#include <aslam/backend/Optimizer.hpp>
// std::partial_sum
#include <numeric>
#include <aslam/backend/ErrorTerm.hpp>
// M.inverse()
#include <Eigen/Dense>
#include <sm/eigen/assert_macros.hpp>
#include <sparse_block_matrix/linear_solver_dense.h>
#include <sparse_block_matrix/linear_solver_cholmod.h>

#ifndef QRSOLVER_DISABLED
#include <sparse_block_matrix/linear_solver_spqr.h>
#endif

#include <aslam/backend/sparse_matrix_functions.hpp>

namespace aslam {
  namespace backend {

    Optimizer::Optimizer(const OptimizerOptions& options) :
      _options(options)
    {
      initializeLinearSolver();
    }

    Optimizer::~Optimizer()
    {
    }

    /// \brief Set up to work on the optimization problem.
    void Optimizer::setProblem(boost::shared_ptr<OptimizationProblemBase> problem)
    {
      _problem = problem;
    }

    void Optimizer::initializeLinearSolver()
    {
      // \todo Add the remaining sparse_block_matrix solvers here.
      if (_options.linearSolver == "cholmod") {
        _options.verbose && std::cout << "Using the cholmod linear solver.\n";
        _solver.reset(new sparse_block_matrix::LinearSolverCholmod<Eigen::MatrixXd>());
      } else if (_options.linearSolver == "dense") {
        _options.verbose && std::cout << "Using the dense linear solver.\n";
        _solver.reset(new sparse_block_matrix::LinearSolverDense<Eigen::MatrixXd>());
      }
#ifndef QRSOLVER_DISABLED
      else if (_options.linearSolver == "spqr") {
        _options.verbose && std::cout << "Using the SPQR linear solver.\n";
        _solver.reset(new sparse_block_matrix::LinearSolverQr<Eigen::MatrixXd>());
      }
#endif
      else {
        _options.verbose && std::cout << "Unknown linear solver specified: " << _options.linearSolver << ". Using cholmod.\n";
        _solver.reset(new sparse_block_matrix::LinearSolverCholmod<Eigen::MatrixXd>());
      }
    }

    /// \brief initialize the optimizer to run on an optimization problem.
    ///        This should be called before calling optimize()
    void Optimizer::initialize()
    {
      Timer init("Optimizer: Initialize Total");
      Timer initDv("Optimizer: Initialize---Design Variables");
      _designVariables.clear();
      std::vector<DesignVariable*> marginalizedDesignVariables;
      // Run through all design variables adding active ones to an active list.
      for (size_t i = 0; i < _problem->numDesignVariables(); ++i) {
        DesignVariable* dv = _problem->designVariable(i);
        if (dv->isActive()) {
          if (dv->isMarginalized() && _options.doSchurComplement) {
            marginalizedDesignVariables.push_back(dv);
          } else {
            _designVariables.push_back(dv);
          }
        }
      }
      SM_ASSERT_FALSE(Exception, _designVariables.empty(), "It is illegal to run the optimizer with all marginalized design variables.");
      // Assign block indices to the design variables.
      // "blocks" will hold the structure of the left-hand-side of Gauss-Newton
      std::vector<int> blocks;
      for (size_t i = 0; i < _designVariables.size(); ++i) {
        _designVariables[i]->setBlockIndex(i);
        blocks.push_back(_designVariables[i]->minimalDimensions());
      }
      _marginalizedStartingBlock = _designVariables.size();
      for (size_t i = 0; i < marginalizedDesignVariables.size(); ++i) {
        _designVariables.push_back(marginalizedDesignVariables[i]);
        marginalizedDesignVariables[i]->setBlockIndex(i + _marginalizedStartingBlock);
        blocks.push_back(marginalizedDesignVariables[i]->minimalDimensions());
      }
      initDv.stop();
      Timer initEt("Optimizer: Initialize---Error Terms");
      // Get all of the error terms that work on these design variables.
      _errorTerms.clear();
      for (size_t i = 0; i < _designVariables.size(); ++i) {
        _problem->getErrors(_designVariables[i], _errorTerms);
      }
      initEt.stop();
      Timer initMx("Optimizer: Initialize---Matrices");
      // Set up the block matrix structure.
      // Surrently "blocks" just has the sizes of the minimal dimensions of each block.
      // the sparse matrix initialization requires the cumulative sum of this.
      std::partial_sum(blocks.begin(), blocks.end(), blocks.begin());
      // Now we can initialized the sparse Hessian matrix.
      _H = SparseBlockMatrix(blocks, blocks);
      //_invH = SparseBlockMatrix(blocks, blocks);
      _rhs.resize(_H.rows());
      // And the marginalized blocks. These are not strictly necessary
      // but they will help with debugging.
      blocks.resize(_marginalizedStartingBlock);
      _A = SparseBlockMatrix(blocks, blocks);
      _b.resize(_A.rows());
      _invVi.resize(numSparseDesignVariables());
      _dx.resize(_H.rowBaseOfBlock(_marginalizedStartingBlock));
      initMx.stop();
      _options.verbose && std::cout << "Optimization problem initialized with " << _designVariables.size() << " design variables and " << _errorTerms.size() << " error terms\n";
      _options.verbose && std::cout << "The dense part of the state is " << _H.rowBaseOfBlock(_marginalizedStartingBlock) << " parameters and the sparse part is " << _H.cols() - _H.rowBaseOfBlock(_marginalizedStartingBlock) << " parameters\n";
    }

    double Optimizer::getLmRho()
    {
      double d1 = _p_J - _J;    // update cost delta
      // L(0) - L(h):
      double d2 = _dx.transpose() * (_lambda * _dx + _b);
      return d1 / d2;
    }

    void Optimizer::setInitialLambda()
    {
      // check if _lambda already set:
      if (_lambda != 0) // if set return
        return;
      if (_options.levenbergMarquardtEstimateLambdaScale <= 0) {
        _lambda = _options.levenbergMarquardtLambdaInit;
      } else {
        double maximum = 0;
        // find the maximum diagonal element in H ans scale by LambdaScale:
        // loop the diagonal blocks:
        for (int i = 0; i < _H.bRows(); i++) {
          const Eigen::MatrixXd* H = _H.block(i, i, false); // do no allocate if missing!
          if (H) {
            for (int j = 0; j < H->rows(); j++) {
              if (fabs((*H)(j, j)) > maximum)
                maximum = fabs((*H)(i, j));
            }
          }
        }
        _lambda = maximum * _options.levenbergMarquardtEstimateLambdaScale;
        _options.verbose && std::cout << "Initialised Lambda: " << _lambda << std::endl;
      }
    }

    /*
    // returns true of stop!
    bool Optimizer::evaluateStoppingCriterion(int iterations)
    {

      // as we have analytic Jacobians we can assume the precision to be:
      double epsilon = std::numeric_limits<double>::epsilon();

      double x_norm = ...;

      // the gradient: is simply the right hand side of GN:
      double grad_norm = _rhs.norm();
      double abs_J = fabs(_J);

      // the first condition:
      bool crit1 = grad_norm < sqrt(epsilon) * (1 + abs_J);

      bool crit2 = _dx.norm() < sqrt(epsilon) * (1 + x_norm);

      bool crit3 = fabs(_J - _p_J) < epsilon * (1 + abs_J);

      bool crit4 = iterations < _options.maxIterations;

      return (crit1 && crit2 && crit3) || crit4;

    }*/

