#include "SparseBlockMatrix.h"

// std lib
#include <iostream>
#include <iomanip>

// qt lib
#include <QDataStream>
#include <QDebug>
#include <QMapIterator>
#include <QListIterator>

// boost lib
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

// Isis lib
#include "IString.h"
#include "LinearAlgebra.h"

using namespace boost::numeric::ublas;

namespace Isis {

  /**
   * Default constructor.
   */
  SparseBlockColumnMatrix::SparseBlockColumnMatrix() {
    m_startColumn = 0;
  }

  /**
   * Destructor. See description of wipe method below.
   */
  SparseBlockColumnMatrix::~SparseBlockColumnMatrix() {
    wipe();
  }

  /**
   * Deletes all pointer elements and removes them from the map. Effectively, a destructor, and in
   *  fact, called by the ~SparseBlockColumnMatrix above.
   */
  void SparseBlockColumnMatrix::wipe() {
    qDeleteAll(values());
    clear();
  }

  /**
   * Copy constructor. Calls copy method immediately below.
   *
   * @param src SparseBlockColumnMatrix to copy
   */
  SparseBlockColumnMatrix::SparseBlockColumnMatrix(const SparseBlockColumnMatrix& src) {
    copy(src);
  }

  /**
   * Copy method.
   *
   * @param src SparseBlockColumnMatrix to copy
   */
  void SparseBlockColumnMatrix::copy(const SparseBlockColumnMatrix& src) {
    // handi-wipe
    wipe();

    // copy matrix blocks from src
    QMapIterator<int, LinearAlgebra::Matrix *> it(src);
    while ( it.hasNext() ) {
      it.next();

      // copy matrix block from src
      LinearAlgebra::Matrix *m = new LinearAlgebra::Matrix(*(it.value()));

      // insert matrix into map
      this->insert(it.key(),m);
    }

    m_startColumn = src.startColumn();
  }

  /**
   * "Equals" operator.
   *
   * @param src SparseBlockColumnMatrix to check against
   */
  SparseBlockColumnMatrix&
      SparseBlockColumnMatrix::operator=(const SparseBlockColumnMatrix& src) {
    if ( this == &src )
      return *this;

    copy(src);

    return *this;
  }

  /**
   * Inserts a "newed" LinearAlgebra::Matrix pointer of size (nRows, nCols) into the map with the
   *  block column number as key. The clear call initializes the matrix elements to zero. If an
   *  entry exists at the key nColumnBlock, no insertion is made.
   *
   * @param nColumnBlock block column number of inserted matrix (key into map)
   * @param nRows number of rows in matrix to be inserted
   * @param nCols number of columns in matrix to be inserted
   *
   * @return bool Returns true if insertion is successful or if block already exists at nColumnBlock
   *              Returns false if attempt to allocate new block fails
   */
  bool SparseBlockColumnMatrix::insertMatrixBlock(int nColumnBlock, int nRows, int nCols) {
    // check if matrix already exists at the key "nColumnBlock"
    if ( this->contains(nColumnBlock) )
      return true;

    // allocate matrix block with nRows and nCols
    LinearAlgebra::Matrix *m = new LinearAlgebra::Matrix(nRows,nCols);

    if ( !m )
      return false;

    // zero matrix elements
    m->clear();

    // insert matrix into map
    this->insert(nColumnBlock,m);

    return true;
  }

  /**
   * Sets starting column for block in full matrix.
   *
   * @param nStartColumn value for starting column in full matrix for this block columns
   */
  void SparseBlockColumnMatrix::setStartColumn(int nStartColumn) {
    m_startColumn = nStartColumn;
  }

  /**
   * Sets starting column for block in full matrix.
   *
   * @return int returns the starting column in the full matrix
   */
  int SparseBlockColumnMatrix::startColumn() const {
    return m_startColumn;
  }

  /**
   * Returns total number of matrix elements in map (NOTE: NOT the number of matrix blocks). The sum
   *  of all the elements in all of the matrix blocks.
   *
   * @return int Total number of matrix elements in SparseBlockColumnMatrix
   */
  int SparseBlockColumnMatrix::numberOfElements() {
    int nElements = 0;

    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();

      if( !it.value() )
        continue;

      nElements += it.value()->size1()*it.value()->size2();
    }

