/*****************************************************************************
*
* Copyright (c) 2003-2018 by The University of Queensland
* http://www.uq.edu.au
*
* Primary Business: Queensland, Australia
* Licensed under the Apache License, version 2.0
* http://www.apache.org/licenses/LICENSE-2.0
*
* Development until 2012 by Earth Systems Science Computational Center (ESSCC)
* Development 2012-2013 by School of Earth Sciences
* Development from 2014 by Centre for Geoscience Computing (GeoComp)
*
*****************************************************************************/

/****************************************************************************/

/* Paso: SystemMatrix: controls iterative linear system solvers  */

/****************************************************************************/

/* Copyrights by ACcESS Australia 2003/04                     */
/* Author: Lutz Gross, l.gross@uq.edu.au                      */

/****************************************************************************/

#include "Solver.h"
#include "Options.h"
#include "SystemMatrix.h"

#include <boost/math/special_functions/fpclassify.hpp>  // for isnan

#include <iostream>

namespace bm = boost::math;

namespace paso {

void Solver_free(SystemMatrix<double>* A)
{
    A->freePreconditioner();
}

///  calls the iterative solver
SolverResult Solver(SystemMatrix_ptr<double> A, double* x, double* b, Options* options,
                    Performance* pp)
{
    const real_t EPSILON = escript::DataTypes::real_t_eps();
    double norm2_of_b,tol,tolerance,time_iter,net_time_start;
    double *r=NULL,norm2_of_residual,last_norm2_of_residual,norm_max_of_b;
    double norm2_of_b_local,norm_max_of_b_local,norm2_of_residual_local;
    double norm_max_of_residual_local,norm_max_of_residual;
    double last_norm_max_of_residual;
#ifdef ESYS_MPI
    double loc_norm;
#endif
    dim_t i,totIter=0,cntIter,method;
    bool finalizeIteration;
    SolverResult errorCode = NoError;
    const dim_t numSol = A->getTotalNumCols();
    const dim_t numEqua = A->getTotalNumRows();
    double *x0=NULL;

    tolerance=options->tolerance;
    if (tolerance < 100.* EPSILON) {
        throw PasoException("Solver: Tolerance is too small.");
    }
    if (tolerance >1.) {
        throw PasoException("Solver: Tolerance must be less than one.");
    }
    method=Options::getSolver(options->method, PASO_PASO, options->symmetric, A->mpi_info);
    /* check matrix type */
    if ((A->type & MATRIX_FORMAT_CSC) || (A->type & MATRIX_FORMAT_OFFSET1) ) {
        throw PasoException("Solver: Iterative solver requires CSR format with unsymmetric storage scheme and index offset 0.");
    }
    if (A->col_block_size != A->row_block_size) {
        throw PasoException("Solver: Iterative solver requires row and column block sizes to be equal.");
    }
    if (A->getGlobalNumCols() != A->getGlobalNumRows()) {
        throw PasoException("Solver: Iterative solver requires a square matrix.");
    }
    time_iter=escript::gettime();
    /* this for testing only */
    if (method==PASO_NONLINEAR_GMRES) {
        LinearSystem* F = new LinearSystem(A, b, options);
        A->solvePreconditioner(x, b);
        errorCode = Solver_NewtonGMRES(F, x, options, pp);
        if (errorCode != NoError) {
            throw PasoException("Solver_NewtonGMRES: an error has occurred.");
        }
        delete F;
        return errorCode;
    }

    r = new double[numEqua];
    x0 = new double[numEqua];
    A->balance();
    options->num_level=0;
    options->num_inner_iter=0;

    /* ========================= */
    Performance_startMonitor(pp, PERFORMANCE_ALL);
    A->applyBalance(r, b, true);
    /* get the norm of the right hand side */
    norm2_of_b=0.;
    norm_max_of_b=0.;
    #pragma omp parallel private(norm2_of_b_local,norm_max_of_b_local)
    {
        norm2_of_b_local=0.;
        norm_max_of_b_local=0.;
        #pragma omp for private(i) schedule(static)
        for (i = 0; i < numEqua ; ++i) {
            norm2_of_b_local += r[i] * r[i];
            norm_max_of_b_local = std::max(std::abs(r[i]),norm_max_of_b_local);
        }
        #pragma omp critical
        {
            norm2_of_b += norm2_of_b_local;
            norm_max_of_b = std::max(norm_max_of_b_local,norm_max_of_b);
        }
    }
#ifdef ESYS_MPI
    /* TODO: use one call */
    loc_norm = norm2_of_b;
    MPI_Allreduce(&loc_norm,&norm2_of_b, 1, MPI_DOUBLE, MPI_SUM, A->mpi_info->comm);
    loc_norm = norm_max_of_b;
    MPI_Allreduce(&loc_norm,&norm_max_of_b, 1, MPI_DOUBLE, MPI_MAX, A->mpi_info->comm);
#endif
    norm2_of_b=sqrt(norm2_of_b);
    /* if norm2_of_b==0 we are ready: x=0 */
    if (bm::isnan(norm2_of_b) || bm::isnan(norm_max_of_b)) {
        throw PasoException("Solver: Matrix or right hand side contains undefined values.");
    } else if (norm2_of_b <= 0.) {
#pragma omp parallel for private(i) schedule(static)
        for (i = 0; i < numSol; i++) x[i]=0.;
        if (options->verbose)
            std::cout << "right hand side is identical to zero." << std::endl;
    } else {
        if (options->verbose) {
            std::cout << "Solver: l2/lmax-norm of right hand side is "
                << norm2_of_b << "/" << norm_max_of_b << "." << std::endl
                << "Solver: l2/lmax-stopping criterion is "
                << norm2_of_b*tolerance << "/" << norm_max_of_b*tolerance
                << "." << std::endl;
            switch (method) {
                case PASO_BICGSTAB:
                    std::cout << "Solver: Iterative method is BiCGStab.\n";
                break;
                case PASO_PCG:
                    std::cout << "Solver: Iterative method is PCG.\n";
                break;
                case PASO_TFQMR:
                    std::cout << "Solver: Iterative method is TFQMR.\n";
                break;
                case PASO_MINRES:
                    std::cout << "Solver: Iterative method is MINRES.\n";
                break;
                case PASO_PRES20:
                    std::cout << "Solver: Iterative method is PRES20.\n";
                break;
                case PASO_GMRES:
                    if (options->restart > 0) {
                        std::cout << "Solver: Iterative method is GMRES("
                            << options->truncation << ","
                            << options->restart << ")." << std::endl;
                    } else {
                        std::cout << "Solver: Iterative method is GMRES("
                            << options->truncation << ")." << std::endl;
                    }
                break;
            }
        }

        // construct the preconditioner
        Performance_startMonitor(pp, PERFORMANCE_PRECONDITIONER_INIT);
        A->setPreconditioner(options);
        Performance_stopMonitor(pp, PERFORMANCE_PRECONDITIONER_INIT);
        options->set_up_time=escript::gettime()-time_iter;
        // get an initial guess by evaluating the preconditioner
        A->solvePreconditioner(x, r);

        totIter = 1;
        finalizeIteration = false;
        last_norm2_of_residual=norm2_of_b;
        last_norm_max_of_residual=norm_max_of_b;
        net_time_start=escript::gettime();

        // main loop
        while (!finalizeIteration) {
            cntIter = options->iter_max - totIter;
            finalizeIteration = true;

            // Set initial residual
            if (totIter > 1) {
                // in the first iteration r = balance * b already
                A->applyBalance(r, b, true);
            }

            A->MatrixVector_CSR_OFFSET0(-1., x, 1., r);
            norm2_of_residual = 0;
            norm_max_of_residual = 0;
            #pragma omp parallel private(norm2_of_residual_local,norm_max_of_residual_local)
            {
                norm2_of_residual_local = 0;
                norm_max_of_residual_local = 0;
                #pragma omp for private(i) schedule(static)
                for (i = 0; i < numEqua; i++) {
                    norm2_of_residual_local+= r[i] * r[i];
                    norm_max_of_residual_local=std::max(std::abs(r[i]),norm_max_of_residual_local);
                }
                #pragma omp critical
                {
                    norm2_of_residual += norm2_of_residual_local;
                    norm_max_of_residual = std::max(norm_max_of_residual_local,norm_max_of_residual);
                }
            }
#ifdef ESYS_MPI
            // TODO: use one call
            loc_norm = norm2_of_residual;
            MPI_Allreduce(&loc_norm,&norm2_of_residual, 1, MPI_DOUBLE, MPI_SUM, A->mpi_info->comm);
            loc_norm = norm_max_of_residual;
            MPI_Allreduce(&loc_norm,&norm_max_of_residual, 1, MPI_DOUBLE, MPI_MAX, A->mpi_info->comm);
#endif
            norm2_of_residual =sqrt(norm2_of_residual);
            options->residual_norm=norm2_of_residual;

            if (options->verbose)
                std::cout << "Solver: Step " << totIter
                    << ": l2/lmax-norm of residual is "
                    << norm2_of_residual << "/" << norm_max_of_residual;

            if (totIter > 1 &&
                    norm2_of_residual >= last_norm2_of_residual &&
                    norm_max_of_residual >= last_norm_max_of_residual) {

                if (options->verbose) std::cout << " divergence!\n";
                throw PasoException("Solver: No improvement during iteration. Iterative solver gives up.");

            } else {
                if (norm2_of_residual>tolerance*norm2_of_b ||
                        norm_max_of_residual>tolerance*norm_max_of_b ) {

                    tol=tolerance*std::min(norm2_of_b,0.1*norm2_of_residual/norm_max_of_residual*norm_max_of_b);
                    if (options->verbose)
                        std::cout << " (new tolerance = " << tol << ").\n";

                    last_norm2_of_residual=norm2_of_residual;
                    last_norm_max_of_residual=norm_max_of_residual;

                    // call the solver
                    switch (method) {
                        case PASO_BICGSTAB:
                            errorCode = Solver_BiCGStab(A, r, x, &cntIter, &tol, pp);
                        break;
                        case PASO_PCG:
                            errorCode = Solver_PCG(A, r, x, &cntIter, &tol, pp);
                        break;
                        case PASO_TFQMR:
                            tol=tolerance*norm2_of_residual/norm2_of_b;
                            errorCode = Solver_TFQMR(A, r, x0, &cntIter, &tol, pp);
                            #pragma omp for private(i) schedule(static)
                            for (i = 0; i < numEqua; i++) {
                                x[i]+= x0[i];
                            }
                        break;
                        case PASO_MINRES:
                            //tol=tolerance*norm2_of_residual/norm2_of_b;
                            errorCode = Solver_MINRES(A, r, x, &cntIter, &tol, pp);
                        break;
                        case PASO_PRES20:
                            errorCode = Solver_GMRES(A, r, x, &cntIter, &tol, 5, 20, pp);
                        break;
                        case PASO_GMRES:
                            errorCode = Solver_GMRES(A, r, x, &cntIter, &tol, options->truncation, options->restart, pp);
                        break;
                    }

                    totIter += cntIter;

                    // error handling
                    if (errorCode == NoError) {
                        finalizeIteration = false;
                    } else if (errorCode == MaxIterReached) {
                        if (options->verbose)
                            std::cout << "Solver: Maximum number of "
                                "iterations reached." << std::endl;
                        break;
                    } else if (errorCode == InputError) {
                        if (options->verbose)
                            std::cout << "Solver: Internal error!\n";
                        break;
                    } else if (errorCode == NegativeNormError) {
                        if (options->verbose)
                            std::cout << "Solver: negative energy norm"
                               " (try other solver or preconditioner)!\n";
                        break;
                    } else if (errorCode == Breakdown) {
                        if (cntIter <= 1) {
                            if (options->verbose)
                                std::cout << "Solver: Uncurable break "
                                    "down!" << std::endl;
                            break;
                        } else {
                            if (options->verbose)
                                std::cout << "Solver: Breakdown at iter "
                                    << totIter << " (residual = "
                                    << tol << "). Restarting ...\n";
                            finalizeIteration = false;
                            errorCode = NoError;
                        }
                    } else {
                        if (options->verbose)
                            std::cout << "Solver: Generic error in solver!\n";
                        break;
                    }
                } else {
                    if (options->verbose)
                        std::cout << " convergence!" << std::endl;
                    options->converged = true;
                }
            }
        } // while
        options->net_time = escript::gettime()-net_time_start;
        options->num_iter = totIter;
        A->applyBalanceInPlace(x, false);
    }
    delete[] r;
    delete[] x0;
    options->time = escript::gettime()-time_iter;
    Performance_stopMonitor(pp, PERFORMANCE_ALL);
    return errorCode;
}

SolverResult Solver(SystemMatrix_ptr<cplx_t> A, cplx_t* x, cplx_t* b, Options* options,
                    Performance* pp)
{
    throw PasoException("Solver(): complex not implemented.");
}

void Solver_free(SystemMatrix<cplx_t>* A)
{
    throw PasoException("Solver_free(): complex not implemented.");
}

} // namespace paso


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "DavidsonDiagonalizer.h"
#include "IndirectPreconditionerEvaluator.h"
#include "IndirectSigmaVectorEvaluator.h"
#include <Utils/IO/Logger.h>
#include <Eigen/Eigenvalues>