          SolutionReturnValue Optimizer::optimizeDogLeg()
          {
            Timer timeGn("Optimizer: build Hessian", true);
            Timer timeErr("Optimizer: evaluate error", true);
            Timer timeSchur("Optimizer: Schur complement", true);
            Timer timeBackSub("Optimizer: Back substitution", true);
            Timer timeSolve("Optimizer: Solve linear system", true);
            Timer timeSD("Optimizer: Solve Steepest Descent", true);

              // Select the design variables and (eventually) the error terms involved in the optimization.
              initialize();

              SolutionReturnValue srv;
              _p_J = 0.0;

              // This sets _J
              evaluateError();

              _p_J = _J;
              srv.JStart = _p_J;

              // *** while not done
              _options.verbose && std::cout << "[" << srv.iterations << ".0]: J: " << _J << std::endl;

              // Set up the estimation problem.
              double deltaX = _options.convergenceDeltaX + 1.0;
              double deltaJ = _options.convergenceDeltaJ + 1.0;

              // choose initial delta to be something between GN and SD but biased towards GN
              // would be ideal but to avoid computing the GN solution in the first step before
              // looping we take the SD norm.
              double _delta = 0;
              double _p_delta = 0;
              // build Gauss newton matrices:
              timeGn.start();
              buildGnMatrices();
              timeGn.stop();

              Eigen::VectorXd _dx_sd(_H.rowBaseOfBlock(_marginalizedStartingBlock));
              Eigen::VectorXd _dx_gn(_H.rowBaseOfBlock(_marginalizedStartingBlock));

              double _dx_sd_norm = 0;
              double _dx_gn_norm = 0;
          double rho = 0;
          double L0 = 0;
          double _sd_scale = 0;
          double beta = 0;

          std::string stepType;

            // Loop until convergence
            while(   srv.iterations   < _options.maxIterations &&
                 deltaX       > _options.convergenceDeltaX &&
                     fabs(deltaJ)   > _options.convergenceDeltaJ)
         // while( !evaluateStoppingCriterion() )
          {
              // calculate steepest descent step:
              timeSD.start();

              Eigen::VectorXd Hrhs(_H.rows(),1);
              _H.multiply(&Hrhs, _rhs);

              _sd_scale = _rhs.squaredNorm() / Hrhs.squaredNorm();
             // std::cout << "  alpha:" << _sd_scale << std::endl;
              _dx_sd = _sd_scale * _rhs;

              timeSD.stop();

              // we need the norm for comparison:
              _dx_sd_norm = _dx_sd.norm();

              int stepIterations = 0;
              int linearSolverFailCounter = 0;

              bool gnComputed = false;
              do
              {
                // Trust Region smaller than SD:
                if (_dx_sd_norm >= _delta && _delta != 0)
                {
                  _dx = _delta / _dx_sd_norm * _dx_sd;  // scale SD step to fit into trust region
                  L0 = _delta * ( 2*_dx_sd_norm - _delta ) / ( 2*_sd_scale );
                  stepType = "SD";
                }
                // otherwise check the GN step
                else
                {
                  // calculate the GN step.
                  if(!gnComputed)
                  {
                    // set lambda to 0 in here...
                    _lambda = 0;

                    timeSchur.start();
                    applySchurComplement(_H, _rhs, _lambda,
                        _marginalizedStartingBlock,
                        _options.doLevenbergMarquardt,
                        _A, _invVi, _b);
                    timeSchur.stop();

                    // Solve the dense system.
                              if(_options.resetSolverEveryIteration)
                              {
                                  _solver->init();
                              }

                              timeSolve.start();
                              bool solutionSuccess = _solver->solve(_A, &_dx_gn[0], &_b[0]);
                              timeSolve.stop();

                              if(!solutionSuccess)
                              {

                                // the default solver failed. try the QR solver as a robust sparse alternative:
                                if(!_fallbackSolver) {  // initialise a new solver
                                  // for now take QR
                                  /// \todo: add an intelligent fallbackSolver structure.
                                  /// maybe add it as a property of the linear solvers and directly name the solvers in their
                                  /// class instead of the optimizer init()
                                  _fallbackSolver.reset(new sparse_block_matrix::LinearSolverQr<Eigen::MatrixXd>());
                                  _fallbackSolver->init();
                                  _options.verbose && std::cout << "Optimizer: The linear solution failed. Fallback to QR. (" << linearSolverFailCounter << ")" << std::endl;
                                }

                                solutionSuccess = _fallbackSolver->solve(_A, &_dx_gn[0], &_b[0]);
                                if(!solutionSuccess) {
                                  _options.verbose && std::cout << "Optimizer: The linear solution failed. Again... (" << linearSolverFailCounter << ")" << std::endl;
                                  _dx_gn = _dx_sd;
                                }

                                // set the default solver to the robust one.
                                if(linearSolverFailCounter > _options.linearSolverMaximumFails)
                                {
                                  _solver.reset(new sparse_block_matrix::LinearSolverQr<Eigen::MatrixXd>());
                                  _solver->init();
                                }
                                linearSolverFailCounter++;  // increment
                                // SM_ASSERT_TRUE(Exception, linearSolverFailCounter <= _options.linearSolverMaximumFails, "The linear solution failed");

                              }
                              gnComputed = true;  // now we have it!
                              // and calculate the norm:
                              _dx_gn_norm = _dx_gn.norm();
                  }

                  // set delta in the first step to take a full GN step:
                  if(_delta == 0) {
                    _delta = (_dx_sd + 0.5 * ( _dx_gn - _dx_sd )).norm();
                  }
                  // now check the size of the gn step:
                  if(_dx_gn_norm <= _delta)
                  {
                    _dx = _dx_gn; // trust region larger than GN step. take it!
                    L0 = _J;
                    stepType = "GN";
                  }
                  else  // otherwise interpolate on the line between the cauchy point and gn step
                  {
                    // get beta:
                    Eigen::VectorXd dgnsd = _dx_gn - _dx_sd;
                    double gdnsd_norm_sqr = dgnsd.squaredNorm();

                    double c = _dx_sd.transpose() * ( dgnsd );
                    if(c <= 0)
                    {
                      beta = -c + sqrt( c*c + gdnsd_norm_sqr*(_delta*_delta - _dx_sd_norm*_dx_sd_norm) );
                      beta /= gdnsd_norm_sqr;
                    }
                    else
                    {
                      beta = (_delta*_delta - _dx_sd_norm*_dx_sd_norm);
                      beta /= c + sqrt( c*c + gdnsd_norm_sqr*(_delta*_delta - _dx_sd_norm*_dx_sd_norm) );
                    }

                    _dx = _dx_sd + beta * ( dgnsd );
                    L0 = 1/2 * _sd_scale * (1-beta)*(1-beta)* _rhs.squaredNorm() + beta*(2-beta)*_J;
                    stepType = "DL";
                  }
                }
                // update:
                timeBackSub.start();
                deltaX = applyStateUpdate();
                timeBackSub.stop();

                // reevaluate error
              timeErr.start();
                evaluateError();
                timeErr.stop();

                // same check as in GN lambda update:
                // rho = getLmRho();
                rho = (_p_J - _J) / L0;
                if(std::isnan((double)rho))
                    rho = -1;

                //std::cout << "RHO: " << rho << " L0: " << L0 << std::endl;
                if(rho > 0)
                {
                  // update GN matrices:
                  timeGn.start();
                  buildGnMatrices();
                  timeGn.stop();
                }
                else
                {
                  revertLastStateUpdate();
                  evaluateError();
                }
                // update trust region
                _p_delta = _delta;
                if( rho > 0.75 ) // step succeeded
                {
                  double _dx_norm3 = 3 * _dx.norm();
                  if ( _delta < _dx_norm3 ) {
                    _delta = _dx_norm3;
                  }
                }
                else if (rho > 0 && rho < 0.25) // step almost failed
                {
                  _delta /= 2.0;
                }
                else if (rho <= 0)  // step failed
                {
                  // if we took a GN step set the trust region to the GN Step / 2
                  if(stepType == "GN")
                    _delta = _dx_gn_norm / 2.0;
                  else
                    _delta /= 2.0;
                }

                deltaJ = _p_J - _J;
                //std::cout << "_p_J:" << _p_J << std::endl;
                //std::cout << "_J:" << _J << std::endl;
                if(_J < _p_J)
                  _p_J = _J;
                else
                  _J = _p_J;

                _options.verbose && std::cout << "   (" << stepIterations << "): J: " << _J << ", dJ: " << deltaJ << ", deltaX: " << deltaX << ", delta: " << _p_delta << ", StepType: " << stepType;

                if(stepType == "DL")
                  std::cout << ", beta: " << beta << std::endl;
                else
                  std::cout << std::endl;

                stepIterations++;

              } while( rho < 0 && _delta > _options.convergenceDeltaX );

              // count iterations
              srv.iterations++;

              _options.verbose && std::cout << "[" << srv.iterations << "]: J: " << _J << ", dJ: " << deltaJ << ", deltaX: " << deltaX << ", delta: " << _delta << std::endl << std::endl;

            }

            srv.JFinal = _J;
            srv.lmLambdaFinal = _lambda;
            srv.dXFinal = deltaX;
            srv.dJFinal = deltaJ;

            return srv;

          }

    SolutionReturnValue Optimizer::optimize()
    {
      Timer timeGn("Optimizer: build Hessian", true);
      Timer timeErr("Optimizer: evaluate error", true);
      Timer timeSchur("Optimizer: Schur complement", true);
      Timer timeBackSub("Optimizer: Back substitution", true);
      Timer timeSolve("Optimizer: Solve linear system", true);
      // Select the design variables and (eventually) the error terms involved in the optimization.
      initialize();
      SolutionReturnValue srv;
      _p_J = 0.0;
      _lambda = 0;
      double gamma = _options.levenbergMarquardtLambdaGamma;
      double beta = _options.levenbergMarquardtLambdaBeta;
      int p = _options.levenbergMarquardtLambdaP;
      double mu = _options.levenbergMarquardtLambdaMuInit;
      // This sets _J
      timeErr.start();
      evaluateError();
      timeErr.stop();
      _p_J = _J;
      srv.JStart = _p_J;
      // *** while not done
      _options.verbose && std::cout << "[" << srv.iterations << ".0]: J: " << _J << std::endl;
      // Set up the estimation problem.
      double deltaX = _options.convergenceDeltaX + 1.0;
      double deltaJ = _options.convergenceDeltaJ + 1.0;
      bool isLmRegression = false;
      // Loop until convergence
      while (srv.iterations <  _options.maxIterations &&
             deltaX > _options.convergenceDeltaX &&
             fabs(deltaJ) > _options.convergenceDeltaJ) {
        // *** build: J, U, Vi, Wi, ea, ebi
        if (! isLmRegression) {
          timeGn.start();
          buildGnMatrices();
          timeGn.stop();
          setInitialLambda();
        }
        // **** Compute the invVis
        // **** Replace A with (U* - sum(YiWit)), and b with (ea = sum(Yi eb))
        timeSchur.start();
        applySchurComplement(_H, _rhs, _lambda,
                             _marginalizedStartingBlock,
                             _options.doLevenbergMarquardt,
                             _A, _invVi, _b);
        timeSchur.stop();
        // Solve the dense system.
        if (_options.resetSolverEveryIteration) {
          _solver->init();
        }
        timeSolve.start();
        bool solutionSuccess = _solver->solve(_A, &_dx[0], &_b[0]);
        timeSolve.stop();
        if (!solutionSuccess) {
          // \todo do something better
          //SM_ASSERT_TRUE(Exception, solutionSuccess, "The linear solution failed");
          isLmRegression = true;
          // **** if J is a regression
          revertLastStateUpdate();
          // ***** Update lambda
          // _lambda *= 1000;
          _lambda *= mu;
          mu *= 2;
          srv.failedIterations++;
        } else {
          /// Apply the state update. _A, _b, _dx, and _H are passed in implicitly.
          timeBackSub.start();
          deltaX = applyStateUpdate();
          timeBackSub.stop();
          // This sets _J
          timeErr.start();
          evaluateError();
          timeErr.stop();
          deltaJ = _p_J - _J;
          if (_options.doLevenbergMarquardt) {
            // update rho:
            double rho = getLmRho();
            // if(_J > _p_J)
            if (rho <= 0) {
              isLmRegression = true;
              // **** if J is a regression
              revertLastStateUpdate();
              // ***** Update lambda
              // _lambda *= 1000;
              _lambda *= mu;
              mu *= 2;
              srv.failedIterations++;
            } else {
              isLmRegression = false;
              // **** otherwise
              // ***** Update lambda
              if (_lambda > 1e-16) {
                // _lambda *= 0.1;
                double u1 = 1 / gamma;
                double u2 = 1 - (beta - 1) * pow((2 * rho - 1), p);
                if (u1 > u2)
                  _lambda *= u1;
                else
                  _lambda *= u2;
                mu = _options.levenbergMarquardtLambdaBeta;
              } else
                _lambda = 1e-15;
            }
          }
        }
        srv.iterations++;
        if (_J < _p_J)
          _p_J = _J;
        _options.verbose && std::cout << "[" << srv.iterations << "]: J: " << _J << ", dJ: " << deltaJ << ", deltaX: " << deltaX << ", lambda: " << _lambda << std::endl;
      }
      srv.JFinal = _J;
      srv.lmLambdaFinal = _lambda;
      srv.dXFinal = deltaX;
      srv.dJFinal = deltaJ;
      return srv;
    }

    DesignVariable* Optimizer::denseVariable(size_t i)
    {
      SM_ASSERT_LT_DBG(Exception, (int)i, _marginalizedStartingBlock, "index out of bounds");
      return _designVariables[i];
    }

    DesignVariable* Optimizer::sparseVariable(size_t i)
    {
      SM_ASSERT_LT_DBG(Exception, i + _marginalizedStartingBlock, _designVariables.size(), "index out of bounds");
      return _designVariables[i + _marginalizedStartingBlock];
    }

    size_t Optimizer::numDenseDesignVariables() const
    {
      return _marginalizedStartingBlock;
    }

    size_t Optimizer::numSparseDesignVariables() const
    {
      return _designVariables.size() - _marginalizedStartingBlock;
    }

    double Optimizer::applyStateUpdate()
    {
      // Apply the update to the dense state.
      int startIdx = 0;
      for (size_t i = 0; i < numDenseDesignVariables(); i++) {
        const int dbd = denseVariable(i)->minimalDimensions();
        Eigen::VectorXd dxS = _dx.segment(startIdx, dbd);
        dxS *= denseVariable(i)->scaling();
        if(dbd > 0) {
          denseVariable(i)->update(&dxS[0], dbd);
        }
        startIdx += dbd;
      }
      // Track the maximum delta
      // \todo: should this be some other metric?
      double deltaX = _dx.array().abs().maxCoeff();
      Eigen::VectorXd dsi;
      // Apply the update to the sparse blocks.
      // This is the back-substitution step.
      for (unsigned i = 0; i < _invVi.size(); i++) {
        buildDsi(i, _H, _rhs, _marginalizedStartingBlock, _invVi[i], _dx, dsi);
        // invert scaling in the update step
        dsi *= sparseVariable(i)->scaling();
        sparseVariable(i)->update(&dsi[0], dsi.size());
        // Track the maximum delta
        // \todo Do something better.
        deltaX = std::max(deltaX, dsi.array().abs().maxCoeff());
      }
      return deltaX;
    }

    void Optimizer::revertLastStateUpdate()
    {
      for (size_t i = 0; i < _designVariables.size(); i++) {
        _designVariables[i]->revertUpdate();
      }
    }

    OptimizerOptions& Optimizer::options()
    {
      return _options;
    }