    return nElements;
  }

  /**
   * Returns total number of columns in map (NOTE: NOT the number of matrix blocks).
   *
   * @return int Total number of columns in SparseBlockColumnMatrix
   */
  int SparseBlockColumnMatrix::numberOfColumns() {

    int nColumns = 0;

    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();

      if( !it.value() )
        continue;

      nColumns = it.value()->size2();
      break;
    }

    return nColumns;
  }

  /**
   * Returns total number of rows in map (this needs to be clarified and maybe rewritten). It's the
   *  number of rows in the block on the diagonal (the last one in the column).
   *
   * @return int Total number of rows in SparseBlockColumnMatrix
   */
  int SparseBlockColumnMatrix::numberOfRows() {

    // iterate to last block (the diagonal one)
    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();

      if( !it.value() )
        continue;
    }

    int nRows = it.value()->size1();

    return nRows;
  }

  /**
   * Prints matrix blocks to std output stream out for debugging. This version makes use of the
   *  Boost matrix library output of matrices as opposed to the printClean method below which
   *  explicitly prints the matrix elements for more control over the format.
   *
   * @param outstream output stream
   */
  void SparseBlockColumnMatrix::print(std::ostream& outstream) {
    if ( size() == 0 ) {
      outstream << "Empty SparseBlockColumnMatrix..." << std::endl;
      return;
    }

    outstream << "Printing SparseBlockColumnMatrix..." << std::endl;
    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();

      if( it.value() )
        outstream << it.key() << std::endl << *(it.value()) << std::endl
                  << std::endl;
      else
        outstream << "NULL block pointer at row[" << IString(it.key())
                  << "]!" << std::endl;
    }
  }

  /**
   * Prints matrix blocks to std output stream out for debugging. Explicitly prints the matrix
   *  elements for more control over the format as opposed to the print method above.
   *
   * @param outstream output stream
   */
  void SparseBlockColumnMatrix::printClean(std::ostream& outstream) {
    if ( size() == 0 ) {
      outstream << "Empty SparseBlockColumnMatrix..." << std::endl;
      return;
    }

    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();

      LinearAlgebra::Matrix * m = it.value();

      int rows = m->size1();
      int cols = m->size2();

      for ( int i = 0; i < rows; i++ ) {
        for ( int j = 0; j < cols; j++ ) {
          double d = m->at_element(i,j);
          if ( j == cols-1 )
            outstream << std::setprecision(12) << d << std::endl;
          else
            outstream << std::setprecision(12) << d << ",";
        }
      }

    }
    outstream << std::endl;
  }

  /**
   * Sets all elements of all matrix blocks to zero.
   */
  void SparseBlockColumnMatrix::zeroBlocks() {
    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();
      it.value()->clear();
    }
  }

  /**
   * Writes matrix to binary disk file pointed to by QDataStream stream
   *
   * @param stream stream pointing to binary disk file
   * @param sbcm SparseBlockColumnMatrix to write
   */
  QDataStream &operator<<(QDataStream &stream, const SparseBlockColumnMatrix &sbcm) {
    // write number of blocks in this column
    int nBlocks = sbcm.size();
    stream << (qint32)nBlocks;

    QMapIterator<int, LinearAlgebra::Matrix *> it(sbcm);
    while ( it.hasNext() ) {
      it.next();

      if( !it.value() )
        continue;

      int nRows = it.value()->size1();
      int nCols = it.value()->size2();

      // write block number (key); rows (size1); and columns (size2)
      stream << it.key() << (qint32)nRows << (qint32)nCols;

      double* data = &it.value()->data()[0];

      // write raw matrix data
      stream.writeRawData((const char*)data, nRows*nCols*sizeof(double));
    }

    return stream;
  }

  /**
   * Reads matrix from binary disk file pointed to by QDataStream stream
   *
   * @param stream stream pointing to binary disk file
   * @param sbcm SparseBlockColumnMatrix to read
   */
  QDataStream &operator>>(QDataStream &stream, SparseBlockColumnMatrix &sbcm) {
    qint32 nBlocks, nBlockNumber, nRows, nCols;
    int i, r, c;

    stream >> nBlocks;

    for ( i = 0; i < nBlocks; i++ ) {
      // read block number (key); rows (size1); and columns (size2)
      stream >> nBlockNumber >> nRows >> nCols;

      double data[nRows*nCols];

      // read raw matrix data
      stream.readRawData((char*)data, nRows*nCols*sizeof(double));

      // insert matrix at correct key
      sbcm.insertMatrixBlock(nBlockNumber, nRows, nCols);

      // get matrix
      LinearAlgebra::Matrix *matrix = sbcm[nBlockNumber];

      // fill with data
      for ( r = 0; r < nRows; r++ ) {
        for ( c = 0; c < nCols; c++ ) {
          int nLocation = r*nRows + c;
          (*matrix)(r,c) = data[nLocation];
        }
      }
    }

    return stream;
  }

  /**
   * Writes matrix to QDebug stream
   *
   * @param dbg debug stream
   * @param sbcm SparseBlockColumnMatrix to write to debug stream
   */
  QDebug operator<<(QDebug dbg, const SparseBlockColumnMatrix &sbcm) {
    dbg.space() << "New Block" << endl;

    QMapIterator<int, LinearAlgebra::Matrix *> it(sbcm);
    while ( it.hasNext() ) {
      it.next();

      if( !it.value() )
        continue;

      // get matrix
      LinearAlgebra::Matrix *matrix = it.value();

      // matrix rows, columns
      int nRows = matrix->size1();
      int nCols = matrix->size2();

      dbg.nospace() << qSetFieldWidth(4);
      dbg.nospace() << qSetRealNumberPrecision(8);

      for ( int r = 0; r < nRows; r++ ) {
        for ( int c = 0; c < nCols; c++ ) {
          dbg.space() << (*matrix)(r,c);
        }
        dbg.space() << endl;
      }

      dbg.space() << endl;
    }

    return dbg;
  }

  //////////////////////////////////////////////////////////////////////////////
  // SparseBlockRowMatrix methods

  /**
   * Destructor. See description of wipe method below.
   */
  SparseBlockRowMatrix::~SparseBlockRowMatrix() {
    wipe();
  }

  /**
   * Deletes all pointer elements and removes them from the map. Effectively, a destructor, and in
   * fact, called by the ~SparseBlockColumnMatrix above.
   */
  void SparseBlockRowMatrix::wipe() {
    qDeleteAll(values());
    clear();
  }

  /**
   * Copy constructor. Calls copy method immediately below.
   *
   * @param src SparseBlockRowMatrix to copy
   */
  SparseBlockRowMatrix::SparseBlockRowMatrix(const SparseBlockRowMatrix& src) {
    copy(src);
  }

  /**
   * Copy method.
   *
   * @param src SparseBlockRowMatrix to copy
   */
  void SparseBlockRowMatrix::copy(const SparseBlockRowMatrix& src) {
    // handi-wipe
    wipe();

    // copy matrix blocks from src
    QMapIterator<int, LinearAlgebra::Matrix *> it(src);
    while ( it.hasNext() ) {
      it.next();

      // copy matrix block from src
      LinearAlgebra::Matrix *m = new LinearAlgebra::Matrix(*(it.value()));

      // insert matrix into map
      this->insert(it.key(),m);
    }
  }

  /**
   * "Equals" operator.
   *
   * @param src SparseBlockRowMatrix to check against
   */
  SparseBlockRowMatrix&
      SparseBlockRowMatrix::operator=(const SparseBlockRowMatrix& src) {
    if ( this == &src )
      return *this;

    copy(src);

    return *this;
  }

  /**
   * Inserts a "newed" LinearAlgebra::Matrix pointer of size (nRows, nCols) into the map with the
   * block row number as key. The matrix::clear call initializes the matrix elements to zero. If an
   * entry exists at the key nRowBlock, no insertion is made.
   *
   * @param nRowBlock block row number of inserted matrix (key into map)
   * @param nRows number of rows in matrix to be inserted
   * @param nCols number of columns in matrix to be inserted
   *
   * @return bool Returns true if insertion successful
   *              Returns false if block already exists at nRowBlock or if allocation of new block
   *              fails
   *              TODO: we return true in the SparseBlockColumnMatrix if the block already exists,
   *                    why is it different here?
   */
  bool SparseBlockRowMatrix::insertMatrixBlock(int nRowBlock, int nRows, int nCols) {
    if ( this->contains(nRowBlock) )
      return false;

    LinearAlgebra::Matrix *m = new LinearAlgebra::Matrix(nRows,nCols);

    if ( !m )
      return false;

    m->clear();

    this->insert(nRowBlock,m);

    return true;
  }

  /**
   * Returns total number of matrix elements in map (NOTE: NOT the number of
   * matrix blocks). The sum of all the elements of all the matrix blocks.
   *
   * @return int Total number of matrix elements in SparseBlockRowMatrix
   */
  int SparseBlockRowMatrix::numberOfElements() {
    int nElements = 0;

    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();

      if( !it.value() )
        continue;

      nElements += it.value()->size1()*it.value()->size2();
    }

    return nElements;
  }

  /**
   * Prints matrix blocks to std output stream out for debugging.
   *
   * @param outstream output stream
   */
  void SparseBlockRowMatrix::print(std::ostream& outstream) {
    if ( size() == 0 ) {
      outstream << "Empty SparseBlockRowMatrix..." << std::endl;
      return;
    }

    outstream << "Printing SparseBlockRowMatrix..." << std::endl;
    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();

      if( it.value() )
        outstream << it.key() << std::endl << *(it.value()) << std::endl
                  << std::endl;
      else
        outstream << "NULL block pointer at column[" << IString(it.key())
                  << "]!" << std::endl;
    }
  }

  /**
   * Prints matrix blocks to std output stream out for debugging.
   *
   * @param outstream output stream
   */
  void SparseBlockRowMatrix::printClean(std::ostream& outstream) {
    if ( size() == 0 ) {
      outstream << "Empty SparseBlockRowMatrix..." << std::endl;
      return;
    }

    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();

      LinearAlgebra::Matrix *m = it.value();

      int rows = m->size1();
      int cols = m->size2();

      for ( int i = 0; i < rows; i++ ) {
        for ( int j = 0; j < cols; j++ ) {
          double d = m->at_element(i,j);
          if ( j == cols-1 )
            outstream << std::setprecision(9) << d << std::endl;
          else
            outstream << std::setprecision(9) << d << ",";
        }
      }

    }
    outstream << std::endl;
  }

  /**
   * Sets all elements of all matrix blocks to zero.
   */
  void SparseBlockRowMatrix::zeroBlocks() {
    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();
      it.value()->clear();
    }
  }

  /**
   * Copies a SparseBlockRowMatrix to a Boost compressed_matrix. This may be a temporary
   *  implementation
   *
   * @param B Boost matrix to copy this SparseBlockRowMatrix to
   */
  void SparseBlockRowMatrix::copyToBoost(compressed_matrix<double>& B) {
    B.clear();

    int ncols, nstart, nend, nrowBlock;
    range rRow = range(0,3);
    range rCol;

    QMapIterator<int, LinearAlgebra::Matrix *> it(*this);
    while ( it.hasNext() ) {
      it.next();

      nrowBlock = it.key();
      LinearAlgebra::Matrix *m = it.value();

      ncols = m->size2();

      nstart = nrowBlock*ncols;
      nend = nstart + ncols;

      rCol = range(nstart,nend);

      matrix_range<compressed_matrix<double> > m1 (B, rRow, rCol);

      m1 = *m;
    }
  }

  /**
   * Sums and returns the number of columns in each matrix block prior to nblockColumn
   *
   * @param nblockColumn
   *
   * @return int Number of leading columns for block at nblockColumn
   */
    int SparseBlockRowMatrix::getLeadingColumnsForBlock(int nblockColumn) {

      if ( nblockColumn == 0 )
        return 0;

      int nLeadingColumnsElements = 0;

      int nCol = 0;

      while ( nCol < nblockColumn ) {
        if ( !(*this)[nCol] ) {
          nCol++;
          continue;
        }

        int ncolumns = (*this)[nCol]->size2();

        if ( ncolumns == -1 )
          continue;

        nLeadingColumnsElements += ncolumns;

        nCol++;
      }

      return nLeadingColumnsElements;
    }

  /**
   * Writes matrix to binary disk file pointed to by QDataStream stream
   *
   * @param stream stream pointing to binary disk file
   * @param sbrm SparseBlockRowMatrix to write
   */
  QDataStream &operator<<(QDataStream &stream, const SparseBlockRowMatrix &sbrm) {
    // write number of blocks in this column
    int nBlocks = sbrm.size();
    stream << (qint32)nBlocks;

    QMapIterator<int, LinearAlgebra::Matrix *> it(sbrm);
    while ( it.hasNext() ) {
      it.next();

      if( !it.value() )
        continue;

      int nRows = it.value()->size1();
      int nCols = it.value()->size2();

      // write block number (key); rows (size1); and columns (size2)
      stream << it.key() << (qint32)nRows << (qint32)nCols;

      double* data = &it.value()->data()[0];

      // write raw matrix data
      stream.writeRawData((const char*)data, nRows*nCols*sizeof(double));
    }

    return stream;
  }

  /**
   * Reads matrix from binary disk file pointed to by QDataStream stream
   *
   * @param stream stream pointing to binary disk file
   * @param sbcm SparseBlockColumnMatrix to read
   */
  QDataStream &operator>>(QDataStream &stream, SparseBlockRowMatrix &sbrm) {
    qint32 nBlocks, nBlockNumber, nRows, nCols;
    int i, r, c;

    stream >> nBlocks;

    for ( i = 0; i < nBlocks; i++ ) {
      // read block number (key); rows (size1); and columns (size2)
      stream >> nBlockNumber >> nRows >> nCols;

      double data[nRows*nCols];

      // read raw matrix data
      stream.readRawData((char*)data, nRows*nCols*sizeof(double));

      // insert matrix at correct key
      sbrm.insertMatrixBlock(nBlockNumber, nRows, nCols);

      // get matrix
      LinearAlgebra::Matrix *matrix = sbrm[nBlockNumber];

      // fill with data
      for ( r = 0; r < nRows; r++ ) {
        for ( c = 0; c < nCols; c++ ) {
          int nLocation = r*nRows + c;
          (*matrix)(r,c) = data[nLocation];
        }
      }
    }

    return stream;
  }

  /**
   * Writes matrix to QDebug stream
   *
   * @param dbt debug stream
   * @param sbcm SparseBlockRowMatrix to write to debug stream
   */
  QDebug operator<<(QDebug dbg, const SparseBlockRowMatrix &sbrm) {
    dbg.space() << "New Block" << endl;

    QMapIterator<int, LinearAlgebra::Matrix *> it(sbrm);
    while ( it.hasNext() ) {
      it.next();

      if( !it.value() )
        continue;

      // get matrix
      LinearAlgebra::Matrix *matrix = it.value();

      // matrix rows, columns
      int nRows = matrix->size1();
      int nCols = matrix->size2();

      dbg.nospace() << qSetFieldWidth(4);
      dbg.nospace() << qSetRealNumberPrecision(8);

      for ( int r = 0; r < nRows; r++ ) {
        for ( int c = 0; c < nCols; c++ ) {
          dbg.space() << (*matrix)(r,c);
        }
        dbg.space() << endl;
      }
      dbg.space() << endl;
    }

    return dbg;
  }

  //////////////////////////////////////////////////////////////////////////////
  // SparseBlockMatrix methods

  /**
   * Destructor.
   */
  SparseBlockMatrix::~SparseBlockMatrix() {
    wipe();
  }

  /**
   * Deletes all pointer elements and removes them from the list. Effectively, a destructor, and in
   *  fact, called by the ~SparseBlockMatrix above.
   */
  void SparseBlockMatrix::wipe() {
    qDeleteAll(*this);
    clear();
  }

  /**
   * Copy constructor. Calls copy method immediately below.
   *
   * @param src SparseBlockMatrix to copy
   */
  SparseBlockMatrix::SparseBlockMatrix(const SparseBlockMatrix& src) {
    copy(src);
  }

  /**
   * Copy method.
   *
   * @param src SparseBlockMatrix to copy
   */
  void SparseBlockMatrix::copy(const SparseBlockMatrix& src) {
    // handi-wipe
    wipe();

    // copy all SparseBlockColumnMatrix objects from src
    for( int i = 0; i < src.size(); i++ ) {
      append( new SparseBlockColumnMatrix(*(src.at(i))));
    }
  }

  /**
   * "Equals" operator.
   *
   * @param src SparseBlockMatrix to check against
   */
  SparseBlockMatrix& SparseBlockMatrix::operator=(const SparseBlockMatrix& src) {
    if ( this == &src )
      return *this;

    copy(src);

    return *this;
  }

  /**
   * Initializes number of columns (SparseBlockColumnMatrix).
   *
   * @param n number of columns to insert
   *
   * @return bool Always returns true
   *              TODO: why bother returning bool? should there be a false condition?
   */
  bool SparseBlockMatrix::setNumberOfColumns( int n ) {

    for( int i = 0; i < n; i++ )
      append( new SparseBlockColumnMatrix() );

    return true;
  }

  /**
   * Inserts a "newed" boost LinearAlgebra::Matrix pointer of size (nRows,
   * nCols) into the matrix at nColumnBlock, nRowBlock. The inserted matrix
   * elements are initialized to zero.
   * If an entry exists at nColumnBlock, RowBlock, no insertion is made.
   *
   * @param nColumnBlock block column number of inserted matrix (QList index)
   * @param nRowBlock block row number of inserted matrix (key into map)
   * @param nRows number of rows in matrix to be inserted
   * @param nCols number of columns in matrix to be inserted
   *
   * @return bool Returns result of SparseBlockColumnMatrix::insertMatrixBlock()
   */
  bool SparseBlockMatrix::insertMatrixBlock(int nColumnBlock, int nRowBlock, int nRows, int nCols) {
    return (*this)[nColumnBlock]->insertMatrixBlock(nRowBlock, nRows, nCols);
  }

  /**
   * Returns total number of blocks in matrix.
   *
   * @return int Total number of blocks in matrix
   */
  int SparseBlockMatrix::numberOfBlocks() {
    int nBlocks = 0;

    for( int i = 0; i < size(); i++ ) {
      if ( !(*this)[i] )
        continue;

      nBlocks += (*this)[i]->size();
    }

    return nBlocks;
  }

  /**
   * Returns number of diagonal matrix blocks (equivalent to size - there is one per column).
   *
   * @return int Number of diagnonal blocks in matrix
   */
  int SparseBlockMatrix::numberOfDiagonalBlocks() {
    int ndiagBlocks = 0;

    for( int i = 0; i < size(); i++ ) {
      SparseBlockColumnMatrix* column = at(i);

      if ( !column )
        continue;

      QMapIterator<int, LinearAlgebra::Matrix *> it(*column);
      while ( it.hasNext() ) {
        it.next();

        if( it.key() == i ) {
          ndiagBlocks++;
          break;
        }
      }
    }

    return ndiagBlocks;
  }

  /**
   * Returns number of off-diagonal matrix blocks.
   *
   * @return int Number of off-diagonal blocks in matrix
   */
  int SparseBlockMatrix::numberOfOffDiagonalBlocks() {
    return (numberOfBlocks() - numberOfDiagonalBlocks());
  }

  /**
   * Returns number of matrix elements in matrix.
   *
   * @return int Total number of matrix elements
   */
  int SparseBlockMatrix::numberOfElements() {
    int nElements = 0;

    for( int i = 0; i < size(); i++ ) {
      if ( !(*this)[i] )
        continue;

      nElements += (*this)[i]->numberOfElements();
    }

    return nElements;
  }

  /**
   * Returns pointer to boost matrix at position (column, row).
   *
   * @param column block column number
   * @param row block row number
   *
   * @return LinearAlgebra::Matrix  Pointer to Boost matrix at position (column,
   *         row)
   */
  LinearAlgebra::Matrix *SparseBlockMatrix::getBlock(int column, int row) {
    return (*(*this)[column])[row];
  }

  /**
   * Sets all elements of all matrix blocks to zero.
   */
  void SparseBlockMatrix::zeroBlocks() {
    for ( int i = 0; i < size(); i++ )
      (*this)[i]->zeroBlocks();
  }

  /**
   * Prints matrix blocks to std output stream out for debugging.
   *
   * @param outstream output stream
   */
  void SparseBlockMatrix::print(std::ostream& outstream) {
    if ( size() == 0 ) {
      outstream << "Empty SparseBlockMatrix..." << std::endl;
      return;
    }

    outstream << "Printing SparseBlockMatrix..." << std::endl;
    for( int i = 0; i < size(); i++ ) {
      SparseBlockColumnMatrix* column = at(i);

      if ( column )
        column->print(outstream);
      else
        outstream << "NULL column pointer at column[" << IString(i)
                  << "]!" << std::endl;
    }
  }

  /**
   * Prints matrix blocks to std output stream out for debugging.
   *
   * @param outstream output stream
   */
  void SparseBlockMatrix::printClean(std::ostream& outstream) {
    if ( size() == 0 ) {
      outstream << "Empty SparseBlockMatrix..." << std::endl;
      return;
    }

    for( int i = 0; i < size(); i++ ) {
      SparseBlockColumnMatrix* column = at(i);

      if ( column )
        column->printClean(outstream);
      else
        outstream << "NULL column pointer at column[" << IString(i)
                  << "]!" << std::endl;
    }
  }

  /**
   * Sums and returns the number of columns in each matrix block prior to nblockColumn
   *
   * @param nblockColumn
   *
   * @return int Number of leading column elements for block at nblockColumn
   */
  int SparseBlockMatrix::getLeadingColumnsForBlock(int nblockColumn) {

    if ( nblockColumn == 0 )
      return 0;

    int nLeadingColumnsElements = 0;

    int nCol = 0;

    while ( nCol < nblockColumn ) {
      if ( !(*this)[nCol] )
        continue;

      int ncolumns = (*this)[nCol]->numberOfColumns();

      if ( ncolumns == -1 )
        continue;

      nLeadingColumnsElements += ncolumns;

      nCol++;
    }

    return nLeadingColumnsElements;
  }

  /**
   * Sums and returns the number of rows in each matrix block prior to nblockRow
   *
   * @param nblockRow
   *
   * @return int Number of leading row elements for block at nblockRow
   */
  int SparseBlockMatrix::getLeadingRowsForBlock(int nblockRow) {

    if ( nblockRow == 0 )
      return 0;

    int i = 0;
    int nLeadingRows = 0;

    while ( i < nblockRow ) {
      SparseBlockColumnMatrix* column = at(i);

      if ( !column )
        continue;

      QMapIterator<int, LinearAlgebra::Matrix *> it(*column);
      // iterate to last element in column
      while ( it.hasNext() ) {
        it.next();

        if( it.key() == i )
          nLeadingRows += it.value()->size1();
      }
      i++;
    }

    return nLeadingRows;
  }

  /**
   * Writes matrix to binary disk file pointed to by QDataStream stream
   *
   * @param stream stream pointing to binary disk file
   * @param sparseBlockMatrix SparseBlockMatrix to write
   */
  QDataStream &operator<<(QDataStream &stream, const SparseBlockMatrix &sparseBlockMatrix) {
    int nBlockColumns = sparseBlockMatrix.size();

    stream << (qint32)nBlockColumns;

    for ( int i =0; i < nBlockColumns; i++ )
      stream << *sparseBlockMatrix.at(i);

    return stream;
  }

  /**
   * Reads matrix from binary disk file pointed to by QDataStream stream
   *
   * @param stream stream pointing to binary disk file
   * @param sparseBlockMatrix SparseBlockMatrix to read
   */
  QDataStream &operator>>(QDataStream &stream, SparseBlockMatrix &sparseBlockMatrix) {
    qint32 nBlockColumns;

    // read and set number of block columns
    stream >> nBlockColumns;
    sparseBlockMatrix.setNumberOfColumns(nBlockColumns);

    for ( int i =0; i < nBlockColumns; i++ )
      stream >> *sparseBlockMatrix.at(i);

    return stream;
  }

  /**
   * Writes matrix to QDebug stream
   *
   * @param dbg debug stream
   * @param m SparseBlockMatrix to write to debug stream
   */
  QDebug operator<<(QDebug dbg, const SparseBlockMatrix &m) {
    int nBlockColumns = m.size();

    for ( int i =0; i < nBlockColumns; i++ )
      dbg << *m.at(i);

    return dbg;
  }
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_UNARY_PLUS_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_UNARY_PLUS_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief unary_plus generic tag

     Represents the unary_plus function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct unary_plus_ : ext::elementwise_<unary_plus_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<unary_plus_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_unary_plus_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::unary_plus_, Site> dispatching_unary_plus_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::unary_plus_, Site>();
   }
   template<class... Args>
   struct impl_unary_plus_;
  }
  /*!
    return the elementwise unary plus of the parameter
    Infix notation can be used with operator '+'
    This is in fact identity.

    @par Semantic:

    For every parameter of type T0

    @code
    T0 r = unary_plus(a0);
    @endcode

    is similar to:

    @code
    T0 r = +a0;
    @endcode

    @par Alias:
    @c identity,  @c id

    @see  @funcref{plus}, @funcref{unary_minus}
    @param a0

    @return a value of the same type as the parameter
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::unary_plus_ , unary_plus  , 1 )
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::unary_plus_ , identity    , 1 )
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::unary_plus_ , id          , 1 )
} }

#include <boost/simd/operator/specific/common.hpp>

#endif


#pragma once

#include "cho_util/core/geometry/geometry_base.hpp"

#include <Eigen/Core>
#include <initializer_list>

namespace cho {
namespace core {

template <typename _Scalar, int N>
class Cuboid : public GeometryBase<Cuboid<_Scalar, N>> {
  using Scalar = _Scalar;

 public:
  explicit constexpr Cuboid() {}
  Cuboid(std::initializer_list<Scalar> args) {
    std::copy(args.begin(), args.end(), data_.data());
  }

  const Scalar* GetPtr() const { return data_.data(); }
  Scalar* GetPtr() { return data_.data(); }
  static constexpr int GetSize() { return N * 2; }

  auto& GetData() { return data_; }
  const auto& GetData() const { return data_; }

  auto GetMin() { return data_.col(0); }
  auto GetMax() { return data_.col(1); }
  auto GetMin() const { return data_.col(0); }
  auto GetMax() const { return data_.col(1); }

  template <typename Derived>
  auto SetMin(const Eigen::DenseBase<Derived>& d) {
    data_.col(0) = d;
  }
  template <typename Derived>
  auto SetMax(const Eigen::DenseBase<Derived>& d) {
    data_.col(1) = d;
  }

 private:
  // [Center, Radius]
  Eigen::Matrix<Scalar, N, 2> data_;

 public:
  friend class GeometryBase<Cuboid>;
  // EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <typename _Scalar, int N>
struct GeometryTraits<Cuboid<_Scalar, N>> {
  using Scalar = _Scalar;
};

}  // namespace core
}  // namespace cho


#ifndef ORIENTATION_TRAJECTORY_H
#define ORIENTATION_TRAJECTORY_H

#include <Eigen/Dense>
#include <iostream>

class TrajOrientation{
public:
	TrajOrientation();
	TrajOrientation(const int & N_size_in, const double & dt_in);
	virtual ~TrajOrientation();

	// Getter functions
	double get_dt();
	int get_trajectory_length();

	virtual void get_quat(const int & index_in, Eigen::Quaterniond & quat_out);
	virtual void get_ang_vel(const int & index_in, Eigen::Vector3d & ang_vel_out);
	virtual void get_ang_acc(const int & index_in, Eigen::Vector3d & ang_acc_out);

	// All of the get_next functions increment the internal index counter
	virtual void get_next_quat(Eigen::Quaterniond & quat_out);
	virtual void get_next_ang_vel(Eigen::Vector3d & ang_vel_out);
	virtual void get_next_ang_acc(Eigen::Vector3d & ang_acc_out);

	virtual void get_next_data(Eigen::Quaterniond & quat_out);
	virtual void get_next_data(Eigen::Quaterniond & quat_out, Eigen::Vector3d & ang_vel_out);
	virtual void get_next_data(Eigen::Quaterniond & quat_out, Eigen::Vector3d & ang_vel_out, Eigen::Vector3d & ang_acc_out);

	// Setter functions
	void set_N_dt(const int & N_size_in, const double & dt_in);
	void set_dt(const double & dt_in);
	void set_quat(const int & index, const Eigen::Quaterniond & quat_in);
	void set_ang_vel(const int & index, const Eigen::Vector3d & ang_vel_in);
	void set_ang_acc(const int & index, const Eigen::Vector3d & ang_acc_in);

	// resets the internal index counter
	void reset_index();

protected:
	std::vector<Eigen::Quaterniond> quat;
	std::vector<Eigen::Vector3d> ang_vel;
	std::vector<Eigen::Vector3d> ang_acc;

	int N_size;
	double dt;
	int index = 0;

	void increment_index();
};

#endif


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \file tests/src/vector_multi_inner_prod.cpp  Tests the performance of multiple inner products with a common vector.
*   \test   Tests the performance of multiple inner products with a common vector.
**/

