//
// GMOM.cpp
//
// Copyright (c) 2017 Shion Hosoda
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

//include{{{
#include <stdlib.h>
#include <boost/program_options.hpp>
#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/collectives.hpp>
#include "include/CsvFileParser.hpp"
#include "include/GibbsSamplerFromGMOM.hpp"
//}}}

using namespace std;

int main(int argc, char *argv[]){

    //Options{{{
    boost::program_options::options_description opt("Options");
    opt.add_options()
    ("help,h", "show help")
    ("otpt,o", boost::program_options::value<string>()->default_value("./"), "directory name for output")
    ("itnm,n", boost::program_options::value<unsigned int>()->default_value(1000), "the number of iteration")
    ("intr,i", boost::program_options::value<unsigned int>()->default_value(5), "sampling interval")
    ("bnin,b", boost::program_options::value<unsigned int>()->default_value(500), "burn-in term")
    ("k,k", boost::program_options::value<double>()->default_value(1.0), "k value")
    ("theta,t", boost::program_options::value<double>()->default_value(1.0), "theta value")
    ("A,A", boost::program_options::value<double>()->default_value(1000.0), "A value")
    ;

    boost::program_options::positional_options_description pd;
    pd.add("orthologfile", 1);
    pd.add("microbefile", 2);

    boost::program_options::options_description hidden("hidden");
    hidden.add_options()
        ("orthologfile", boost::program_options::value<string>(), "hidden")
        ("microbefile", boost::program_options::value<string>(), "hidden")
        ;
    boost::program_options::options_description cmdline_options;
    cmdline_options.add(opt).add(hidden);

    boost::program_options::variables_map vm;
    try{
        boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(cmdline_options).positional(pd).run(), vm);
    }catch(const boost::program_options::error_with_option_name& e){
        cout<<e.what()<<endl;
    }
    boost::program_options::notify(vm);
    string orthologFilename;
    string microbeFilename;
    double k, theta, A;
    unsigned int iterationNumber;
    unsigned int samplingInterval;
    unsigned int burnIn;
    string outputDirectory;
    string PFilename;
    string VFilename;
    string logLikelihoodFilename;
    if (vm.count("help") || !vm.count("orthologfile") || !vm.count("microbefile")){
        cout<<"Usage:\n GMOM [ortholog file] [microbe file] [-options] "<<endl;
        cout<<endl;
        cout<<opt<<endl;
        exit(1);
    }else{
        orthologFilename = vm["orthologfile"].as<std::string>();
        microbeFilename = vm["microbefile"].as<std::string>();
        if(vm.count("k"))k = vm["k"].as<double>();
        if(vm.count("theta"))theta = vm["theta"].as<double>();
        if(vm.count("A"))A = vm["A"].as<double>();
        if(vm.count("itnm"))iterationNumber = vm["itnm"].as<unsigned int>();
        if(vm.count("intr"))samplingInterval = vm["intr"].as<unsigned int>();
        if(vm.count("bnin"))burnIn = vm["bnin"].as<unsigned int>();
        if(vm.count("otpt"))outputDirectory = vm["otpt"].as<std::string>();
        if(outputDirectory[outputDirectory.size()-1] != '/')outputDirectory.push_back('/');
        PFilename = outputDirectory + "P.csv";
        VFilename = outputDirectory + "V.csv";
        logLikelihoodFilename = outputDirectory + "LogLikelihood.csv";
    }

    CsvFileParser<double> orthologFile(orthologFilename);
    CsvFileParser<double> microbeFile(microbeFilename);
    orthologFile.convertLog();
    //}}}

//estimation{{{
    GibbsSamplerFromGMOM *estimator;
    bmpi::environment env(argc, argv);
    bmpi::communicator world;
    estimator = new GibbsSamplerFromGMOM(orthologFile, microbeFile, A, k, theta, iterationNumber, burnIn, samplingInterval, world);
    estimator->runIteraions();
    if(world.rank() == 0){
        estimator->writeParameters(PFilename, VFilename);
        estimator->writeLogLikelihood(logLikelihoodFilename);
    }
    delete estimator;
//}}}
    return 0;
}


//==================================================================================================
/*!
  @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_SIMD_DIVROUND_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_DIVROUND_INCLUDED

#include <boost/simd/function/scalar/divround.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/simd/function/divround.hpp>

#endif


/** This is free and unencumbered software released into the public domain.
The authors of ISIS do not claim copyright on the contents of this file.
For more details about the LICENSE terms and the AUTHORS, you will
find files of those names at the top level of this repository. **/

/* SPDX-License-Identifier: CC0-1.0 */
#include "jama/jama_svd.h"
#include "jama/jama_qr.h"

#include <armadillo>

#include "LeastSquares.h"
#include "IException.h"
#include "IString.h"

namespace Isis {
  /**
   * Creates a LeastSquares Object.
   *
   * @param basis A BasisFunction. This parameter allows for the least squares
   *              fitting to be applied to arbitrary equations.
   */
  LeastSquares::LeastSquares(Isis::BasisFunction &basis, bool sparse,
                             int sparseRows, int sparseCols, bool jigsaw) {
    p_jigsaw = jigsaw;
    p_basis = &basis;
    p_solved = false;
    p_sparse = sparse;
    p_sigma0 = 0.;

    p_sparseRows = sparseRows;
    p_sparseCols = sparseCols;

    if (p_sparse) {

      //  make sure sparse nrows/ncols have been set
      if (sparseRows == 0  ||  sparseCols == 0) {
        QString msg = "If solving using sparse matrices, you must enter the "
                      "number of rows/columns";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }

      p_sparseA.set_size(sparseRows, sparseCols);
      p_normals.set_size(sparseCols, sparseCols);
      p_ATb.resize(sparseCols, 1);
      p_xSparse.resize(sparseCols);

      if( p_jigsaw ) {
        p_epsilonsSparse.resize(sparseCols);
        std::fill_n(p_epsilonsSparse.begin(), sparseCols, 0.0);

        p_parameterWeights.resize(sparseCols);
      }

    }
    p_currentFillRow = -1;
  }

  //! Destroys the LeastSquares object.
  LeastSquares::~LeastSquares() {
  }