    double Optimizer::evaluateError()
    {
      // \todo Make multi-threaded
      _J = 0.0;
      std::set<ErrorTerm*>::iterator it, it_end;
      it = _errorTerms.begin();
      it_end = _errorTerms.end();
      for (; it != it_end; ++it) {
        _J += (*it)->evaluateError();
      }
      return _J;
    }

    void Optimizer::buildGnMatrices()
    {
      // Do some initialization
      zeroMatrices();
      std::set<ErrorTerm*>::iterator it, it_end;
      it = _errorTerms.begin();
      it_end = _errorTerms.end();
      const bool useMEstimator = true;
      for (; it != it_end; ++it) {
        (*it)->buildHessian(_H, _rhs, useMEstimator);
      }
    }

    void Optimizer::zeroMatrices()
    {
      // clear(false) zeros all of the blocks but does not de-allocate them.
      _H.clear(false);
      _rhs.setZero();
      //_J = 0.0;
    }

    /// \brief return the full Hessian
    const SparseBlockMatrix& Optimizer::H() const
    {
      return _H;
    }

    /// \brief return the full rhs
    const Eigen::VectorXd& Optimizer::rhs() const
    {
      return _rhs;
    }

    /// \brief return the reduced system lhs
    const SparseBlockMatrix& Optimizer::A() const
    {
      return _A;
    }

    /// \brief return the reduced system rhs
    const Eigen::VectorXd& Optimizer::b() const
    {
      return _b;
    }

    /// \brief return the reduced system dx
    const Eigen::VectorXd& Optimizer::dx() const
    {
      return _dx;
    }

    /// The value of the objective function.
    double Optimizer::J() const
    {
      return _J;
    }

    void Optimizer::printTiming() const
    {
      sm::timing::Timing::print(std::cout);
    }

    void Optimizer::computeDiagonalCovariances()
    {
      std::vector<std::pair<int, int> > blockIndices;
      for (size_t i = 0; i < _designVariables.size(); ++i) {
        blockIndices.push_back(std::make_pair(i, i));
      }
      computeCovarianceBlocks(blockIndices);
    }

    void Optimizer::computeCovarianceBlocks(const std::vector<std::pair<int, int> > & blockIndices)
    {
      /// \todo Figure out properly why I have to create a new linear solver here.
      boost::shared_ptr<sparse_block_matrix::LinearSolverCholmod<Eigen::MatrixXd> > solver(new sparse_block_matrix::LinearSolverCholmod<Eigen::MatrixXd>());
      bool success = solver->solvePattern(_invH, blockIndices, _H);
      SM_ASSERT_TRUE(Exception, success, "Unable to retrieve covariance");
    }

    void Optimizer::computeCovariances()
    {
      std::vector<std::pair<int, int> > blockIndices;
      for (size_t i = 0; i < _designVariables.size(); ++i) {
        for (size_t j = i; j < _designVariables.size(); ++j) {
          blockIndices.push_back(std::make_pair(i, j));
        }
      }
      computeCovarianceBlocks(blockIndices);
    }

    const Eigen::MatrixXd* Optimizer::getCovarianceBlock(int blockRow, int blockCol) const
    {
      return _invH.block(blockRow, blockCol);
    }

    const SparseBlockMatrix& Optimizer::P() const
    {
      return _invH;
    }

    const SparseBlockMatrix& Optimizer::getCovariance() const
    {
      return _invH;
    }

    void Optimizer::checkProblemSetup()
    {
      // Check that all error terms are hooked up to design variables.
    }

  } // namespace backend
} // namespace aslam


#pragma once

#include <rai/lib/blocks.hpp>
#include <rai/secure/utility.hpp>

#include <boost/iterator/transform_iterator.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/variant.hpp>

#include <unordered_map>

#include <blake2/blake2.h>

namespace boost
{
template <>
struct hash<rai::uint256_union>
{
	size_t operator() (rai::uint256_union const & value_a) const
	{
		std::hash<rai::uint256_union> hash;
		return hash (value_a);
	}
};
}
namespace rai
{
const uint8_t protocol_version = 4;
// Accepted minimum protocol version, may be epoch-dependent, access it through rai::protocol_version_min_get()
uint8_t protocol_version_min_get ();
const uint8_t protocol_version_min_epoch_new = 4;
const uint8_t protocol_version_min_epoch_old = 3; // Irrelevant after new epoch has started
const uint8_t protocol_version_legacy_min = 1; // Not used as of version 1

class block_store;

/**
 * A key pair. The private key is generated from the random pool, or passed in
 * as a hex string. The public key is derived using ed25519.
 */
class keypair
{
public:
	keypair ();
	keypair (std::string const &);
	keypair (rai::raw_key &&);
	rai::public_key pub;
	rai::raw_key prv;
};

std::unique_ptr<rai::block> deserialize_block (MDB_val const &);

/**
 * Legacy account info, v12, does not yet contain comment_block
 */
class account_info_v12
{
public:
	account_info_v12 (rai::mdb_val const &);
	size_t size_in_db () const;
	// members, they must be all value types
	rai::block_hash head;
	rai::block_hash rep_block; // to deprecate, all blocks have the representative
	rai::block_hash open_block;
	rai::amount balance;
	::uint64_t last_block_time_intern; // in fact this is a rai::timestamp_t, 4-byte, but for alignment reasons stored on 8 bytes
	::uint64_t block_count;
};

/**
 * Latest information about an account
 */
class account_info
{
public:
	account_info ();
	account_info (rai::mdb_val const &);
	account_info (rai::account_info const &) = default;
	account_info (rai::block_hash const &, rai::block_hash const &, rai::block_hash const &, rai::block_hash const &, rai::amount const &, rai::timestamp_t, uint64_t);
	account_info (rai::account_info_v12 const &);
	bool operator== (rai::account_info const &) const;
	bool operator!= (rai::account_info const &) const;
	rai::amount balance_with_manna (rai::account const &, rai::timestamp_t) const;
	rai::mdb_val serialize_to_db () const;
	void deserialize_from_db (rai::mdb_val const &);
	size_t size_in_db () const;
	rai::timestamp_t last_block_time () const
	{
		return static_cast<rai::timestamp_t> (last_block_time_intern);
	}
	void last_block_time_set (rai::timestamp_t t)
	{
		last_block_time_intern = t;
	}
	// members, they must be all value types
	rai::block_hash head;
	rai::block_hash rep_block; // to deprecate, all blocks have the representative
	rai::block_hash open_block;
	rai::block_hash comment_block; // last account comment block
	rai::amount balance;
	::uint64_t last_block_time_intern; // in fact this is a rai::timestamp_t, 4-byte, but for alignment reasons stored on 8 bytes
	::uint64_t block_count;
};

/**
 * Information on an uncollected send
 */
class pending_info
{
public:
	pending_info ();
	pending_info (rai::mdb_val const &);
	pending_info (rai::account const &, rai::amount const &);
	bool operator== (rai::pending_info const &) const;
	rai::mdb_val serialize_to_db () const;
	void deserialize_from_db (rai::mdb_val const &);
	size_t size_in_db () const;
	rai::account source;
	rai::amount amount;
};

class pending_key
{
public:
	pending_key (rai::account const &, rai::block_hash const &);
	pending_key (MDB_val const &);
	void serialize (rai::stream &) const;
	bool deserialize (rai::stream &);
	bool operator== (rai::pending_key const &) const;
	rai::mdb_val val () const;
	rai::account account;
	rai::block_hash hash;
};

class block_info
{
public:
	block_info ();
	block_info (rai::mdb_val const &);
	block_info (rai::account const &, rai::amount const &);
	bool operator== (rai::block_info const &) const;
	rai::mdb_val serialize_to_db () const;
	void deserialize_from_db (rai::mdb_val const &);
	size_t size_in_db () const;
	rai::account account;
	rai::amount balance;
};
class block_counts
{
public:
	block_counts ();
	size_t sum ();
	size_t state;
	size_t comment;
};
typedef std::vector<boost::variant<std::shared_ptr<rai::block>, rai::block_hash>>::const_iterator vote_blocks_vec_iter;
class iterate_vote_blocks_as_hash
{
public:
	iterate_vote_blocks_as_hash () = default;
	rai::block_hash operator() (boost::variant<std::shared_ptr<rai::block>, rai::block_hash> const & item) const;
};
class vote
{
public:
	vote () = default;
	vote (rai::vote const &);
	vote (bool &, rai::stream &);
	vote (bool &, rai::stream &, rai::block_type);
	vote (rai::account const &, rai::raw_key const &, uint64_t, std::shared_ptr<rai::block>);
	vote (rai::account const &, rai::raw_key const &, uint64_t, std::vector<rai::block_hash>);
	vote (MDB_val const &);
	std::string hashes_string () const;
	rai::uint256_union hash () const;
	bool operator== (rai::vote const &) const;
	bool operator!= (rai::vote const &) const;
	void serialize (rai::stream &, rai::block_type);
	void serialize (rai::stream &);
	bool deserialize (rai::stream &);
	bool validate ();
	boost::transform_iterator<rai::iterate_vote_blocks_as_hash, rai::vote_blocks_vec_iter> begin () const;
	boost::transform_iterator<rai::iterate_vote_blocks_as_hash, rai::vote_blocks_vec_iter> end () const;
	std::string to_json () const;
	// Vote round sequence number
	uint64_t sequence;
	// The blocks, or block hashes, that this vote is for
	std::vector<boost::variant<std::shared_ptr<rai::block>, rai::block_hash>> blocks;
	// Account that's voting
	rai::account account;
	// Signature of sequence + block hashes
	rai::signature signature;
	static const std::string hash_prefix;
};
enum class vote_code
{
	invalid, // Vote is not signed correctly
	replay, // Vote does not have the highest sequence number, it's a replay
	vote // Vote has the highest sequence number
};

enum class process_result
{
	progress = 0, // Hasn't been seen before, signed correctly
	bad_signature = 1, // Signature was bad, forged or transmission error
	old = 2, // Already seen and was valid
	negative_spend = 3, // Malicious attempt to spend a negative amount
	fork = 4, // Malicious fork based on previous
	unreceivable = 5, // Source block doesn't exist, has already been received, or requires an account upgrade (epoch blocks)
	gap_previous = 6, // Block marked as previous is unknown
	gap_source = 7, // Block marked as source is unknown
	opened_burn_account = 8, // The impossible happened, someone found the private key associated with the public key '0'.
	balance_mismatch = 9, // Balance and amount delta don't match
	representative_mismatch = 10, // 0x0A Representative is changed when it is not allowed
	block_position = 11, // 0x0B This block cannot follow the previous block (e.g. due to epoch)
	invalid_state_block = 12, // 0x0C a state block with undefined subtype
	invalid_block_creation_time = 13, // 0x0D Out-of-order block, or invalid block creation time
	send_same_account = 14, // 0x0E send to self
	invalid_comment_block = 15, // 0x0F a comment block with invalid parameters
	invalid_comment_block_legacy = 16, // 0x10 Comment block is not allowed before an epoch time
};
class process_return
{
public:
	rai::process_result code;
	rai::account account;
	rai::amount amount;
	rai::account pending_account;
	rai::state_block_subtype state_subtype;
};
enum class tally_result
{
	vote,
	changed,
	confirm
};
extern rai::keypair const & zero_key;
extern rai::keypair const & test_genesis_key;
extern rai::account const & rai_test_genesis_account;
extern rai::account const & rai_beta_genesis_account;
extern rai::account const & rai_live_genesis_account;
extern std::string const & rai_test_genesis;
extern std::string const & rai_beta_genesis;
extern std::string const & rai_live_genesis;
extern rai::keypair const & test_manna_ep1_key;
extern rai::keypair const & test_manna_ep2_key;
extern std::string const & genesis_block;
extern rai::account const & genesis_account;
extern rai::account const & burn_account;
extern rai::amount_t const & genesis_amount;
extern rai::timestamp_t const genesis_time;
extern rai::account const & manna_account_epoch1;
extern rai::account const & manna_account_epoch2;
// A block hash that compares inequal to any real block hash
extern rai::block_hash const & not_a_block;
// An account number that compares inequal to any real account number
extern rai::block_hash const & not_an_account;

class genesis
{
public:
	explicit genesis ();
	void initialize (MDB_txn *, rai::block_store &) const;
	rai::block_hash hash () const;
	rai::state_block const & block () const;
	rai::block_hash root () const;
	std::unique_ptr<rai::state_block> genesis_block;
};

class manna_control
{
public:
	static uint32_t manna_start_epoch1;
	static uint32_t manna_start_epoch2;
	static uint32_t manna_freq;
	static rai::amount_t manna_increment;