//
// *** System
//
#include <iostream>
#include <iomanip>

//
// *** Boost
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

//
// *** ViennaCL
//
//#define VIENNACL_DEBUG_ALL
#define VIENNACL_WITH_UBLAS 1
#include "viennacl/vector.hpp"
#include "viennacl/vector_proxy.hpp"
#include "viennacl/linalg/inner_prod.hpp"
#include "viennacl/linalg/norm_1.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/norm_inf.hpp"

#include "Random.hpp"

using namespace boost::numeric;

//
// -------------------------------------------------------------
//
template<typename ScalarType>
ScalarType diff(ScalarType const & s1, ScalarType const & s2)
{
   viennacl::backend::finish();
   if (s1 != s2)
      return (s1 - s2) / std::max(std::fabs(s1), std::fabs(s2));
   return 0;
}
//
// -------------------------------------------------------------
//
template<typename ScalarType>
ScalarType diff(ScalarType const & s1, viennacl::scalar<ScalarType> const & s2)
{
   viennacl::backend::finish();
   if (s1 != s2)
      return (s1 - s2) / std::max(std::fabs(s1), std::fabs(s2));
   return 0;
}
//
// -------------------------------------------------------------
//
template<typename ScalarType>
ScalarType diff(ScalarType const & s1, viennacl::entry_proxy<ScalarType> const & s2)
{
   viennacl::backend::finish();
   if (s1 != s2)
      return (s1 - s2) / std::max(std::fabs(s1), std::fabs(s2));
   return 0;
}
//
// -------------------------------------------------------------
//
template<typename ScalarType, typename ViennaCLVectorType>
ScalarType diff(ublas::vector<ScalarType> const & v1, ViennaCLVectorType const & vcl_vec)
{
   ublas::vector<ScalarType> v2_cpu(vcl_vec.size());
   viennacl::backend::finish();
   viennacl::copy(vcl_vec, v2_cpu);

   for (unsigned int i=0;i<v1.size(); ++i)
   {
      if ( std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) ) > 0 )
         v2_cpu[i] = std::fabs(v2_cpu[i] - v1[i]) / std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) );
      else
         v2_cpu[i] = 0.0;
   }

   return ublas::norm_inf(v2_cpu);
}