namespace Scine {
namespace Utils {

template<class MatrixType, DavidsonBalancedType s>
DavidsonDiagonalizer<MatrixType, s>::DavidsonDiagonalizer(int eigenvaluesToCompute, int numberGuessVectors, int totalDimension)
  : eigenvaluesToCompute_(eigenvaluesToCompute), blockSize_(numberGuessVectors), subspaceDimension_(numberGuessVectors) {
  assert(eigenvaluesToCompute > 0 &&
         "Unintended behaviour: calculate negative amount of eigenvalues in Davidson diagonalizer.");
  if (eigenvaluesToCompute_ > blockSize_)
    throw InvalidDavidsonInputException();
  initialize(totalDimension);
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::initialize(int dimension) {
  if (eigenvaluesToCompute_ > dimension) {
    eigenvaluesToCompute_ = dimension;
    Utils::Log::warning() << "Too many eigenvalues requested (" << eigenvaluesToCompute_ << "), they were set to "
                          << dimension << ".";
  }
  if (subspaceDimension_ > dimension) {
    subspaceDimension_ = eigenvaluesToCompute_;
    blockSize_ = subspaceDimension_;
    Utils::Log::warning() << "Initial subspace dimension too big (" << subspaceDimension_ << "), it was reduced to "
                          << eigenvaluesToCompute_ << ".";
  }
  assert(dimension >= blockSize_ && "Matrix dimension cannot be smaller than number of guess vectors!");
  guessVectors_ = Eigen::MatrixXd::Zero(dimension, subspaceDimension_);
  if (s == DavidsonBalancedType::standard) {
    guessVectors_.block(0, 0, subspaceDimension_, subspaceDimension_) =
        Eigen::MatrixXd::Identity(subspaceDimension_, subspaceDimension_);
  }
  else {
    srand(seed_);
    guessVectors_.block(0, 0, subspaceDimension_, subspaceDimension_) =
        Eigen::MatrixXd::Random(subspaceDimension_, subspaceDimension_);
  }
  maxDimension_ = dimension;
  maxIterations_ = dimension;
  sigmaVectorEvaluator_.reset();
  preconditionerEvaluator_.reset();
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setMaxIterations(int maxIterations) {
  maxIterations_ = maxIterations;
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setMatrixToDiagonalize(const MatrixType& matrix) {
  if (type_ == DavidsonDirectType::direct) { // if a direct method is in use, no matrix is needed.
    return;
  }
  matrixToDiagonalize_ = matrix;
  originalDiagonal_ = matrixToDiagonalize_.diagonal();
  initialize(matrixToDiagonalize_.cols());
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setGuess(const Eigen::MatrixXd& guessVectors) {
  guessVectors_ = guessVectors.leftCols(subspaceDimension_);
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setDavidsonType(DavidsonDirectType type) {
  type_ = type;
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setSeed(unsigned seed) {
  seed_ = seed;
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setSigmaVectorEvaluator(std::unique_ptr<SigmaVectorEvaluator<MatrixType>>&& evaluator) {
  sigmaVectorEvaluator_ = std::move(evaluator);
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setPreconditionerEvaluator(std::unique_ptr<PreconditionerEvaluator>&& evaluator) {
  preconditionerEvaluator_ = std::move(evaluator);
}

template<class MatrixType, DavidsonBalancedType s>
EigenContainer DavidsonDiagonalizer<MatrixType, s>::solve() {
  checkEvaluators();

  // Set initial size of guess matrix
  subspaceDimension_ = blockSize_;
  for (int i = 0; i < maxIterations_; ++i) {
    performIteration();
    Log::info() << "Iteration number: " << i << "\tSubspace dimension: " << subspaceDimension_ << ".";
    if (converged_) { // if full matrix still calculate everything
      Log::info() << "Converged in " << i << " iterations, dimensionality " << subspaceDimension_ << ".";
      return eigenPairs_;
    }
  }
  throw DavidsonNotConvergedException();
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::performIteration() {
  // Orthogonalize the projection matrix
  if (s == DavidsonBalancedType::standard) {
    orthogonalizeSubspace(guessVectors_);
    guessVectors_.leftCols(subspaceDimension_).colwise().normalize();
  }

  const Eigen::MatrixXd& projector = guessVectors_.leftCols(subspaceDimension_);
  if (s == DavidsonBalancedType::balanced) {
    for (int i = basisOverlap_.cols(); i < subspaceDimension_; ++i) {
      basisOverlap_.conservativeResize(subspaceDimension_, subspaceDimension_);
      basisOverlap_.col(i) = projector.transpose() * projector.col(i);
      Eigen::VectorXd symmetricRow = basisOverlap_.col(i);
      basisOverlap_.row(i) = symmetricRow;
    }
  }

  Eigen::MatrixXd sigmaMatrix = sigmaVectorEvaluator_->evaluateSigmaVector(projector);
  Eigen::MatrixXd projectedMatrix = projector.transpose() * sigmaMatrix;

  Eigen::MatrixXd subspaceEVectors;
  Eigen::VectorXd subspaceEValues;

  if (s == DavidsonBalancedType::standard) {
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> subspaceDiagonalizer(projectedMatrix);
    subspaceEVectors = subspaceDiagonalizer.eigenvectors();
    subspaceEValues = subspaceDiagonalizer.eigenvalues();
  }
  else if (s == DavidsonBalancedType::balanced) {
    Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> subspaceDiagonalizer(projectedMatrix, basisOverlap_);
    subspaceEVectors = subspaceDiagonalizer.eigenvectors();
    subspaceEValues = subspaceDiagonalizer.eigenvalues();
  }

  // Check convergence:
  // Get a matrix with the first *eigenvaluesToCompute_* residual vectors as column.
  // basically, it calculated r_k = H*Psi_{k, approx} - h_k*Psi_{k, approx}
  // = (sigmaMatrix - h_k * bMatrix) * v_k
  // H: matrix to diagonalize
  // Psi_{k, approx} k-th approximated Ritz eigenvectors for the subspace
  // h_k: k-th approximated Ritz eigenvalue
  // sigmaMatrix: matrix with as columns the sigma vectors
  // bMatrix: matrix with as column the tentative vectors
  // v_k: k-th subspace eigenvector
  Eigen::MatrixXd residualVectors(sigmaMatrix.rows(), eigenvaluesToCompute_);
  for (int col = 0; col < eigenvaluesToCompute_; ++col) {
    residualVectors.col(col) = (sigmaMatrix - subspaceEValues(col) * projector) * subspaceEVectors.col(col);
  }

  Eigen::VectorXd residualNorms = residualVectors.colwise().norm();
  converged_ = true;
  for (int i = 0; i < residualNorms.size(); ++i) {
    if (residualNorms(i) > eigenvalueTol)
      converged_ = false;
  }
  if (converged_) {
    eigenPairs_ = {subspaceEValues.head(eigenvaluesToCompute_), projector * subspaceEVectors.leftCols(eigenvaluesToCompute_)};
    return;
  }
  expandSubspace(subspaceEValues, residualVectors, projector, residualNorms);
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::expandSubspace(const Eigen::VectorXd& subspaceEValues,
                                                         const Eigen::MatrixXd& residualVectors, const Eigen::MatrixXd& projector,
                                                         const Eigen::VectorXd& residualNorms) {
  // look at each residual vector
  for (int i = 0; i < eigenvaluesToCompute_; ++i) {
    if (residualNorms(i) > eigenvalueTol) {
      Eigen::VectorXd correctionVector =
          residualVectors.col(i).cwiseProduct(-preconditionerEvaluator_->evaluate(subspaceEValues(i)));
      if (s == DavidsonBalancedType::standard) {
        Eigen::VectorXd orthogonalizedCorrectionVector(guessVectors_.rows());
        orthogonalizedCorrectionVector = orthogonalizeToSubspace(correctionVector.normalized(), projector);

        if (orthogonalizedCorrectionVector.norm() / correctionVector.norm() > /*correctionTol*/ 1e-3) {
          if (subspaceDimension_ < maxDimension_) {
            ++subspaceDimension_;
            guessVectors_.conservativeResize(Eigen::NoChange, subspaceDimension_);
            guessVectors_.col(subspaceDimension_ - 1) = orthogonalizedCorrectionVector.normalized();
          }
        }
      }
      else {
        if (subspaceDimension_ < maxDimension_) {
          ++subspaceDimension_;
          guessVectors_.conservativeResize(Eigen::NoChange, subspaceDimension_);
          guessVectors_.col(subspaceDimension_ - 1) = correctionVector;
        }
      }
    }
  }
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::checkEvaluators() {
  if (!sigmaVectorEvaluator_ || !preconditionerEvaluator_) {
    switch (type_) {
      case DavidsonDirectType::standard:
        setSigmaVectorEvaluator(std::make_unique<IndirectSigmaVectorEvaluator<MatrixType>>(matrixToDiagonalize_));
        setPreconditionerEvaluator(std::make_unique<IndirectPreconditionerEvaluator>(originalDiagonal_));
        break;
      case DavidsonDirectType::direct:
        throw InvalidSigmaVectorEvaluator();
      default:
        throw InvalidDavidsonTypeException();
    }
  }
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::orthogonalizeSubspace(Eigen::MatrixXd& trialSpace) {
  int spaceDimension = trialSpace.rows();
  // Calculate modified Gram-Schmidt QR decomposition
  Eigen::MatrixXd Q(spaceDimension, subspaceDimension_);
  Eigen::MatrixXd R(subspaceDimension_, subspaceDimension_);

  for (int i = 0; i < subspaceDimension_; ++i) {
    R(i, i) = trialSpace.col(i).norm();
    Q.col(i) = trialSpace.col(i) / R(i, i);
    for (int j = i + 1; j < subspaceDimension_; ++j) {
      R(i, j) = Q.col(i).transpose() * trialSpace.col(j);
      trialSpace.col(j) -= Q.col(i) * R(i, j);
    }
  }
}
template<class MatrixType, DavidsonBalancedType s>
Eigen::VectorXd DavidsonDiagonalizer<MatrixType, s>::orthogonalizeToSubspace(const Eigen::VectorXd& vector,
                                                                             const Eigen::MatrixXd& subspace) const {
  Eigen::VectorXd orthonormalizedVector = vector;
  for (int basis = 0; basis < subspace.cols(); ++basis) {
    orthonormalizedVector -= subspace.col(basis).dot(vector) * subspace.col(basis) / subspace.col(basis).norm();
  }
  return orthonormalizedVector;
}

template class DavidsonDiagonalizer<Eigen::MatrixXd>;
template class DavidsonDiagonalizer<Eigen::SparseMatrix<double>>;
template class DavidsonDiagonalizer<Eigen::MatrixXd, DavidsonBalancedType::balanced>;
template class DavidsonDiagonalizer<Eigen::SparseMatrix<double>, DavidsonBalancedType::balanced>;

} // namespace Utils
} // namespace Scine


#include "core/simulations/LightDark.h"

#include "core/Util.h"
#include <boost/functional/hash.hpp>
#include <iostream>
#include <opencv2/imgproc.hpp>
#include <random>
#include <stdexcept>

namespace simulations {

LightDark::Action LightDark::Action::Rand() {
  Action action{std::uniform_real_distribution<float>(0, 2 * PI)(Rng())};
  action.trigger = std::bernoulli_distribution(0.2)(Rng());
  return action;
}

uint64_t LightDark::Observation::Discretize() const {
  float grid_size = DELTA * EGO_SPEED;
  list_t<int> data {
    static_cast<int>(floorf(ego_agent_position.x / grid_size)),
    static_cast<int>(floorf(ego_agent_position.y / grid_size))
  };
  return boost::hash_value(data);
}

list_t<list_t<LightDark::Action>> LightDark::Action::CreateHandcrafted(size_t length) {
  list_t<list_t<Action>> macro_actions;
  for (size_t i = 0; i < 8; i++) {
    macro_actions.emplace_back();
    for (size_t j = 0; j < length; j++) {
      macro_actions.back().push_back({static_cast<float>(i) * 2 * PI / 8});
    }
  }
  Action trigger_action;
  trigger_action.trigger = true;
  macro_actions.emplace_back();
  macro_actions.back().emplace_back(trigger_action);
  return macro_actions;
}

list_t<list_t<LightDark::Action>> LightDark::Action::Deserialize(const list_t<float>& params, size_t macro_length) {
  list_t<list_t<LightDark::Action>> macro_actions = StandardMacroActionDeserialization<LightDark::Action>(params, macro_length);
  Action trigger_action;
  trigger_action.trigger = true;
  macro_actions.emplace_back();
  macro_actions.back().emplace_back(trigger_action);
  return macro_actions;
}

/* ====== Construction functions ====== */

LightDark::LightDark() : step(0), _is_terminal(false), _is_failure(false) {

}

LightDark LightDark::CreateRandom() {

  do {
    EGO_START_MEAN.x = std::uniform_real_distribution<float>(RANDOMIZATION_REGION[0].x, RANDOMIZATION_REGION[2].x)(Rng());
    EGO_START_MEAN.y = std::uniform_real_distribution<float>(RANDOMIZATION_REGION[0].y, RANDOMIZATION_REGION[2].y)(Rng());
    GOAL.x = std::uniform_real_distribution<float>(RANDOMIZATION_REGION[0].x, RANDOMIZATION_REGION[2].x)(Rng());
    GOAL.y = std::uniform_real_distribution<float>(RANDOMIZATION_REGION[0].y, RANDOMIZATION_REGION[2].y)(Rng());
    LIGHT_POS = std::uniform_real_distribution<float>(RANDOMIZATION_REGION[0].x, RANDOMIZATION_REGION[2].x)(Rng());
  } while ((EGO_START_MEAN - GOAL).norm() < 4 || (std::abs(EGO_START_MEAN.x - LIGHT_POS) < 4 || (std::abs(GOAL.x - LIGHT_POS) < 4)));

  /*
  EGO_START_MEAN = {-2.0f, -2.5f};
  GOAL = {-3.5f, 2.9f};
  LIGHT_POS = 3.5f;
  */

  return SampleBeliefPrior();
}

/* ====== Belief related functions ======*/

LightDark LightDark::SampleBeliefPrior() {
  LightDark sim;
  sim.ego_agent_position.x = std::normal_distribution<float>(
      EGO_START_MEAN.x, EGO_START_STD)(Rng());
  sim.ego_agent_position.y =std::normal_distribution<float>(
      EGO_START_MEAN.y, EGO_START_STD)(Rng());
  return sim;
}

float LightDark::Error(const LightDark& other) const {
  return (ego_agent_position - other.ego_agent_position).norm();
}

/* ====== Bounds related functions ====== */
// Actually a high probability estimate, assuming actuation noise samples below 3 s.d.
float LightDark::BestReward() const {
  if (_is_terminal) { return 0; } // Return value of 0 needed for DESPOT.

  float distance = std::max(0.0f, (GOAL - ego_agent_position).norm() - EGO_RADIUS);
  float max_distance_per_step = EGO_SPEED * DELTA;
  size_t steps = static_cast<size_t>(round(ceilf(distance / max_distance_per_step)));
  if (steps <= 1) {
    return GOAL_REWARD;
  } else {
    return (1 - powf(GAMMA, static_cast<float>(steps) - 1)) / (1 - static_cast<float>(steps)) * STEP_REWARD +
      powf(GAMMA, static_cast<float>(steps) - 1) * GOAL_REWARD;
  }
}

/* ====== Stepping functions ====== */

template <bool compute_log_prob>
std::tuple<LightDark, float, LightDark::Observation, float> LightDark::Step(
    const LightDark::Action& action, const LightDark::Observation* observation) const {
  if (_is_terminal) { throw std::logic_error("Cannot step terminal simulation."); }

  LightDark next_sim = *this;
  float reward;

  /* ====== Step 1: Update state.  ======*/
  if (action.trigger) {
    if ((next_sim.ego_agent_position - GOAL).norm() <= EGO_RADIUS) {
      reward = GOAL_REWARD;
      next_sim._is_failure = false;
    } else {
      reward = COLLISION_REWARD;
      next_sim._is_failure = true;
    }
    next_sim._is_terminal = true;
    next_sim.step++;
  } else {
    next_sim.ego_agent_position += DELTA * vector_t(EGO_SPEED, 0).rotated(action.orientation);
    reward = STEP_REWARD;
    next_sim.step++;
    if (next_sim.step == MAX_STEPS) {
      reward = COLLISION_REWARD;
      next_sim._is_terminal = true;
      next_sim._is_failure = true;
    }
  }

  /* ====== Step 2: Generate observation. ====== */
  Observation new_observation;
  if (observation) {
    new_observation = *observation;
  }
  float log_prob = 0;

  bool in_light = abs(next_sim.ego_agent_position.x - LIGHT_POS) <= LIGHT_WIDTH;
  if (!observation) {
    if (in_light) {
      new_observation.ego_agent_position = next_sim.ego_agent_position;
      new_observation.ego_agent_position.x += std::normal_distribution<float>(0.0, OBSERVATION_NOISE)(RngDet());
      new_observation.ego_agent_position.y += std::normal_distribution<float>(0.0, OBSERVATION_NOISE)(RngDet());
    } else {
      new_observation.ego_agent_position.x = std::numeric_limits<float>::quiet_NaN();
      new_observation.ego_agent_position.y = std::numeric_limits<float>::quiet_NaN();
    }
  }
  if constexpr (compute_log_prob) {
    if (in_light) {
      if (std::isnan(new_observation.ego_agent_position.x) && std::isnan(new_observation.ego_agent_position.y)) {
        log_prob += -std::numeric_limits<float>::infinity();
      } else {
        log_prob += NormalLogProb(next_sim.ego_agent_position.x, OBSERVATION_NOISE, new_observation.ego_agent_position.x);
        log_prob += NormalLogProb(next_sim.ego_agent_position.y, OBSERVATION_NOISE, new_observation.ego_agent_position.y);
      }
    } else {
      if (std::isnan(new_observation.ego_agent_position.x) && std::isnan(new_observation.ego_agent_position.y)) {
        log_prob += 0;
      } else {
        log_prob += -std::numeric_limits<float>::infinity();
      }
    }
  }

  return std::make_tuple(next_sim, reward, observation ? Observation() : new_observation, log_prob);
}
template std::tuple<LightDark, float, LightDark::Observation, float> LightDark::Step<true>(
    const LightDark::Action& action, const LightDark::Observation* observation) const;
template std::tuple<LightDark, float, LightDark::Observation, float> LightDark::Step<false>(
    const LightDark::Action& action, const LightDark::Observation* observation) const;

/* ====== Serialization functions ====== */

void LightDark::Encode(list_t<float>& data) const {
  data.emplace_back(static_cast<float>(step));
  ego_agent_position.Encode(data);
}

void LightDark::EncodeContext(list_t<float>& data) {
  GOAL.Encode(data);
  data.emplace_back(LIGHT_POS);
}

cv::Mat LightDark::Render(const list_t<LightDark>& belief_sims,
    const list_t<list_t<Action>>& macro_actions, const vector_t& macro_action_start) const {

  constexpr float SCENARIO_MIN = -7.0f;
  constexpr float SCENARIO_MAX = 7.0f;
  constexpr float RESOLUTION = 0.02f;
  auto to_frame = [&](const vector_t& vector) {
    return cv::Point{
      static_cast<int>((vector.x - SCENARIO_MIN) / RESOLUTION),
      static_cast<int>((SCENARIO_MAX - vector.y) / RESOLUTION)
    };
  };
  auto to_frame_dist = [&](float d) {
    return static_cast<int>(d / RESOLUTION);
  };

  cv::Mat frame(
      static_cast<int>((SCENARIO_MAX - SCENARIO_MIN) / RESOLUTION),
      static_cast<int>((SCENARIO_MAX - SCENARIO_MIN) / RESOLUTION),
      CV_8UC3,
      cv::Scalar(255, 255, 255));

  for (int x = 0; x < frame.cols; x++) {
    float x_world = static_cast<float>(x) * RESOLUTION + SCENARIO_MIN;
    float intensity = abs(x_world - LIGHT_POS) <= LIGHT_WIDTH ? 255 : 128;
    frame.col(x).setTo(cv::Scalar(intensity, intensity, intensity));
  }

  cv::drawMarker(frame, to_frame(EGO_START_MEAN),
      cv::Scalar(255, 255, 0), cv::MARKER_TILTED_CROSS, 30, 2, cv::LINE_AA);
  cv::drawMarker(frame, to_frame(GOAL),
      cv::Scalar(0, 255, 0), cv::MARKER_TILTED_CROSS, 30, 2, cv::LINE_AA);

  for (const simulations::LightDark& belief_sim : belief_sims) {
    cv::drawMarker(frame, to_frame(belief_sim.ego_agent_position), cv::Scalar(0, 255, 255),
        cv::MARKER_CROSS, 3, 2, cv::LINE_4);
  }

  // Draw ego agent.
  cv::circle(frame, to_frame(ego_agent_position), to_frame_dist(EGO_RADIUS),
      cv::Scalar(255, 0, 0), -1, cv::LINE_AA);
  cv::circle(frame, to_frame(ego_agent_position), to_frame_dist(EGO_RADIUS),
      cv::Scalar(0, 0, 0), 2, cv::LINE_AA);

  const static list_t<cv::Scalar> colors = {
    {75, 25, 230},
    {49, 130, 245},
    {25, 225, 255},
    {240, 240, 70},
    {75, 180, 60},
    {180, 30, 145},
    {230, 50, 240},
    {216, 99, 67}
  };

  for (size_t i = 0; i < macro_actions.size(); i++) {
    vector_t s = macro_action_start;
    for (const Action& a : macro_actions[i]) {
      vector_t e = s + vector_t(DELTA * EGO_SPEED, 0).rotated(a.orientation);
      if (!a.trigger) {
        cv::line(frame, to_frame(s), to_frame(e),
            colors[i], 5, cv::LINE_AA);
      }
      s = e;
    }
  }

  if (_is_terminal) {
    if ((ego_agent_position - GOAL).norm() <= EGO_RADIUS) {
      cv::putText(frame,
          "Stop (Success)",
          to_frame(ego_agent_position + vector_t(1.0, - EGO_RADIUS / 2)),
          cv::FONT_HERSHEY_DUPLEX,
          1.0,
          cv::Scalar(0, 255, 0),
          2,
          cv::LINE_AA);
    } else {
      cv::putText(frame,
          "Stop (Failure)",
          to_frame(ego_agent_position + vector_t(1.0, - EGO_RADIUS / 2)),
          cv::FONT_HERSHEY_DUPLEX,
          1.0,
          cv::Scalar(0, 0, 255),
          2,
          cv::LINE_AA);
    }
  }

  return frame;
}

}


#include <iostream>
#include <fstream>
#include <string>

#include <stdio.h>

#include <GL/glew.h>
#include <GL/freeglut.h>

#include <Eigen/Dense>

GLuint VBO;

std::string readFile(std::string &&inputFilePath)
{
    std::fstream input{inputFilePath, std::ios::in | std::ios::ate};

    // Fill string with 0-byte values.
    std::string inputStr(input.tellg(), '\0');
    input.seekg(0);

    if (! input.read(&inputStr[0], inputStr.size()))
        return "";

    input.close();

    return inputStr;
}

void createVertexBuffer()
{
    Eigen::Vector3f vertices[3];

    vertices[0] = Eigen::Vector3f({-1.0f, -1.0f, 0.0f});
    vertices[1] = Eigen::Vector3f({1.0f, -1.0f, 0.0f});
    vertices[2] = Eigen::Vector3f({0.0f, 1.0f, 0.0f});

    glGenBuffers(1, &VBO);
    glBindBuffer(GL_ARRAY_BUFFER, VBO);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);
}

void renderCallback()
{
    glClear(GL_COLOR_BUFFER_BIT);

    glEnableVertexAttribArray(0);
    glBindBuffer(GL_ARRAY_BUFFER, VBO);
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0);

    glDrawArrays(GL_TRIANGLES, 0, 3);

    glDisableVertexAttribArray(0);
    glutSwapBuffers();
}

static void addShader(GLuint ShaderProgram, const char* pShaderText, GLenum ShaderType)
{
    GLuint ShaderObj = glCreateShader(ShaderType);

    if (ShaderObj == 0) {
        fprintf(stderr, "Error creating shader type %d\n", ShaderType);
        exit(0);
    }

    const GLchar* p[1];
    p[0] = pShaderText;
    GLint Lengths[1];
    Lengths[0]= strlen(pShaderText);
    glShaderSource(ShaderObj, 1, p, Lengths);
    glCompileShader(ShaderObj);
    GLint success;
    glGetShaderiv(ShaderObj, GL_COMPILE_STATUS, &success);
    if (!success) {
        GLchar InfoLog[1024];
        glGetShaderInfoLog(ShaderObj, 1024, NULL, InfoLog);
        fprintf(stderr, "Error compiling shader type %d: '%s'\n", ShaderType, InfoLog);
        exit(1);
    }

    glAttachShader(ShaderProgram, ShaderObj);
}

static void compileShaders(
    std::string &&vertexShaderPath, std::string &&fragmentShaderPath)
{
    GLuint ShaderProgram = glCreateProgram();

    if (ShaderProgram == 0)
    {
        std::clog << "Error compiling shaders." << std::endl;
        exit(1);
    }

    std::string vertexShaderSource = readFile(std::move(vertexShaderPath));
    std::string fragmentShaderSource = readFile(std::move(fragmentShaderPath));

    if ("" == vertexShaderSource || "" == fragmentShaderSource)
    {
        exit(1);
    }

    addShader(ShaderProgram, vertexShaderSource.c_str(), GL_VERTEX_SHADER);
    addShader(ShaderProgram, fragmentShaderSource.c_str(), GL_FRAGMENT_SHADER);

    GLint Success = 0;
    GLchar ErrorLog[1024] = { 0 };

    glLinkProgram(ShaderProgram);
    glGetProgramiv(ShaderProgram, GL_LINK_STATUS, &Success);
	if (Success == 0) {
		glGetProgramInfoLog(ShaderProgram, sizeof(ErrorLog), NULL, ErrorLog);
		fprintf(stderr, "Error linking shader program: '%s'\n", ErrorLog);
        exit(1);
	}

    glValidateProgram(ShaderProgram);
    glGetProgramiv(ShaderProgram, GL_VALIDATE_STATUS, &Success);
    if (!Success) {
        glGetProgramInfoLog(ShaderProgram, sizeof(ErrorLog), NULL, ErrorLog);
        fprintf(stderr, "Invalid shader program: '%s'\n", ErrorLog);
        exit(1);
    }

    glUseProgram(ShaderProgram);
}

int main(int argc, char** argv)
{
    // CLI args pass-through.
    glutInit(&argc, argv);

    // Set up window frame.
    glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGBA);
    glutInitWindowSize(640, 480);
    glutInitWindowPosition(100, 100);
    glutCreateWindow("Test App");

    // Add callbacks.
    glutDisplayFunc(renderCallback);

    // Must init glew after glut.
    GLenum res = glewInit();
    if (res != GLEW_OK)
    {
        std::clog << "Error: '" << glewGetErrorString(res) << '\'' << std::endl;
        return 1;
    }

    glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

    createVertexBuffer();

    compileShaders("shader.fs", "shader.vs");

    glutMainLoop();

    return 0;
}


//==============================================================================
//         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 NT2_CORE_FUNCTIONS_SCALAR_RANDN_HPP_INCLUDED
#define NT2_CORE_FUNCTIONS_SCALAR_RANDN_HPP_INCLUDED

#include <nt2/core/functions/randn.hpp>
#include <nt2/core/utility/randstream.hpp>
#include <nt2/core/include/functions/as_size.hpp>

#include <nt2/sdk/parameters.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/repetition/repeat_from_to.hpp>

namespace nt2 { namespace ext
{
  /// INTERNAL ONLY
  /// target
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::randn_, tag::cpu_
                            , (T)
                            , (target_< scalar_< floating_<T> > >)
                            )
  {
    typedef typename T::type result_type;

    BOOST_FORCEINLINE result_type operator()(T const&) const
    {
      result_type that;
      current_randstream->randn(&that,0,1);
      return that;
    }
  };

  /// INTERNAL ONLY
  /// of_size + target
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::randn_, tag::cpu_
                            , (T)(A0)
                            , (fusion_sequence_<A0>)
                              (target_< scalar_< floating_<T> > >)
                            )
  {
    typedef typename boost::remove_const<A0>::type            size_type;
    typedef typename  boost::proto::result_of
                    ::make_expr < nt2::tag::randn_, container::domain
                                , box<size_type>
                                , T
                                >::type                   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& tgt) const
    {
      return  boost::proto
            ::make_expr<nt2::tag::randn_,container::domain>( boxify(a0), tgt );
    }
  };

  /// INTERNAL ONLY
  /// of_size
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::randn_, tag::cpu_
                            , (A0)
                            , (fusion_sequence_<A0>)
                            )
  {
    typedef typename boost::remove_const<A0>::type            size_type;
    typedef typename  boost::proto::result_of
                    ::make_expr < nt2::tag::randn_, container::domain
                                , box<size_type>
                                , meta::as_<double>
                                >::type                   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return  boost::proto
            ::make_expr < nt2::tag::randn_
                        , container::domain
                        >( boxify(a0), meta::as_<double>() );
    }
  };

  #define M2(z,n,t) (BOOST_PP_CAT(A,n))
  #define M1(z,n,t) (scalar_< arithmetic_<BOOST_PP_CAT(A,n)> >)

  #define M0(z,n,t)                                                             \
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::randn_, tag::cpu_                        \
                            , BOOST_PP_REPEAT(n,M2,~)                           \
                            , BOOST_PP_REPEAT(n,M1,~)                           \
                            )                                                   \
  {                                                                             \
    BOOST_DISPATCH_RETURNS( n                                                   \
                          , ( BOOST_PP_ENUM_BINARY_PARAMS(n,const A,& a) )      \
                          ,  nt2::randn(nt2::as_size(BOOST_PP_ENUM_PARAMS(n,a))) \
                          )                                                     \
  };                                                                            \
                                                                                \
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::randn_, tag::cpu_                        \
                            , BOOST_PP_REPEAT(n,M2,~)(T)                        \
                            , BOOST_PP_REPEAT(n,M1,~)                           \
                              (target_< scalar_< floating_<T> > >)              \
                            )                                                   \
  {                                                                             \
    BOOST_DISPATCH_RETURNS( BOOST_PP_INC(n)                                     \
                          , ( BOOST_PP_ENUM_BINARY_PARAMS(n,const A,& a)        \
                            , T const& tgt                                      \
                            )                                                   \
                          , (nt2::randn( nt2::as_size(BOOST_PP_ENUM_PARAMS(n,a)) \
                                      , tgt                                     \
                                      )                                         \
                            )                                                   \
                          )                                                     \
  };                                                                            \
  /**/

  BOOST_PP_REPEAT_FROM_TO(1, BOOST_PP_INC(NT2_MAX_DIMENSIONS), M0, ~)

  #undef M0
  #undef M1
  #undef M2

} }

#endif


/*=========================================================================
 *
 *   Program:   Bifurcation Analysis Library
 *   Module:    main.cpp
 *
 *   Copyright (C) 2009 Daniele Linaro
 *
 *   This program is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *=========================================================================*/

#include <unistd.h>
#include <iostream>
#include <cstdlib>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
using boost::property_tree::ptree;

#include <nvector/nvector_serial.h>

#include "balCommon.h"
#include "balParameters.h"
#include "balLogger.h"
#include "balODESolver.h"
#include "balBifurcationDiagram.h"
#include "balBifurcationParameters.h"
#include "balPLL.h"
using namespace bal;

const realtype pi = 3.1415926535897931;

int main(int argc, char *argv[]) {
  if(argc == 1) {
    fprintf(stderr, "Usage: %s ConfigFile\n", argv[0]);
    exit(1);
  }
  ptree config;
  read_xml(argv[1], config);
  char name[100];

  int steps[PLL::npar];
  BifurcationParameters * bp = BifurcationParameters::Create();
  bp->SetNumber(PLL::npar);

  for(int i=0; i<PLL::npar; i++) {
    sprintf(name,"pll.%s.steps",PLL::parname[i]);
    steps[i] = config.get<int>(name);
    if(steps[i] == 1) {
      sprintf(name,"pll.%s.min",PLL::parname[i]);
      bp->SetIthParameter(i,config.get<double>(name));
    }
    else {
      sprintf(name,"pll.%s.min",PLL::parname[i]);
      bp->SetIthParameterLowerBound(i,config.get<double>(name));
      sprintf(name,"pll.%s.max",PLL::parname[i]);
      bp->SetIthParameterUpperBound(i,config.get<double>(name));
    }
  }

#ifdef WITHPHIERR
#ifdef EXTEND
  realtype x0[7] = {0.,0.,0.,0.,pi,0.,0.};
#else
  realtype x0[5] = {0.,0.,0.,0.,pi};
#endif
#else
#ifdef EXTEND
  realtype x0[6] = {0.,0.,0.,0.,0.,0.};
#else
  realtype x0[4] = {0.,0.,0.,0.};
#endif
#endif
  x0[1] = config.get<double>("pll.vdd.min");

  bp->SetNumberOfSteps(steps);
  PLL * pll = PLL::Create();
  pll->SetParameters(bp);
  BifurcationDiagram * bifd = BifurcationDiagram::Create();
  bifd->RestartFromX0(true);
  bifd->SetDynamicalSystem(pll);
  bifd->SetFilename((char *) config.get<std::string>("simulation.outputfile").c_str());
  if(config.get<bool>("simulation.trajectory"))
    bifd->GetODESolver()->SetIntegrationMode(BOTH);
  else
    bifd->GetODESolver()->SetIntegrationMode(EVENTS);
  bifd->GetODESolver()->SetTransientDuration(config.get<double>("simulation.ttran"));
  bifd->GetODESolver()->HaltAtEquilibrium(false);
  bifd->GetODESolver()->HaltAtCycle(false);
  bifd->GetODESolver()->SetFinalTime(config.get<double>("simulation.tout"));
  bifd->GetODESolver()->SetTimeStep(1e-11);
  bifd->GetODESolver()->SetMaxNumberOfIntersections((int) 1e7);
  bifd->GetODESolver()->SetX0(x0);
  bifd->GetODESolver()->SetRelativeTolerance(1e-10);
  bifd->SetNumberOfThreads(1);
  bifd->ComputeDiagram();
  bifd->Destroy();
  pll->Destroy();
  bp->Destroy();

  return EXIT_SUCCESS;
}


// Copyright (c) 2017 Shota SUGIHARA
// Distributed under the MIT License.
#include <iostream>
#include <boost/format.hpp>
#include <Eigen/Core>
#include "condition.h"
#include "grid.h"
#include "particles.h"

// Sample code using TinyMPS library.
int main(int argc, char* argv[]) {
  try {
    std::string output_path = "./output/";
    std::string input_data = "./input/input.data";
    std::string input_grid = "./input/dam.grid";
    if (argc >= 2) output_path = argv[1];
    if (argc >= 3) input_data = argv[2];
    if (argc >= 4) input_grid = argv[3];
    output_path += "output_%1%.vtk";
    tiny_mps::Condition condition(input_data);
    tiny_mps::Particles particles(input_grid, condition);
    tiny_mps::Timer timer(condition);
    Eigen::Vector3d minpos(-0.1, -0.1, 0);
    Eigen::Vector3d maxpos(1.1, 2.1, 0);
    while(particles.nextLoop(output_path, timer)) {
      particles.calculateTemporaryVelocity(condition.gravity, timer);
      particles.updateTemporaryPosition(timer);
      particles.giveCollisionRepulsionForce();
      particles.updateTemporaryPosition(timer);
      particles.calculateTemporaryParticleNumberDensity();
      particles.checkSurfaceParticles();
      particles.solvePressurePoisson(timer);
      particles.setZeroOnNegativePressure();
      particles.correctVelocity(timer);
      particles.updateTemporaryPosition(timer);
      particles.updateVelocityAndPosition();
      particles.removeOutsideParticles(minpos, maxpos);
    }
    return 0;
  } catch (const std::exception& e) {
    std::cerr << e.what() << std::endl;
    exit(EXIT_FAILURE);
  }
  return 1;
}


//==============================================================================
//         Copyright 2009 - 2014 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_SWAR_FUNCTIONS_SIMD_SSE_SSE2_DETAILS_TOPOLOGY_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_DETAILS_TOPOLOGY_HPP_INCLUDED

#include <boost/simd/swar/functions/details/permuted.hpp>
#include <boost/simd/swar/functions/details/topology.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/if.hpp>

namespace boost { namespace simd { namespace details
{
  //============================================================================
  // Make a MM_SHUFFLE from a Permutation and Cardinal
  //============================================================================
  template<typename P, std::size_t Cardinal> struct  shuffle_mask;