	// Check if this account is a manna account
	static bool is_manna_account (rai::account const &);
	static rai::amount_t adjust_balance_with_manna (rai::account const &, rai::amount_t, rai::timestamp_t, rai::timestamp_t);

private:
	static bool is_manna_account_epoch1 (rai::account const &);
	static bool is_manna_account_epoch2 (rai::account const &);
	// Compute the manna increment between time points.  Account is needed as it may influence the epoch.
	static rai::amount_t compute_manna_increment_account (rai::account const &, rai::timestamp_t, rai::timestamp_t);
	// Compute the manna increment, within epoch1 (starting from genesis, and ending with epoch2 start)
	static rai::amount_t compute_manna_increment_epoch1 (rai::timestamp_t, rai::timestamp_t);
	// Compute the manna increment, within epoch2 (starting with epoch2 start)
	static rai::amount_t compute_manna_increment_epoch2 (rai::timestamp_t, rai::timestamp_t);
	// Compute the manna increment between time points, bounded by the manna epoch period between manna_start and manna_end
	static rai::amount_t compute_manna_increment_within_period (rai::timestamp_t, rai::timestamp_t, rai::timestamp_t, rai::timestamp_t);
};
}


#include "NICS-module.h"
#include <vector>
#include <string>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <math.h>
#include <Eigen/Dense>

//-------------------------------------------------------------
// Program to find the centers of rings fo NICS(0) and NICS(1)
// calculation and write some quazi-.gjf file
//-------------------------------------------------------------

int main() {

   //---------------------------------------------
   // Get the information out of the input file:
   //---------------------------------------------

   // Variables to get out of input file:
   int nmols;
   std::vector<int> nrings_per_molecule;
   std::vector<std::string> filenames;
   std::vector<std::vector<int>> rings_atoms; // atoms making up rings

   // Read input file:
   read_input_file(nmols, filenames, nrings_per_molecule, rings_atoms);
   std::cout << "nmols: " << nmols << std::endl;
   print_vector_string(filenames);
   std::cout << "nrings_per_molecule: " ;
   print_vector_int(nrings_per_molecule);
   print_matrix_int(rings_atoms);

   //--------------------------------------------------------
   // Get the coordinates of the molecules and rings from
   // the .xyz files, move and reorient them, and print the
   // final coordinates:
   //--------------------------------------------------------

   // Variables:
   std::string in_xyz_file_name, out_ring_xyz_file_name, out_xyz_file_name;
   std::string line, temp;
   std::vector<std::string> lables_ring, lables;
   std::vector<double> mass, xring, yring, zring,  x, y, z;
   double temp_double;
   double xsum, ysum, zsum, xcenter, ycenter, zcenter;
   int i, j, k, kk, line_num, ring_num;
   Eigen::Vector3d vec1, vec2, normal1, normal2;
   Eigen::Matrix3d inert;
   Eigen::MatrixXd ring_coordinates(1,1), coordinates(1,1);

   ring_num = 0;
   for (i=0; i<nmols; i=i+1) { // for each of the molecules:
      for (j=0; j<nrings_per_molecule[i]; j=j+1) { // for each ring in mol

         // Reset lables and coordinates arrays:
         mass.resize(0);
         lables.resize(0);
         x.resize(0);
         y.resize(0);
         z.resize(0);
         lables_ring.resize(0);
         xring.resize(0);
         yring.resize(0);
         zring.resize(0);

         // Open the input file for that molecule and ring:
         in_xyz_file_name = filenames[i] + ".xyz";
         std::ifstream in_xyz_file;
         in_xyz_file.open(in_xyz_file_name);

         // get the coordinates of the rings and the molecule:
         line_num = 1;
         while (!in_xyz_file.eof()) {
            getline(in_xyz_file,line);
            if (line_num > 2) {
               std::stringstream ssin(line);
               std::vector<std::string> split_line;
               split_line.resize(0);
               while (ssin.good()){
                  ssin >> temp;
                  split_line.push_back(temp);
               }
               // uncomment if segmentation fault stod
               //std::cout << print_vector_string(split_line) << std::endl;
               if (split_line.size() > 1) {
                  // get lables and coordinates for whole molecule
                  lables.push_back(split_line[0]);
                  temp_double = std::stod(split_line[1]);
                  x.push_back(temp_double);
                  temp_double = std::stod(split_line[2]);
                  y.push_back(temp_double);
                  temp_double = std::stod(split_line[3]);
                  z.push_back(temp_double);
                  for (k=0; k<rings_atoms[ring_num].size(); k=k+1){
                     if (line_num == rings_atoms[ring_num][k] + 2) {
                        // get lables and coordinates for ring:
                        lables_ring.push_back(split_line[0]);
                        temp_double = std::stod(split_line[1]);
                        xring.push_back(temp_double);
                        temp_double = std::stod(split_line[2]);
                        yring.push_back(temp_double);
                        temp_double = std::stod(split_line[3]);
                        zring.push_back(temp_double);
                        continue;
                     }
                  }
               }
            }
            line_num = line_num + 1;
         }

         // Find center of ring:
         xsum = 0.0;
         ysum = 0.0;
         zsum = 0.0;
         for (k=0; k<xring.size(); k=k+1) {
            xsum = xsum + xring[k];
            ysum = ysum + yring[k];
            zsum = zsum + zring[k];
         }
         xcenter = xsum/xring.size();
         ycenter = ysum/yring.size();
         zcenter = zsum/zring.size();

         // Move the coordinates vectors for the ring
         // so that the center of the ring is positioned
         // at (0,0,0):
         for (k=0; k<xring.size(); k=k+1) {
            xring[k] = xring[k] - xcenter;
            yring[k] = yring[k] - ycenter;
            zring[k] = zring[k] - zcenter;
         }

         // Move the coordinates vectors for the
         // entire molecule so that the center of the
         // ring is positioned at (0,0,0):
         for (k=0; k<x.size(); k=k+1) {
            x[k] = x[k] - xcenter;
            y[k] = y[k] - ycenter;
            z[k] = z[k] - zcenter;
         }

         // find plane passing through all the points of
         // the ring using the equation
         // C=(XY.transpose*XY).inverse*XY.transpose*Z
         Eigen::Vector3d Cvec;
         Eigen::VectorXd Zvec;
         Eigen::MatrixXd XYmat;

         Zvec.resize(zring.size());
         XYmat.resize(xring.size(),3);
         for (k=0; k<xring.size(); k=k+1) {
            XYmat(k,0) = 1;
            XYmat(k,1) = xring[k];
            XYmat(k,2) = yring[k];
            Zvec(k) = zring[k];
         }

         Cvec = (XYmat.transpose()*XYmat).inverse()*XYmat.transpose()*Zvec;

         // Define two vectors in this plane:
         vec1(0) = 0.8;
         vec1(1) = 1.5;
         vec1(2) = Cvec(0) + Cvec(1)*vec1(0) + Cvec(2)*vec1(1);
         vec2(0) = -1.2;
         vec2(1) = 0.6;
         vec2(2) = Cvec(0) + Cvec(1)*vec2(0) + Cvec(2)*vec2(1);

         // find a normal to this plane at (0,0,0)
         normal1 = vec1.cross(vec2);
         std::cout << "normal:" << std::endl;
         std::cout << normal1 << std::endl;

         double norm_of_normal;
         norm_of_normal = (sqrt(normal1(0)*normal1(0) +
                  normal1(1)*normal1(1) + normal1(2)*normal1(2)));
         std::cout << "norm of normal: " << normal1.norm() << std::endl;

         // normalize normal 1 to length 1:
         for (k=0; k<normal1.size(); k=k+1) {
            normal1(k) = normal1(k) / norm_of_normal;
         }

         norm_of_normal = (sqrt(normal1(0)*normal1(0) +
                  normal1(1)*normal1(1) + normal1(2)*normal1(2)));
         std::cout << "norm of normal: " << norm_of_normal << std::endl;

         // convert vectors xring, yring, and zring to a
         // ring_coordinates matrix:
         ring_coordinates.resize(xring.size(),3);
         for(k=0; k<xring.size(); k=k+1) {
            ring_coordinates(k,0) = xring[k];
            ring_coordinates(k,1) = yring[k];
            ring_coordinates(k,2) = zring[k];
         }

         // Convert vector x, y, and z to a coordinates matrix
         // and add dummy atoms:
         coordinates.resize(x.size() + 3,3);
         for (k=0; k<x.size(); k=k+1) {
            coordinates(k,0) = x[k];
            coordinates(k,1) = y[k];
            coordinates(k,2) = z[k];
         }
         coordinates(x.size(),0) = normal1(0);
         coordinates(x.size(),1) = normal1(1);
         coordinates(x.size(),2) = normal1(2);
         coordinates(x.size()+1,0) = 0.0;
         coordinates(x.size()+1,1) = 0.0;
         coordinates(x.size()+1,2) = 0.0;
         coordinates(x.size()+2,0) = -normal1(0);
         coordinates(x.size()+2,1) = -normal1(1);
         coordinates(x.size()+2,2) = -normal1(2);

         // Print final coordinates for the ring
         out_ring_xyz_file_name = filenames[i] + "_ring_"
            + std::to_string(j) + ".xyz";
         if_file_exist_delete(out_ring_xyz_file_name);
         std::ofstream out_ring_xyz_file;
         out_ring_xyz_file.open(out_ring_xyz_file_name);

         out_ring_xyz_file << rings_atoms[ring_num].size() + 3 << std::endl;
         out_ring_xyz_file << "sldgjksjg" << std::endl;
         for (kk=0; kk<rings_atoms[ring_num].size(); kk=kk+1) {
            out_ring_xyz_file << lables_ring[kk] << "         "
                         << ring_coordinates(kk,0) << "         "
                         << ring_coordinates(kk,1) << "         "
                         << ring_coordinates(kk,2) << std::endl;
         }
         out_ring_xyz_file << "N          " << normal1(0) << "         "
                           << normal1(1) << "         " << normal1(2)
                           << std::endl;
         out_ring_xyz_file << "O          " << vec1(0) << "         "
                           << vec1(1) << "         " << vec1(2)
                           << std::endl;
         out_ring_xyz_file << "O          " << vec2(0) << "         "
                           << vec2(1) << "         " << vec2(2)
                           << std::endl;

         // Align normal2 to z-axis, so that
         // the ring is in the xy plane"
         align(normal1, coordinates);
         align(normal1, ring_coordinates);

         // Print final coordinates for the molecule,
         // adding the dummy atoms:
         out_xyz_file_name = filenames[i] + "_" +
                                  std::to_string(j) + ".xyz";
         if_file_exist_delete(out_xyz_file_name);
         std::ofstream out_xyz_file;
         out_xyz_file.open(out_xyz_file_name);

         // printing:
         out_xyz_file << x.size() + 3 << std::endl;
         out_xyz_file << "0 1" << std::endl;
         for (kk=0; kk<x.size(); kk=kk+1) {
            out_xyz_file << std::left << std::setw(2) << lables[kk]
                         << std::setw(18) << std::right
                         << std::fixed << std::setprecision(7)
                         << coordinates(kk,0)
                         << std::setw(19) << std::right
                         << std::fixed << std::setprecision(7)
                         << coordinates(kk,1)
                         << std::setw(19) << std::right
                         << std::fixed << std::setprecision(7)
                         << coordinates(kk,2) << std::endl;
         }
         for (kk=x.size(); kk<x.size()+3; kk=kk+1) {
         out_xyz_file << std::left << std::setw(2) << "Bq"
                      << std::setw(18) << std::right
                      << std::fixed << std::setprecision(7)
                      << coordinates(kk,0)
                      << std::setw(19) << std::right
                      << std::fixed << std::setprecision(7)
                      << coordinates(kk,1)
                      << std::setw(19) << std::right
                      << std::fixed << std::setprecision(7)
                      << coordinates(kk,2) << std::endl;
         }

         // End of loop, reset for next ring:
         ring_num = ring_num + 1;
         in_xyz_file.close();
         out_ring_xyz_file.close();
         out_xyz_file.close();
      }

   }

   return 0;
}


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