  /**
   * Invoke this method for each set of knowns. Given our example in the
   * description, we have three knowns and expecteds. They are
   * @f[
   * (1,1) = 3
   * @f]
   * @f[
   * (-2,3) = 1
   * @f]
   * @f[
   * (2,-1) = 2
   * @f]
   *
   * @param data A vector of knowns.
   *
   * @param result The expected value for the knowns.
   *
   * @param weight (Default = 1.0) How strongly to weight this known. Weight less
   *               than 1 increases residual for this known, while weight greater
   *               than 1 decreases the residual for this known.
   *
   * @throws Isis::IException::Programmer - Number of elements in data does not
   *                                        match basis requirements
   *
   * @internal
   * @history 2008-04-22  Tracie Sucharski,  Fill sparse matrix.
   * @history 2009-12-21  Jeannie Walldren,  Modified code to add
   *          the square root of the weight to the vector
   *          p_sqrtweight.
   *
   */
  void LeastSquares::AddKnown(const std::vector<double> &data, double result,
                              double weight) {
    if((int) data.size() != p_basis->Variables()) {
      QString msg = "Number of elements in data does not match basis [" +
                        p_basis->Name() + "] requirements";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    p_expected.push_back(result);

    if (weight == 1) {
      p_sqrtWeight.push_back(weight);
    }
    else {
      p_sqrtWeight.push_back(sqrt(weight));
    }

    if(p_sparse) {
      FillSparseA(data);
    }
    else {
      p_input.push_back(data);
    }
  }

  /**
   *  Invoke this method for each set of knowns for sparse solutions.  The A
   *  sparse matrix must be filled as we go or we will quickly run out of memory
   *  for large solutions. So, expand the basis function, apply weights (which is
   *  done in the Solve method for the non-sparse case.
   *
   * @param data A vector of knowns.
   *
   * @internal
   * @history 2008-04-22  Tracie Sucharski - New method for sparse solutions.
   * @history 2009-12-21  Jeannie Walldren - Changed variable name
   *          from p_weight to p_sqrtweight.
   *
   */
  void LeastSquares::FillSparseA(const std::vector<double> &data) {

    p_basis->Expand(data);

    p_currentFillRow++;

    int ncolumns = (int)data.size();

    for(int c = 0;  c < ncolumns; c++) {
      p_sparseA(p_currentFillRow, c) = p_basis->Term(c) * p_sqrtWeight[p_currentFillRow];
    }
  }

  /**
   * This method returns the data at the given row.
   *
   * @param row
   *
   * @return std::vector<double>
   */
  std::vector<double> LeastSquares::GetInput(int row) const {
    if((row >= Rows()) || (row < 0)) {
      QString msg = "Index out of bounds [Given = " + toString(row) + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_input[row];
  }

  /**
   * This method returns the expected value at the given row.
   *
   * @param row
   *
   * @return double
   */
  double LeastSquares::GetExpected(int row) const {
    if((row >= Rows()) || (row < 0)) {
      QString msg = "Index out of bounds [Given = " + toString(row) + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_expected[row];
  }

  /**
   * This methods returns the number of rows in the matrix.
   *
   *
   * @return int
   */
  int LeastSquares::Rows() const {
    return (int)p_input.size();
  }

  /**
   * After all the data has been registered through AddKnown, invoke this
   * method to solve the system of equations. You can then use the Evaluate
   * and Residual methods freely.
   *
   * @internal
   * @history  2008-04-16 Debbie Cook / Tracie Sucharski, Added SolveSparse.
   * @history  2009-04-08 Tracie Sucharski - Added return value which will
   *                          pass on what is returned from SolveSparse which
   *                          is a column number of a column that contained
   *                          all zeros.
   * @history  2010-12-12 Debbie A. Cook  Fixed "no data" test for SPARSE
   *                          case
   */
  int LeastSquares::Solve(Isis::LeastSquares::SolveMethod method) {

    if((method == SPARSE  &&  p_sparseRows == 0)  ||
       (method != SPARSE  &&  Rows() == 0 )) {
      p_solved = false;
      QString msg = "No solution available because no input data was provided";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    if(method == SVD) {
      SolveSVD();
    }
    else if(method == QRD) {
      SolveQRD();
    }
    else if(method == SPARSE) {
      int column = SolveSparse();
      return column;
    }
    return 0;
  }

  /**
   * After all the data has been registered through AddKnown, invoke this
   * method to solve the system of equations. You can then use the Evaluate
   * and Residual methods freely.
   * @internal
   * @history 2009-12-21  Jeannie Walldren - Changed variable name
   *          from p_weight to p_sqrtweight.
   *
   */
  void LeastSquares::SolveSVD() {

    // We are solving Ax=b ... start by creating A
    TNT::Array2D<double> A(p_input.size(), p_basis->Coefficients());
    for(int r = 0; r < A.dim1(); r++) {
      p_basis->Expand(p_input[r]);
      for(int c = 0; c < A.dim2(); c++) {
        A[r][c] = p_basis->Term(c) * p_sqrtWeight[r];
      }
    }

    // Ok use singular value decomposition to solve for the coefficients
    // A = [U][S][V']  where [U] is MxN, [S] is NxN, [V'] is NxN transpose
    // of [V].  We are solving for [A]x=b and need inverse of [A] such
    // that x = [invA]b. Since inverse may not exist we use the
    // pseudo-inverse [A+] from SVD which is [A+] = [V][invS][U']
    // Our coefficents are then x = [A+]b where b is p_b.
    JAMA::SVD<double> svd(A);

    TNT::Array2D<double> V;
    svd.getV(V);

    // The inverse of S is the 1 over each diagonal element of S
    TNT::Array2D<double> invS;
    svd.getS(invS);

    for(int i = 0; i < invS.dim1(); i++) {
      if(invS[i][i] != 0.0) invS[i][i] = 1.0 / invS[i][i];
    }

    // Transpose U
    TNT::Array2D<double> U;
    svd.getU(U);
    TNT::Array2D<double> transU(U.dim2(), U.dim1());

    for(int r = 0; r < U.dim1(); r++) {
      for(int c = 0; c < U.dim2(); c++) {
        transU[c][r] = U[r][c];
      }
    }

    // Now multiply everything together to get [A+]
    TNT::Array2D<double> VinvS = TNT::matmult(V, invS);
    TNT::Array2D<double> Aplus = TNT::matmult(VinvS, transU);

    // Using Aplus and our b we can solve for the coefficients
    TNT::Array2D<double> b(p_expected.size(), 1);

    for(int r = 0; r < (int)p_expected.size(); r++) {
      b[r][0] = p_expected[r] * p_sqrtWeight[r];
    }

    TNT::Array2D<double> coefs = TNT::matmult(Aplus, b);

    // If the rank of the matrix is not large enough we don't
    // have enough coefficients for the solution
    if (coefs.dim1() < p_basis->Coefficients()) {
      QString msg = "Unable to solve least-squares using SVD method. No "
                    "solution available. Not enough knowns or knowns are "
                    "co-linear ... [Unknowns = "
                    + toString(p_basis->Coefficients()) + "] [Knowns = "
                    + toString(coefs.dim1()) + "]";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    // Set the coefficients in our basis equation
    std::vector<double> bcoefs;
    for (int i = 0; i < coefs.dim1(); i++) bcoefs.push_back(coefs[i][0]);

    p_basis->SetCoefficients(bcoefs);

    // Compute the errors
    for(int i = 0; i < (int)p_input.size(); i++) {
      double value = p_basis->Evaluate(p_input[i]);
      p_residuals.push_back(value - p_expected[i]);
      p_sigma0 += p_residuals[i]*p_residuals[i]*p_sqrtWeight[i]*p_sqrtWeight[i];
    }
    // calculate degrees of freedom (or redundancy)
    // DOF = # observations + # constrained parameters - # unknown parameters
    p_degreesOfFreedom = p_basis->Coefficients() - coefs.dim1();

    if( p_degreesOfFreedom > 0.0 )  {
      p_sigma0 = p_sigma0/(double)p_degreesOfFreedom;
    }

    // check for p_sigma0 < 0
    p_sigma0 = sqrt(p_sigma0);

    // All done
    p_solved = true;
  }

  /**
   * After all the data has been registered through AddKnown, invoke this
   * method to solve the system of equations with a QR
   * decomposition of A = QR. You can then use the Evaluate and
   * Residual methods freely. The QR decomposition is only slightly
   * less reliable than the SVD, but much faster.
   * @internal
   * @history 2009-12-21  Jeannie Walldren - Changed variable name
   *          from p_weight to p_sqrtweight.
   *
   */
  void LeastSquares::SolveQRD() {

    // We are solving Ax=b ... start by creating an MxN matrix, A
    TNT::Array2D<double> A(p_input.size(), p_basis->Coefficients());
    for(int r = 0; r < A.dim1(); r++) {
      p_basis->Expand(p_input[r]);
      for(int c = 0; c < A.dim2(); c++) {
        A[r][c] = p_basis->Term(c) * p_sqrtWeight[r];
      }
    }

    // Ok use  to solve for the coefficients
    // [A] = [Q][R]  where [Q] is MxN and orthogonal and  [R] is an NxN,
    // upper triangular matrix.  TNT provides the solve method that inverts
    // [Q] and backsolves [R] to get the coefficients in the vector x.
    // That is, we solve the system Rx = Q^T b
    JAMA::QR<double> qr(A);

    // Using A and our b we can solve for the coefficients
    TNT::Array1D<double> b(p_expected.size());
    for(int r = 0; r < (int)p_expected.size(); r++) {
      b[r] = p_expected[r] * p_sqrtWeight[r];
    }// by construction, we know the size of b is equal to M, so b is conformant

    // Check to make sure the matrix is full rank before solving
    // -- rectangular matrices must be full rank in order for the solve method
    //    to be successful
    int full = qr.isFullRank();
    if(full == 0) {
      QString msg = "Unable to solve-least squares using QR Decomposition. "
                    "The upper triangular R matrix is not full rank";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    TNT::Array1D<double> coefs = qr.solve(b);

    // Set the coefficients in our basis equation
    std::vector<double> bcoefs;
    for(int i = 0; i < coefs.dim1(); i++) {
      bcoefs.push_back(coefs[i]);
    }
    p_basis->SetCoefficients(bcoefs);

    // Compute the errors
    for(int i = 0; i < (int)p_input.size(); i++) {
      double value = p_basis->Evaluate(p_input[i]);
      p_residuals.push_back(value - p_expected[i]);
    }

    // All done
    p_solved = true;
  }

  /**
   * @brief  Solve using sparse class
   *
   * After all the data has been registered through AddKnown, invoke this
   * method to solve the system of equations Nx = b, where
   *
   * N = ATPA
   * b = ATPl, and
   *
   * N is the "normal equations matrix;
   * A is the so-called "design" matrix;
   * P is the observation weight matrix (typically diagonal);
   * l is the "computed - observed" column vector;
   *
   * The solution is achieved using a sparse matrix formulation of
   * the LU decomposition of the normal equations.
   *
   * You can then use the Evaluate and Residual methods freely.
   *
   * @internal
   * @history  2008-04-16 Debbie Cook / Tracie Sucharski, New method
   * @history  2008-04-23 Tracie Sucharski,  Fill sparse matrix as we go in
   *                          AddKnown method rather than in the solve method,
   *                          otherwise we run out of memory very quickly.
   * @history  2009-04-08 Tracie Sucharski - Added return value which is a
   *                          column number of a column that contained all zeros.
   * @history 2009-12-21  Jeannie Walldren - Changed variable name
   *                          from p_weight to p_sqrtweight.
   * @history 2010        Ken Edmundson
   * @history 2010-11-20  Debbie A. Cook Merged Ken Edmundson verion with system version
   * @history 2011-03-17  Ken Edmundson Corrected computation of residuals
   *
   */
  int LeastSquares::SolveSparse() {

    // form "normal equations" matrix by multiplying ATA
    p_normals = p_sparseA.t()*p_sparseA;

    // Create the right-hand-side column vector 'b'
    arma::mat b(p_sparseRows, 1);

    // multiply each element of 'b' by it's associated weight
    for ( int r = 0; r < p_sparseRows; r++ )
      b(r,0) = p_expected[r] * p_sqrtWeight[r];

    // form ATb
    p_ATb = p_sparseA.t()*b;

    // apply parameter weighting if Jigsaw (bundle adjustment)
    if ( p_jigsaw ) {
      for( int i = 0; i < p_sparseCols; i++) {
        double weight = p_parameterWeights[i];

        if( weight <= 0.0 )
          continue;

        p_normals(i, i) += weight;
        p_ATb(i, 0) -= p_epsilonsSparse[i]*weight;
      }
    }

    bool status = spsolve(p_xSparse, p_normals, p_ATb, "superlu");

    if (status == false) {
      QString msg = "Could not solve sparse least squares problem.";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    // Set the coefficients in our basis equation
    p_basis->SetCoefficients(arma::conv_to< std::vector<double> >::from(p_xSparse));

    // if Jigsaw (bundle adjustment)
    // add corrections into epsilon vector (keeping track of total corrections)
    if ( p_jigsaw ) {
      for( int i = 0; i < p_sparseCols; i++ )
        p_epsilonsSparse[i] += p_xSparse[i];
    }

    // Compute the image coordinate residuals and sum into Sigma0
    // (note this is exactly what was being done before, but with less overhead - I think)
    // ultimately, we should not be using the A matrix but forming the normals
    // directly. Then we'll have to compute the residuals by back projection

    p_residuals.resize(p_sparseRows);
    p_residuals = arma::conv_to< std::vector<double> >::from(p_sparseA*p_xSparse);
    p_sigma0 = 0.0;

    for ( int i = 0; i < p_sparseRows; i++ ) {
        p_residuals[i] = p_residuals[i]/p_sqrtWeight[i];
        p_residuals[i] -= p_expected[i];
        p_sigma0 += p_residuals[i]*p_residuals[i]*p_sqrtWeight[i]*p_sqrtWeight[i];
    }

    // if Jigsaw (bundle adjustment)
    // add contibution to Sigma0 from constrained parameters
    if ( p_jigsaw ) {
      double constrained_vTPv = 0.0;

      for ( int i = 0; i < p_sparseCols; i++ ) {
        double weight = p_parameterWeights[i];

        if ( weight <= 0.0 )
          continue;

        constrained_vTPv += p_epsilonsSparse[i]*p_epsilonsSparse[i]*weight;
      }
      p_sigma0 += constrained_vTPv;
    }
    // calculate degrees of freedom (or redundancy)
    // DOF = # observations + # constrained parameters - # unknown parameters
    p_degreesOfFreedom = p_sparseRows + p_constrainedParameters - p_sparseCols;

    if( p_degreesOfFreedom <= 0.0 ) {
      p_sigma0 = 1.0;
    }
    else {
      p_sigma0 = p_sigma0/(double)p_degreesOfFreedom;
    }

    // check for p_sigma0 < 0
    p_sigma0 = sqrt(p_sigma0);

    // All done
    p_solved = true;
    return 0;
  }

  void LeastSquares::Reset ()
  {
    if ( p_sparse ) {
      p_sparseA.zeros();
      p_ATb.zeros();
      p_normals.zeros();
      p_currentFillRow = -1;
    }
    else {
      p_input.clear();
    }
      p_sigma0 = 0.;
    p_residuals.clear();
    p_expected.clear();
    p_sqrtWeight.clear();
    p_solved = false;
  }

  /**
   * Invokes the BasisFunction Evaluate method.
   *
   * @param data The input variables to evaluate.
   *
   * @return The evaluation for the input variable.
   *
   * @throws Isis::IException::Programmer - Unable to evaluate until a
   *                                        solution has been computed
   */
  double LeastSquares::Evaluate(const std::vector<double> &data) {
    if(!p_solved) {
      QString msg = "Unable to evaluate until a solution has been computed";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_basis->Evaluate(data);
  }

  /**
   * Returns a vector of residuals (errors). That is, the difference between the
   * evaluation of a known with the solution against the expected value.
   *
   * @return The vector of residuals.
   *
   * @throws Isis::IException::Programmer - Unable to return residuals until a
   *                                        solution has been computed
   */
  std::vector<double> LeastSquares::Residuals() const {
    if(!p_solved) {
      QString msg = "Unable to return residuals until a solution has been computed";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_residuals;
  }

  /**
   * Returns the ith residual. That is, the difference between the evaluation of
   * a known with the solution against the expected value. There is one residual
   * for each time AddKnown was invoked.
   *
   * @param i The number of times AddKnown was invoked to be evaluated.
   *
   * @return The output value of the residual.
   *
   * @throws Isis::IException::Programmer - Unable to return residuals until a
   *                                        solution has been computed
   */
  double LeastSquares::Residual(int i) const {
    if(!p_solved) {
      QString msg = "Unable to return residuals until a solution has been computed";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_residuals[i];
  }

  /**
   * Reset the weight for the ith known. This weight will not be used unless
   * the system is resolved using the Solve method.
   *
   * @param index The position in the array to assign the given weight value
   * @param weight A weight factor to apply to the ith known. A weight less
   *               than one increase the residual for this known while a
   *               weight greater than one decrease the residual for this
   *               known.
   * @internal
   * @history 2009-12-21  Jeannie Walldren,  Modified code to add
   *          the square root of the weight to the vector
   *          p_sqrtweight.
   *
   */

  void LeastSquares::Weight(int index, double weight) {
    if(weight == 1) {
      p_sqrtWeight[index] = weight;
    }
    else {
      p_sqrtWeight[index] = sqrt(weight);
    }
  }

} // end namespace isis


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2018, Rawashdeh Research Group
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **********************************************************************/
// Author: Mohamed Aladem

#include "lvt_pnp_solver.h"
#include "lvt_logging_utils.h"
#include "lvt_local_map.h"
#include "lvt_image_features_struct.h"
#include <Eigen/StdVector>
#include <numeric>
#include <g2o/config.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/icp/types_icp.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

#define N_PASSES 2

lvt_pnp_solver::lvt_pnp_solver(double fx, double fy, double cx, double cy, double baseline)
    : m_fx(fx), m_fy(fy), m_cx(cx), m_cy(cy), m_baseline(baseline), m_optimizer(nullptr)
{
    m_optimizer = new g2o::SparseOptimizer();
    m_optimizer->setVerbose(false);
    g2o::BlockSolver_6_3::LinearSolverType *linear_solver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
    g2o::BlockSolver_6_3 *block_solver = new g2o::BlockSolver_6_3(linear_solver);
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(block_solver);
    m_optimizer->setAlgorithm(solver);
}

lvt_pnp_solver::~lvt_pnp_solver()
{
    delete m_optimizer;
}

lvt_pose lvt_pnp_solver::compute_pose(const lvt_pose &cam_pose, lvt_image_features_struct *left_struct,
                                      const lvt_vector3_array &map_points, const std::vector<int> &matches_left)
{
    LVT_LOG("Started computing pose...");
    // add camera
    const Eigen::Quaterniond orientation = cam_pose.get_orientation_quaternion();
    const Eigen::Vector3d position = cam_pose.get_position();
    g2o::SBACam sba_cam(orientation, position);
    sba_cam.setKcam(m_fx, m_fy, m_cx, m_cy, m_baseline);
    g2o::VertexCam *cam_vertex = new g2o::VertexCam();
    cam_vertex->setId(0);
    cam_vertex->setEstimate(sba_cam);
    cam_vertex->setFixed(false);
    m_optimizer->addVertex(cam_vertex);

    // add mono measurments
    static const double mono_chi = sqrt(LVT_REPROJECTION_TH2);
    int vertex_id = 1;
    std::vector<g2o::EdgeProjectP2MC *> mono_edges(map_points.size());
    for (size_t i = 0, count = map_points.size(); i < count; i++)
    {
        g2o::VertexSBAPointXYZ *point_vertex = new g2o::VertexSBAPointXYZ();
        point_vertex->setId(vertex_id++);
        point_vertex->setMarginalized(false);
        point_vertex->setEstimate(map_points[i]);
        point_vertex->setFixed(true);
        m_optimizer->addVertex(point_vertex);

        g2o::EdgeProjectP2MC *edge = new g2o::EdgeProjectP2MC();
        edge->setVertex(0, point_vertex);
        edge->setVertex(1, cam_vertex);
        cv::Point2f mp_cv = left_struct->get_keypoint(matches_left[i]).pt;
        lvt_vector2 img_pt;
        img_pt << mp_cv.x, mp_cv.y;
        edge->setMeasurement(img_pt);
        edge->information() = Eigen::Matrix2d::Identity();
        g2o::RobustKernel *rkh = new g2o::RobustKernelCauchy;
        edge->setRobustKernel(rkh);
        rkh->setDelta(mono_chi);
        m_optimizer->addEdge(edge);
        mono_edges[i] = edge;
    }

    // perform optimzation
    std::vector<int> mono_inlier_marks(mono_edges.size(), 1);
    for (int i = 0; i < N_PASSES; i++)
    {
        m_optimizer->initializeOptimization(0);
        m_optimizer->optimize(5);
        for (int k = 0; k < mono_edges.size(); k++)
        {
            if (mono_edges[k]->chi2() > LVT_REPROJECTION_TH2)
            {
                mono_edges[k]->setLevel(1);
                mono_inlier_marks[k] = 0;
            }
        }
    }

    // retrieve optimized pose
    const Eigen::Vector3d op_position = cam_vertex->estimate().translation();
    const Eigen::Quaterniond op_orientation = cam_vertex->estimate().rotation();
    lvt_pose op_pose = lvt_pose(op_position, op_orientation);

    m_optimizer->clear();
    LVT_LOG("Finished computing pose.");
    LVT_RECORD("inlier count", std::accumulate(mono_inlier_marks.begin(), mono_inlier_marks.end(), 0));
    return op_pose;
}


#include <cmath>
#include <ctime>
#include <string>
#include <sstream>
#include <algorithm>
#include <ctime>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>	// for converting int to string
#include "boost/date_time/gregorian/gregorian.hpp"		// for date
#include "globals.h"
#include "agent.h"
#include "resource.h"
#include "marketplace.h"
#include "ran.h"
#include "statstracker.h"
#include "devmarketplace.h"
#include "utils.h"
#include "logging.h"

/**
 * Random number generator. Generates random numbers between 0 and 1.
 */
Ran01<double>	randGen;
/**
 * Random number generator. Generates random numbers using binomial distribution.
 */
RanBinomial<double>	randBinomial;

/**
 * Define an array of device types.
 */
string device_names[] = { "Tool", "Machine", "Factory", "Industry", "DevMachine", "DevFactory", "NoDevice!!" };

/**
 * Constructor.
 */
Globals::Globals()
{
    REMOVE_RES = false;
    REMOVE_AGENT = false;
    END_SAVE = false;
    SAVE_DAY_STATUS = false;
    SIM_NAME = "default";
    PARALLEL_TRADES = false;
}

/**
 * initialize random seed, EXPERIENCE_FOR_MAKING, RES_DECAY_SLOWNESS, RES_VALUES.
 */
void Globals::initGlobalStructures()
{
    if (randomSeedSet) {
        randGen = Ran01<double>(randomSeed);
    } else {
        randGen = Ran01<double>();
    }
    if (randomSeedSet) {
        randBinomial = RanBinomial<double>(randomSeed);
    } else {
        randBinomial = RanBinomial<double>();
    }

    discoveredDevices.resize(NUM_DEVICE_TYPES);	// there are 6 empty vectors in this vector.
    EXPERIENCE_FOR_MAKING.resize(NUM_DEVICE_TYPES);	// there are 6 empty vectors in this vector
    RES_DECAY_SLOWNESS.resize(NUM_RESOURCES);
    RES_VALUES.resize(NUM_RESOURCES);

    if (START_DAY == 0) {		// if not loading a previous run
        EXPERIENCE_FOR_MAKING.resize(NUM_DEVICE_TYPES);
        EXPERIENCE_FOR_MAKING[TOOL] = 1.0;
        EXPERIENCE_FOR_MAKING[MACHINE] = 2.0;
        EXPERIENCE_FOR_MAKING[FACTORY] = 4.0;
        EXPERIENCE_FOR_MAKING[INDUSTRY] = 8.0;
        EXPERIENCE_FOR_MAKING[DEVMACHINE] = 4.0;
        EXPERIENCE_FOR_MAKING[DEVFACTORY] = 8.0;
    } else {		// not START_DAY == 0
    	DAY_STATUS_LOAD_FOLDER = "";
        int slashIdx = configFilename.find_last_of('/');
        for (unsigned i = 0; i < (unsigned) slashIdx; i++) {
        	DAY_STATUS_LOAD_FOLDER += configFilename[i];
        }
//        loadDayStatus();	// from utils.cpp
    }

    // CODE HERE IN config.py to do GRAPH_X_VALUES, QUArtILE_COLORS.  Don't need to do this
    // for the cpp code.  leave in python.

    RES_IN_DEV.resize(NUM_DEVICE_TYPES);   // make room for a value for each device name.
    RES_IN_DEV[TOOL] = RESOURCES_IN_TOOL;
    RES_IN_DEV[MACHINE] = RESOURCES_IN_TOOL * NUM_DEVICE_COMPONENTS;
    RES_IN_DEV[FACTORY] = RESOURCES_IN_TOOL * NUM_DEVICE_COMPONENTS * NUM_DEVICE_COMPONENTS;
    RES_IN_DEV[INDUSTRY] = RESOURCES_IN_TOOL * NUM_DEVICE_COMPONENTS *
        NUM_DEVICE_COMPONENTS * NUM_DEVICE_COMPONENTS;
    RES_IN_DEV[DEVMACHINE] = RESOURCES_IN_TOOL * NUM_DEVICE_COMPONENTS * NUM_DEVICE_COMPONENTS;
    RES_IN_DEV[DEVFACTORY] = RESOURCES_IN_TOOL * NUM_DEVICE_COMPONENTS *
        NUM_DEVICE_COMPONENTS * NUM_DEVICE_COMPONENTS;

    // inizialize memoMUs
    for (int i = 0; i < NUM_AGENT_GROUPS; i++) {
        memoMUs.push_back(map<pair<int,int>, double>());
    }
}
/**
 * Read agents values in from configAgentFilename file (the one the user set using -p or the default one),
 * and initialize each agent with its specific values.
 */
void Globals::initializeAgents()
{
    ifstream agentValsFile(configAgentFilename.c_str());
    /*
     * the agent values file is organized this way:
     * # comment line -- skip this.
     * Each non comment line has 12 values in it:
     *  steepness, scaling, minResEff, maxResEff, maxResExp, minDevEff, maxDevEff, maxDevExp, lifetime, resTradePower, devTradePower, patent, group
     * First there is a line of this data for each of the resources for agent 0.  (typically 24 lines)
     * Then there is a line of this data for each of the resources for agent 1.   (typically 24 lines)
     * Repeat for each agent.
     * At the very end are two lines like this:
     * # MODES: lifetime_mode, dev_mode, util_mode, res_mode
     * homogeneous, homogeneous, homogeneous, homogeneous
     *
     * resTradePower: default is 1, can be anything except for 0.
     */
    NUM_AGENTS_IN_GROUP = vector<int>(NUM_AGENT_GROUPS, 0);
    NUM_ACTIVE_AGENTS_IN_GROUP = vector<int>(NUM_AGENT_GROUPS, 0);
    agent.clear();
    for (int aId = 0; aId < NUM_AGENTS; aId++) {
        vector< vector<double> > agValues;
        for (int resId = 0; resId < NUM_RESOURCES; ) {
            string line;
            getline(agentValsFile, line);	// read line from file into string
            stringstream sline(line);		// make a stringstream of the line
            if (line[0] == '#') {		// if first char is #, skip line
                continue;
            }
            /* read in a line of 13 values for each resource, comma-separated. */
            vector<double> t(14, 0.0);
            for (int i = 0; i < 14; i++) {
                sline >> t[i];
                if (sline.peek() == ',') {
                    char comma;
                    sline >> comma;
                }
                /*
                 * Compute the number of agents in each group.
                 * For now, the resources of one agent are the same.
                 * So the type for all resources in one agent is the same.
                 * Only need to check once.
                 */
                if ((resId == 0) && (i == 13)) {
                    NUM_AGENTS_IN_GROUP[t[i]]++;
                }
            }
            agValues.push_back(t);  // put the temporary vector of 9 values on the end.
            // We read in values for the resource, so now we get ready for
            // the next resource.
            resId++;
        }
        agent.push_back(new Agent(aId, agValues));
    }

    activeAgents = 0;
    for (int aId = 0; aId < NUM_AGENTS; aId++) {
        if (agent[aId]->inSimulation) {
            activeAgents++;
            NUM_ACTIVE_AGENTS_IN_GROUP[agent[aId]->group]++;
        }
    }
}

/**
 * Create a directory and its "parent / grandparent" directories specified in the path if they don't exist.
 */
void Globals::createDirectory(string path)
{
    const char* pathChar = path.c_str();
    string subPath = "";
    for (unsigned int i = 1; i < strlen(pathChar); i++) {  // don't need to check the first char
        /*
         * Check for each parent directory and see if it exists.
         */
        if (pathChar[i] == '/') {
            subPath = path.substr(0, i);
            if (! boost::filesystem::exists(subPath)) {
                boost::filesystem::create_directory(subPath);
            }
        }
        if (i == strlen(pathChar)-1) {
            if (! boost::filesystem::exists(path)) {
                boost::filesystem::create_directory(path);
            }
        }
    }
}

/**
 * Set some global variables according to the command line the user enters.
 */
void Globals::setAdvancedOptions()
{
    REMOVE_RES = false;
    if (removeResMidRun) {
        REMOVE_RES = true;
        RES_TO_REMOVE = removeResId;
        REMOVE_RES_DAY = removeResDay;
        ELIMINATE_RESERVES = removeResHoldings;
    } else {
		REMOVE_RES_DAY=-99;
		ELIMINATE_RESERVES=false;
		RES_TO_REMOVE=-99;
	}

    REMOVE_AGENT = false;
    if (removeAgentMidRun) {
        REMOVE_AGENT = true;
        AGENT_TO_REMOVE = removeAgentId;
        REMOVE_AGENT_DAY = removeAgentDay;
    } else {
		AGENT_TO_REMOVE=-99;
		REMOVE_AGENT_DAY=-99;
	}

    SAVE_DAY_STATUS = false;
    if (saveInMiddle) {
        DAY_STATUS_SAVE_FOLDER = saveInMiddleFoldername;
        DAY_FOR_SAVE = saveInMiddleDay;
        createDirectory(DAY_STATUS_SAVE_FOLDER);
        SAVE_DAY_STATUS = true;
    } else {
		DAY_FOR_SAVE=-99;
	}

    END_SAVE = false;
    if (saveFileFolderSet) {
        END_SAVE = true;
        SAVE_FOLDER = saveFileFolder;
        createDirectory(SAVE_FOLDER);
        createDirectory(SAVE_FOLDER + "/configFiles");
    }

    if (! END_SAVE) {
        // simTitle has default value of "000" if not set explicitly.
        SIM_NAME = simTitle;
    } else {
        if (simTitle == "000") {	// sim title (run number) wasn't explicity tracked with -t.
        	/*
        	 * TODO: not sure about the name. python mkdtemp generates a random name with prefix sim_.
        	 * For path spliting, any easier way?
        	 */
        	SIM_SAVE_FOLDER = SAVE_FOLDER + "/sim_default";
        	createDirectory(SIM_SAVE_FOLDER);

            int slashIdx = SIM_SAVE_FOLDER.find_last_of('/');
            SIM_NAME = "";
            for (unsigned i = slashIdx + 1; i < SIM_SAVE_FOLDER.length(); i++) {
            	SIM_NAME += SIM_SAVE_FOLDER[i];
            }
        } else {
            SIM_NAME = simTitle;
            createDirectory(SAVE_FOLDER);
            createDirectory(SAVE_FOLDER + "/configFiles");

            SIM_SAVE_FOLDER = SAVE_FOLDER + '/' + SIM_NAME;
            createDirectory(SIM_SAVE_FOLDER);
        }
    }

    SAVE_TRADES = glob.saveExchangeRateData;
}

/**
 * Reinitialize the variables, saving paths that need to be reinitialized for each new run.
 */
void Globals::perRunInitialization()
{
    res.clear();
    discoveredDevices.clear();
    discoveredDevices.resize(NUM_DEVICE_TYPES);
    if (START_DAY == 0) {
        for (int resId = 0; resId < NUM_RESOURCES; resId++) {
            res.push_back(Resource(resId));
            for (int devId = TOOL; devId < NUM_DEVICE_TYPES; devId++) {
                discoveredDevices[devId].push_back(NULL);
            }
        }
    } else {
        /*
         * TODO: saveDayStatus and loadDayStatus in utils.cpp
         */
        DAY_STATUS_LOAD_FOLDER = "";
        int slashIdx = configFilename.find_last_of('/');
        for (unsigned i = 0; i < (unsigned) slashIdx; i++) {
            DAY_STATUS_LOAD_FOLDER += configFilename[i];
        }
    }
    setGlobalMarketPlaces();
    setGlobalStats();
    initializeAgents();

}

/**
 * Create new instances of ResourceMarketplace and DeviceMarketplace.
 */
void Globals::setGlobalMarketPlaces()
{
    resourceMarket = new ResourceMarketplace();
    deviceMarket = new DeviceMarketplace();
}

/**
 * Create new instances of TradeStats, ProductionStats, OtherStats.
 */
void Globals::setGlobalStats()
{
    tradeStats = new TradeStats();
    productionStats = new ProductionStats();
    otherStats = new OtherStats();
}

/**
 * Set the start time of the run.
 */
void Globals::startTimer()
{
    startTime = time(0);
}

/**
 * Set the end time of the run.
 */
void Globals::endTimer()
{
    endTime = time(0);
}

/**
 * Remove a certain agent on a specific day during the run.
 * \param agentNum the index of the agent to be removed
 * \param day the day that the agent needs to be removed
 */
void Globals::removeAgent(int agentNumber, int day)
{
    agent[agentNumber]->remove();
    activeAgents--;
    NUM_ACTIVE_AGENTS_IN_GROUP[agent[agentNumber]->group]--;
    LOG(3) << "Agent " << agentNumber << " has been removed on day " << day;
}

/**
 * Remove a certain resource on a specific day during the run.
 * \param resNumber the index of the resource to be removed
 * \param day the day that the resource needs to be removed
 */
void Globals::removeRes(int resNumber, int day)
{
    res[resNumber].remove();
    BOOST_FOREACH(Agent *ag, agent) {
        for (int dev = TOOL; dev <= DEVFACTORY; dev++) {
            ag->devProp[dev][resNumber].deviceHeld = 0;
        }
    }
    if (ELIMINATE_RESERVES) {
        BOOST_FOREACH(Agent *ag, agent) {
            ag->resProp[resNumber].setHeld(0);
            ag->resProp[resNumber].experience = 0;
        }
    }
    LOG(4) << "Res " << resNumber << " has been removed on day " << day;
}

/**
 * \return a random double in range [lower, upper).
 */
double Globals::random_range(double lower, double upper) {
    return (randGen() * (upper - lower)) + lower;
}
/**
 * \return a random double in range [0.0, 1.0).
 */
double Globals::random_01() {
    return random_range(0.0, 1.0);
}
/**
 * \return an random integer value in range [lower, upper).
 */
int Globals::random_int(int lower, int upper) {
    return ((int) random_range(lower, upper));
}
/**
 * \returns an random integer value in range [lower, upper].
 */
int Globals::random_int_inclusive(int lower, int upper) {
    return random_int(lower, upper + 1);
}
/**
 * \param vec a vector of integers
 * \return an int randomly chosen from the vector.
 */
int Globals::random_choice(vector<int> &vec)
{
    return vec[random_int(0, vec.size())];
}
/**
 * \param n the number of independent experiements.
 * \param p the probability of success
 * \return an int randomly from a binomial distribution.
 */
int Globals::random_binomial(int n, double p)
{
    return randBinomial(n, p);
}

/**
 * Reinitialize SIM_NAME and SIM_SAVE_FOLDER.
 */
void Globals::reinitialize()
{
    pair<string, string> names = calcSimName(year + month + day);
    SIM_NAME = names.first;
    SIM_SAVE_FOLDER = SAVE_FOLDER + '/' + SIM_NAME;
    createDirectory(SIM_SAVE_FOLDER);
}

/**
 * \return the scenario number (1-4)
 */
int Globals::calcScenarioNumber()
{
    int scenarioNum = 1;
    if (TRADE_EXISTS) {
        scenarioNum = 2;
        if (DEVICES_EXIST) {
            scenarioNum = 4;
            if (TOOLS_ONLY) {
                scenarioNum = 3;
            }
        }
    }
    return scenarioNum;
}

/**
 * \return the simulation number (001 - 999)
 */
string Globals::calcSimNumber(string simName)
{
    char simNumChar[3];
    string simNumStr = "001";
    int simNumInt = 1;
    int temp;

    while ( boost::filesystem::exists(SAVE_FOLDER + '/' + simName + '_' + 'r' + simNumStr) ) {
        simNumInt++;
        if (((int) log10( (double) simNumInt ) + 1) == 1) {
             temp = sprintf(simNumChar, "00%d", simNumInt);
        } else if (((int) log10( (double) simNumInt ) + 1) == 2) {
            temp = sprintf(simNumChar, "0%d", simNumInt);
        } else if (((int) log10( (double) simNumInt ) + 1) == 3) {
            temp = sprintf(simNumChar, "%d", simNumInt);
        } else if (((int) log10( (double) simNumInt ) + 1) == 4) {
            cout << "Too much simulations!" << endl;
            return 0;
        }
        simNumStr = string(simNumChar);
    }
    return simNumStr;
}

/**
 * \return a pair of (simName_r_simNumber, simName)
 */
pair<string, string> Globals::calcSimName(string nameBegin)
{
    int scenarioNum = calcScenarioNumber();
    string simName = nameBegin + '_' + boost::lexical_cast<string>(NUM_RESOURCES)
            + '_' + boost::lexical_cast<string>(NUM_AGENTS)
            + '_' + boost::lexical_cast<string>(NUM_DAYS)
            + '_' + boost::lexical_cast<string>(scenarioNum);
    string simNumber = calcSimNumber(simName);
    pair<string, string> result = make_pair(simName + '_' + 'r' + simNumber, simName);
    return result;
}

/**
 * Print the information in config file.
 */
void Globals::printConfig()
{
    cout << "Simulation variables:" << endl;
    cout << "Day length is " << DAY_LENGTH << endl;
    cout << "Number of days is " << NUM_DAYS << endl;
    cout << "Number of agents is " << NUM_AGENTS << endl;
    cout << "Number of resources is " << NUM_RESOURCES << endl << endl;
    cout << "Number of groups/types of agents is " << NUM_AGENT_GROUPS << endl;

    cout << "Trade Context variables: " << endl;
    cout << "Number of resource trade rounds is " << RES_TRADE_ROUNDS << endl;
    cout << "Epsilon of trade " << TRADE_EPSILON << endl;
    cout << "Number of resource trade attempts is " << RES_TRADE_ATTEMPTS << endl;
    cout << "Number of device trade rounds is " << DEVICE_TRADE_ROUNDS << endl;
    cout << "Number of device trade attempts is " << DEVICE_TRADE_ATTEMPTS << endl;
    cout << "Menu size is " << MENU_SIZE << endl << endl;

    cout << "Input booleans " << endl;
    cout << "A resource will be removed: " << REMOVE_RES << endl;
    cout << "Trade exists: " << TRADE_EXISTS << endl;
    cout << "Devices exist: " << DEVICES_EXIST << endl << endl;

    cout << "Agent Capacity variables: " << endl;
    cout << "Device trade memory length is " << DEVICE_TRADE_MEMORY_LENGTH << endl;
    cout << "Device production memory length is " <<
        DEVICE_PRODUCTION_MEMORY_LENGTH << endl;
    cout << "Daily experience loss for idle resources is " << DAILY_EXP_PENALTY << endl;
    cout << "Maximum resource experience is " << MAX_RES_EXPERIENCE << endl;
    cout << "Minimum effort that can go into a resource is " << MIN_RES_EFFORT << endl;
    cout << "Days of device to hold " << DAYS_OF_DEVICE_TO_HOLD << endl << endl;

    cout << "Extraction Efficiency variables: " << endl;
    cout << "Factor of a tool " << TOOL_FACTOR << endl;
    cout << "Lifetime of a tool " << TOOL_LIFETIME << endl;
    cout << "Factor of a machine " << MACHINE_FACTOR << endl;
    cout << "Lifetime of a machine " << MACHINE_LIFETIME << endl;
    cout << "Factor of a factory " << FACTORY_FACTOR << endl;
    cout << "Lifetime of a factory " << FACTORY_LIFETIME << endl;
    cout << "Factor of a industry " << INDUSTRY_FACTOR << endl;
    cout << "Lifetime of a industry " << INDUSTRY_LIFETIME << endl;
    cout << "Factor of a device machine " << DEV_MACHINE_FACTOR << endl;
    cout << "Lifetime of a device machine " << DEV_MACHINE_LIFETIME << endl;
    cout << "Factor of a device factory " << DEV_FACTORY_FACTOR << endl;
    cout << "Lifetime of a device factory " << DEV_FACTORY_LIFETIME << endl << endl;

    cout << "Production Efficiency variables: " << endl;
    cout << "Number of resources to make a tool is " << RESOURCES_IN_TOOL << endl;
    cout << "Experience the inventor gains for the invention is " << INVENTOR_DEVICE_EXPERIENCE << endl;
    cout << "Number of components that go into a device is " << NUM_DEVICE_COMPONENTS << endl;
    cout << "Maximum experience someone can have in a device is " << MAX_DEVICE_EXPERIENCE << endl;
    cout << "Amount of decay devices experience on a daily basis " << DAILY_DEVICE_DECAY << endl;
    cout << "Production Epsilon is " << PRODUCTION_EPSILON << endl << endl;

    cout << "Innovation Volume/Speed variables: " << endl;
    cout << "Minimum resources required for device construction is " << MIN_RES_HELD_FOR_DEVICE_CONSIDERATION << endl;
    cout << "Minimum device experience for an agent that holds the device is " << MIN_HELD_DEVICE_EXPERIENCE << endl;
    cout << "Minimum devices made recently to consider obtaining a device making device is " << MIN_DEVICE_FOR_DEV_DEVICE_CONSIDERATION << endl << endl;

}


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

#ifndef BOOST_HISTOGRAM_AXIS_INTEGER_HPP
#define BOOST_HISTOGRAM_AXIS_INTEGER_HPP

#include <boost/histogram/axis/iterator.hpp>
#include <boost/histogram/axis/option.hpp>
#include <boost/histogram/detail/compressed_pair.hpp>
#include <boost/histogram/detail/meta.hpp>
#include <boost/histogram/fwd.hpp>
#include <boost/throw_exception.hpp>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <type_traits>
#include <utility>

namespace boost {
namespace histogram {
namespace axis {

/**
  Axis for an interval of integer values with unit steps.

  Binning is a O(1) operation. This axis bins faster than a regular axis.

  @tparam Value input value type. Must be integer or floating point.
  @tparam MetaData type to store meta data.
  @tparam Options see boost::histogram::axis::option (all values allowed).
 */
template <class Value, class MetaData, class Options>
class integer : public iterator_mixin<integer<Value, MetaData, Options>> {
  static_assert(std::is_integral<Value>::value || std::is_floating_point<Value>::value,
                "integer axis requires type floating point or integral type");

  using value_type = Value;
  using local_index_type = std::conditional_t<std::is_integral<value_type>::value,
                                              index_type, real_index_type>;

  using metadata_type = detail::replace_default<MetaData, std::string>;
  using options_type =
      detail::replace_default<Options, join<option::underflow, option::overflow>>;

  static_assert(!test<options_type, option::circular>::value ||
                    std::is_floating_point<value_type>::value ||
                    !test<options_type, option::overflow>::value,
                "integer axis with integral type cannot have overflow");

public:
  constexpr integer() = default;

  /** Construct over semi-open integer interval [start, stop).
   *
   * \param start    first integer of covered range.
   * \param stop     one past last integer of covered range.
   * \param meta     description of the axis.
   */
  integer(value_type start, value_type stop, metadata_type meta = {})
      : size_meta_(static_cast<index_type>(stop - start), std::move(meta)), min_(start) {
    if (stop <= start) BOOST_THROW_EXCEPTION(std::invalid_argument("bins > 0 required"));
  }

  /// Constructor used by algorithm::reduce to shrink and rebin.
  integer(const integer& src, index_type begin, index_type end, unsigned merge)
      : integer(src.value(begin), src.value(end), src.metadata()) {
    if (merge > 1)
      BOOST_THROW_EXCEPTION(std::invalid_argument("cannot merge bins for integer axis"));
    if (test<options_type, option::circular>::value && !(begin == 0 && end == src.size()))
      BOOST_THROW_EXCEPTION(std::invalid_argument("cannot shrink circular axis"));
  }

  /// Return index for value argument.
  index_type index(value_type x) const noexcept {
    return index_impl(std::is_floating_point<value_type>(), x);
  }

  /// Returns index and shift (if axis has grown) for the passed argument.
  auto update(value_type x) noexcept {
    auto impl = [this](long x) {
      const auto i = x - min_;
      if (i >= 0) {
        const auto k = static_cast<axis::index_type>(i);
        if (k < size()) return std::make_pair(k, 0);
        const auto n = k - size() + 1;
        size_meta_.first() += n;
        return std::make_pair(k, -n);
      }
      const auto k = static_cast<axis::index_type>(
          detail::static_if<std::is_floating_point<value_type>>(
              [](auto x) { return std::floor(x); }, [](auto x) { return x; }, i));
      min_ += k;
      size_meta_.first() -= k;
      return std::make_pair(0, -k);
    };

    return detail::static_if<std::is_floating_point<value_type>>(
        [this, impl](auto x) {
          if (std::isfinite(x)) return impl(static_cast<long>(std::floor(x)));
          // this->size() is workaround for gcc-5 bug
          return std::make_pair(x < 0 ? -1 : this->size(), 0);
        },
        impl, x);
  }

  /// Return value for index argument.
  value_type value(local_index_type i) const noexcept {
    if (!test<options_type, option::circular>::value) {
      if (i < 0) return detail::lowest<value_type>();
      if (i > size()) { return detail::highest<value_type>(); }
    }
    return min_ + i;
  }

  /// Return bin for index argument.
  decltype(auto) bin(index_type idx) const noexcept {
    return detail::static_if<std::is_floating_point<local_index_type>>(
        [this](auto idx) { return interval_view<integer>(*this, idx); },
        [this](auto idx) { return this->value(idx); }, idx);
  }

  /// Returns the number of bins, without over- or underflow.
  index_type size() const noexcept { return size_meta_.first(); }
  /// Returns the options.
  static constexpr unsigned options() noexcept { return options_type::value; }
  /// Returns reference to metadata.
  metadata_type& metadata() noexcept { return size_meta_.second(); }
  /// Returns reference to const metadata.
  const metadata_type& metadata() const noexcept { return size_meta_.second(); }

  template <class V, class M, class O>
  bool operator==(const integer<V, M, O>& o) const noexcept {
    return size() == o.size() && detail::relaxed_equal(metadata(), o.metadata()) &&
           min_ == o.min_;
  }

  template <class V, class M, class O>
  bool operator!=(const integer<V, M, O>& o) const noexcept {
    return !operator==(o);
  }

  template <class Archive>
  void serialize(Archive&, unsigned);

private:
  index_type index_impl(std::false_type, int x) const noexcept {
    const auto z = x - min_;
    if (test<options_type, option::circular>::value)
      return static_cast<index_type>(z - std::floor(float(z) / size()) * size());
    if (z < size()) return z >= 0 ? z : -1;
    return size();
  }

  template <typename T>
  index_type index_impl(std::true_type, T x) const noexcept {
    // need to handle NaN, cannot simply cast to int and call int-implementation
    const auto z = x - min_;
    if (test<options_type, option::circular>::value) {
      if (std::isfinite(z))
        return static_cast<index_type>(std::floor(z) - std::floor(z / size()) * size());
    } else if (z < size()) {
      return z >= 0 ? static_cast<index_type>(z) : -1;
    }
    return size();
  }

  detail::compressed_pair<index_type, metadata_type> size_meta_{0};
  value_type min_{0};

  template <class V, class M, class O>
  friend class integer;
};

#if __cpp_deduction_guides >= 201606

template <class T>
integer(T, T)->integer<detail::convert_integer<T, index_type>>;

template <class T>
integer(T, T, const char*)->integer<detail::convert_integer<T, index_type>>;

template <class T, class M>
integer(T, T, M)->integer<detail::convert_integer<T, index_type>, M>;

#endif

} // namespace axis
} // namespace histogram
} // namespace boost

#endif


#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>


// Copyright (c) 2015-2021 The Dogecoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <arith_uint256.h>
#include <chain.h>
#include <chainparams.h>
#include <dogecoin.h>

#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(dogecoin_tests, TestingSetup)

BOOST_AUTO_TEST_CASE(subsidy_test)
{
    const int nHeight = 36;
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    const auto params = chainParams->GetConsensus();
    uint256 prevHash = uint256S("0x4eb7c3f5914a84a25ea0ae12c39d1e5390a8ea576490035b63dcd03fcc14d106"); // Block 35

    CAmount nSubsidy = GetDogecoinBlockSubsidy(nHeight, params, prevHash);
    BOOST_CHECK_EQUAL(nSubsidy, 228450 * COIN);
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test_first_100k)
{
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    const auto params = chainParams->GetConsensus();
    CAmount nSum = 0;
    arith_uint256 prevHash = UintToArith256(uint256S("0"));

    for (int nHeight = 0; nHeight <= 100000; nHeight++) {
        CAmount nSubsidy = GetDogecoinBlockSubsidy(nHeight, params, ArithToUint256(prevHash));
        BOOST_CHECK(MoneyRange(nSubsidy));
        BOOST_CHECK(nSubsidy <= 1000000 * COIN);
        nSum += nSubsidy;
        // Use nSubsidy to give us some variation in previous block hash, without requiring full block templates
        prevHash += nSubsidy;
    }

    const CAmount expected = 54894174438 * COIN;
    BOOST_CHECK_EQUAL(expected, nSum);
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test_100k_145k)
{
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    const auto params = chainParams->GetConsensus();
    CAmount nSum = 0;
    arith_uint256 prevHash = UintToArith256(uint256S("0"));

    for (int nHeight = 100000; nHeight <= 145000; nHeight++) {
        CAmount nSubsidy = GetDogecoinBlockSubsidy(nHeight, params, ArithToUint256(prevHash));
        BOOST_CHECK(MoneyRange(nSubsidy));
        BOOST_CHECK(nSubsidy <= 500000 * COIN);
        nSum += nSubsidy;
        // Use nSubsidy to give us some variation in previous block hash, without requiring full block templates
        prevHash += nSubsidy;
    }

    const CAmount expected = 12349960000 * COIN;
    BOOST_CHECK_EQUAL(expected, nSum);
}

// Check the simplified rewards after block 145,000
BOOST_AUTO_TEST_CASE(subsidy_limit_test_post_145k)
{
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    const auto params = chainParams->GetConsensus();
    CAmount nSum = 0;
    const uint256 prevHash = uint256S("0");

    for (int nHeight = 145000; nHeight < 600000; nHeight++) {
        CAmount nSubsidy = GetDogecoinBlockSubsidy(nHeight, params, prevHash);
        CAmount nExpectedSubsidy = (500000 >> (nHeight / 100000)) * COIN;
        BOOST_CHECK(MoneyRange(nSubsidy));
        BOOST_CHECK_EQUAL(nSubsidy, nExpectedSubsidy);
        nSum += nSubsidy;
    }

    // Test reward at 600k+ is constant
    CAmount nConstantSubsidy = GetDogecoinBlockSubsidy(600000, params, prevHash);
    BOOST_CHECK_EQUAL(nConstantSubsidy, 10000 * COIN);

    nConstantSubsidy = GetDogecoinBlockSubsidy(700000, params, prevHash);
    BOOST_CHECK_EQUAL(nConstantSubsidy, 10000 * COIN);
}

BOOST_AUTO_TEST_CASE(get_next_work_difficulty_limit)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus();

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1386474927; // Block # 1

    pindexLast.nHeight = 239;
    pindexLast.nTime = 1386475638; // Block #239
    pindexLast.nBits = 0x1e0ffff0;
    BOOST_CHECK_EQUAL(CalculateDogecoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1e00ffffU);
}

BOOST_AUTO_TEST_CASE(get_next_work_pre_digishield)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus();

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1386942008; // Block 9359

    pindexLast.nHeight = 9599;
    pindexLast.nTime = 1386954113;
    pindexLast.nBits = 0x1c1a1206;
    BOOST_CHECK_EQUAL(CalculateDogecoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1c15ea59U);
}

BOOST_AUTO_TEST_CASE(get_next_work_digishield)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus();

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1395094427;

    // First hard-fork at 145,000, which applies to block 145,001 onwards
    pindexLast.nHeight = 145000;
    pindexLast.nTime = 1395094679;
    pindexLast.nBits = 0x1b499dfd;
    BOOST_CHECK_EQUAL(CalculateDogecoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1b671062U);
}

BOOST_AUTO_TEST_CASE(get_next_work_digishield_modulated_upper)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus();

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1395100835;

    // Test the upper bound on modulated time using mainnet block #145,107
    pindexLast.nHeight = 145107;
    pindexLast.nTime = 1395101360;
    pindexLast.nBits = 0x1b3439cd;
    BOOST_CHECK_EQUAL(CalculateDogecoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1b4e56b3U);
}

BOOST_AUTO_TEST_CASE(get_next_work_digishield_modulated_lower)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus();

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1395380517;

    // Test the lower bound on modulated time using mainnet block #149,423
    pindexLast.nHeight = 149423;
    pindexLast.nTime = 1395380447;
    pindexLast.nBits = 0x1b446f21;
    BOOST_CHECK_EQUAL(CalculateDogecoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1b335358U);
}

BOOST_AUTO_TEST_CASE(get_next_work_digishield_rounding)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus();

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1395094679;

    // Test case for correct rounding of modulated time - this depends on
    // handling of integer division, and is not obvious from the code
    pindexLast.nHeight = 145001;
    pindexLast.nTime = 1395094727;
    pindexLast.nBits = 0x1b671062;
    BOOST_CHECK_EQUAL(CalculateDogecoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1b6558a4U);
}

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef BOOST_GRAPH_AUGMENT_HPP
#define BOOST_GRAPH_AUGMENT_HPP

#include <boost/graph/filtered_graph.hpp>

namespace boost
{
namespace detail
{

    template < class Graph, class ResCapMap >
    filtered_graph< const Graph, is_residual_edge< ResCapMap > > residual_graph(
        const Graph& g, ResCapMap residual_capacity)
    {
        return filtered_graph< const Graph, is_residual_edge< ResCapMap > >(
            g, is_residual_edge< ResCapMap >(residual_capacity));
    }

    template < class Graph, class PredEdgeMap, class ResCapMap,
        class RevEdgeMap >
    inline void augment(const Graph& g,
        typename graph_traits< Graph >::vertex_descriptor src,
        typename graph_traits< Graph >::vertex_descriptor sink, PredEdgeMap p,
        ResCapMap residual_capacity, RevEdgeMap reverse_edge)
    {
        typename graph_traits< Graph >::edge_descriptor e;
        typename graph_traits< Graph >::vertex_descriptor u;
        typedef typename property_traits< ResCapMap >::value_type FlowValue;

        // find minimum residual capacity along the augmenting path
        FlowValue delta = (std::numeric_limits< FlowValue >::max)();
        e = get(p, sink);
        do
        {
            BOOST_USING_STD_MIN();
            delta = min BOOST_PREVENT_MACRO_SUBSTITUTION(
                delta, get(residual_capacity, e));
            u = source(e, g);
            e = get(p, u);
        } while (u != src);

        // push delta units of flow along the augmenting path
        e = get(p, sink);
        do
        {
            put(residual_capacity, e, get(residual_capacity, e) - delta);
            put(residual_capacity, get(reverse_edge, e),
                get(residual_capacity, get(reverse_edge, e)) + delta);
            u = source(e, g);
            e = get(p, u);
        } while (u != src);
    }

} // namespace detail
} // namespace boost

#endif /* BOOST_GRAPH_AUGMENT_HPP */


//==============================================================================
//         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_TOOLBOX_FDLIBM_FUNCTIONS_SCALAR_FREXP_HPP_INCLUDED
#define NT2_TOOLBOX_FDLIBM_FUNCTIONS_SCALAR_FREXP_HPP_INCLUDED
#include <nt2/toolbox/fdlibm/functions/frexp.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/at.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
  extern "C"{
    extern double fd_frexp ( double,int* );
  }
/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is fundamental_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::fdlibm::tag::frexp_, tag::cpu_,
			     (A0)(A1),
                       (scalar_<double_<A0> > )
		       (scalar_<int32_<A1> > )
                      )
  {
    typedef A0 result_type;

    inline result_type operator()(A0 const& a0,A1 & a1) const
    {
      return fd_frexp(a0, &a1);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::fdlibm::tag::frexp_, tag::cpu_,
                       (A0),
                       (scalar_<double_<A0> > )
                      )
  {
    typedef boost::fusion::vector<A0, nt2::int32_t>    result_type;

    NT2_FUNCTOR_CALL(1)
    {
      result_type res;
      boost::fusion::at_c<0>(res) =nt2::fdlibm::frexp(a0, boost::fusion::at_c<1>(res));
      return res;
    }
  };
} }
#endif


#include "frame_loader.h"

#include <string>
#include <stdint.h>
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <armadillo>

using namespace std;

FrameLoader::FrameLoader() {
	// empty
}

bool FrameLoader::LoadBackground(session_data &session) {

	bool bckg_loaded = true;

	image_size.clear();
	bckg_images.clear();

	for (int i=0; i<session.N_cam; i++) {

		string img_name = session.session_loc+"/"+session.bckg_loc+"/"+session.bckg_img_names[i];

		cv::Mat image = cv::imread(img_name, cv::IMREAD_GRAYSCALE);

		if (image.empty()) {
			bckg_loaded = false;
		}
		else {
			arma::Col<int> bckg_vec = FrameLoader::CVMat2Vector(image);
			bckg_images.push_back(bckg_vec);
			tuple<int, int> img_size = make_tuple(image.rows, image.cols);
			image_size.push_back(img_size);
		}

	}

	return bckg_loaded;
}

void FrameLoader::SetFrame(session_data &session, frame_data &frame) {
	frame.N_cam = session.N_cam;
	frame.image_size.clear();
	for (int i=0; i<session.N_cam; i++) {
		frame.image_size.push_back(image_size[i]);
	}
}

bool FrameLoader::LoadFrame(session_data &session, frame_data &frame, int MovNR, int FrameNR) {
	bool success;
	try {
		frame.mov_nr = MovNR;
		frame.frame_nr = FrameNR;
		frame.raw_frame.clear();
		for (int i=0; i<frame.N_cam; i++) {
			string img_name = 	session.session_loc+"/"+session.mov_names[MovNR]+"/"+session.cam_names[i]+"/"+
								session.frame_name+to_string(session.chrono_frames[FrameNR])+"."+session.frame_img_format;
			cv::Mat image = cv::imread(img_name, cv::IMREAD_GRAYSCALE);
			if (image.empty()) {
				success = false;
			}
			else {
				arma::Col<int> img_vec = FrameLoader::CVMat2Vector(image);
				frame.raw_frame.push_back(FrameLoader::BackgroundSubtract(img_vec,bckg_images[i]));
			}
		}
	}
	catch(...) {
		success = false;
	}
	return success;
}

arma::Col<int> FrameLoader::BackgroundSubtract(arma::Col<int> &img_vec, arma::Col<int> &bckg_vec) {
	arma::Col<int> s255;
	s255.set_size(img_vec.n_rows);
	s255.fill(255);
	arma::Col<int> sub_img = img_vec-(bckg_vec-s255);
	return sub_img;
}

np::ndarray FrameLoader::ReturnFrame(session_data &session, frame_data &frame, int CamNR) {

	int N_row = get<0>(image_size[CamNR]);
	int N_col = get<1>(image_size[CamNR]);

	p::tuple shape = p::make_tuple(N_row,N_col);
	np::dtype dtype = np::dtype::get_builtin<int>();
	np::ndarray array_out = np::zeros(shape,dtype);

	for (int i=0; i<N_row; i++) {
		for (int j=0; j<N_col; j++) {
			array_out[i][j] = frame.raw_frame[CamNR](i*N_row+j);
		}
	}

	return array_out;
}

arma::Col<int> FrameLoader::CVMat2Vector(cv::Mat &img) {

	arma::Col<int> img_vec;

	img.convertTo(img, CV_8UC1);
	arma::Mat<uint8_t> img_mat(reinterpret_cast<uint8_t*>(img.data), img.rows, img.cols);
	img_vec = arma::vectorise(arma::conv_to<arma::Mat<int>>::from(img_mat));

	return img_vec;
}

cv::Mat FrameLoader::Vector2CVMat(arma::Col<int> &img_vec, int N_row, int N_col) {

	arma::Mat<int> img_mat;

	img_mat = img_vec;

	img_mat.reshape(N_row,N_col);

	cv::Mat img(img_mat.n_rows, img_mat.n_cols, CV_32SC1, img_mat.memptr());

	img.convertTo(img, CV_8UC1);

	return img;
}


#include <neat/src/Net.hpp>
#include <algorithm>
#include <numeric>
#include <gsl/gsl>
#include <boost/iterator/indirect_iterator.hpp>
#include <neat/src/Neuron.hpp>
#include <neat/src/Weight.hpp>
#include <neat/src/NetInputNeuron.hpp>

namespace spacey::neat {

Net::Net(InputSize inputSize, OutputSize outputSize) {
    Expects(inputSize.value >= 1);
    Expects(outputSize.value >= 1);
    neurons.reserve(inputSize.value + outputSize.value);
    netInputs.reserve(inputSize.value);
    netOutputs.reserve(outputSize.value);
    while (inputSize.value--) {
        auto& neuron = neurons.emplace_back(std::make_unique<NetInputNeuron>(neurons));
        netInputs.emplace_back(neuron.get());
    }
    while (outputSize.value--) {
        auto& neuron = neurons.emplace_back(std::make_unique<Neuron>(neurons));
        netOutputs.emplace_back(neuron.get());
    }
}

auto Net::getNetInputs() const -> const ObservedNeuronList& {
    return netInputs;
}

auto Net::getNetOutputs() const -> const ObservedNeuronList& {
    return netOutputs;
}

auto Net::addNeuron() -> gsl::not_null<Neuron*> {
    auto& neuron = neurons.emplace_back(std::make_unique<Neuron>(neurons));
    return gsl::not_null{ neuron.get() };
}

std::vector<Response> Net::gatherOutputsResponses() {
    std::vector<Response> responses;
    std::transform(std::begin(netOutputs), std::end(netOutputs), std::back_inserter(responses), [](auto&& node) {
        return node->getResponse();
    });
    return responses;
}

auto Net::getNetResponseFor(const std::vector<NetActivation>& inputs) -> std::vector<Response> {
    Expects(inputs.size() == netInputs.size());
    insertNetInputValues(inputs);
    auto visited = VisitedNeuronsMap{};
    markNetInputsAsVisited(visited);
    propagateNetInputs(visited);
    auto responses = gatherOutputsResponses();
    Ensures(responses.size() == netOutputs.size());
    return responses;
}

void Net::insertNetInputValues(const std::vector<NetActivation>& inputs) {
    const auto indirectNetInputs =
        boost::indirect_iterator<ObservedNeuronList::iterator, Neuron>(netInputs.begin());
    // false positive with indirect_iterator
#pragma warning(suppress : 26444)
    std::transform(std::begin(inputs), std::end(inputs), indirectNetInputs, [](auto&& input) {
        // no scaling of the input
        const auto inputWeight = Weight{ 1.0L };
        return Neuron::activationFunction(input * inputWeight);
    });
}

void Net::markNetInputsAsVisited(Net::VisitedNeuronsMap& visited) {
    std::for_each(std::begin(netInputs), std::end(netInputs), [&visited](auto&& node) {
        // after setting the responses of the inputNodes, set them to be visited ones
        // this will stop the recursion
        visited[node] = true;
    });
}

void Net::propagateNetInputs(Net::VisitedNeuronsMap& visited) {
    std::for_each(std::begin(netOutputs), std::end(netOutputs), [this, &visited](auto&& node) {
        // propagate the input with DFS
        calculateResponseFor(*node, visited);
    });
}

void Net::calculateResponseFor(Neuron& neuron, VisitedNeuronsMap& visited) {
    auto&& inputs = neuron.getInputs();
    std::for_each(std::begin(inputs), std::end(inputs), [this, &visited](auto&& input) {
        auto& [inputNeuron, inputNeuronWeight] = input;
        if (!visited[inputNeuron]) {
            visited[inputNeuron] = true;
            calculateResponseFor(*inputNeuron, visited);
        }
    });
    // note: input.response may be read for nodes that are marked visited, but not
    // calculated yet. That's normal, it's just a cycle in the network, it will
    // behave like a recurrent neuron (it will use value from previous evaluation)
    const auto weightedActivation = std::accumulate(std::begin(inputs), std::end(inputs),
    WeightedActivation{ 0.0L }, [](auto&& accumulatedActivation, auto&& input) {
        auto& [inputNeuron, inputNeuronWeight] = input;
        return accumulatedActivation + inputNeuron->getResponse() * inputNeuronWeight;
    });
    neuron.setResponse(Neuron::activationFunction(weightedActivation));
}

}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> test_int_D_D(const Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> & M)
{
	return M;
}
void export_int_D_D()
{
	boost::python::def("test_int_D_D",test_int_D_D);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, Eigen::Dynamic, 1> test_int_D_1(const Eigen::Matrix<int, Eigen::Dynamic, 1> & M)
{
	return M;
}
void export_int_D_1()
{
	boost::python::def("test_int_D_1",test_int_D_1);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, Eigen::Dynamic, 2> test_int_D_2(const Eigen::Matrix<int, Eigen::Dynamic, 2> & M)
{
	return M;
}
void export_int_D_2()
{
	boost::python::def("test_int_D_2",test_int_D_2);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, Eigen::Dynamic, 3> test_int_D_3(const Eigen::Matrix<int, Eigen::Dynamic, 3> & M)
{
	return M;
}
void export_int_D_3()
{
	boost::python::def("test_int_D_3",test_int_D_3);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, Eigen::Dynamic, 4> test_int_D_4(const Eigen::Matrix<int, Eigen::Dynamic, 4> & M)
{
	return M;
}
void export_int_D_4()
{
	boost::python::def("test_int_D_4",test_int_D_4);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, Eigen::Dynamic, 5> test_int_D_5(const Eigen::Matrix<int, Eigen::Dynamic, 5> & M)
{
	return M;
}
void export_int_D_5()
{
	boost::python::def("test_int_D_5",test_int_D_5);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, Eigen::Dynamic, 6> test_int_D_6(const Eigen::Matrix<int, Eigen::Dynamic, 6> & M)
{
	return M;
}
void export_int_D_6()
{
	boost::python::def("test_int_D_6",test_int_D_6);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 1, Eigen::Dynamic> test_int_1_D(const Eigen::Matrix<int, 1, Eigen::Dynamic> & M)
{
	return M;
}
void export_int_1_D()
{
	boost::python::def("test_int_1_D",test_int_1_D);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 1, 1> test_int_1_1(const Eigen::Matrix<int, 1, 1> & M)
{
	return M;
}
void export_int_1_1()
{
	boost::python::def("test_int_1_1",test_int_1_1);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 1, 2> test_int_1_2(const Eigen::Matrix<int, 1, 2> & M)
{
	return M;
}
void export_int_1_2()
{
	boost::python::def("test_int_1_2",test_int_1_2);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 1, 3> test_int_1_3(const Eigen::Matrix<int, 1, 3> & M)
{
	return M;
}
void export_int_1_3()
{
	boost::python::def("test_int_1_3",test_int_1_3);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 1, 4> test_int_1_4(const Eigen::Matrix<int, 1, 4> & M)
{
	return M;
}
void export_int_1_4()
{
	boost::python::def("test_int_1_4",test_int_1_4);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 1, 5> test_int_1_5(const Eigen::Matrix<int, 1, 5> & M)
{
	return M;
}
void export_int_1_5()
{
	boost::python::def("test_int_1_5",test_int_1_5);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 1, 6> test_int_1_6(const Eigen::Matrix<int, 1, 6> & M)
{
	return M;
}
void export_int_1_6()
{
	boost::python::def("test_int_1_6",test_int_1_6);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 2, Eigen::Dynamic> test_int_2_D(const Eigen::Matrix<int, 2, Eigen::Dynamic> & M)
{
	return M;
}
void export_int_2_D()
{
	boost::python::def("test_int_2_D",test_int_2_D);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 2, 1> test_int_2_1(const Eigen::Matrix<int, 2, 1> & M)
{
	return M;
}
void export_int_2_1()
{
	boost::python::def("test_int_2_1",test_int_2_1);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 2, 2> test_int_2_2(const Eigen::Matrix<int, 2, 2> & M)
{
	return M;
}
void export_int_2_2()
{
	boost::python::def("test_int_2_2",test_int_2_2);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 2, 3> test_int_2_3(const Eigen::Matrix<int, 2, 3> & M)
{
	return M;
}
void export_int_2_3()
{
	boost::python::def("test_int_2_3",test_int_2_3);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 2, 4> test_int_2_4(const Eigen::Matrix<int, 2, 4> & M)
{
	return M;
}
void export_int_2_4()
{
	boost::python::def("test_int_2_4",test_int_2_4);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 2, 5> test_int_2_5(const Eigen::Matrix<int, 2, 5> & M)
{
	return M;
}
void export_int_2_5()
{
	boost::python::def("test_int_2_5",test_int_2_5);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 2, 6> test_int_2_6(const Eigen::Matrix<int, 2, 6> & M)
{
	return M;
}
void export_int_2_6()
{
	boost::python::def("test_int_2_6",test_int_2_6);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 3, Eigen::Dynamic> test_int_3_D(const Eigen::Matrix<int, 3, Eigen::Dynamic> & M)
{
	return M;
}
void export_int_3_D()
{
	boost::python::def("test_int_3_D",test_int_3_D);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 3, 1> test_int_3_1(const Eigen::Matrix<int, 3, 1> & M)
{
	return M;
}
void export_int_3_1()
{
	boost::python::def("test_int_3_1",test_int_3_1);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 3, 2> test_int_3_2(const Eigen::Matrix<int, 3, 2> & M)
{
	return M;
}
void export_int_3_2()
{
	boost::python::def("test_int_3_2",test_int_3_2);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 3, 3> test_int_3_3(const Eigen::Matrix<int, 3, 3> & M)
{
	return M;
}
void export_int_3_3()
{
	boost::python::def("test_int_3_3",test_int_3_3);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 3, 4> test_int_3_4(const Eigen::Matrix<int, 3, 4> & M)
{
	return M;
}
void export_int_3_4()
{
	boost::python::def("test_int_3_4",test_int_3_4);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 3, 5> test_int_3_5(const Eigen::Matrix<int, 3, 5> & M)
{
	return M;
}
void export_int_3_5()
{
	boost::python::def("test_int_3_5",test_int_3_5);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 3, 6> test_int_3_6(const Eigen::Matrix<int, 3, 6> & M)
{
	return M;
}
void export_int_3_6()
{
	boost::python::def("test_int_3_6",test_int_3_6);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 4, Eigen::Dynamic> test_int_4_D(const Eigen::Matrix<int, 4, Eigen::Dynamic> & M)
{
	return M;
}
void export_int_4_D()
{
	boost::python::def("test_int_4_D",test_int_4_D);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 4, 1> test_int_4_1(const Eigen::Matrix<int, 4, 1> & M)
{
	return M;
}
void export_int_4_1()
{
	boost::python::def("test_int_4_1",test_int_4_1);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 4, 2> test_int_4_2(const Eigen::Matrix<int, 4, 2> & M)
{
	return M;
}
void export_int_4_2()
{
	boost::python::def("test_int_4_2",test_int_4_2);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 4, 3> test_int_4_3(const Eigen::Matrix<int, 4, 3> & M)
{
	return M;
}
void export_int_4_3()
{
	boost::python::def("test_int_4_3",test_int_4_3);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 4, 4> test_int_4_4(const Eigen::Matrix<int, 4, 4> & M)
{
	return M;
}
void export_int_4_4()
{
	boost::python::def("test_int_4_4",test_int_4_4);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 4, 5> test_int_4_5(const Eigen::Matrix<int, 4, 5> & M)
{
	return M;
}
void export_int_4_5()
{
	boost::python::def("test_int_4_5",test_int_4_5);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 4, 6> test_int_4_6(const Eigen::Matrix<int, 4, 6> & M)
{
	return M;
}
void export_int_4_6()
{
	boost::python::def("test_int_4_6",test_int_4_6);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 5, Eigen::Dynamic> test_int_5_D(const Eigen::Matrix<int, 5, Eigen::Dynamic> & M)
{
	return M;
}
void export_int_5_D()
{
	boost::python::def("test_int_5_D",test_int_5_D);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 5, 1> test_int_5_1(const Eigen::Matrix<int, 5, 1> & M)
{
	return M;
}
void export_int_5_1()
{
	boost::python::def("test_int_5_1",test_int_5_1);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 5, 2> test_int_5_2(const Eigen::Matrix<int, 5, 2> & M)
{
	return M;
}
void export_int_5_2()
{
	boost::python::def("test_int_5_2",test_int_5_2);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 5, 3> test_int_5_3(const Eigen::Matrix<int, 5, 3> & M)
{
	return M;
}
void export_int_5_3()
{
	boost::python::def("test_int_5_3",test_int_5_3);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 5, 4> test_int_5_4(const Eigen::Matrix<int, 5, 4> & M)
{
	return M;
}
void export_int_5_4()
{
	boost::python::def("test_int_5_4",test_int_5_4);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 5, 5> test_int_5_5(const Eigen::Matrix<int, 5, 5> & M)
{
	return M;
}
void export_int_5_5()
{
	boost::python::def("test_int_5_5",test_int_5_5);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 5, 6> test_int_5_6(const Eigen::Matrix<int, 5, 6> & M)
{
	return M;
}
void export_int_5_6()
{
	boost::python::def("test_int_5_6",test_int_5_6);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, Eigen::Dynamic> test_int_6_D(const Eigen::Matrix<int, 6, Eigen::Dynamic> & M)
{
	return M;
}
void export_int_6_D()
{
	boost::python::def("test_int_6_D",test_int_6_D);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 1> test_int_6_1(const Eigen::Matrix<int, 6, 1> & M)
{
	return M;
}
void export_int_6_1()
{
	boost::python::def("test_int_6_1",test_int_6_1);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 2> test_int_6_2(const Eigen::Matrix<int, 6, 2> & M)
{
	return M;
}
void export_int_6_2()
{
	boost::python::def("test_int_6_2",test_int_6_2);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 3> test_int_6_3(const Eigen::Matrix<int, 6, 3> & M)
{
	return M;
}
void export_int_6_3()
{
	boost::python::def("test_int_6_3",test_int_6_3);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 4> test_int_6_4(const Eigen::Matrix<int, 6, 4> & M)
{
	return M;
}
void export_int_6_4()
{
	boost::python::def("test_int_6_4",test_int_6_4);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 5> test_int_6_5(const Eigen::Matrix<int, 6, 5> & M)
{
	return M;
}
void export_int_6_5()
{
	boost::python::def("test_int_6_5",test_int_6_5);
}

#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 6> test_int_6_6(const Eigen::Matrix<int, 6, 6> & M)
{
	return M;
}
void export_int_6_6()
{
	boost::python::def("test_int_6_6",test_int_6_6);
}


#include <boost/hana/concept/group.hpp>


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2020 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

/** \file NSLawMatrix.hpp
 *  \brief Base (abstract) class for a symmetric matrix of nonsmooth laws
*/

#ifndef NSLAWMATRIX_H
#define NSLAWMATRIX_H

#include <SiconosSerialization.hpp>
#include <NonSmoothLaw.hpp>
#include <boost/numeric/ublas/symmetric.hpp>

/** This class uses (extends) a symmetric matrix to store
 *  NonSmoothLaws associated to pairs of integers.  It can be used
 *  e.g. to maintain a list of non-smooth laws associated with contact
 *  between types of objects. */
class NSLawMatrix : public boost::numeric::ublas::symmetric_matrix < SP::NonSmoothLaw >
{
protected:
  /** serialization hooks
  */
  ACCEPT_SERIALIZATION(NSLawMatrix);

public:
  NSLawMatrix(NSLawMatrix::size_type i = 1)
    : boost::numeric::ublas::symmetric_matrix < SP::NonSmoothLaw >(i) {}
};

#endif


//
// Created by pedro on 27/11/18.
//

#ifndef IFTWT_RG_GLOBALS_H
#define IFTWT_RG_GLOBALS_H

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/graph_utility.hpp>
#include <pcl/pcl_base.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>

#include <ctime>
#include <math.h>
#include <set>
#include <algorithm>
#include <iostream>
#include <map>
#include <vector>
#include <queue>

namespace global{
    /** PCL types **/
    typedef pcl::PointXYZ Point;
    typedef pcl::PointCloud<Point> Cloud;
    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> CloudT;
    typedef pcl::PointXYZI PointI;
    typedef pcl::PointCloud<PointI> CloudI;
    typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointI, float> graph_i;
    typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float> graph_t;

    /** boost types **/
    struct VertexBundle{
        unsigned int cost;
        PointI point;
    };
    class EdgeBundle {
    public:
        EdgeBundle( unsigned int weight=0) : weight(weight){}

        unsigned int weight;
    };

    typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS,
    VertexBundle,EdgeBundle> graph_v;
    typedef boost::graph_traits<graph_v>::vertex_descriptor pcd_vx_descriptor;
    typedef boost::graph_traits<graph_v>::vertex_iterator pcd_vx_iterator;

    template<class T,
            class Container = std::vector<T>,
            class Compare = std::less<typename Container::value_type>>
    class custom_priority_queue : public std::priority_queue<T,Container,Compare>
    {
    public:
        bool remove(const T& value)
        {
            auto it = std::find(this->c.begin(), this->c.end(), value);
            if(it != this->c.end())
            {
                this->c.erase(it);
                std::make_heap(this->c.begin(), this->c.end(), this->comp);
                return true;
            } else
                return false;
        }
        Container& getContainer() { return this->c; }
        void print()
        {
            for(auto it = this->c.begin(); it != this->c.end(); ++it)
                std::cout << *it << " ";
            std::cout << std::endl;
        }
    };

    static bool p_equal (const global::PointI p1, const global::PointI p2){
        return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z;
    };
}

#endif //IFTWT_RG_GLOBALS_H