  template<typename P>
  struct  shuffle_mask<P,2>
        : boost::mpl::int_
          < _MM_SHUFFLE2( (details::permuted<details::fix_zero<P>,1,2>::value)
                        , (details::permuted<details::fix_zero<P>,0,2>::value)
                        )
          >
  {};

  template<typename P>
  struct  shuffle_mask<P,4>
        : boost::mpl::int_
          < _MM_SHUFFLE ( (details::permuted<details::fix_zero<P>,3,4>::value)
                        , (details::permuted<details::fix_zero<P>,2,4>::value)
                        , (details::permuted<details::fix_zero<P>,1,4>::value)
                        , (details::permuted<details::fix_zero<P>,0,4>::value)
                        )
          >
  {};

  //============================================================================
  // Markup for SSE2 topology
  // direct_    means the pattern is of the shape { a0 | a1 }
  // indirect_  means the pattern is of the shape { a1 | a0 }
  // mixed_     means the pattern is random
  //============================================================================
  typedef boost::mpl::int_<-1>  indirect_;
  typedef boost::mpl::int_<0>   mixed_;
  typedef boost::mpl::int_<1>   direct_;

  //============================================================================
  // Check if a permutation follows SSE2 shuffle restriction in a way or another
  //============================================================================
  template<typename Permutation, std::size_t Cardinal>
  struct sse2_topology
  {
    typedef indirect_ type;
  };