#include <boost/mpl/apply.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/char.hpp>

#define BOOST_SIMD_MM_PERM2(i0, i1) (i0+(i1 << 4))                             \
/**/

namespace boost { namespace simd { namespace details
{
  // Result of the meta permutation
  template<class P, class Card, int I>
  struct index_ : boost::mpl::char_< boost::mpl::apply< typename P::type
                                                      , boost::mpl::int_<I>
                                                      , Card
                                                      >::type::value>
  {};

  // Generate the the correct mask
  template<class P, class Card, char Offset, char Idx, char Inc>
  struct generate_ : boost::mpl::char_<((0>index_<P,Card,Idx>::value)?-1:(Offset+Inc))>
  {};

  // Permute specialization
  template<class P, int N>
  struct permute {};

  template<class P>
  struct permute<P,2>
  {
    typedef P permut_t;
    typedef boost::mpl::int_<2> card_t;

    template<char I, char J>
    struct mask_
    : boost::mpl::char_< generate_< permut_t
                                  , card_t
                                  , ((index_<permut_t,card_t,I>::value & 1) << 3)
                                  , I
                                  , J
                                  >::value
                        >
    {};

#ifdef BOOST_SIMD_HAS_SSSE3_SUPPORT
    static __m128i call()
    {
      return _mm_setr_epi8( mask_<0,0>::value, mask_<0,1>::value
                          , mask_<0,2>::value, mask_<0,3>::value
                          , mask_<0,4>::value, mask_<0,5>::value
                          , mask_<0,6>::value, mask_<0,7>::value
                          , mask_<1,0>::value, mask_<1,1>::value
                          , mask_<1,2>::value, mask_<1,3>::value
                          , mask_<1,4>::value, mask_<1,5>::value
                          , mask_<1,6>::value, mask_<1,7>::value
                          );
    }
#elif defined(BOOST_SIMD_HAS_VMX_SUPPORT)
    static __vector unsigned char call()
    {
      __vector unsigned char that = { mask_<0,0>::value, mask_<0,1>::value
                                    , mask_<0,2>::value, mask_<0,3>::value
                                    , mask_<0,4>::value, mask_<0,5>::value
                                    , mask_<0,6>::value, mask_<0,7>::value
                                    , mask_<1,0>::value, mask_<1,1>::value
                                    , mask_<1,2>::value, mask_<1,3>::value
                                    , mask_<1,4>::value, mask_<1,5>::value
                                    , mask_<1,6>::value, mask_<1,7>::value };
      return that;
    }
#endif
  };

  template<class P>
  struct permute<P,4>
  {
    typedef P permut_t;
    typedef boost::mpl::int_<4> card_t;

    template<char I, char J>
    struct mask_
    : boost::mpl::char_< generate_< permut_t
                                  , card_t
                                  , ((index_<permut_t,card_t,I>::value & 3) << 2)
                                  , I
                                  , J
                                  >::value
                        >
    {};

#ifdef BOOST_SIMD_HAS_SSSE3_SUPPORT
    static __m128i call()
    {
      return _mm_setr_epi8( mask_<0, 0>::value, mask_<0, 1>::value
                          , mask_<0, 2>::value, mask_<0, 3>::value
                          , mask_<1, 0>::value, mask_<1, 1>::value
                          , mask_<1, 2>::value, mask_<1, 3>::value
                          , mask_<2, 0>::value, mask_<2, 1>::value
                          , mask_<2, 2>::value, mask_<2, 3>::value
                          , mask_<3, 0>::value, mask_<3, 1>::value
                          , mask_<3, 2>::value, mask_<3, 3>::value
                          );
    }
#elif defined(BOOST_SIMD_HAS_VMX_SUPPORT)
    static __vector unsigned char call()
    {
      __vector unsigned char that = { mask_<0, 0>::value, mask_<0, 1>::value
                                    , mask_<0, 2>::value, mask_<0, 3>::value
                                    , mask_<1, 0>::value, mask_<1, 1>::value
                                    , mask_<1, 2>::value, mask_<1, 3>::value
                                    , mask_<2, 0>::value, mask_<2, 1>::value
                                    , mask_<2, 2>::value, mask_<2, 3>::value
                                    , mask_<3, 0>::value, mask_<3, 1>::value
                                    , mask_<3, 2>::value, mask_<3, 3>::value };
      return that;
    }
#endif
  };

  template<class P>
  struct permute<P,8>
  {
    typedef P permut_t;
    typedef boost::mpl::int_<8> card_t;

    template<char I, char J>
    struct mask_
    : boost::mpl::char_< generate_< permut_t
                                  , card_t
                                  , ((index_<permut_t,card_t,I>::value & 7) << 1)
                                  , I
                                  , J
                                  >::value
                        >
    {};

#ifdef BOOST_SIMD_HAS_SSSE3_SUPPORT
    static __m128i call()
    {
      return _mm_setr_epi8( mask_<0, 0>::value, mask_<0, 1>::value
                          , mask_<1, 0>::value, mask_<1, 1>::value
                          , mask_<2, 0>::value, mask_<2, 1>::value
                          , mask_<3, 0>::value, mask_<3, 1>::value
                          , mask_<4, 0>::value, mask_<4, 1>::value
                          , mask_<5, 0>::value, mask_<5, 1>::value
                          , mask_<6, 0>::value, mask_<6, 1>::value
                          , mask_<7, 0>::value, mask_<7, 1>::value
                          );
    }
#elif defined(BOOST_SIMD_HAS_VMX_SUPPORT)
    static __vector unsigned char call()
    {
      __vector unsigned char that = { mask_<0, 0>::value, mask_<0, 1>::value
                                    , mask_<1, 0>::value, mask_<1, 1>::value
                                    , mask_<2, 0>::value, mask_<2, 1>::value
                                    , mask_<3, 0>::value, mask_<3, 1>::value
                                    , mask_<4, 0>::value, mask_<4, 1>::value
                                    , mask_<5, 0>::value, mask_<5, 1>::value
                                    , mask_<6, 0>::value, mask_<6, 1>::value
                                    , mask_<7, 0>::value, mask_<7, 1>::value };
      return that;
    }
#endif
  };

  template<class P>
  struct permute<P,16>
  {
    typedef P permut_t;
    typedef boost::mpl::int_<16> card_t;

    template<char I>
    struct mask_
    : boost::mpl::char_<(index_<P,card_t,I>::value)<0?-1:index_<P,card_t,I>::value>
    {};

#ifdef BOOST_SIMD_HAS_SSSE3_SUPPORT
    static __m128i call()
    {
      return _mm_setr_epi8( mask_< 0>::value, mask_< 1>::value
                          , mask_< 2>::value, mask_< 3>::value
                          , mask_< 4>::value, mask_< 5>::value
                          , mask_< 6>::value, mask_< 7>::value
                          , mask_< 8>::value, mask_< 9>::value
                          , mask_<10>::value, mask_<11>::value
                          , mask_<12>::value, mask_<13>::value
                          , mask_<14>::value, mask_<15>::value
                          );
    }
#elif defined(BOOST_SIMD_HAS_VMX_SUPPORT)
    static __vector unsigned char call()
    {
      __vector unsigned char that = { mask_< 0>::value, mask_< 1>::value
                                    , mask_< 2>::value, mask_< 3>::value
                                    , mask_< 4>::value, mask_< 5>::value
                                    , mask_< 6>::value, mask_< 7>::value
                                    , mask_< 8>::value, mask_< 9>::value
                                    , mask_<10>::value, mask_<11>::value
                                    , mask_<12>::value, mask_<13>::value
                                    , mask_<14>::value, mask_<15>::value };
      return that;
    }
#endif
  };

#ifdef BOOST_SIMD_HAS_AVX_SUPPORT
template< unsigned int i0, unsigned int i1>
BOOST_FORCEINLINE
__m256 perm2(__m256 const lower, __m256 const upper)
{
  return _mm256_permute2f128_ps( lower, upper
                          , BOOST_SIMD_MM_PERM2(i0, i1)
                          );
}
template< unsigned int i0, unsigned int i1>
BOOST_FORCEINLINE
__m256d perm2(__m256d const lower, __m256d const upper)
{
  return _mm256_permute2f128_pd( lower, upper
                          , BOOST_SIMD_MM_PERM2(i0, i1)
                          );
}
template< unsigned int i0, unsigned int i1>
BOOST_FORCEINLINE
__m256i perm2(__m256i const lower, __m256i const upper)
{
  return _mm256_permute2f128_si256( lower, upper
                          , BOOST_SIMD_MM_PERM2(i0, i1)
                          );
}

#undef BOOST_SIMD_MM_PERM2
#endif
} } }
#undef V
#undef H1
#endif


#pragma once

#include <ros/ros.h>
#include <geometry_msgs/Point.h>

#include <Eigen/Dense>
#include <iostream>
#include "utils/types.hpp"
#include "generate_dynamic_scene/obstacle.hpp"
#include "generate_dynamic_scene/human.hpp"
#include <hdi_plan/obstacle_info.h>
#include <hdi_plan/point_array.h>

#include <stack>
#include <queue>
#include <vector>
#include <memory>
#include <cmath>
#include <limits>
#include <cstdlib>
#include <time.h>
#include <string>
#include <map>
#include <queue>
#include <algorithm>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Empty.h>

#include "dynamic_motion_planner/chomp_trajectory.hpp"
#include "dynamic_motion_planner/chomp.hpp"
#include "generate_dynamic_scene/human.hpp"
#include "utils/utility_functions.hpp"
#include <hdi_plan/point_array.h>

#include <iostream>
#include <fstream>

namespace hdi_plan {

class GenerateObstacle {
public:
	GenerateObstacle(const ros::NodeHandle &nh, const ros::NodeHandle &pnh);
	~GenerateObstacle();

private:
	ros::NodeHandle nh_;
	ros::NodeHandle pnh_;

	ros::Subscriber trajectory_sub_;

	ros::Publisher human_movement_pub_;
	ros::Publisher update_human_obstacle_pub_;
	ros::Publisher pub_optimized_path_;
	ros::Publisher pub_get_new_path_;
	ros::Publisher path_spot_pub_;