template<typename ScalarType, typename ViennaCLVectorType>
ScalarType diff(ublas::vector_slice<ublas::vector<ScalarType> > const & v1, ViennaCLVectorType const & vcl_vec)
{
   ublas::vector<ScalarType> v2_cpu(vcl_vec.size());
   viennacl::backend::finish();
   viennacl::copy(vcl_vec, v2_cpu);

   for (unsigned int i=0;i<v1.size(); ++i)
   {
      if ( std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) ) > 0 )
         v2_cpu[i] = std::fabs(v2_cpu[i] - v1[i]) / std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) );
      else
         v2_cpu[i] = 0.0;
   }

   return ublas::norm_inf(v2_cpu);
}

template<typename T1, typename T2>
int check(T1 const & t1, T2 const & t2, double epsilon)
{
  int retval = EXIT_SUCCESS;

  double temp = std::fabs(diff(t1, t2));
  if (temp > epsilon)
  {
    std::cout << "# Error! Relative difference: " << temp << std::endl;
    retval = EXIT_FAILURE;
  }
  return retval;
}

//
// -------------------------------------------------------------
//
template< typename NumericT, typename Epsilon,
          typename UblasVectorType1,    typename UblasVectorType2,    typename UblasVectorType3,    typename UblasVectorType4,
          typename ViennaCLVectorType1, typename ViennaCLVectorType2, typename ViennaCLVectorType3, typename ViennaCLVectorType4 >