  //============================================================================
  // Cardinal 2 is either direct or indirect, no room for mixed
  //============================================================================
  template<typename Permutation>
  struct sse2_topology<Permutation,2>
  {
    static const int p0 = permuted<Permutation,0,2>::value;
    static const int p1 = permuted<Permutation,1,2>::value;

    typedef typename mpl::if_c< (p0 <= 1) && (p1 >= 2)
                              , direct_
                              , indirect_
                              >::type type;
  };

  //============================================================================
  // Cardinal 4 can be anything
  //============================================================================
  template<typename Permutation>
  struct sse2_topology<Permutation,4>
  {
    static const int p0 = permuted<Permutation,0,4>::value;
    static const int p1 = permuted<Permutation,1,4>::value;
    static const int p2 = permuted<Permutation,2,4>::value;
    static const int p3 = permuted<Permutation,3,4>::value;

    static const bool is_direct =   (p0 < 4 ) &&  (p1 < 4 )
                                &&  (p2 >= 4) &&  (p3 >= 4);

    static const bool is_indirect =   (p2 < 4 ) &&  (p3 < 4 )
                                  &&  (p0 >= 4) &&  (p1 >= 4);

    typedef typename mpl::eval_if_c < is_direct
                                    , direct_
                                    , mpl::eval_if_c< is_indirect
                                                    , indirect_
                                                    , mixed_
                                                    >
                                    >::type type;
  };
} } }

#endif


//****************************************************************************
// (c) 2012 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
#ifndef openOR_HistogramAnalyser_hpp
#define openOR_HistogramAnalyser_hpp

#include <openOR/Callable.hpp> //basic
#include <openOR/DataSettable.hpp> //basic
#include <openOR/Utility/Types.hpp> //openOR_core
#include <openOR/Math/vector.hpp> //openOr__core

#include <string>
#include <memory>

#include <openOR/Image/Image1DData.hpp> //Image_ImageData

#include <boost/shared_array.hpp>

#include <openOR/Defs/Image_Utility.hpp>

namespace openOR {
   namespace Image {