	std::vector<hdi_plan::obstacle_info> path_spot_list_;
	std::vector<double> goal_position_param_;

	void publish_goal_position();
	bool load_params();

    void publish_human_movement_1();
	void publish_human_movement_2();
	void publish_obstacle();
	void publish_obstacle2();
	void remove_obstacle();
    double get_distance(const Eigen::Vector2d &point1, const Eigen::Vector2d &point2);
    hdi_plan::obstacle_info get_obstacle_message(bool operation, int human_id, double position_x = 100.0, double position_y = 100.0);
	void trajectory_callback(const hdi_plan::point_array::ConstPtr &msg);
	void publish_path_spot(int stop_index);
    /*
	void publish_trajectory();
	bool load_params();

	// chomp trajectory
	double discretization_;

	// chomp
	double quadrotor_radius_;
	double quadrotor_speed_;
	double total_plan_time_;
	double collision_threshold_;
	double planning_time_limit_;
	int max_iterations_;
	int max_iterations_after_collision_free_;
	double learning_rate_;
	double obstacle_cost_weight_;
	double dynamic_obstacle_cost_weight_;
	double dynamic_collision_factor_;
	double smoothness_cost_weight_;
	double smoothness_cost_velocity_;
	double smoothness_cost_acceleration_;
	double smoothness_cost_jerk_;
	double ridge_factor_;
	double min_clearence_;
	double joint_update_limit_;*/
};
}


#include <boost/geometry/geometries/multi_polygon.hpp>


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is 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.
 */

#include <calibration/camera_target_based_intrinsics_calibrator_params.h>
#include <calibration/camera_target_based_intrinsics_calibrator.h>
#include <calibration/parameter_reader.h>
#include <ff_common/init.h>
#include <ff_common/utils.h>
#include <localization_common/image_correspondences.h>
#include <localization_common/logger.h>
#include <localization_common/utilities.h>
#include <optimization_common/fov_distorter.h>
#include <optimization_common/rad_distorter.h>
#include <optimization_common/radtan_distorter.h>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

namespace ca = calibration;
namespace lc = localization_common;
namespace oc = optimization_common;
namespace po = boost::program_options;

lc::ImageCorrespondences LoadTargetMatches(const std::string& match_file) {
  std::vector<Eigen::Vector2d> image_points;
  std::vector<Eigen::Vector3d> points_3d;
  std::cout << "Reading: " << match_file << std::endl;
  std::ifstream cr(match_file.c_str());
  std::string line;
  while (std::getline(cr, line)) {
    double val;
    std::vector<double> vals;
    std::istringstream is(line);
    while (is >> val) vals.push_back(val);
    if (vals.size() != 5) LogFatal("Each line of " << match_file << " must have 5 entries.");
    points_3d.emplace_back(Eigen::Vector3d(vals[1], vals[2], 0));
    image_points.emplace_back(Eigen::Vector2d(vals[3], vals[4]));
  }

  return lc::ImageCorrespondences(image_points, points_3d);
}

std::vector<lc::ImageCorrespondences> LoadAllTargetMatches(const std::string& corners_directory,
                                                           const int max_num_match_sets) {
  std::vector<std::string> corners_files;
  std::vector<lc::ImageCorrespondences> all_matches;
  ff_common::ListFiles(corners_directory, "txt", &corners_files);
  int i = 0;
  for (const auto& corners_file : corners_files) {
    const auto& matches = LoadTargetMatches(corners_file);
    if (matches.size() < 4) {
      LogError("Too few matches, only " << matches.size() << ".");
      continue;
    }
    all_matches.emplace_back(matches);
    if (++i > max_num_match_sets) break;
  }
  return all_matches;
}

void WriteCalibrationResultsToFile(const Eigen::Vector2d& focal_lengths, const Eigen::Vector2d& principal_points,
                                   const Eigen::VectorXd& distortion, const std::string& output_filename) {
  std::ofstream output_file;
  output_file.open(output_filename);
  output_file << focal_lengths[0] << " " << focal_lengths[1] << std::endl;
  output_file << principal_points[0] << " " << principal_points[1] << std::endl;
  int i = 0;
  for (; i < distortion.size() - 1; ++i) {
    output_file << distortion[i] << " ";
  }
  output_file << distortion[i];
  output_file.close();
}

template <typename DISTORTER>
bool Calibrate(const ca::RunCalibratorParams& params, const std::vector<lc::ImageCorrespondences>& target_matches,
               const std::string& output_file) {
  ca::CameraTargetBasedIntrinsicsCalibrator<DISTORTER> calibrator(params.camera_target_based_intrinsics_calibrator);
  ca::StateParameters initial_state_parameters;
  initial_state_parameters.focal_lengths = params.camera_params->GetFocalVector();
  initial_state_parameters.principal_points = params.camera_params->GetOpticalOffset();
  initial_state_parameters.distortion = params.camera_params->GetDistortion();
  ca::StateParameters calibrated_state_parameters;
  ca::StateParametersCovariances covariances;
  const bool success = calibrator.EstimateInitialTargetPosesAndCalibrate(target_matches, initial_state_parameters,
                                                                         calibrated_state_parameters, covariances);
  if (!success) {
    LogError("Calibrate: Calibration failed.");
    return false;
  }
  if (params.camera_target_based_intrinsics_calibrator.calibrate_focal_lengths) {
    LogInfo("initial focal lengths: " << std::endl << initial_state_parameters.focal_lengths.matrix());
    LogInfo("calibrated focal lengths: " << std::endl << calibrated_state_parameters.focal_lengths.matrix());
    LogInfo("calibrated focal length covariance: " << std::endl << covariances.focal_lengths.matrix());
  }
  if (params.camera_target_based_intrinsics_calibrator.calibrate_principal_points) {
    LogInfo("initial principal points: " << std::endl << initial_state_parameters.principal_points.matrix());
    LogInfo("calibrated principal points: " << std::endl << calibrated_state_parameters.principal_points.matrix());
    LogInfo("calibrated principal points covariance: " << std::endl << covariances.principal_points.matrix());
  }
  if (params.camera_target_based_intrinsics_calibrator.calibrate_distortion) {
    LogInfo("initial distortion: " << std::endl << initial_state_parameters.distortion.matrix());
    LogInfo("calibrated distortion: " << std::endl << calibrated_state_parameters.distortion.matrix());
    LogInfo("calibrated distortion covariance: " << std::endl << covariances.distortion.matrix());
  }
  WriteCalibrationResultsToFile(calibrated_state_parameters.focal_lengths, calibrated_state_parameters.principal_points,
                                calibrated_state_parameters.distortion, output_file);
  return true;
}

int main(int argc, char** argv) {
  std::string robot_config_file;
  std::string world;
  std::string output_file;
  po::options_description desc("Calibrates camera intrinsics using target detections.");
  desc.add_options()("help", "produce help message")("corners-directory", po::value<std::string>()->required(),
                                                     "Directory containing target detections to use for calibration.")(
    "config-path,c", po::value<std::string>()->required(), "Config path")(
    "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("config/robots/bumble.config"),
    "Robot config file")("world,w", po::value<std::string>(&world)->default_value("iss"), "World name")(
    "output-file,o", po::value<std::string>(&output_file)->default_value("calibrated_params.txt"),
    "File where calibrated parameters are saved.");
  po::positional_options_description p;
  p.add("corners-directory", 1);
  p.add("config-path", 1);
  po::variables_map vm;
  try {
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);
  } catch (std::exception& e) {
    std::cerr << "Error: " << e.what() << "\n";
    return 1;
  }

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 1;
  }

  const std::string corners_directory = vm["corners-directory"].as<std::string>();
  const std::string config_path = vm["config-path"].as<std::string>();

  // Only pass program name to free flyer so that boost command line options
  // are ignored when parsing gflags.
  int ff_argc = 1;
  ff_common::InitFreeFlyerApplication(&ff_argc, &argv);

  lc::SetEnvironmentConfigs(config_path, world, robot_config_file);
  config_reader::ConfigReader config;
  config.AddFile("geometry.config");
  config.AddFile("cameras.config");
  config.AddFile("tools/camera_target_based_intrinsics_calibrator.config");
  if (!config.ReadFiles()) {
    LogFatal("Failed to read config files.");
  }
  ca::RunCalibratorParams params;
  LoadRunCalibratorParams(config, params);

  std::vector<lc::ImageCorrespondences> target_matches;
  if (!boost::filesystem::is_directory(corners_directory)) {
    LogFatal("Corners directory " << corners_directory << " not found.");
  }
  LogInfo("Calibrating " << robot_config_file << ", camera: " << params.camera_name);
  target_matches =
    LoadAllTargetMatches(corners_directory, params.camera_target_based_intrinsics_calibrator.max_num_match_sets);
  LogInfo("Number of target match sets: " << target_matches.size());
  if (params.distortion_type == "fov") {
    Calibrate<oc::FovDistorter>(params, target_matches, output_file);
  } else if (params.distortion_type == "rad") {
    Calibrate<oc::RadDistorter>(params, target_matches, output_file);
  } else if (params.distortion_type == "radtan") {
    Calibrate<oc::RadTanDistorter>(params, target_matches, output_file);
  } else {
    LogFatal("Invalid distortion type provided.");
  }
}


#ifndef CORE_H
#define CORE_H

#include <Eigen/Core>
#include <memory>

namespace sequential_line_search
{
    class GaussianProcessRegressor;
}

class Core
{
public:
    Core();

    static Core& getInstance()
    {
        static Core core;
        return core;
    }

    std::shared_ptr<sequential_line_search::GaussianProcessRegressor> regressor;

    Eigen::MatrixXd X;
    Eigen::VectorXd y;

    double evaluateObjectiveFunction(Eigen::VectorXd x) const;

    bool   show_slider_value;
    double x_slider;
    double y_slider;

    // For optimization
    void            proceedOptimization();
    Eigen::VectorXd findNextPoint() const;
    Eigen::VectorXd x_max;
    double          y_max;

    // For regression
    void addData(const Eigen::VectorXd& x, double y);
    void computeRegression();
};

#endif // CORE_H


#ifndef SENSORS_STEREO_HUD_HPP
#define SENSORS_STEREO_HUD_HPP

#include "opencv2/opencv.hpp"
#include <cv.h>
#include <iostream>
#include <string>
#include <bot_core/bot_core.h>
#include <Eigen/Core>

using namespace cv;

#define PI 3.14159265

class Hud {

    private:
        float airspeed_, airspeed_unchecked_, altitude_, q0_, q1_, q2_, q3_, gps_speed_, gps_heading_, battery_voltage_, x_accel_, y_accel_, z_accel_, throttle_, elevonL_, elevonR_;
        long timestamp_;
        int frame_number_, video_number_;
        int plane_number_, log_number_;
        int scale_factor_;
        Scalar hud_color_ ;
        int box_line_width_;
        int text_font_;
        double hud_font_scale_;
        double hud_font_scale_small_;
        float text_thickness_;
        float pitch_range_of_lens_;
        int is_autonomous_;
        int clutter_level_;
        int traj_number_;
        std::string state_machine_state_ = "State Unknown";
        double u0_left_elevon_ = 0;
        double u0_right_elevon_ = 0;
        double u0_throttle_ = 0;

        void PutHudText(Mat hud_img, string str_in, Point text_orgin);
        void PutHudTextSmall(Mat hud_img, string str_in, Point text_orgin);