int test(Epsilon const& epsilon,
         UblasVectorType1    & ublas_v1, UblasVectorType2    & ublas_v2, UblasVectorType3    & ublas_v3, UblasVectorType4    & ublas_v4,
         ViennaCLVectorType1 &   vcl_v1, ViennaCLVectorType2 &   vcl_v2, ViennaCLVectorType3 &   vcl_v3, ViennaCLVectorType4 &   vcl_v4)
{
  int retval = EXIT_SUCCESS;

  for (std::size_t i=0; i<ublas_v1.size(); ++i)
  {
    ublas_v1[i] = NumericT(1.0) + random<NumericT>();
    ublas_v2[i] = NumericT(1.0) + random<NumericT>();
    ublas_v3[i] = NumericT(1.0) + random<NumericT>();
    ublas_v4[i] = NumericT(1.0) + random<NumericT>();
  }

  viennacl::copy(ublas_v1.begin(), ublas_v1.end(), vcl_v1.begin());  //resync
  viennacl::copy(ublas_v2.begin(), ublas_v2.end(), vcl_v2.begin());
  viennacl::copy(ublas_v3.begin(), ublas_v3.end(), vcl_v3.begin());
  viennacl::copy(ublas_v4.begin(), ublas_v4.end(), vcl_v4.begin());

  std::cout << "Checking for successful copy..." << std::endl;
  if (check(ublas_v1, vcl_v1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  if (check(ublas_v2, vcl_v2, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  if (check(ublas_v3, vcl_v3, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  if (check(ublas_v4, vcl_v4, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;

  ublas::vector<NumericT> ref_result = ublas::scalar_vector<NumericT>(40, 0.0);
  viennacl::vector<NumericT> result = viennacl::scalar_vector<NumericT>(40, 0.0);

  std::cout << "Testing inner_prod with two vectors..." << std::endl;
  ref_result(2) = ublas::inner_prod(ublas_v1, ublas_v1);
  ref_result(5) = ublas::inner_prod(ublas_v1, ublas_v2);
  viennacl::project(result, viennacl::slice(2, 3, 2)) = viennacl::linalg::inner_prod(vcl_v1, viennacl::tie(vcl_v1, vcl_v2));
  if (check(ref_result, result, epsilon) != EXIT_SUCCESS)
  {
    std::cout << ref_result << std::endl;
    std::cout << result << std::endl;
    return EXIT_FAILURE;
  }

  ref_result(3) = ublas::inner_prod(ublas_v1, ublas_v3);
  ref_result(7) = ublas::inner_prod(ublas_v1, ublas_v4);
  viennacl::project(result, viennacl::slice(3, 4, 2)) = viennacl::linalg::inner_prod(vcl_v1, viennacl::tie(vcl_v3, vcl_v4));
  if (check(ref_result, result, epsilon) != EXIT_SUCCESS)
  {
    std::cout << ref_result << std::endl;
    std::cout << result << std::endl;
    return EXIT_FAILURE;
  }

  std::cout << "Testing inner_prod with three vectors..." << std::endl;
  ref_result(1) = ublas::inner_prod(ublas_v1, ublas_v1);
  ref_result(3) = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(5) = ublas::inner_prod(ublas_v1, ublas_v3);
  viennacl::project(result, viennacl::slice(1, 2, 3)) = viennacl::linalg::inner_prod(vcl_v1, viennacl::tie(vcl_v1, vcl_v2, vcl_v3));
  if (check(ref_result, result, epsilon) != EXIT_SUCCESS)
  {
    std::cout << ref_result << std::endl;
    std::cout << result << std::endl;
    return EXIT_FAILURE;
  }

  ref_result(2)  = ublas::inner_prod(ublas_v1, ublas_v3);
  ref_result(6)  = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(10) = ublas::inner_prod(ublas_v1, ublas_v4);
  viennacl::project(result, viennacl::slice(2, 4, 3)) = viennacl::linalg::inner_prod(vcl_v1, viennacl::tie(vcl_v3, vcl_v2, vcl_v4));
  if (check(ref_result, result, epsilon) != EXIT_SUCCESS)
  {
    std::cout << ref_result << std::endl;
    std::cout << result << std::endl;
    return EXIT_FAILURE;
  }

  std::cout << "Testing inner_prod with four vectors..." << std::endl;
  ref_result(4) = ublas::inner_prod(ublas_v1, ublas_v1);
  ref_result(5) = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(6) = ublas::inner_prod(ublas_v1, ublas_v3);
  ref_result(7) = ublas::inner_prod(ublas_v1, ublas_v4);
  viennacl::project(result, viennacl::slice(4, 1, 4)) = viennacl::linalg::inner_prod(vcl_v1, viennacl::tie(vcl_v1, vcl_v2, vcl_v3, vcl_v4));
  if (check(ref_result, result, epsilon) != EXIT_SUCCESS)
  {
    std::cout << ref_result << std::endl;
    std::cout << result << std::endl;
    return EXIT_FAILURE;
  }

  ref_result(3)  = ublas::inner_prod(ublas_v1, ublas_v3);
  ref_result(6)  = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(9)  = ublas::inner_prod(ublas_v1, ublas_v4);
  ref_result(12) = ublas::inner_prod(ublas_v1, ublas_v1);
  viennacl::project(result, viennacl::slice(3, 3, 4)) = viennacl::linalg::inner_prod(vcl_v1, viennacl::tie(vcl_v3, vcl_v2, vcl_v4, vcl_v1));
  if (check(ref_result, result, epsilon) != EXIT_SUCCESS)
  {
    std::cout << ref_result << std::endl;
    std::cout << result << std::endl;
    return EXIT_FAILURE;
  }

  std::cout << "Testing inner_prod with five vectors..." << std::endl;
  ref_result(1) = ublas::inner_prod(ublas_v1, ublas_v1);
  ref_result(3) = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(5) = ublas::inner_prod(ublas_v1, ublas_v3);
  ref_result(7) = ublas::inner_prod(ublas_v1, ublas_v4);
  ref_result(9) = ublas::inner_prod(ublas_v1, ublas_v2);
  viennacl::project(result, viennacl::slice(1, 2, 5)) = viennacl::linalg::inner_prod(vcl_v1, viennacl::tie(vcl_v1, vcl_v2, vcl_v3, vcl_v4, vcl_v2));
  if (check(ref_result, result, epsilon) != EXIT_SUCCESS)
  {
    std::cout << ref_result << std::endl;
    std::cout << result << std::endl;
    return EXIT_FAILURE;
  }

  ref_result(2)  = ublas::inner_prod(ublas_v1, ublas_v3);
  ref_result(4)  = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(6)  = ublas::inner_prod(ublas_v1, ublas_v4);
  ref_result(8)  = ublas::inner_prod(ublas_v1, ublas_v1);
  ref_result(10) = ublas::inner_prod(ublas_v1, ublas_v2);
  viennacl::project(result, viennacl::slice(2, 2, 5)) = viennacl::linalg::inner_prod(vcl_v1, viennacl::tie(vcl_v3, vcl_v2, vcl_v4, vcl_v1, vcl_v2));
  if (check(ref_result, result, epsilon) != EXIT_SUCCESS)
  {
    std::cout << ref_result << std::endl;
    std::cout << result << std::endl;
    return EXIT_FAILURE;
  }

  std::cout << "Testing inner_prod with eight vectors..." << std::endl;
  ref_result(1)  = ublas::inner_prod(ublas_v1, ublas_v1);
  ref_result(5)  = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(9)  = ublas::inner_prod(ublas_v1, ublas_v3);
  ref_result(13) = ublas::inner_prod(ublas_v1, ublas_v4);
  ref_result(17) = ublas::inner_prod(ublas_v1, ublas_v3);
  ref_result(21) = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(25) = ublas::inner_prod(ublas_v1, ublas_v1);
  ref_result(29) = ublas::inner_prod(ublas_v1, ublas_v2);
  std::vector<viennacl::vector_base<NumericT> const *> vecs1(8);
  vecs1[0] = &vcl_v1;
  vecs1[1] = &vcl_v2;
  vecs1[2] = &vcl_v3;
  vecs1[3] = &vcl_v4;
  vecs1[4] = &vcl_v3;
  vecs1[5] = &vcl_v2;
  vecs1[6] = &vcl_v1;
  vecs1[7] = &vcl_v2;
  viennacl::vector_tuple<NumericT> tuple1(vecs1);
  viennacl::project(result, viennacl::slice(1, 4, 8)) = viennacl::linalg::inner_prod(vcl_v1, tuple1);
  if (check(ref_result, result, epsilon) != EXIT_SUCCESS)
  {
    std::cout << ref_result << std::endl;
    std::cout << result << std::endl;
    return EXIT_FAILURE;
  }

  ref_result(3)  = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(5)  = ublas::inner_prod(ublas_v1, ublas_v4);
  ref_result(7)  = ublas::inner_prod(ublas_v1, ublas_v1);
  ref_result(9)  = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(11) = ublas::inner_prod(ublas_v1, ublas_v2);
  ref_result(13) = ublas::inner_prod(ublas_v1, ublas_v1);
  ref_result(15) = ublas::inner_prod(ublas_v1, ublas_v4);
  ref_result(17) = ublas::inner_prod(ublas_v1, ublas_v2);
  std::vector<viennacl::vector_base<NumericT> const *> vecs2(8);
  vecs2[0] = &vcl_v2;
  vecs2[1] = &vcl_v4;
  vecs2[2] = &vcl_v1;
  vecs2[3] = &vcl_v2;
  vecs2[4] = &vcl_v2;
  vecs2[5] = &vcl_v1;
  vecs2[6] = &vcl_v4;
  vecs2[7] = &vcl_v2;
  viennacl::vector_tuple<NumericT> tuple2(vecs2);
  viennacl::project(result, viennacl::slice(3, 2, 8)) = viennacl::linalg::inner_prod(vcl_v1, tuple2);
  if (check(ref_result, result, epsilon) != EXIT_SUCCESS)
  {
    std::cout << ref_result << std::endl;
    std::cout << result << std::endl;
    return EXIT_FAILURE;
  }

  // --------------------------------------------------------------------------
  return retval;
}

template< typename NumericT, typename Epsilon >
int test(Epsilon const& epsilon)
{
  int retval = EXIT_SUCCESS;
  std::size_t size = 8 * 1337;

  std::cout << "Running tests for vector of size " << size << std::endl;

  //
  // Set up UBLAS objects
  //
  ublas::vector<NumericT> ublas_full_vec1(size);
  ublas::vector<NumericT> ublas_full_vec2(ublas_full_vec1.size());

  for (std::size_t i=0; i<ublas_full_vec1.size(); ++i)
  {
    ublas_full_vec1[i]  = NumericT(1.0) + random<NumericT>();
    ublas_full_vec2[i] = NumericT(1.0) + random<NumericT>();
  }

  ublas::slice s1(    ublas_full_vec1.size() / 8, 3, ublas_full_vec1.size() / 8);
  ublas::slice s2(2 * ublas_full_vec2.size() / 8, 1, ublas_full_vec2.size() / 8);
  ublas::slice s3(4 * ublas_full_vec1.size() / 8, 2, ublas_full_vec1.size() / 8);
  ublas::slice s4(3 * ublas_full_vec2.size() / 8, 4, ublas_full_vec2.size() / 8);
  ublas::vector_slice< ublas::vector<NumericT> > ublas_slice_vec1(ublas_full_vec1, s1);
  ublas::vector_slice< ublas::vector<NumericT> > ublas_slice_vec2(ublas_full_vec2, s2);
  ublas::vector_slice< ublas::vector<NumericT> > ublas_slice_vec3(ublas_full_vec1, s3);
  ublas::vector_slice< ublas::vector<NumericT> > ublas_slice_vec4(ublas_full_vec2, s4);

  //
  // Set up ViennaCL objects
  //
  viennacl::vector<NumericT> vcl_full_vec1(ublas_full_vec1.size());
  viennacl::vector<NumericT> vcl_full_vec2(ublas_full_vec2.size());

  viennacl::fast_copy(ublas_full_vec1.begin(), ublas_full_vec1.end(), vcl_full_vec1.begin());
  viennacl::copy     (ublas_full_vec2.begin(), ublas_full_vec2.end(), vcl_full_vec2.begin());

  viennacl::slice vcl_s1(    vcl_full_vec1.size() / 8, 3, vcl_full_vec1.size() / 8);
  viennacl::slice vcl_s2(2 * vcl_full_vec2.size() / 8, 1, vcl_full_vec2.size() / 8);
  viennacl::slice vcl_s3(4 * vcl_full_vec1.size() / 8, 2, vcl_full_vec1.size() / 8);
  viennacl::slice vcl_s4(3 * vcl_full_vec2.size() / 8, 4, vcl_full_vec2.size() / 8);
  viennacl::vector_slice< viennacl::vector<NumericT> > vcl_slice_vec1(vcl_full_vec1, vcl_s1);
  viennacl::vector_slice< viennacl::vector<NumericT> > vcl_slice_vec2(vcl_full_vec2, vcl_s2);
  viennacl::vector_slice< viennacl::vector<NumericT> > vcl_slice_vec3(vcl_full_vec1, vcl_s3);
  viennacl::vector_slice< viennacl::vector<NumericT> > vcl_slice_vec4(vcl_full_vec2, vcl_s4);

  viennacl::vector<NumericT> vcl_short_vec1(vcl_slice_vec1);
  viennacl::vector<NumericT> vcl_short_vec2 = vcl_slice_vec2;
  viennacl::vector<NumericT> vcl_short_vec3 = vcl_slice_vec2 + vcl_slice_vec1;
  viennacl::vector<NumericT> vcl_short_vec4 = vcl_short_vec1 + vcl_slice_vec2;

  ublas::vector<NumericT> ublas_short_vec1(ublas_slice_vec1);
  ublas::vector<NumericT> ublas_short_vec2(ublas_slice_vec2);
  ublas::vector<NumericT> ublas_short_vec3 = ublas_slice_vec2 + ublas_slice_vec1;
  ublas::vector<NumericT> ublas_short_vec4 = ublas_short_vec1 + ublas_slice_vec2;

  std::cout << "Testing creation of vectors from slice..." << std::endl;
  if (check(ublas_short_vec1, vcl_short_vec1, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  if (check(ublas_short_vec2, vcl_short_vec2, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  if (check(ublas_short_vec3, vcl_short_vec3, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;
  if (check(ublas_short_vec4, vcl_short_vec4, epsilon) != EXIT_SUCCESS)
    return EXIT_FAILURE;

  //
  // Now start running tests for vectors, ranges and slices:
  //

  std::cout << " ** [vector|vector|vector|vector] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec1, ublas_short_vec2, ublas_short_vec2, ublas_short_vec2,
                            vcl_short_vec1,   vcl_short_vec2,   vcl_short_vec3,   vcl_short_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [vector|vector|vector|slice] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec1, ublas_short_vec2, ublas_short_vec2, ublas_slice_vec2,
                            vcl_short_vec1,   vcl_short_vec2,   vcl_short_vec3,   vcl_slice_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [vector|vector|slice|vector] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec1, ublas_short_vec2, ublas_slice_vec2, ublas_short_vec2,
                            vcl_short_vec1,   vcl_short_vec2,   vcl_slice_vec3,   vcl_short_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [vector|vector|slice|slice] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec1, ublas_short_vec2, ublas_slice_vec2, ublas_slice_vec2,
                            vcl_short_vec1,   vcl_short_vec2,   vcl_slice_vec3,   vcl_slice_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [vector|slice|vector|vector] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec1, ublas_slice_vec2, ublas_short_vec2, ublas_short_vec2,
                            vcl_short_vec1,   vcl_slice_vec2,   vcl_short_vec3,   vcl_short_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [vector|slice|vector|slice] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec1, ublas_slice_vec2, ublas_short_vec2, ublas_slice_vec2,
                            vcl_short_vec1,   vcl_slice_vec2,   vcl_short_vec3,   vcl_slice_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [vector|slice|slice|vector] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec1, ublas_slice_vec2, ublas_slice_vec2, ublas_short_vec2,
                            vcl_short_vec1,   vcl_slice_vec2,   vcl_slice_vec3,   vcl_short_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [vector|slice|slice|slice] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_short_vec1, ublas_slice_vec2, ublas_slice_vec2, ublas_slice_vec2,
                            vcl_short_vec1,   vcl_slice_vec2,   vcl_slice_vec3,   vcl_slice_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  //////////////////

  std::cout << " ** [slice|vector|vector|vector] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_slice_vec1, ublas_short_vec2, ublas_short_vec2, ublas_short_vec2,
                            vcl_slice_vec1,   vcl_short_vec2,   vcl_short_vec3,   vcl_short_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [slice|vector|vector|slice] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_slice_vec1, ublas_short_vec2, ublas_short_vec2, ublas_slice_vec2,
                            vcl_slice_vec1,   vcl_short_vec2,   vcl_short_vec3,   vcl_slice_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [slice|vector|slice|vector] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_slice_vec1, ublas_short_vec2, ublas_slice_vec2, ublas_short_vec2,
                            vcl_slice_vec1,   vcl_short_vec2,   vcl_slice_vec3,   vcl_short_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [slice|vector|slice|slice] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_slice_vec1, ublas_short_vec2, ublas_slice_vec2, ublas_slice_vec2,
                            vcl_slice_vec1,   vcl_short_vec2,   vcl_slice_vec3,   vcl_slice_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [slice|slice|vector|vector] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_slice_vec1, ublas_slice_vec2, ublas_short_vec2, ublas_short_vec2,
                            vcl_slice_vec1,   vcl_slice_vec2,   vcl_short_vec3,   vcl_short_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [slice|slice|vector|slice] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_slice_vec1, ublas_slice_vec2, ublas_short_vec2, ublas_slice_vec2,
                            vcl_slice_vec1,   vcl_slice_vec2,   vcl_short_vec3,   vcl_slice_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [slice|slice|slice|vector] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_slice_vec1, ublas_slice_vec2, ublas_slice_vec2, ublas_short_vec2,
                            vcl_slice_vec1,   vcl_slice_vec2,   vcl_slice_vec3,   vcl_short_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  std::cout << " ** [slice|slice|slice|slice] **" << std::endl;
  retval = test<NumericT>(epsilon,
                          ublas_slice_vec1, ublas_slice_vec2, ublas_slice_vec2, ublas_slice_vec2,
                            vcl_slice_vec1,   vcl_slice_vec2,   vcl_slice_vec3,   vcl_slice_vec4);
  if (retval != EXIT_SUCCESS)
    return EXIT_FAILURE;

  return EXIT_SUCCESS;
}

//
// -------------------------------------------------------------
//
int main()
{
   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "## Test :: Vector multiple inner products" << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << std::endl;

   int retval = EXIT_SUCCESS;

   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << std::endl;
   {
      typedef float NumericT;
      NumericT epsilon = static_cast<NumericT>(1.0E-4);
      std::cout << "# Testing setup:" << std::endl;
      std::cout << "  eps:     " << epsilon << std::endl;
      std::cout << "  numeric: float" << std::endl;
      retval = test<NumericT>(epsilon);
      if ( retval == EXIT_SUCCESS )
         std::cout << "# Test passed" << std::endl;
      else
         return retval;
   }
   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << std::endl;
#ifdef VIENNACL_WITH_OPENCL
   if ( viennacl::ocl::current_device().double_support() )
#endif
   {
      {
         typedef double NumericT;
         NumericT epsilon = 1.0E-12;
         std::cout << "# Testing setup:" << std::endl;
         std::cout << "  eps:     " << epsilon << std::endl;
         std::cout << "  numeric: double" << std::endl;
         retval = test<NumericT>(epsilon);
         if ( retval == EXIT_SUCCESS )
           std::cout << "# Test passed" << std::endl;
         else
           return retval;
      }
      std::cout << std::endl;
      std::cout << "----------------------------------------------" << std::endl;
      std::cout << std::endl;
   }

  std::cout << std::endl;
  std::cout << "------- Test completed --------" << std::endl;
  std::cout << std::endl;

   return retval;
}


/*
 * Copyright (c) 2011 Adrian Michel
 * http://www.amichel.com
 *
 * Permission to use, copy, modify, distribute and sell this
 * software and its documentation for any purpose is hereby
 * granted without fee, provided that both the above copyright
 * notice and this permission notice appear in all copies and in
 * the supporting documentation.
 *
 * This library is distributed in the hope that it will be
 * useful. However, Adrian Michel makes no representations about
 * the suitability of this software for any purpose.  It is
 * provided "as is" without any express or implied warranty.
 *
 * Should you find this library useful, please email
 * info@amichel.com with a link or other reference
 * to your work.
*/

#ifndef DE_CONSTRAINTS_HPP_INCLUDED
#define DE_CONSTRAINTS_HPP_INCLUDED

// MS compatible compilers support #pragma once

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif

#include <set>
#include <boost/tokenizer.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>

#include "random_generator.hpp"
#include "de_types.hpp"

namespace de
{

/**
 * Exception thrown in case of a constraint error
 *
 * @author adrian (12/1/2011)
 */
class constraints_exception : public exception
{
public:
	/**
	 * constructor that takes the error message as argument
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param message
	 */
	constraints_exception( const std::string& message )
	: exception( message.c_str() )
	{
	}
};

/**
 * Abstract base class for concrete constraint classes
 *
 * A constraint class describes certain characteristics and
 * limits of the input variables fed to the objective function.
 *
 * @author adrian (12/1/2011)
 */
class constraint
{
public:
	virtual ~constraint(){}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double get_rand_value() = 0;

	/**
	 * returns a random value limited to the type and range of the
	 * constraint based on a previous value and an origin (see
	 * specific implementation in derived classes)
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param value
	 * @param origin
	 *
	 * @return double
	 */
	virtual double get_rand_value( double value, double origin ) = 0;

	/**
	 * returns the min limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double min() const = 0;

	/**
	 * returns the max limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double max() const = 0;

	/**
	 * Gets a random value within the limits set for the constraint,
	 * but further limited to a range defined by its origin and the
	 * width of the zone around this origin in pct of the total
	 * width
	 *
	 * @author adrian (12/13/2011)
	 *
	 * @param origin the origin (center) of the zone further
	 *  			 limiting the constraint
	 * @param zonePct the width of the zone in pct of the total
	 *  			  width, around the origin
	 *
	 * @return double
	 */
	virtual double get_rand_value_in_zone( double origin, double zonePct ) const = 0;

	/**
	 * Gets the point midway between min and max - will only work
	 * for range constraints
	 *
	 * @author adrian (12/16/2011)
	 *
	 * @return double
	 */
	virtual double get_middle_point() = 0;
};

/**
 * A smart pointer to a Constraint
 */
typedef boost::shared_ptr< constraint > constraint_ptr;

/**
 * Base class for constraints that are range based. Each such
 * constraint has a min and a max value
 *
 * @author adrian (12/1/2011)
 */
class range_constraint : public constraint
{
private:
	double m_min;
	double m_max;

public:

	/**
	 * constructor that takes the min and max limits of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param min
	 * @param max
	 */
	range_constraint( double min, double max )
	: m_min( min ), m_max( max )
	{
		assert( min <= max );
	}

	/**
	 * returns the min limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double min() const { return m_min; }

	/**
	 * returns the max limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double max() const { return m_max; }

};

/**
 * A real constraint. Specifies that variables can have any
 * double value, whitin the specified limits.
 *
 * @author adrian (12/1/2011)
 */
class real_constraint : public range_constraint
{
public:

	/**
	 * constructor that takes the min and max limit of the real
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param min
	 * @param max
	 */
	real_constraint( double min, double max )
	: range_constraint( min, max )
	{
		assert( min <= max );
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double get_rand_value()
	{
		return genrand( range_constraint::min(), range_constraint::max() );
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint based on a previous value and an origin
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param value
	 * @param origin
	 *
	 * @return double
	 */
	double get_rand_value( double value, double origin )
	{
		double ret = value;

		while( ret < range_constraint::min() )
		{
			ret = range_constraint::min() + genrand() * ( origin - range_constraint::min() );
		}

		while( ret > range_constraint::max() )
		{
			ret = range_constraint::max() + genrand() * ( origin - range_constraint::max() );
		}

		return ret;
	}

	virtual double get_rand_value_in_zone( double origin, double zonePct ) const
	{
		if( origin > max() )
			throw constraints_exception( "origin coordinate > max" );
		if( origin < min() )
			throw constraints_exception( "origin coordinate < min" );

		if( zonePct > 100.0 )
			throw constraints_exception( "zonePct > 100%" );

		if( zonePct < 0 )
			throw constraints_exception( "zonePct < 0%" );

		if( zonePct == 0 )
			throw constraints_exception( "zonePct == 0%" );

		double zoneSize = ( max() - min() ) * zonePct / 100.0;

		double _min = std::max( min(), origin - zoneSize/2.0 );
		double _max = std::min( max(), origin + zoneSize/2.0 );

		return genrand( _min, _max );
	}

	virtual double get_middle_point()
	{
		return ( max() + min() )/2.0;
	}

};

/**
 * An integer constraint. Specifies that variables can have any
 * integer values within the specified limits.
 *
 * @author adrian (12/1/2011)
 */
class int_constraint : public range_constraint
{
public:
	/**
	 * constructor that takes the min and max limit of the integer
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param min
	 * @param max
	 */
	int_constraint( double min, double max )
	: range_constraint( min, max )
	{
		assert( min <= max );
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double get_rand_value()
	{
		return genintrand( range_constraint::min(), range_constraint::max() );
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint based on a previous value and an origin
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param value
	 * @param origin
	 *
	 * @return double
	 */
	double get_rand_value( double value, double origin )
	{
		double ret = boost::math::round( value );

		while( ret < range_constraint::min() )
		{
			ret = range_constraint::min() + genrand() * ( origin - range_constraint::min() );
			ret = boost::math::round( ret );
		}

		while( ret > range_constraint::max() )
		{
			ret = range_constraint::max() + genrand() * ( origin - range_constraint::max() );
			ret = boost::math::round( ret );
		}

		return ret;
	}

	virtual double get_rand_value_in_zone( double origin, double zonePct ) const
	{
		if ( origin > max() )
			throw constraints_exception( "origin coordinate > max" );
		if ( origin < min() )
			throw constraints_exception( "origin coordinate < min" );

		if ( zonePct > 100.0 )
			throw constraints_exception( "zonePct > 100%" );

		if( zonePct < 0 )
			throw constraints_exception( "zonePct < 0%" );

		if( zonePct == 0  )
			throw constraints_exception( "zonePct == 0%" );

		double zoneSize = ( max() - min() ) * zonePct/100.0;

		double _min = std::max( min(), origin - zoneSize/2.0 );
		double _max = std::min( max(), origin + zoneSize/2.0 );

		double val = boost::math::round( genrand( _min, _max ) );

		for ( ;val < _min || val > _max; val = boost::math::round( genrand( _min, _max ) ) );

		return val;
	}

	virtual double get_middle_point()
	{
		return boost::math::round( ( max() - min() )/2.0 );
	}

};

/**
 * A set constraint. Specifies that variables can take any
 * values from a predefined set. Doesn't require min or max.
 *
 * Note that duplicate values will be removed
 *
 * @author adrian (12/1/2011)
 */
class set_constraint : public constraint
{
private:
	class unique : public std::unary_function < Double, bool >
	{
	public:
		bool operator ()( Double d ) const
		{
			return m_unique.insert( d ).second;
		}

	public:
		double min() const
		{
			if( m_unique.size() > 0 )
				return *m_unique.begin();
			else
				throw constraints_exception( "could not get the min value of an empty set constraint" );
		}

		double max() const
		{
			if( m_unique.size() > 0 )
				return *m_unique.rbegin();
			else
				throw constraints_exception( "could not get the max value of an empty set constraint" );
		}

	private:
		mutable std::set< Double > m_unique;
	};
private:
	unique m_unique;
	de::DVector m_values;

public:
	/**
	 * Constructs the set from a vector of Double values, and
	 * removes duplicates to ensure a uniform distribution for
	 * randomization
	 *
	 * @author adrian (12/10/2011)
	 *
	 * @param values
	 */
	set_constraint( const de::DVector& values )
	{
		// making sure the values in the "set" are unique
		std::remove_copy_if (values.begin (), values.end (), std::back_inserter( m_values ), m_unique );
	}

	/**
	 * adds an individual value to the "set", won't create duplicate
	 * values.
	 *
	 * @author adrian (12/10/2011)
	 *
	 * @param value
	 */
	void add_value( de::Double value )
	{
		if( m_unique( value ) )
			m_values.push_back( value );
	}

	/**
	 * returns a value randomly chosen from the set of available
	 * values
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double get_rand_value()
	{
		de::DVector::size_type index( genintrand( 0, m_values.size() - 1 ) );

		return m_values[ index ];
	}

	/**
	 * returns a value randomly chosen from the set of available
	 * values
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double get_rand_value(double value, double origin)
	{
		return get_rand_value();
	}

	/**
	 * returns the min limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double min() const
	{
		return m_unique.min();
	}

	/**
	 * returns the max limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double max() const
	{
		return m_unique.max();
	}

	virtual double get_rand_value_in_zone( double origin, double zonePct ) const
	{
		throw constraints_exception( "get_rand_value_in_zone only supported for range constraints" );
	}

	virtual double get_middle_point()
	{
		throw constraints_exception( "get_middle_point not supported by set constraint" );
	}

};

/**
 * A boolean constraint. Specifies that variables can take a
 * boolean value - true or false.
 *
 * @author adrian (12/1/2011)
 */
class boolean_constraint : public constraint
{
public:
	/**
	 * returns a random boolean value
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double get_rand_value()
	{
		return genrand() < 0.5;
	}

	/**
	 * returns a random boolean value
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double get_rand_value(double value, double origin)
	{
		return get_rand_value();
	}

	/**
	 * returns the min limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double min() const
	{
		return 0;
	}

	/**
	 * returns the max limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double max() const
	{
		return 1;
	}

	virtual double get_rand_value_in_zone( double origin, double zonePct ) const
	{
		throw constraints_exception( "get_rand_value_in_zone only supported for range constraints" );
	}

	virtual double get_middle_point()
	{
		throw constraints_exception( "get_middle_point not supported by bool constraint" );
	}

};

typedef std::vector< constraint_ptr > constraints_base;

/**
 * A collection of constraints, implemented as a vector.
 *
 * Is used to define the constraints for all variables used
 * during an optimization session.
 *
 * @author adrian (12/1/2011)
 */
class constraints : public constraints_base
{
private:
	typedef boost::char_separator< char > separator;
	typedef boost::tokenizer< separator > tokenizer;

public:
	/**
	 * Initializes a collection of constraints with default values.
	 * All constraints are of type real and have the same default
	 * min and max.
	 *
	 * @author adrian (12/1/2011)
	 *
	 * @param varCount number of constraints
	 * @param defMin default min limit
	 * @param defMax default max limit
	 */
	constraints( size_t varCount, double defMin, double defMax )
	: constraints_base( varCount, boost::make_shared< real_constraint >( defMin, defMax ) )
	{
	}

	/**
	 * Initializes a collection of constraints from string
	 * descsriptions. Currently used only for range based
	 * constraints.
	 *
	 * A constraint can be described as "type;min;max" where type
	 * can be real or integer and min and max are the range limits.
	 *
	 * @author adrian (12/1/2011)
	 *
	 * @param str a a collection (vector) of constraint description
	 *  		  strings, each string describing constraints for
	 *  		  one variable
	 * @param var_count the total number of variables (can be
	 *  			   different than the number of strings). If
	 *  			   there are more variables than strings, the
	 *  			   extra constraints are set to be real and use
	 *  			   the default min and max arguments for the
	 *  			   range
	 * @param def_min default min value in case the number of
	 *  			 variables is higher than the number of
	 *  			 constraints specified as strings.
	 * @param def_max default max value in case the number of
	 *  			 variables is higher than the number of
	 *  			 constraints specified as strings.
	 */
	constraints( const std::vector< std::string >& str, size_t var_count , double def_min, double def_max )
	: constraints_base( var_count, boost::make_shared< real_constraint >( def_min, def_max ) )
	{
		for( std::vector< std::string >::size_type i = 0; i < str.size(); ++i )
		{
			tokenizer tokens( str[ i ], separator( ";," ) );

			std::string type;
			double _min;
			double _max;

			size_t count( 0 );

			for( tokenizer::const_iterator j = tokens.begin(); j != tokens.end(); ++j, ++count )
			{
				const std::string token( boost::trim_copy( *j ) );

				try
				{
					switch( count )
					{
						case 0:
							type = token;
							break;
						case 1:
							_min = boost::lexical_cast< double >( token.c_str() );
							break;
						case 2:
							_max = boost::lexical_cast< double >( token.c_str() );
							break;
						default:
							// too many fields
							throw constraints_exception( ( boost::format( "wrong variable format in \"%1%\" - too many fields" ) % str[ i ] ).str() );

					}
				}
				catch( const boost::bad_lexical_cast& )
				{
					throw constraints_exception( ( boost::format( "wrong floating point number format: %1%") % token ).str() );
				}
			}

			// too few fields
			if( count < 3 )
				throw constraints_exception( ( boost::format( "wrong variable format in \"%1%\" - too few fields" ) % str[ i ] ).str() );

			if( i < var_count )
				constraints_base::at( i ) = str_to_constraint( type, _min, _max );
			else
				constraints_base::push_back( str_to_constraint( type, _min, _max ) );
		}
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double get_rand_value( size_t index )
	{
		if( index < constraints_base::size() )
			return (*this)[ index ]->get_rand_value();
		else
			throw constraints_exception( ( boost::format( "invalid constraint index: %1%, higher than max number of constraints: %2%" ) % index % constraints_base::size() ).str() );
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint based on a previous value and an origin
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param index the constraint index
	 * @param value previous value
	 * @param origin origin
	 *
	 * @return double
	 */
	double get_rand_value( size_t index, double value, double origin )
	{
		if( index < constraints_base::size() )
			return (*this)[ index ]->get_rand_value( value, origin );
		else
			throw constraints_exception( ( boost::format( "invalid constraint index: %1%, higher than max number of constraints: %2%" ) % index % constraints_base::size() ).str() );
	}

	/**
	 * Generates a set of random values within a "hypercube" region
	 * defined by an origin and an area expressed as a percentage of
	 * the entire range
	 *
	 * @author adrian (12/13/2011)
	 *
	 * @param origin the origin coordinates of the region
	 * @param sidePct the side of the hypercube in pct of the entire
	 *  			  side
	 *
	 * @return DVectorPtr
	 */
	DVectorPtr get_square_zone_rand_values( const DVectorPtr origin, double sidePct ) const
	{
		assert( origin );
		assert( sidePct > 0 && sidePct <= 100 );

		if( origin->size() == constraints_base::size() )
		{
			DVectorPtr square( boost::make_shared< DVector >( origin->size() ) );

			for( constraints_base::size_type n = 0; n < constraints_base::size(); ++n )
				(*square)[ n ] = (*this)[ n ]->get_rand_value_in_zone( (*origin)[ n ], sidePct );

			return square;

		}
		else
			throw constraints_exception( "The origin vector must have the same number of elements as there are constraints" );
	}

	DVectorPtr get_middle_point()
	{
		DVectorPtr r( boost::make_shared< DVector >( constraints_base::size() ) );

		for( constraints_base::size_type n = 0; n < constraints_base::size(); ++n )
			(*r)[ n ] = (*this)[ n ]->get_middle_point();

		return r;

	}

	/**
	 * Get a set of random values within the limits set by the
	 * constraints
	 *
	 * @author adrian (12/13/2011)
	 *
	 * @return DVectorPtr
	 */
	DVectorPtr get_rand_values() const
	{
		DVectorPtr r( boost::make_shared< DVector >( constraints_base::size() ) );

		for( constraints_base::size_type n = 0; n < constraints_base::size(); ++n )
			(*r)[ n ] = (*this)[ n ]->get_rand_value();

		return r;
	}

private:
	constraint_ptr str_to_constraint( const std::string& type, double min, double max )
	{
		if( boost::to_lower_copy( type ) == "real" )
			return boost::make_shared< real_constraint >( min, max );
		else if( boost::to_lower_copy( type ) == "int" || boost::to_lower_copy( type ) == "integer" )
			return boost::make_shared< int_constraint >( min, max );
		else
			throw constraints_exception( ( boost::format( "invalid constraint type \"%1%\"" ) % type ).str() );
	}
};

typedef boost::shared_ptr< constraints > constraints_ptr;

}

#endif //DE_CONSTRAINTS_HPP_INCLUDED


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_BITINTEGER_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_BITINTEGER_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/bitinteger.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_positive.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/constants/signmask.hpp>
#include <boost/dispatch/meta/adapted_traits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::bitinteger_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type a00 = simd::bitwise_cast<result_type>(a0);
      return select(boost::simd::is_positive(a0), a00, Signmask<result_type>()-a00 );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::bitinteger_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return simd::bitwise_cast<result_type>(a0);
    }
  };
} } }

#endif


// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// this project is translated from this project : digraph [https://github.com/damian0815/digraph]

//  Author:  MingXing Liu
//  Date:    2017-12-25
//  version:
//  v0.1
//  1.init verion

#ifndef BOOST_GRAPH_SUGIYAMA_LAYOUT_HPP
#define BOOST_GRAPH_SUGIYAMA_LAYOUT_HPP

#include <math.h>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/simple_point.hpp>
#include <boost/graph/circle_layout.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/static_assert.hpp>
#include <boost/unordered_map.hpp>
#include <boost/property_map/property_map.hpp>

namespace boost {

static const int    MAX_SWEEPS = 100;

template<typename GraphType>
static void splitIntoLayers(const GraphType& g,
                            std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                            std::map<typename graph_traits<GraphType>::vertex_iterator, int> &nodemap) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;
    typedef typename graph_traits<GraphType>::vertex_descriptor VertexDescriptor;
    typedef typename graph_traits<GraphType>::out_edge_iterator OutEdgeIter;

    typedef std::deque< VertexDescriptor > V_VEC;
    V_VEC sorted;
    topological_sort(g, std::front_inserter(sorted));

    std::map<VertexDescriptor, int> lmap;
    for (typename V_VEC::value_type &vt:sorted){
        lmap[vt] = 0;
    }

    int h = 1;
    for (typename V_VEC::iterator it = sorted.begin(); it != sorted.end(); ++it )
    {
        VertexDescriptor n1 = *it;

        OutEdgeIter out, out_end;
        tie(out, out_end) = out_edges(n1, g);
        for(; out != out_end; ++out){
            VertexDescriptor n2=target(*out,g);

            int inc = 1;
            lmap[n2] = std::max(lmap[n1] + inc, lmap[n2]);
            h = std::max(h, lmap[n2] + 1);
        }
    }

    //stack_init(h, (int)sorted.size());
    layers.resize(h);

    Viter n, nend;
    tie(n,nend) = vertices(g);
    for (typename V_VEC::value_type &vt:sorted){
        //stack_add(n+vt, lmap[vt]);
        int layerIndex=lmap[vt];

        layers[layerIndex].push_back(n+vt);
        nodemap[n+vt] = layerIndex;
    }
}

template<typename GraphType,typename IndexMap>
static void setOrderedIndexes(IndexMap &index,
                       std::vector<typename graph_traits<GraphType>::vertex_iterator>& ln) {
    for (int i = 0; i < ln.size(); i++) {
        index[*(ln[i])]=i;
    }
}

template<typename GraphType,typename RectMap>
static double maxHeight(RectMap &rect,
                 const std::vector<typename graph_traits<GraphType>::vertex_iterator> &ln) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    double mh = 0;
    for (typename std::vector<Viter>::const_iterator it = ln.begin(); it != ln.end(); ++it ) {
        Viter n = (*it);
        mh = std::max(mh, (double)(rect[*n][1]));
    }
    return mh;
}

template<typename GraphType,typename PositionMap,typename RectMap>
static double avgX(PositionMap &position,
            RectMap &rect,
            const std::vector<typename graph_traits<GraphType>::vertex_iterator>& ln) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    double m = 0;
    for (typename std::vector<Viter>::const_iterator it = ln.begin(); it != ln.end(); ++it ) {
        Viter n = *it;
        m += (position[*n][0]+rect[*n][0]/2);
    }
    return m / ln.size();
}

template<typename GraphType,typename IndexMap>
static int barycenter( IndexMap &index,
                const std::vector<typename graph_traits<GraphType>::vertex_iterator>& ln) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    if (ln.size() == 0) {
        return 0;
    }

    double bc = 0;
    for (typename std::vector<Viter>::const_iterator it = ln.begin(); it != ln.end(); ++it ) {
        Viter n = *it;

        bc += index[*n];
    }
    return (int) floor((bc / ln.size())+0.5f);
}

template<typename GraphType>
std::vector<typename graph_traits<GraphType>::vertex_iterator>
static stack_getConnectedTo(const GraphType& g,
                     std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                     typename graph_traits<GraphType>::vertex_iterator n1,
                     int layerIndex) {
    typedef typename graph_traits<GraphType>::vertex_iterator        Viter;
    typedef typename boost::graph_traits<GraphType>::edge_descriptor EdgeDescriptor;

    std::vector<Viter> ln;
    if (layerIndex < layers.size() && layerIndex >= 0) {
        for (typename std::vector<Viter>::iterator it = layers.at(layerIndex).begin(); it != layers.at(layerIndex).end(); ++it )
        {
            Viter n2 = *it;
            std::pair<EdgeDescriptor, bool> test1 = edge(*n1, *n2, g);
            if (test1.second == true) { // 判断vd1和vd2所指的两节点是否相邻
                ln.push_back(n2);
            }

            /*std::pair<EdgeDescriptor, bool> test2 = edge( *n2,*n1, g);
              if (test2.second == true) { // 判断vd1和vd2所指的两节点是否相邻
                 ln.push_back(n2);
             }*/
        }
    }
    return ln;
}

template<typename GraphType,typename IndexMap>
static void stack_reduceCrossings2L(const GraphType& g,
                             std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                             IndexMap &index,
                             int staticIndex, int flexIndex) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    assert( flexIndex < layers.size() );
    std::vector<Viter>& flex = layers[flexIndex];
    for (typename std::vector<Viter>::const_iterator it = flex.begin(); it != flex.end(); ++it )
    {
        Viter n = (*it);
        std::vector<Viter> neighbors = stack_getConnectedTo(g,layers,n, staticIndex);

        index[*n]=barycenter<GraphType>(index,neighbors);
    }

    sort( flex.begin(), flex.end(),  [&index]( Viter& n1,  Viter& n2 )->bool{
        return index[*n1]< index[*n2];
    });

    setOrderedIndexes<GraphType>(index,flex);
}

template<typename GraphType,typename PositionMap,typename RectMap>
static void stack_layerHeights(std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                        PositionMap &position,
                        RectMap &rect,
                        int &yspacing) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    double offset = 0;
    for (int l = 0; l < layers.size(); l++) {
        std::vector<Viter>& ln = layers[l];
        double maxh = maxHeight<GraphType>(rect,ln);
        for (typename std::vector<Viter>::iterator it = ln.begin(); it != ln.end(); ++it ) {
            Viter n = (*it);
            //if (n->isVirtual()) {
            if (false) {
                position[*n][1]=offset+maxHeight<GraphType>(rect,ln)/2.0;
            } else {
                position[*n][1]=offset;
            }
        }
        offset += maxh + yspacing;
    }
}

template<typename GraphType, typename PositionMap,typename RectMap>
static void stack_xPosPack(std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                    PositionMap &position,
                    RectMap &rect,
                    int flexIndex,
                    int &xspacing) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    assert( flexIndex < layers.size() );
    std::vector<Viter>& flex = layers[flexIndex];
    double offset = 0;
    for (typename std::vector<Viter>::const_iterator it = flex.begin(); it != flex.end(); ++it ) {
        Viter n = (*it);
        position[*n][0]=offset;
        offset = position[*n][0] + rect[*n][0] + xspacing;
    }
}

template<typename GraphType, typename PositionMap,typename RectMap>
static  void stack_xPosDown(const GraphType& g,
                    PositionMap &position,
                    RectMap &rect,
                    std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                    int staticIndex, int flexIndex,
                    int &xspacing) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    assert( flexIndex < layers.size() );
    std::vector<Viter> flex = layers[flexIndex];
    for (int i = 0; i < flex.size(); i++) {
        Viter n = flex[i];
        std::vector<Viter> neighbors = stack_getConnectedTo(g,layers,n, staticIndex);
        double avg = avgX<GraphType>(position,rect,neighbors);
        double min = (i > 0) ? (position[*(flex[i-1])][0] + rect[*(flex[i-1])][0] + xspacing) : (-std::numeric_limits<double>::max());
        if (!isnan(avg)) {
            position[*n][0]= std::max(min, avg -  rect[*n][0]/2.0);
        }
    }
}

template<typename GraphType, typename PositionMap,typename RectMap>
static  void stack_xPosUp(const GraphType& g,
                  PositionMap &position,
                  RectMap &rect,
                  std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                  int staticIndex, int flexIndex,
                  int &xspacing) {//staticIndex=flexIndex+1
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    assert( flexIndex < layers.size() );
    std::vector<Viter> flex = layers[flexIndex];
    for (int i = flex.size() - 1; i >= 0; i--) {
        Viter n = flex[i];
        std::vector<Viter> neighbors = stack_getConnectedTo(g,layers,n, staticIndex);

        //for ( int i=0; i<neighbors.size() ;i++ )
        double avg = avgX<GraphType>(position,rect,neighbors);
        // calculate min, max
        double min = (i>0)?(position[*(flex[i-1])][0] + rect[*(flex[i-1])][0] + xspacing) : -std::numeric_limits<double>::max();
        double max =std::numeric_limits<double>::max();
        if(i<flex.size()-1){
            max=position[*(flex[i+1])][0] - rect[*n][0] - xspacing;
        }

        if (!isnan(avg)) {
            position[*n][0]=std::max(min, std::min(max, avg - rect[*n][0] / 2.0));
        }
    }
}

template<typename GraphType, typename PositionMap,typename RectMap,typename IndexMap>
void sugiyama_graph_layout(const GraphType& g,
                           PositionMap &position,
                           RectMap &rect,
                           IndexMap &index,
                           int xspacing=20,
                           int yspacing=30)
{
    BOOST_STATIC_ASSERT (property_traits<PositionMap>::value_type::dimensions >= 2);
    BOOST_STATIC_ASSERT (property_traits<RectMap>::value_type::dimensions >= 2);

    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    typedef  std::vector<Viter>     Viter_VEC;
    typedef  std::vector<Viter_VEC> Viter_VEC_VEC;
    Viter_VEC_VEC        layers;
    std::map<Viter, int> nodemap;

    //removeCycles
    //removeCycles();

    //splitIntoLayers
    splitIntoLayers(g,layers,nodemap);

    //insertDummies
    //insertDummies();

    //initIndexes
    for (typename Viter_VEC_VEC::iterator it = layers.begin(); it != layers.end(); ++it )
    {
        Viter_VEC& l = *it;

        /*sort( l.begin(), l.end(), [&getXYWH]( Viter n1, Viter n2 )->bool{
              return getXYWH[*n1]->id < getXYWH[*n2]->id;
          });*/
        /*
          @todo figure out how to do this
          Collections.sort(l, new Comparator<Viter>() {
              public int compare(Viter n1, Viter n2) {
                  return n1.getIndex() - n2.getIndex();
              }
          });*/

        setOrderedIndexes<GraphType>(index,l);
    }

    //reduceCrossings
    for (int round = 0; round < MAX_SWEEPS; round++) {
        if (round % 2 == 0) {
            for (int l = 0; l < layers.size() - 1; l++) {
                stack_reduceCrossings2L(g,layers,index,l, l + 1);
            }
        } else {
            for (int l = layers.size() - 1; l > 0; l--) {
                stack_reduceCrossings2L(g,layers,index,l, l - 1);
            }
        }
    }

    //undoRemoveCycles
    //undoRemoveCycles();

    //layerHeights
    stack_layerHeights<GraphType>(layers,position,rect,yspacing);

    //xPos
    for (int l = 0; l < layers.size(); l++) {
        stack_xPosPack<GraphType>(layers,position,rect,l,xspacing);
    }

    printf("2\n");
    for (int l = 0; l < layers.size() - 1; l++) {
        stack_xPosDown(g,position,rect,layers,l, l + 1,xspacing);
    }
    for (int l = layers.size() - 1; l >0; l--) {
        stack_xPosUp(g,position,rect,layers,l, l - 1,xspacing);
    }
}

}

#endif


// Author Dmytro Bobkov, dmytro.bobkov@tum.de
// Chair of Media Technology, Technical University of Munich, 2018-2019
// Parts of these were done while being at research visit to group of Bernd Girod IVMS at Stanford University

#include <iostream>
#include <fstream>
#include <iomanip>
#include <chrono>

#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include <boost/filesystem.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>

namespace po = boost::program_options;
namespace fs = boost::filesystem;

#include "projection_helpers.h"
#include "io_helpers.h"

#include "rendering_tools.h"

#define IMG_WIDTH 4096
#define IMG_HEIGHT 2048

void
parseCommandLineOptions(int argc,
                        const char** argv,
                        Configuration& config_rendering,
                        std::string& in_pano_dir,
                        std::string& in_poses_dir,
                        std::string& output_folder,
                        bool& render_views_from_panoramas )
{
    po::options_description desc("Allowed options");
    desc.add_options()
            ("in_pano_dir", po::value<std::string>()->required(), "Input pano directory")
            ("in_poses_dir", po::value<std::string>()->required(), "Input poses directory")
            ("out_rgb_view_dir", po::value<std::string>()->required(), "Out directory for the rendered views")
            ("render_views_from_panoramas", po::bool_switch(&render_views_from_panoramas), "Optional argument for rendering")
            ("num_yaw_angles", po::value<int>()->required(), "num_yaw_angles");;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("in_pano_dir")) {
        in_pano_dir = vm["in_pano_dir"].as<std::string>();
        std::cout << "In pano dir set to " << in_pano_dir << ".\n";
    }

    if (vm.count("in_poses_dir")) {
        in_poses_dir = vm["in_poses_dir"].as<std::string>();
        std::cout << "In poses dir set to " << in_poses_dir << ".\n";
    }

    if (vm.count("out_rgb_view_dir")) {
        output_folder = vm["out_rgb_view_dir"].as<std::string>();
        std::cout << "Out dir set to " << output_folder << ".\n";
    }

    if (vm.count("num_yaw_angles")) {
        config_rendering.number_yaw_angles = vm["num_yaw_angles"].as<int>();
        std::cout << "num_yaw_angles set to " << config_rendering.number_yaw_angles << ".\n";
    }
}

int main( int argc, const char** argv )
{
    std::string in_pano_dir, output_folder, in_poses_dir;
    bool render_views_from_panoramas;
    Configuration config_rendering;

    parseCommandLineOptions(argc, argv, config_rendering,
                            in_pano_dir, in_poses_dir,
                            output_folder, render_views_from_panoramas);

    std::map<std::string, Panorama> pano_poses;
    parseInLocPanoramaPoses( in_poses_dir, pano_poses );

    PanoConfig pano_config;
    pano_config.shift_yaw_deg = 90;
    pano_config.invert_yaw = true;

    std::vector<std::string> pano_filenames;
    parsePanoramaFilenames( in_pano_dir, pano_filenames );

    assert(pano_filenames.size() == pano_poses.size());

    if( boost::filesystem::exists(output_folder)==false ) {
        boost::filesystem::create_directory(output_folder);
    }

    bool generate_multiple_yaw_rates = config_rendering.number_yaw_angles==-1;
    if( generate_multiple_yaw_rates ) {
        std::vector<int> yaw_numbers = {4, 8, 12, 16};
        initConfigurationMultipleRates(  yaw_numbers, config_rendering );
    }
    else {
        initConfiguration(  config_rendering );
    }
    // NOTE: caution we are using portrait dimensions because our queries are portrait!
    config_rendering.mat_width = 640;
    config_rendering.mat_height = 480;

    std::vector<RenderedImage> rendered_images;
    generateRenderedImages( config_rendering, pano_filenames, pano_poses, rendered_images );

    std::cout << "Finished rendered images " << rendered_images.size() << std::endl;

    // now write to JSON all data
    std::string json_output_all_rendered_view = output_folder + "/view_poses.json";

    writeViewsAndConfigToJSON( config_rendering, rendered_images, json_output_all_rendered_view );

    if( !render_views_from_panoramas ) {
        return 0;
    }

    // now iterate over all panos
    for( size_t ind_pano_raw=0; ind_pano_raw<pano_filenames.size(); ind_pano_raw++ ) {
        std::cout << "Rendering from pano " << ind_pano_raw << "/" << pano_filenames.size() << std::endl;

        std::string name_image = pano_filenames[ind_pano_raw];

        // get index from the filename correctly
        boost::filesystem::path p_i(name_image);
        std::string name_file = p_i.stem().string();
        std::string name_prefix = name_file + '_';

        cv::Mat img_src = cv::imread(name_image.c_str(), CV_LOAD_IMAGE_COLOR);
        if( img_src.empty() ) {//is image loaded?
            std::cerr << "Error: Could not load image!" << std::endl;
            continue;
        }

        // resize to standard size
        cv::Size standard_size(IMG_WIDTH, IMG_HEIGHT);
        cv::resize(img_src, img_src, standard_size);
        assert(img_src.rows() == IMG_HEIGHT);
        assert(img_src.cols() == IMG_WIDTH);

        processOnePano( config_rendering,
                        pano_config,
                        img_src,
                        output_folder,
                        name_prefix );

    }

    return 0;
}