      //! Ported from FBPARTViewer (VolumeHistogramAnalyser)
      class OPENOR_IMAGE_UTILITY_API HistogramAnalyser : public Callable, public DataSettable {

      public:
         HistogramAnalyser();
         virtual ~HistogramAnalyser();

         void operator()() const;
         void setData(const AnyPtr& data, const std::string& name = "");
         uint32 objectThreshold() const;
         uint32 backgroundPeak() const;

         bool carrierMaterial() const;
         void setCarrierMaterial(bool newCarrierMaterial);

         bool background() const;
         void setBackground(bool newBackground);

         bool searchforhighdensitycarriermaterial() const;
         void setSearchforhighdensitycarriermaterial(bool newValue);

      private:
         // openOR-secific stuff
         std::tr1::shared_ptr<Image1DData<Triple<size_t> > > m_pRegions;
         std::tr1::shared_ptr<Image1DDataUI64> m_pHistogram;
         std::tr1::shared_ptr<uint32> m_pObjectThreshold, m_pBackground;
         bool m_background, m_carrierMaterial, m_searchforhighdensitycarriermaterial;

         // FBPARTViewer copied stuff
         // needed to create a new class because of const constraint on operator()()
         struct HistogramAnalyserImpl {
            HistogramAnalyserImpl();
            ~HistogramAnalyserImpl();

            std::vector<Math::Vector3ui> getDensityIntervals(bool bBackground = true,bool bCarrierMaterial=true);
            uint getBackgroundPeakGreyValue();
            uint getObjectThreshold();

            void setParameter(uint nBlockSize,uint nBlockedHistogramFilterSize,uint nFirstDerivationFilterSize,uint nSecondDerivationFilterSize,float fHistThreshold,float fSecondDerivationThreshold,float fAbsoluteSectionDifferenceThreshold,float fRelativePeakSectionDifferenceThreshold, float fMinNumOfVoxelInSection,float fCarrierMaterialDistance,float fRelativePeakDistanceThreshold, float fHighDensityCarrierMaterialDistance);
            bool computeDensityIntervals(boost::shared_array<long long>& m_Histogram, uint m_nHistogramDataSize, bool bSearchforhighdensitycarriermaterial = true);

         private:

            // blocking size
            uint m_nBlockSize;

            // filter length for the blocked histogram
            uint m_nBlockedHistogramFilterSize;

            // filter length for first derivation
            uint m_nFirstDerivationFilterSize;

            // filter length for second derivation
            uint m_nSecondDerivationFilterSize;

            // mimimum voxel number for maxima detection in percent
            float m_fHistThreshold;

            // minimum second derivation height for maxima detection relative to number of voxels in percent
            float m_fSecondDerivationThreshold;

            // minimum difference between two sections in percent relative to voxel number
            float m_fAbsoluteSectionDifferenceThreshold;

            // minimum difference between a peak and section borders in percent
            float m_fRelativePeakSectionDifferenceThreshold;

            //min number of voxel in section
            float m_fMinNumOfVoxelInSection;

            //minimum distance between two peaks in percent of total histogram size
            float m_fRelativePeakDistanceThreshold;

            // relative density based on background gray value
            float m_fCarrierMaterialDistance;

            // relative density based on background gray value
            float m_fHighDensityCarrierMaterialDistance;

            bool blockHistogram(const boost::shared_array<long long>& refHistogram,const uint nHistogramDataSize, std::vector<double>& refBlockedHistogram,const uint nBlockSize,uint& nMaxValue) const;
            bool filterHistogram(const std::vector<double>& refOriginalHistogram, std::vector<double>& refFilteredHistogram,const uint nFilterSize) const;
            bool deriveHistogram(const std::vector<double>& refOriginalHistogram, std::vector<double>& refDerivedHistogram) const;

            bool detectExtremeValues(std::vector<int>& vecExtremeValues,long long NumOfVoxels, std::vector<double>& FilteredHistogram, uint nFilterSizeFilteredHistogram, std::vector<double>& FirstDerivation, uint nFilterSizeFirstDerivation,std::vector<double>& SecondDerivation, uint nFilterSizeSecondDerivation, float fHistThreshold, float SecondDerivationThreshold);
            bool detectSections(std::vector<int>& vecExtremeValues,std::vector<double>& BlockedHistogram,long long nNumOfVoxels,float fAbsoluteSectionDifferenceThreshold,float fRelativePeakSectionDifferenceThreshold,std::vector<uint>& vecSections,float fRelativePeakDistanceThreshold,uint nBlockedHistogramFilterSize);
            bool buildIntervals(std::vector<uint>& vecSections,std::vector<Math::Vector3ui>& vecDensityIntervals);
            bool findMissingMaxima(std::vector<double>& refBlockedHistogram,std::vector<Math::Vector3ui>&   vecDensityIntervals,long long nNumOfVoxels,float fAbsoluteSectionDifferenceThreshold,float fRelativePeakSectionDifferenceThreshold);
            bool mergeIntervals(std::vector<Math::Vector3ui>& vecDensityIntervals,std::vector<double>& refBlockedHistogram,uint nNumOfVoxels,float fRelativePeakSectionDifferenceThreshold, float fAbsoluteSectionDifferenceThreshold,uint nBlockedHistogramFilterSize, float fMinNumOfVoxelInSection);
            bool rescaleIntervals(std::vector<Math::Vector3ui>& vecDensityIntervals,uint nBlockSize);
            bool analyzeDetectedIntervals(std::vector<Math::Vector3ui>& vecDensityIntervals,std::vector<Math::Vector3ui>& vecDensityIntervalsWithoutBackground,std::vector<Math::Vector3ui>& vecDensityIntervalsWithoutBackgroundAndCarrierMaterial,uint nBackgroundPeakGreyValue,float fCarrierMaterialCoeff, float fHighDensityCarrierMaterialDistance, uint nBlockSize,const std::vector<double>& refBlockedHistogram, uint nTotalGreyScaleInterval);
            bool findMinMax(const std::vector<double>& BlockedHistogram,const uint nStartIndex,const uint nEndIndex, int& nMinIndex, double& fMinValue, int& nMaxIndex, double& fMaxValue) const;
            //has interval enough voxels?
            bool hasIntervalEnoughVoxels(std::vector<double>& refBlockedHistogram,Math::Vector3ui& vecRegion, double fThreshold);