        void DrawAirspeed(Mat hud_img);
        void DrawAltitude(Mat hud_img);
        void DrawLadder(Mat hud_img, float value, bool for_airspeed, int major_increment, int minor_increment);
        void DrawFrameNumber(Mat hud_img);
        void DrawGpsSpeed(Mat hud_img);
        void DrawArtificialHorizon(Mat hud_img);
        void DrawCompass(Mat hud_img);
        void DrawBatteryVoltage(Mat hud_img);
        void DrawStateMachineState(Mat hud_img);

        void DrawHashBoxFill(Mat hud_img, Point top_left, Point top_right, Point bottom_left, Point bottom_right);

        void DrawDateTime(Mat hud_img);

        void DrawElevon(Mat hud_img, float elevon_value, float elevon_u0_value, float roll, int center_delta, int width, int position_left, int position_right, bool is_left);

        void DrawAutonomous(Mat hud_img);

        void DrawCenterMark(Mat hud_img);
        void DrawThrottle(Mat hud_img);
        void DrawPlaneAndLogNumbers(Mat hud_img);

        void DrawAllAccelerationIndicators(Mat hud_img);
        void DrawGraphIndicator(Mat hud_img, int left, int top, string label, int min_value, int max_value, int mark_increment, string plus_format, string minus_format, float value, bool zero_in_center = false, bool reverse_graph_direction = false);

        void GetEulerAngles(float *yaw, float *pitch, float *roll);

        int GetLadderBoxTop(Mat hud_img) { return hud_img.rows * 0.394; }
        int GetLadderBoxWidth(Mat hud_img) { return hud_img.cols * 0.10; }
        int GetLadderBoxHeight(Mat hud_img) { return hud_img.rows * 0.072; }
        int GetLadderArrowWidth(Mat hud_img) { return hud_img.cols * 0.025; }
        int GetAirspeedLeft(Mat hud_img) { return hud_img.cols * .045; }
        int GetAltitudeLeft(Mat hud_img) { return hud_img.cols * .833; }

    public:
        Hud(Scalar hud_color = Scalar(0.45, 0.95, 0.48)); // default is green

        void SetAirspeed(float airspeed_in_mps) { airspeed_
            = 2.23694*airspeed_in_mps; }

        void SetAirspeedUnchecked(float airspeed_unchecked_in_mps) { airspeed_unchecked_
            = 2.23694*airspeed_unchecked_in_mps; }

        void SetAltitude(float altitude_in_meters) { altitude_ = 3.28084*altitude_in_meters; }

        void SetGpsSpeed(float gps_speed_in_mps) { gps_speed_ = gps_speed_in_mps * 2.23694; }

        void SetPlaneNumber(int num) { plane_number_ = num; }
        void SetLogNumber(int num) { log_number_ = num; }

        void SetOrientation(float q0, float q1, float q2, float q3) {
            q0_ = q0;
            q1_ = q1;
            q2_ = q2;
            q3_ = q3;
        }

        void SetAcceleration(float x, float y, float z) {
            x_accel_ = x/9.81;
            y_accel_ = y/9.81;
            z_accel_ = z/9.81 - 1; // minus one since normal G is 1.0
        }

        void SetAutonomous(int autonomous) { is_autonomous_ = autonomous; }

        void SetServoCommands(float throttle, float elevonL, float elevonR) {
            throttle_ = throttle;
            elevonL_ = elevonL;
            elevonR_ = 100-elevonR; // because the servos are mounted in opposite directions
        }

        int GetClutterLevel() { return clutter_level_; }
        void SetClutterLevel(int clutter_level) { clutter_level_ = clutter_level; }

        void SetVideoNumber(int video_number) { video_number_ = video_number; }

        void SetGpsHeading(float gps_heading) { gps_heading_ = gps_heading; }

        void SetBatteryVoltage(float voltage) { battery_voltage_ = voltage; }

        void SetPitchRangeOfLens(float pitch_range_of_lens) { pitch_range_of_lens_ = pitch_range_of_lens; }
        float GetPitchRangeOfLens() { return pitch_range_of_lens_; }

        void SetFrameNumber(int frame_number_in) { frame_number_ = frame_number_in; }

        void SetTimestamp(long timestamp) { timestamp_ = timestamp; }

        void SetTrajectoryNumber(int traj_number) { traj_number_ = traj_number; }

        void SetStateMachineState(std::string state) { state_machine_state_ = state; }

        void SetUCommand(double left_elevon, double right_elevon, double throttle) {
            u0_left_elevon_ = left_elevon;
            u0_right_elevon_ = right_elevon;
            u0_throttle_ = throttle;
        }

        void SetImageScaling(float scale_factor) { scale_factor_ = scale_factor; }
        int GetImageScaling() { return scale_factor_; }

        int GetVideoNumber() { return video_number_; }
        int GetFrameNumber() { return frame_number_; }

        void DrawHud(InputArray _input_image, OutputArray _output_image);

};

#endif


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

  @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_FUNCTION_DEFINITION_ISINCLUDED_C_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_ISINCLUDED_C_HPP_INCLUDED

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

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

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, isincluded_c_);
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::isincluded_c_,isincluded_c);

} }

#endif


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include <gmock/gmock.h>
#include <mars/buffer.h>
#include <mars/core_logic.h>
#include <mars/core_state.h>
#include <mars/sensors/imu/imu_measurement_type.h>
#include <mars/sensors/imu/imu_sensor_class.h>
#include <mars/sensors/pose/pose_sensor_class.h>
#include <mars/sensors/sensor_abs_class.h>
#include <mars/type_definitions/buffer_data_type.h>
#include <mars/type_definitions/core_type.h>
#include <Eigen/Dense>
#include <memory>

class mars_core_logic_test : public testing::Test
{
public:
  int test(int a)
  {
    return a;
  }
};

TEST_F(mars_core_logic_test, CTOR)
{
  // Setup the core definition
  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();

  // Create the CoreLogic and link the core states
  mars::CoreLogic core_logic(core_states_sptr);
}

TEST_F(mars_core_logic_test, INITIALIZE)
{
  Eigen::Vector3d p_wi_init(0, 0, 5);
  Eigen::Quaterniond q_wi_init = Eigen::Quaterniond::Identity();

  // Setup the core definition
  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::ImuSensorClass> imu_sensor_sptr = std::make_shared<mars::ImuSensorClass>("IMU");
  core_states_sptr.get()->set_propagation_sensor(imu_sensor_sptr);

  // Create the CoreLogic and link the core states
  mars::CoreLogic core_logic(core_states_sptr);

  // Call on empty buffer
  ASSERT_FALSE(core_logic.Initialize(p_wi_init, q_wi_init));

  int timestamp = 0;
  mars::IMUMeasurementType imu_meas(Eigen::Vector3d(1, 2, 3), Eigen::Vector3d(4, 5, 6));

  mars::BufferDataType data;
  data.set_sensor_data(std::make_shared<mars::IMUMeasurementType>(imu_meas));

  mars::BufferEntryType buffer_entry(timestamp, data, imu_sensor_sptr, mars::BufferMetadataType::measurement);

  core_logic.buffer_prior_core_init_.AddEntrySorted(buffer_entry);

  // Call on non-empty buffer

  ASSERT_TRUE(core_logic.Initialize(p_wi_init, q_wi_init));
}

TEST_F(mars_core_logic_test, GENERATE_STATE_TRANSITION_BLOCK)
{
  const int test_size = 10;
  const int first_test_idx = 3;
  const int last_test_idx = 7;

  // Setup the core definition
  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::ImuSensorClass> imu_sensor_sptr = std::make_shared<mars::ImuSensorClass>("IMU");
  core_states_sptr.get()->set_propagation_sensor(imu_sensor_sptr);

  // Create the CoreLogic and link the core states
  mars::CoreLogic core_logic(core_states_sptr);

  // Generate test data
  std::vector<mars::CoreType> state_transition_test_data(test_size);
  for (int k = 0; k < test_size; k++)
  {
    // Generate test data
    state_transition_test_data[k].state_transition_ = mars::CoreStateMatrix::Random();
    double timestamp = k;
    // Generate buffer entrys
    mars::BufferDataType data;
    data.set_core_data(std::make_shared<mars::CoreType>(state_transition_test_data[k]));

    mars::BufferEntryType buffer_entry(timestamp, data, imu_sensor_sptr, mars::BufferMetadataType::core_state);
    core_logic.buffer_.AddEntrySorted(buffer_entry);
  }

  mars::CoreStateMatrix state_transition_test_result;
  state_transition_test_result = core_logic.GenerateStateTransitionBlock(first_test_idx, last_test_idx);

  mars::CoreStateMatrix state_transition_correct_result(mars::CoreStateMatrix::Identity());
  for (int k = first_test_idx; k <= last_test_idx; k++)
  {
    state_transition_correct_result = state_transition_test_data[k].state_transition_ * state_transition_correct_result;
  }

  ASSERT_TRUE(state_transition_correct_result.isApprox(state_transition_test_result, 1e-16));

  EXPECT_EQ(state_transition_correct_result, state_transition_test_result);

  // Test that negativ idx call an assert and terminate the program
  EXPECT_DEBUG_DEATH(core_logic.GenerateStateTransitionBlock(-1, 2), "");
  EXPECT_DEBUG_DEATH(core_logic.GenerateStateTransitionBlock(1, -2), "");
  EXPECT_DEBUG_DEATH(core_logic.GenerateStateTransitionBlock(-1, -2), "");
}

TEST_F(mars_core_logic_test, REWORK_BUFFER)
{
  // Setup the core definition
  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();

  // Create the CoreLogic and link the core states
  mars::CoreLogic core_logic(core_states_sptr);

  // Test that negativ idx call an assert and terminate the program
  EXPECT_DEATH(core_logic.ReworkBufferStartingAtIndex(-1), "");
}

TEST_F(mars_core_logic_test, PERFORM_SENSOR_UPDATE)
{
  // Setup the core definition
  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();

  // Create the CoreLogic and link the core states
  mars::CoreLogic core_logic(core_states_sptr);

  // Create a sensor instance
  std::shared_ptr<mars::PoseSensorClass> sensor_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose", core_states_sptr);

  // Prepare Dummy data
  int sensor_dummy = 15;
  mars::BufferDataType data(std::make_shared<mars::CoreType>(), std::make_shared<int>(sensor_dummy));
  std::shared_ptr<mars::BufferDataType> data_sptr = std::make_shared<mars::BufferDataType>(data);

  // Call sensor update with prepared objects
  int sensor_update_status;
  mars::BufferEntryType new_buffer_entry;
  sensor_update_status = core_logic.PerformSensorUpdate(&new_buffer_entry, sensor_sptr, 10, data_sptr);

  ASSERT_EQ(sensor_update_status, 0);
}