            //scan the histogram for masking grew value => bRemove=true removes it bRemove=false adds it
            bool modifyHistogram_MaskingGreyValue(bool bRemove,double fThreshold = 5.0,double fMaxDiffThreshold = 100.0);

            // copmute the total number of voxels of the volume
            long long getNumOfVoxel(const boost::shared_array<long long>& refHistogram,const uint nHistogramDataSize) const;

            void init();

            std::vector<double>  m_vecBlockedHistogram;
            std::vector<double>  m_vecFilteredHistogram;
            std::vector<double>  m_vecFirstDerivation;
            std::vector<double>  m_vecSecondDerivation;
            std::vector<double>  m_vecTemp;
            std::vector<int>   m_vecExtremeValues;
            std::vector<uint> m_vecSections;
            uint m_nBackgroundPeakGreyValue;
            uint m_nObjectThreshold;
            Math::Vector2ll m_vecMaskingPeak;
            std::vector<Math::Vector3ui>   m_vecDensityIntervals;
            std::vector<Math::Vector3ui>   m_vecDensityIntervalsWithoutBackground;
            std::vector<Math::Vector3ui>   m_vecDensityIntervalsWithoutBackgroundAndCarrierMaterial;

            uint m_nSectionBeginValue;
            uint m_nSectionMaxvalue;
         };
      };
   }
}
#endif //openOR_HistogramAnalyser_hpp


//
//  synthetic_data_helper.cpp
//  synthetic_data_helper
//
//  Created by Cristián Garay on 10/15/16.
//  Copyright © 2016 Cristian Garay. All rights reserved.
//

#define NPY_NO_DEPRECATED_API NPY_1_11_API_VERSION

#include <iostream>
#include <stdint.h>
#include <alloca.h>
#include <Eigen/Core>
#include <boost/python.hpp>
#include <boost/python/numeric.hpp>
#include <boost/python/ptr.hpp>
#include <Python.h>
#include <numpy/ndarrayobject.h>

using namespace Eigen;
using namespace std;
using namespace boost::python;

#if PY_VERSION_HEX >= 0x03000000
void *
#else
void
#endif
init_numpy(){
    //Py_Initialize;
    import_array();
}

struct double_to_python_float
{
    static PyObject* convert(double const& d)
      {
        return boost::python::incref(
          boost::python::object(d).ptr());
      }
};

//numpy scalar converters.
template <typename T, NPY_TYPES NumPyScalarType>
struct enable_numpy_scalar_converter
{
  enable_numpy_scalar_converter()
  {
    // Required NumPy call in order to use the NumPy C API within another
    // extension module.
    // import_array();
    init_numpy();

    boost::python::converter::registry::push_back(
      &convertible,
      &construct,
      boost::python::type_id<T>());
  }

  static void* convertible(PyObject* object)
  {
    // The object is convertible if all of the following are true:
    // - is a valid object.
    // - is a numpy array scalar.
    // - its descriptor type matches the type for this converter.
    return (
      object &&                                                    // Valid
      PyArray_CheckScalar(object) &&                               // Scalar
      PyArray_DescrFromScalar(object)->type_num == NumPyScalarType // Match
    )
      ? object // The Python object can be converted.
      : NULL;
  }

  static void construct(
    PyObject* object,
    boost::python::converter::rvalue_from_python_stage1_data* data)
  {
    // Obtain a handle to the memory block that the converter has allocated
    // for the C++ type.
    namespace python = boost::python;
    typedef python::converter::rvalue_from_python_storage<T> storage_type;
    void* storage = reinterpret_cast<storage_type*>(data)->storage.bytes;

    // Extract the array scalar type directly into the storage.
    PyArray_ScalarAsCtype(object, storage);

    // Set convertible to indicate success.
    data->convertible = storage;
  }
};

dict create_synthetic_data(dict& model, numeric::array& starts, numeric::array& lengths, numeric::array& resources){
    //TODO: check if parameters are null.
    //TODO: check that dicts have the required members.
    //TODO: check that all parameters have the right sizes.
    //TODO: i'm not sending any error messages.

    numeric::array learns = extract<numeric::array>(model["learns"]);
    int num_resources = len(learns);

    numeric::array forgets = extract<numeric::array>(model["forgets"]);
    numeric::array guesses = extract<numeric::array>(model["guesses"]);

    numeric::array slips = extract<numeric::array>(model["slips"]);
    int num_subparts = len(slips);

    Vector2d initial_distn;
    double prior = extract<double>(model["prior"]);
    initial_distn << 1-prior, prior;

    MatrixXd As(2, 2*num_resources);
    for (int n=0; n<num_resources; n++) {
        double learn = extract<double>(learns[n]);
        double forget = extract<double>(forgets[n]);
        As.col(2*n) << 1-learn, learn;
        As.col(2*n+1) << forget, 1-forget;
    }

    int num_sequences = len(starts);

    int64_t bigT = 0;
    for (int k=0; k<num_sequences; k++) {
        bigT += extract<int64_t>(lengths[k]); //extract this as int??
    }

    //// outputs
    int all_stateseqs[1][bigT]; //used to be int8_t
    int all_data[num_subparts][bigT]; //used to be int8_t
    all_data[0][0] = 0;
    dict result;

    /* COMPUTATION */

    for (int sequence_index=0; sequence_index < num_sequences; sequence_index++) {
        int64_t sequence_start = extract<int64_t>(starts[sequence_index]) - 1; //should i extract these as ints?
        int64_t T = extract<int64_t>(lengths[sequence_index]);

        Vector2d nextstate_distr = initial_distn;

        for (int t=0; t<T; t++) {
            all_stateseqs[0][sequence_start + t] = nextstate_distr(0) < ((double) rand()) / ((double) RAND_MAX); //always all_stateseqs[0]?
            for (int n=0; n<num_subparts; n++) {
                all_data[n][sequence_start+t] = ((all_stateseqs[0][sequence_start + t]) ? extract<double>(slips[n]) : (1-extract<double>(guesses[n]))) < (((double) rand()) / ((double) RAND_MAX));
            }

            nextstate_distr = As.col(2*(extract<int64_t>(resources[sequence_start + t])-1)+all_stateseqs[0][sequence_start + t]); //extract int is right??
        }
    }

    //wrapping results in numpy objects.
    npy_intp all_stateseqs_dims[2] = {1, bigT}; //just put directly this array into the PyArray_SimpleNewFromData function?
    PyObject * all_stateseqs_pyObj = PyArray_SimpleNewFromData(2, all_stateseqs_dims, NPY_INT, all_stateseqs); //this should be NPY_INT8.
    boost::python::handle<> all_stateseqs_handle( all_stateseqs_pyObj );
    boost::python::numeric::array all_stateseqs_handle_arr( all_stateseqs_handle );

    npy_intp all_data_dims[2] = {num_subparts, bigT}; //just put directly this array into the PyArray_SimpleNewFromData function?
    PyObject * all_data_pyObj = PyArray_SimpleNewFromData(2, all_data_dims, NPY_INT, all_data); //this should be NPY_INT8.
    boost::python::handle<> all_data_handle( all_data_pyObj );
    boost::python::numeric::array all_data_arr( all_data_handle );

    result["stateseqs"] = all_stateseqs_handle_arr;
    result["data"] = all_data_arr;
    return(result);

}

BOOST_PYTHON_MODULE(synthetic_data_helper){
    //import_array();
    init_numpy();
    /*if(PyArray_API == NULL)
	{
	    import_array();
	}*/
    numeric::array::set_module_and_type("numpy", "ndarray");
    to_python_converter<double, double_to_python_float>();
    enable_numpy_scalar_converter<boost::int8_t, NPY_INT8>();
    enable_numpy_scalar_converter<boost::int16_t, NPY_INT16>();
    enable_numpy_scalar_converter<boost::int32_t, NPY_INT32>();
    enable_numpy_scalar_converter<boost::int64_t, NPY_INT64>();

    def("create_synthetic_data", create_synthetic_data);

}


/// @file DeconvolverEntropy.tcc
/// @brief Class for a deconvolver based on entropy or min L1 norm
/// @details This concrete class defines a deconvolver used to estimate an
/// image from a residual image, psf optionally using a weights image.
/// @ingroup Deconvolver
///
/// @copyright (c) 2007 CSIRO
/// Australia Telescope National Facility (ATNF)
/// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
/// PO Box 76, Epping NSW 1710, Australia
/// atnf-enquiries@csiro.au
///
/// This file is part of the ASKAP software distribution.
///
/// The ASKAP software distribution is free software: you can redistribute it
/// and/or modify it under the terms of the GNU General Public License as
/// published by the Free Software Foundation; either version 2 of the License,
/// or (at your option) any later version.
///
/// This program is distributed in the hope that it will be useful,
/// but WITHOUT ANY WARRANTY; without even the implied warranty of
/// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
/// GNU General Public License for more details.
///
/// You should have received a copy of the GNU General Public License
/// along with this program; if not, write to the Free Software
/// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
///
/// @author Tim Cornwell <tim.cornwell@csiro.au>
///

#include <string>

#include <casacore/casa/aips.h>
#include <casacore/casa/Arrays/Array.h>
#include <casacore/casa/Arrays/ArrayMath.h>
#include <boost/shared_ptr.hpp>
#include <askap/AskapLogging.h>
ASKAP_LOGGER(decentropylogger, ".deconvolution.entropy");

#include <deconvolution/DeconvolverEntropy.h>
#include <deconvolution/EntropyBase.h>
#include <deconvolution/EntropyI.h>
#include <deconvolution/Emptiness.h>

using namespace casa;

namespace askap {

    namespace synthesis {

        /// @brief Class for a deconvolver based on the Entropy algorithm of Cornwell and Evans
        /// @details This base class defines a deconvolver used to estimate an
        /// image from a residual image, psf optionally using a weights image.
        /// The template argument T is the type, and FT is the transform
        /// e.g. DeconvolverEntropy<Double, DComplex>
        /// @ingroup Deconvolver

        template<class T, class FT>
        DeconvolverEntropy<T, FT>::~DeconvolverEntropy()
        {
        };

        template<class T, class FT>
        DeconvolverEntropy<T, FT>::DeconvolverEntropy(Vector<Array<T> >& dirty, Vector<Array<T> >& psf)
                : DeconvolverBase<T, FT>::DeconvolverBase(dirty, psf)
        {
            if (this->itsNumberDirtyTerms > 1) {
                throw(AskapError("Entropy deconvolver cannot perform multi-term deconvolutions"));
            }
        };

        template<class T, class FT>
        DeconvolverEntropy<T, FT>::DeconvolverEntropy(Array<T>& dirty, Array<T>& psf)
                : DeconvolverBase<T, FT>::DeconvolverBase(dirty, psf)
        {
        };
        template<class T, class FT>
        void DeconvolverEntropy<T, FT>::finalise()
        {
            // Find residuals for current model model
            this->updateResiduals(this->itsModel);
        }

        template<class T, class FT>
        void DeconvolverEntropy<T, FT>::initialise()
        {
            DeconvolverBase<T, FT>::initialise();

            // Set the initial value of Q
            T Q;
            Q = sum(this->psf()(this->psf() > T(0.1)));
            ASKAPLOG_INFO_STR(decentropylogger, "Initial value of Q = " << Q << " pixels");

            this->itsEntropy->setQ(Q);
        }

        // This is basically the algorithm described in the Cornwell-Evans paper of 1985, with
        // some improvements as suggested by Bob Sault.
        template<class T, class FT>
        bool DeconvolverEntropy<T, FT>::deconvolve()
        {
            this->initialise();

            ASKAPLOG_INFO_STR(decentropylogger, "Performing Entropy deconvolution for "
                                  << this->control()->targetIter() << " iterations");

            ASKAPLOG_INFO_STR(decentropylogger, "Target rms fit = "
                                  << this->control()->targetObjectiveFunction() << " Jy/beam");

            uInt numberPixels = this->itsModel.shape().product();
            T targetChisq = square(this->control()->targetObjectiveFunction()) * numberPixels;
            T chisq;

            Array<T> trialModel(this->model().shape());
            trialModel.set(T(0.0));

            this->updateResiduals(this->model());

            T lipschitz(10.0);

            this->model() = this->dirty().copy() / lipschitz;

            Array<T> step(this->model().shape());
            step.set(T(0.0));

            T absPeakVal(max(abs(this->dirty())));
            T peakSidelobe(0.1);
            T aFit = max(peakSidelobe * absPeakVal, rms(this->dirty())) / lipschitz;
            ASKAPLOG_INFO_STR(decentropylogger, "Scaling = " << aFit << " Jy/pixel");
            this->itsEntropy->setScale(aFit);

            this->model().set(aFit);

            this->updateResiduals(this->model());

            do {
                // Find the current fit
                chisq = sum(square(this->dirty()));

                Matrix<T> GDG(this->itsEntropy->formGDGStep(this->model(), this->dirty(), step));

                // Check to see if Alpha and Beta need to be initialised. If so then we need to
                // do so and recalculate the gradients and step
                if (this->itsEntropy->initialiseAlphaBeta(GDG)) {
                    GDG = this->itsEntropy->formGDGStep(this->model(), this->dirty(), step);
                }

                T flux = sum(this->model());
                this->itsEntropy->changeAlphaBeta(GDG, targetChisq, chisq, this->control()->targetFlux(), flux);

                // Now find the normalised gradient - we will use this to limit the step taken
                T length(this->itsEntropy->formLength(GDG));
                if (length <= T(0.0)) {
                    length = GDG(F, F);
                }
                T normGrad = GDG(J, J) / length;

                //        relaxMin();

                // We limit the step to less than the tolerance e.g. 0.1 so that the
                // quadratic approximation in the Newton-Raphson is still valid.
                T scale = 1.0;
                T scalem = 1.0;
                if (normGrad > 0.0) {
                    scalem = this->control()->tolerance() / normGrad;
                }
                scale = min(T(1.0), scalem);

                // OK - now we take the proposed step and evaluate the
                // gradient there.
                trialModel = this->model() + scale * step;

                // Calculate residual for this new trial image
                this->updateResiduals(trialModel);
                chisq = sum(square(this->dirty()));

                // Form the scalar Gradient . Step at this new location. Ideally this should be
                // zero. Once we know the value of the gradient initially and for the trial image, we
                // can determine the optimal step by interpolating the gradient to zero. Ideally the
                // step should be O(1) times the original step
                T eps = 1.0;
                T gradDotStep0 = GDG(J, J);
                T gradDotStep1(this->itsEntropy->formGDS(this->model(), this->dirty(), step));

                if (gradDotStep0 != gradDotStep1) {
                    eps = gradDotStep0 / (gradDotStep0 - gradDotStep1);
                }
                if (scale != T(0.0)) eps = min(eps, T(scalem / scale));
                if (eps <= T(0.0)) {
                    eps = T(1.0);
                }

                // Step to optimum point
                this->model() = this->model() + scale * eps * step;

                // Recalculate residual for the new image
                this->updateResiduals(this->model());

                chisq = sum(square(this->dirty()));

                // readjust beam volume
                //      itsQ = itsQ*(T(1.0)/max(T(0.5), min(T(2.0),eps))+T(1.0))/T(2.0);

                flux = sum(this->model());
                this->itsEntropy->changeAlphaBeta(GDG, targetChisq, chisq, this->control()->targetFlux(), flux);

                absPeakVal = max(abs(this->dirty()));

                T lipschitz(10.0);

                aFit = max(peakSidelobe * absPeakVal, rms(this->dirty())) / lipschitz;
                ASKAPLOG_INFO_STR(decentropylogger, "Scaling = " << aFit << " Jy/pixel");
                this->itsEntropy->setScale(aFit);

                this->state()->setPeakResidual(absPeakVal);
                this->state()->setObjectiveFunction(sqrt(chisq / numberPixels));
                this->state()->setTotalFlux(flux);

                this->monitor()->monitor(*(this->state()));
                this->state()->incIter();
            } while (!this->control()->terminate(*(this->state())));

            ASKAPLOG_INFO_STR(decentropylogger, "Performed Entropy deconvolution for "
                                  << this->state()->currentIter() << " iterations");

            ASKAPLOG_INFO_STR(decentropylogger, this->control()->terminationString());

            this->finalise();

            return True;
        }

        template<class T, class FT>
        void DeconvolverEntropy<T, FT>::configure(const LOFAR::ParameterSet& parset)
        {
            DeconvolverBase<T, FT>::configure(parset);

            String algorithm(parset.getString("algorithm", "Emptiness"));
            this->control()->setAlgorithm(algorithm);

            if (algorithm == "EntropyI") {
                ASKAPLOG_INFO_STR(decentropylogger, "Maximising information entropy of model image");
                itsEntropy = boost::shared_ptr<EntropyBase<T> >(new EntropyI<T>());
                itsEntropy->setTolerance(parset.getFloat("tolerance", 0.3));
            } else {
                ASKAPLOG_INFO_STR(decentropylogger, "Maximising emptiness (negative L1 norm) of model image");
                itsEntropy = boost::shared_ptr<EntropyBase<T> >(new Emptiness<T>());
                itsEntropy->setTolerance(parset.getFloat("tolerance", 0.3));
            }
        }

    } // namespace synthesis

} // namespace askap


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_MASK2LOGICAL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_MASK2LOGICAL_HPP_INCLUDED

#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/simd/is_nez.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( mask2logical_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::arithmetic_<A0> >
                          )
  {
    auto operator() ( A0 const& a0) const BOOST_NOEXCEPT -> decltype(is_nez(a0))
    {
      BOOST_ASSERT_MSG(assert_is_mask(a0), "Argument to mask2logical is not a valid logical mask");
      return is_nez(a0);
    }
  };
} } }

#endif


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE4_1_SIMD_FUNCTION_GROUP_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE4_1_SIMD_FUNCTION_GROUP_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/function/saturated.hpp>
#include <boost/simd/detail/dispatch/meta/downgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( group_
                          , (typename A0)
                          , bs::sse4_1_
                          , bs::saturated_tag
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE bd::downgrade_t<A0>
    operator()(const saturated_tag &,const A0 & a0, const A0 & a1 ) const BOOST_NOEXCEPT
    {
      A0 mask1 = bs::Constant<A0,0x7FFFFFFF>();
      A0 mask2 = bs::Constant<A0,0xF0000000>();
      return _mm_packus_epi32( bitwise_and(a0, mask1) | shift_right(bitwise_and(a0, mask2), 1)
                             , bitwise_and(a1, mask1) | shift_right(bitwise_and(a1, mask2), 1)
                             );
    }
  };
} } }

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DEFINITION_SELINC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_SELINC_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG(ext, selinc_, boost::dispatch::elementwise_<selinc_>);
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, selinc_);
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::selinc_,selinc);

} }

#endif


/*
==== Author:

Relja Arandjelovic (relja@robots.ox.ac.uk)
Visual Geometry Group,
Department of Engineering Science
University of Oxford

==== Copyright:

The library belongs to Relja Arandjelovic and the University of Oxford.
No usage or redistribution is allowed without explicit permission.
*/

#include "train_hamming.h"

#include <fstream>
#include <vector>

#include <boost/filesystem.hpp>

#ifdef RR_MPI
#include <boost/mpi/collectives.hpp>
#endif

#include <Eigen/SVD>

#include "clst_centres.h"
#include "flat_desc_file.h"
#include "hamming_data.pb.h"
#include "median_computer.h"
#include "mpi_queue.h"
#include "par_queue.h"
#include "protobuf_util.h"
#include "same_random.h"
#include "timing.h"

namespace buildIndex {

typedef std::vector<float> trainHammingResult; // medians after cluster centres were subtracted

class trainHammingManager : public managerWithTiming<trainHammingResult> {
    public:

        trainHammingManager(uint32_t const nJobs,
                            std::string const trainHammFn,
                            std::vector<float> const &rot,
                            uint32_t const numDims,
                            uint32_t const numClst,
                            uint32_t const vocChunkSize)
                            : managerWithTiming<trainHammingResult>(nJobs, "trainHammingManager"),
                              trainHammFn_(trainHammFn),
                              vocChunkSize_(vocChunkSize),
                              hammEmbBits_(rot.size() / numDims),
                              rot_(&rot) {
                ASSERT(rot.size() % numDims==0);

                hamm_.set_k(numClst);
                hamm_.set_numdims(numDims);
                hamm_.set_numbits(hammEmbBits_);

                // resize the array to its final size
                median_.resize(numClst*hammEmbBits_, 0);
            }

        ~trainHammingManager();

        void
            compute( uint32_t jobID, trainHammingResult &result );

    private:
        std::string trainHammFn_;
        uint32_t const vocChunkSize_, hammEmbBits_;
        rr::hammingData hamm_;
        std::vector<float> const *rot_;
        std::vector<float> median_;
        DISALLOW_COPY_AND_ASSIGN(trainHammingManager)
};

trainHammingManager::~trainHammingManager(){
    // organization:
    // magicNumber, sizeof(header), header, rotation, medians
    std::string headerStr;
    hamm_.SerializeToString(&headerStr);
    uint32_t const magicNumber= 0xF1234987;
    uint32_t const headerSize= headerStr.length();

    FILE *f= fopen(trainHammFn_.c_str(), "wb");

    fwrite( &(magicNumber), sizeof(uint32_t), 1, f );
    fwrite( &(headerSize), sizeof(uint32_t), 1, f );
    fwrite( &(headerStr[0]), sizeof(char), headerSize, f );
    fwrite( &((*rot_)[0]), sizeof(float), rot_->size(), f );
    fwrite( &median_[0], sizeof(float), median_.size(), f );

    fclose(f);
}

void
trainHammingManager::compute( uint32_t jobID, trainHammingResult &result ){
    // make sure results are saved sorted by job!
    uint32_t wordStart= jobID*vocChunkSize_;
    uint32_t const wordEnd= std::min( (jobID+1)*vocChunkSize_, hamm_.k() );
    ASSERT( (wordEnd-wordStart)*hammEmbBits_ == result.size());
    std::memcpy( &median_[wordStart*hammEmbBits_],
                 &result[0],
                 result.size() * sizeof(float) );
}

class trainHammingWorker : public queueWorker<trainHammingResult> {
    public:
        trainHammingWorker(std::string const trainDescsFn,
                           std::string const trainAssignsFn,
                           bool const RootSIFT,
                           std::vector<float> const &rot,
                           clstCentres const &clstCentres_obj,
                           uint32_t const vocChunkSize)
                           : descFile_(trainDescsFn, RootSIFT),
                             hammEmbBits_(rot.size() / descFile_.numDims()),
                             numClst_(clstCentres_obj.numClst),
                             vocChunkSize_(vocChunkSize),
                             numDims_(descFile_.numDims()),
                             numDescs_(descFile_.numDescs()),
                             rot_(&rot),
                             clstCentres_obj_(&clstCentres_obj){
                  ASSERT(clstCentres_obj_->numDims==numDims_);
                  ASSERT(rot.size() % numDims_==0);
                  f_= fopen(trainAssignsFn.c_str(), "rb");
                  fd_= fileno(f_);
            }