TEST_F(mars_core_logic_test, PROPAGATE_SENSOR_CROSS_COV)
{
  constexpr int sensor_cov_dim = 20;
  constexpr int core_cov_dim = mars::CoreStateType::size_error_;
  using SensorCovMatrix = Eigen::Matrix<double, core_cov_dim + sensor_cov_dim, core_cov_dim + sensor_cov_dim>;

  // Setup the core definition
  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();

  // Create the CoreLogic and link the core states
  mars::CoreLogic core_logic(core_states_sptr);

  // Generate symmetryc sensor covariance
  SensorCovMatrix prior_sensor_covariance;
  prior_sensor_covariance.setRandom();
  prior_sensor_covariance = (prior_sensor_covariance * prior_sensor_covariance.transpose()) / 2;

  // Generate symmetryc core covariance
  mars::CoreStateMatrix core_cov;
  core_cov.setRandom();
  core_cov = (core_cov * core_cov.transpose()) / 2;

  mars::CoreStateMatrix state_transition(mars::CoreStateMatrix::Random());
  Eigen::MatrixXd cov_result;
  cov_result = core_logic.PropagateSensorCrossCov(prior_sensor_covariance, core_cov, state_transition);

  // Ensure that core elements remain the same
  ASSERT_TRUE(core_cov.isApprox(cov_result.block(0, 0, core_cov_dim, core_cov_dim), 1e-16));

  // Ensure that inter-sensor covariance remain the same
  Eigen::Matrix<double, sensor_cov_dim, sensor_cov_dim> sensor_cov_before, sensor_cov_after;
  sensor_cov_before = prior_sensor_covariance.block(core_cov_dim, core_cov_dim, sensor_cov_dim, sensor_cov_dim);
  sensor_cov_after = cov_result.block(core_cov_dim, core_cov_dim, sensor_cov_dim, sensor_cov_dim);
  ASSERT_TRUE(sensor_cov_before.isApprox(sensor_cov_after, 1e-16));

  // Ensure symmetry
  SensorCovMatrix zero_cov(SensorCovMatrix::Zero());
  ASSERT_TRUE(zero_cov.isApprox(cov_result - cov_result.transpose()));

  // Ensure correct propagation
  Eigen::Matrix<double, core_cov_dim, sensor_cov_dim> sensor_cross_cov_before, sensor_cross_cov_after;
  sensor_cross_cov_before = prior_sensor_covariance.block(0, core_cov_dim, core_cov_dim, sensor_cov_dim);
  sensor_cross_cov_after = cov_result.block(0, core_cov_dim, core_cov_dim, sensor_cov_dim);

  ASSERT_TRUE(sensor_cross_cov_after.isApprox(state_transition * sensor_cross_cov_before, 1e-16));
}


/*
 * Copyright 2020 Tier IV, Inc. All rights reserved.
 *
 * 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.
 */

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <tf2/transform_datatypes.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <chrono>
#include <cluster_data_association/node.hpp>
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace cluster_data_association
{
ClusterDataAssociationNode::ClusterDataAssociationNode()
: nh_(""),
  pnh_("~"),
  tf_listener_(tf_buffer_),
  cluster0_sub_(pnh_, "input/clusters0", 1),
  cluster1_sub_(pnh_, "input/clusters1", 1),
  sync_(SyncPolicy(10), cluster0_sub_, cluster1_sub_)
{
  sync_.registerCallback(boost::bind(&ClusterDataAssociationNode::clusterCallback, this, _1, _2));

  associated_cluster_pub_ =
    pnh_.advertise<autoware_perception_msgs::DynamicObjectWithFeatureArray>("output/clusters", 10);
}

void ClusterDataAssociationNode::clusterCallback(
  const autoware_perception_msgs::DynamicObjectWithFeatureArray::ConstPtr & input_cluster0_msg,
  const autoware_perception_msgs::DynamicObjectWithFeatureArray::ConstPtr & input_cluster1_msg)
{
  // Guard
  if (associated_cluster_pub_.getNumSubscribers() < 1) return;

  // build output msg
  autoware_perception_msgs::DynamicObjectWithFeatureArray output_msg;
  output_msg.header = input_cluster0_msg->header;

  /* global nearest neighbor */
  std::unordered_map<int, int> direct_assignment;
  std::unordered_map<int, int> reverse_assignment;
  Eigen::MatrixXd score_matrix =
    data_association_.calcScoreMatrix(*input_cluster1_msg, *input_cluster0_msg);
  data_association_.assign(score_matrix, direct_assignment, reverse_assignment);
  for (size_t cluster0_idx = 0; cluster0_idx < input_cluster0_msg->feature_objects.size();
       ++cluster0_idx) {
    if (direct_assignment.find(cluster0_idx) != direct_assignment.end())  // found
    {
      output_msg.feature_objects.push_back(input_cluster0_msg->feature_objects.at(cluster0_idx));
    } else  // not found
    {
      output_msg.feature_objects.push_back(input_cluster0_msg->feature_objects.at(cluster0_idx));
    }
  }
  for (size_t cluster1_idx = 0; cluster1_idx < input_cluster1_msg->feature_objects.size();
       ++cluster1_idx) {
    if (reverse_assignment.find(cluster1_idx) != reverse_assignment.end())  // found
    {
    } else  // not found
    {
      output_msg.feature_objects.push_back(input_cluster1_msg->feature_objects.at(cluster1_idx));
    }
  }

  // publish output msg
  associated_cluster_pub_.publish(output_msg);
}
}  // namespace cluster_data_association


/*
 *    Filename: main-ortho-from-pcl.cc
 *  Created on: Jun 25, 2017
 *      Author: Timo Hinzmann
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

// NON-SYSTEM
#include <aerial-mapper-dense-pcl/stereo.h>
#include <aerial-mapper-grid-map/aerial-mapper-grid-map.h>
#include <aerial-mapper-io/aerial-mapper-io.h>
#include <aerial-mapper-ortho/ortho-from-pcl.h>
#include <aerial-mapper-utils/utils-nearest-neighbor.h>
#include <Eigen/Core>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <ros/ros.h>

DEFINE_bool(ortho_from_pcl_show_orthomosaic_opencv, true,
            "Show the orthomosaic using opencv?");
DEFINE_bool(ortho_from_pcl_use_adaptive_interpolation, false,
            "Use adaptive interpolation in the orthomosaic generation? Used to "
            "handle missing data.");
DEFINE_int32(ortho_from_pcl_interpolation_radius, 10,
             "Interpolation radius [m].");
DEFINE_double(ortho_from_pcl_center_easting, 0.0,
              "Center [m] of the grid_map (easting).");
DEFINE_double(ortho_from_pcl_center_northing, 0.0,
              "Center [m] of the grid_map (northing).");
DEFINE_double(ortho_from_pcl_delta_easting, 100.0,
              "Width [m] of the grid_map, starting from center.");
DEFINE_double(ortho_from_pcl_delta_northing, 100.0,
              "Height [m] of the grid_map, starting from center");
DEFINE_double(ortho_from_pcl_resolution, 1.0,
              "Resolution of the grid_map [m].");
DEFINE_string(data_directory, "",
              "Directory to poses, images, and calibration file.");
DEFINE_string(ortho_from_pcl_orthomosaic_jpg_filename, "",
              "Name of the output image storing the orthomsaic.");
DEFINE_string(filename_poses, "",
              "Name of the file that contains positions and orientations for "
              "every camera in the global/world frame, i.e. T_G_B");
DEFINE_string(prefix_images, "",
              "Prefix of the images to be loaded, e.g. 'images_'");
DEFINE_string(
    filename_camera_rig, "",
    "Name of the camera calibration file (intrinsics). File ending: .yaml");
DEFINE_bool(load_point_cloud_from_file, false,
            "Load point cloud from file? Otherwise generate the point cloud "
            "from the provided images, camera poses, camera intrinsicspoint "
            "cloud from images.");
DEFINE_string(filename_point_cloud, "",
              "Name of the file that contains the point cloud.");
DEFINE_int32(dense_pcl_use_every_nth_image, 10,
             "Only use every n-th image in the densification process.");
DEFINE_bool(use_BM, true,
            "Use BM Blockmatching if true. Use SGBM (=Semi-Global-) "
            "Blockmatching if false.");

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);
  google::InstallFailureSignalHandler();

  ros::init(argc, argv, "ortho_from_pcl");
  ros::NodeHandle nh;

  // Parse input parameters.
  const std::string& base = FLAGS_data_directory;
  const std::string& filename_camera_rig = FLAGS_filename_camera_rig;
  const std::string& filename_poses = FLAGS_filename_poses;
  const std::string& filename_images = base + FLAGS_prefix_images;

  // Load camera rig from file.
  io::AerialMapperIO io_handler;
  const std::string& filename_camera_rig_yaml = base + filename_camera_rig;
  std::shared_ptr<aslam::NCamera> ncameras =
      io_handler.loadCameraRigFromFile(filename_camera_rig_yaml);
  CHECK(ncameras);

  // Load body poses from file.
  Poses T_G_Bs;
  const std::string& path_filename_poses = base + filename_poses;
  io::PoseFormat pose_format = io::PoseFormat::Standard;
  io_handler.loadPosesFromFile(pose_format, path_filename_poses, &T_G_Bs);

  // Load images from file.
  size_t num_poses = T_G_Bs.size();
  Images images;
  io_handler.loadImagesFromFile(filename_images, num_poses, &images);

  // Retrieve dense point cloud.
  AlignedType<std::vector, Eigen::Vector3d>::type point_cloud;
  std::vector<int> point_cloud_intensities;
  if (FLAGS_load_point_cloud_from_file) {
    // Either load point cloud from file..
    CHECK(!FLAGS_filename_point_cloud.empty());
    io_handler.loadPointCloudFromFile(FLAGS_filename_point_cloud, &point_cloud,
                                      &point_cloud_intensities);
  } else {
    // .. or generate via dense reconstruction from poses and images.
    stereo::Settings settings_dense_pcl;
    settings_dense_pcl.use_every_nth_image =
        FLAGS_dense_pcl_use_every_nth_image;
    LOG(INFO) << "Perform dense reconstruction using planar rectification.";
    stereo::BlockMatchingParameters block_matching_params;
    block_matching_params.use_BM = FLAGS_use_BM;
    stereo::Stereo stereo(ncameras, settings_dense_pcl, block_matching_params);
    stereo.addFrames(T_G_Bs, images, &point_cloud, &point_cloud_intensities);
  }

  LOG(INFO) << "Initialize layered map.";
  grid_map::Settings settings_aerial_grid_map;
  settings_aerial_grid_map.center_easting = FLAGS_ortho_from_pcl_center_easting;
  settings_aerial_grid_map.center_northing =
      FLAGS_ortho_from_pcl_center_northing;
  settings_aerial_grid_map.delta_easting = FLAGS_ortho_from_pcl_delta_easting;
  settings_aerial_grid_map.delta_northing = FLAGS_ortho_from_pcl_delta_northing;
  settings_aerial_grid_map.resolution = FLAGS_ortho_from_pcl_resolution;
  grid_map::AerialGridMap map(settings_aerial_grid_map);

  // Orthomosaic from point cloud.
  ortho::Settings settings;
  settings.interpolation_radius = FLAGS_ortho_from_pcl_interpolation_radius;
  settings.use_adaptive_interpolation =
      FLAGS_ortho_from_pcl_use_adaptive_interpolation;
  settings.show_orthomosaic_opencv =
      FLAGS_ortho_from_pcl_show_orthomosaic_opencv;
  settings.orthomosaic_jpg_filename =
      FLAGS_ortho_from_pcl_orthomosaic_jpg_filename;
  CHECK(point_cloud.size() > 0);
  CHECK(point_cloud.size() == point_cloud_intensities.size());

  // Generate the orthomosaic from the point cloud.
  ortho::OrthoFromPcl mosaic(settings);
  mosaic.process(point_cloud, point_cloud_intensities, map.getMutable());

  LOG(INFO) << "Publish until shutdown.";
  map.publishUntilShutdown();

  return 0;
}


#include <boost/hana/fwd/concept/monad_plus.hpp>


#include <boost/regex.hpp>
#include <cstdlib>

#include "eval_add/eval_add.h"

using namespace boost;

const regex re(R"(\s*(\d+)\s*\+\s*(\d+)\s*)");

bool eval_add(const char *str, int *out) {
  cmatch match;
  if (regex_match(str, match, re)) {
    *out = stoi(match[1].str()) + stoi(match[2].str());
    return true;
  } else {
    return false;
  }
}