        ~trainHammingWorker(){ fclose(f_); }

        void
            operator() ( uint32_t jobID, trainHammingResult &result ) const;

    private:
        flatDescsFile const descFile_;
        FILE *f_;
        int fd_;
        uint32_t const hammEmbBits_, numClst_, vocChunkSize_, numDims_, numDescs_;
        std::vector<float> const *rot_;
        clstCentres const *clstCentres_obj_;

        DISALLOW_COPY_AND_ASSIGN(trainHammingWorker)
};

void
trainHammingWorker::operator() ( uint32_t jobID, trainHammingResult &result ) const {

    result.clear();

    uint32_t wordStart= jobID*vocChunkSize_;
    uint32_t const wordEnd= std::min( (jobID+1)*vocChunkSize_, numClst_ );
    std::vector<medianComputer> medianComp( (wordEnd-wordStart)*hammEmbBits_ );

    uint32_t const descChunkSize= 1000;
    uint32_t clusterIDs[descChunkSize];

    for (uint32_t iDescStart= 0; iDescStart<numDescs_; ){
        uint32_t iDescEnd= std::min(iDescStart + descChunkSize, numDescs_);

        // descriptors
        float *descs;
        descFile_.getDescs(iDescStart, iDescEnd, descs);
        // clusters
        ASSERT( pread64(fd_, clusterIDs,
                       (iDescEnd-iDescStart)*sizeof(uint32_t),
                       static_cast<uint64_t>(iDescStart)*sizeof(uint32_t)) > 0 );

        uint32_t const count= iDescEnd-iDescStart;
        float *itDesc= descs;
        uint32_t *itClusterID= clusterIDs;

        for (uint32_t iDesc= 0; iDesc<count; ++iDesc, ++itClusterID){
            // process only the descriptors with cluster inside [wordStart,wordEnd)
            if (*itClusterID < wordStart || *itClusterID >= wordEnd){
                itDesc+= numDims_;
                continue;
            }

            float * const thisDesc= itDesc;

            // subtract cluster centres
            float const *itC= clstCentres_obj_->clstC_flat + (*itClusterID) * numDims_;
            for (uint32_t iDim= 0; iDim<numDims_; ++iDim, ++itDesc, ++itC)
                *itDesc-= *itC;

            // rotate and add to median computer for every projection dimension
            medianComputer *itMC= &medianComp[0] + (*itClusterID-wordStart) * hammEmbBits_;
            float projection;
            float const *itDescEnd= thisDesc+numDims_;
            float const *itDescC;
            float const *itRot= &((*rot_)[0]);

            for (uint32_t iDim= 0; iDim < hammEmbBits_; ++iDim, ++itMC){
                projection= 0.0f;
                for (itDescC= thisDesc; itDescC!=itDescEnd; ++itDescC, ++itRot)
                    projection+= *itDescC * (*itRot);

                itMC->add(projection);
            }
        }

        delete []descs;

        iDescStart= iDescEnd;
    }

    result.clear();
    result.reserve( (wordEnd-wordStart)*hammEmbBits_ );
    for (medianComputer *itMC= &medianComp[0]; wordStart<wordEnd; ++wordStart){
        for (uint32_t iDim= 0; iDim < hammEmbBits_; ++iDim, ++itMC)
            result.push_back( itMC->getMedian() );
    }

}

void
computeHamming(
        std::string const clstFn,
        bool const RootSIFT,
        std::string const trainDescsFn,
        std::string const trainAssignsFn,
        std::string const trainHammFn,
        uint32_t const hammEmbBits){

    MPI_GLOBAL_ALL;

    if (boost::filesystem::exists(trainHammFn)){
        if (rank==0)
            std::cout<<"buildIndex::computeHamming: trainHammFn already exist ("<<trainHammFn<<")\n";
        return;
    }
    ASSERT( boost::filesystem::exists(trainDescsFn) );
    ASSERT( boost::filesystem::exists(trainAssignsFn) );
    ASSERT( hammEmbBits<=64 );

    // clusters
    if (rank==0)
        std::cout<<"buildIndex::computeHamming: Loading cluster centres\n";
    double t0= timing::tic();
    clstCentres const clstCentres_obj( clstFn.c_str(), true );
    uint32_t numClst= clstCentres_obj.numClst;
    uint32_t numDims= clstCentres_obj.numDims;
    if (rank==0)
        std::cout<<"buildIndex::computeHamming: Loading cluster centres - DONE ("<< timing::toc(t0) <<" ms)\n";
    ASSERT(numDims>=hammEmbBits);

    bool useThreads= detectUseThreads();
    uint32_t numWorkerThreads= 4;

    // rotation
    std::vector<float> rot;

    if (rank==0){
        #if 1
        // Find the rotation (projection) of the descriptor residuals
        // Original paper does random rotation
        // Better to do PCA to get the top hammEmbBits directions
        // Then do random rotation only on top of this, in order to balance the variances
        // e.g. see the Iterative Quantization paper (this would be even better but too complicated so an overkill)

        // --- do PCA of input descriptors (well, residuals as this is roughly 0-centred) to pick

        // descriptors & assigned clusters
        flatDescsFile descFile(trainDescsFn, RootSIFT);
        uint32_t const numTrainDescsTotal= descFile.numDescs();
        uint32_t const numDims= descFile.numDims();
        FILE *f= fopen(trainAssignsFn.c_str(), "rb");
        ASSERT(f!=NULL);
        int fd= fileno(f);
        uint32_t numTrainDescsPCA= std::min( numTrainDescsTotal, static_cast<uint32_t>(100000) );
        uint32_t const blockSize= std::min(numTrainDescsPCA, static_cast<uint32_t>(1000));
        numTrainDescsPCA-= numTrainDescsPCA%blockSize;
        uint32_t const blockStep= numTrainDescsTotal / (numTrainDescsPCA/blockSize);

        float *descs= new float[numTrainDescsPCA*numDims];
        float *itDesc= descs;
        float const *descsEnd= descs + numTrainDescsPCA*numDims;
        uint32_t *clusterIDs= new uint32_t[blockSize];
        std::cout<<"buildIndex::computeHamming: Reading training "<<numTrainDescsPCA<<" descriptors for PCA\n";

        for (uint32_t iDescStart= 0; itDesc!=descsEnd; iDescStart+= blockStep){
            float *thisBlockDescs;
            // descriptors
            descFile.getDescs(iDescStart, iDescStart+blockSize, thisBlockDescs);
            float const *thisDescIt= thisBlockDescs;
            // clusters
            ASSERT( pread64(fd, clusterIDs,
                            blockSize*sizeof(uint32_t),
                            static_cast<uint64_t>(iDescStart)*sizeof(uint32_t)) > 0 );
            // subtract cluster centres
            uint32_t const *endClstID= clusterIDs + blockSize;
            for (uint32_t const *itClusterID= clusterIDs; itClusterID!=endClstID; ++itClusterID){
                float const *itC= clstCentres_obj.clstC_flat + (*itClusterID) * numDims;
                for (uint32_t iDim= 0; iDim<numDims; ++iDim, ++itDesc, ++itC, ++thisDescIt)
                    *itDesc= *thisDescIt - *itC;
            }
            delete []thisBlockDescs;
        }
        fclose(f);
        delete []clusterIDs;

        double t0= timing::tic();
        std::cout<<"buildIndex::computeHamming: Computing PCA\n";
        Eigen::Map<Eigen::MatrixXf> trainData(descs, numDims, numTrainDescsPCA);
        Eigen::JacobiSVD<Eigen::MatrixXf> svdForPCA(trainData, Eigen::ComputeThinU);
        // doing SVD of trainData is like eig(trainData*trainData'), leftmost columns of U are largest eigenvectors
        Eigen::MatrixXf PCA= svdForPCA.matrixU().block(0,0, numDims,hammEmbBits);
        ASSERT(PCA.rows()==numDims && PCA.cols()==hammEmbBits);
        delete []descs;

        // --- get the random rotation (hammEmbBits x hammEmbBits)
        std::cout<<"buildIndex::computeHamming: Computing random rotation\n";

        // make random matrix
        sameRandomUint32 sr(hammEmbBits * hammEmbBits, 43);
        sameRandomStreamUint32 srS(sr);
        Eigen::MatrixXf randMatrix(hammEmbBits, hammEmbBits);
        for (uint32_t i= 0; i<hammEmbBits; ++i)
            for (uint32_t j= 0; j<hammEmbBits; ++j)
                randMatrix(i,j)= srS.getNextFloat();

        // get random orthonormal matrix by doing SVD
        Eigen::JacobiSVD<Eigen::MatrixXf> svdForRR(randMatrix, Eigen::ComputeFullU);
        Eigen::MatrixXf R= svdForRR.matrixU();
        ASSERT(R.cols()==hammEmbBits && R.rows()==hammEmbBits);
        // check stuff orthogonal just in case, but there is no way it isn't
        for (uint32_t i= 0; i<hammEmbBits; ++i)
            ASSERT( fabs( R.row(i).norm() -1)<1e-4 );

        // --- finally, compute the final rotation via R*PCA'
        R*= PCA.transpose();

        #else

        // just do random rotation
        double t0= timing::tic();

        // --- get the random rotation (hammEmbBits x numDims)
        std::cout<<"buildIndex::computeHamming: Computing random rotation\n";

        // make random matrix
        sameRandomUint32 sr(numDims * numDims, 43);
        sameRandomStreamUint32 srS(sr);
        Eigen::MatrixXf randMatrix(numDims, numDims);
        for (uint32_t i= 0; i<numDims; ++i)
            for (uint32_t j= 0; j<numDims; ++j)
                randMatrix(i,j)= srS.getNextFloat();

        // get random orthonormal matrix by doing SVD
        Eigen::JacobiSVD<Eigen::MatrixXf> svdForRR(randMatrix, Eigen::ComputeFullU);
        Eigen::MatrixXf R= svdForRR.matrixU().block(0,0, hammEmbBits,numDims);
        ASSERT(R.cols()==numDims && R.rows()==hammEmbBits);
        // check stuff orthogonal just in case, but there is no way it isn't
        for (uint32_t i= 0; i<hammEmbBits; ++i)
            ASSERT( fabs( R.row(i).norm() -1)<1e-4 );

        #endif

        // copy the rotation matrix into vector
        rot.reserve(hammEmbBits*numDims);
        for (uint32_t i= 0; i<hammEmbBits; ++i)
            for (uint32_t j= 0; j<numDims; ++j)
                rot.push_back(R(i,j));

        std::cout<<"buildIndex::computeHamming: Done with rotation ("<< timing::toc(t0) <<" ms)\n";
    }

    #ifdef RR_MPI
    // communicate the rotation to everyone
    if (!useThreads)
        boost::mpi::broadcast(comm, rot, 0);
    #endif

    // Parallelization is done a bit differently than normally, due to memory:
    // Each worker will process a range of visual words to find the medians
    uint32_t const vocChunkSize=
        std::min( static_cast<uint32_t>(5000),
                  static_cast<uint32_t>(
                      std::ceil(static_cast<double>(numClst)/std::max(numWorkerThreads, numProc))) );
    uint32_t const nJobs= static_cast<uint32_t>( std::ceil(static_cast<double>(numClst)/vocChunkSize) );

    // compute hamming stuff

    #ifdef RR_MPI
    if (!useThreads) comm.barrier();
    #endif

    trainHammingManager *manager= (rank==0) ?
        new trainHammingManager(nJobs, trainHammFn, rot, numDims, numClst, vocChunkSize) :
        NULL;

    trainHammingWorker worker(trainDescsFn, trainAssignsFn,
                              RootSIFT,
                              rot, clstCentres_obj, vocChunkSize);

    if (useThreads)
        threadQueue<trainHammingResult>::start( nJobs, worker, *manager, numWorkerThreads );
    else
        mpiQueue<trainHammingResult>::start( nJobs, worker, manager );

    if (rank==0) delete manager;

}

};