//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2009
//
//============================================================================

#include "StdLinearSolver.hpp"
#include "../IDenseMatrix.hpp"
#include "../ICompressedSparseMatrix.hpp"
#include "NNLS/nnls.h"
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/QR>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>
#include <Eigen/SVD>
#include <type_traits>

namespace Thea {
namespace Algorithms {

namespace StdLinearSolverInternal {

// Implementation of StdLinearSolver functions.
class THEA_DLL_LOCAL StdLinearSolverImpl
{
  public:
    // Constructor.
    StdLinearSolverImpl(StdLinearSolver::Method method_, StdLinearSolver::Constraint constraint_)
    : method(method_), constraint(constraint_), tolerance(-1), max_iters(-1), ndims(0), has_solution(false)
    {}

    // Solve the linear system Ax = b for a dense double-precision matrix A.
    template < typename MatrixT, typename ScalarT,
               typename std::enable_if< std::is_same<typename MatrixT::value_type, ScalarT>::value, int >::type = 0 >
    bool solve(Eigen::MatrixBase<MatrixT> const & a, ScalarT const * b, IOptions const * options = nullptr)
    {
      if (a.rows() < a.cols())
        THEA_WARNING << "StdLinearSolver: Fewer objectives than dimensions -- the solution will not be unique";

      has_solution = false;
      try
      {
        if (a.rows() <= 0 || a.cols() <= 0)
          throw Error("Empty coefficient matrix");

        intx num_objectives = a.rows();
        ndims = a.cols();

        switch (constraint)
        {
          case StdLinearSolver::Constraint::NON_NEGATIVE:
          {
            if (method != StdLinearSolver::Method::DEFAULT && method != StdLinearSolver::Method::NNLS)
              throw Error("Unsupported method for non-negative least squares problems");

            // Values will be overwritten anyway, so make copies
            MatrixX<double, MatrixLayout::COLUMN_MAJOR> nnls_a = a.template cast<double>();  // NNLS needs Fortran COLUMN-MAJOR
            VectorX<double> nnls_b = Eigen::Map< VectorX<ScalarT> const >(b, num_objectives).template cast<double>();

            solution.resize(ndims);

            double         rnorm;
            Array<double>  w((size_t)ndims);
            Array<double>  zz((size_t)num_objectives);
            Array<int>     index((size_t)ndims);
            int            mode;

            int mda = (int)num_objectives, im = (int)num_objectives, in = (int)ndims;

            // NOTE: Assume float64 == double, for passing solution vector to NNLS
            nnls_c(nnls_a.data(), &mda, &im, &in, nnls_b.data(), solution.data(), &rnorm, &w[0], &zz[0], &index[0], &mode);

            if (mode == 1)
              has_solution = true;
            else
            {
              switch (mode)
              {
                // Should never be 2 since we've checked for this above, but recheck all the same
                case 2:  THEA_DEBUG << "StdLinearSolver: NNLS error (bad problem dimensions)"; break;
                case 3:  THEA_DEBUG << "StdLinearSolver: NNLS error (iteration count exceeded)"; break;
                default: THEA_DEBUG << "StdLinearSolver: Unknown NNLS error";
              }
            }

            break;
          }

          case StdLinearSolver::Constraint::UNCONSTRAINED:
          {
            switch (method)
            {
              case StdLinearSolver::Method::HOUSEHOLDER_QR:
              {
                Eigen::HouseholderQR<typename MatrixT::PlainObject> solver(a);
                solution = solver.solve(Eigen::Map< VectorX<ScalarT> const >(b, num_objectives));
                has_solution = true;
                break;
              }

              case StdLinearSolver::Method::DEFAULT:  // slower than plain Householder, but more accurate
              case StdLinearSolver::Method::COL_PIV_HOUSEHOLDER_QR:
              {
                Eigen::ColPivHouseholderQR<typename MatrixT::PlainObject> solver(a);
                if (tolerance >= 0) solver.setThreshold(tolerance);
                solution = solver.solve(Eigen::Map< VectorX<ScalarT> const >(b, num_objectives));
                has_solution = true;
                break;
              }

              case StdLinearSolver::Method::FULL_PIV_HOUSEHOLDER_QR:
              {
                Eigen::FullPivHouseholderQR<typename MatrixT::PlainObject> solver(a);
                if (tolerance >= 0) solver.setThreshold(tolerance);
                solution = solver.solve(Eigen::Map< VectorX<ScalarT> const >(b, num_objectives));
                has_solution = true;
                break;
              }

              case StdLinearSolver::Method::COMPLETE_ORTHOGONAL_DECOMPOSITION:
              {
                Eigen::CompleteOrthogonalDecomposition<typename MatrixT::PlainObject> solver(a);
                if (tolerance >= 0) solver.setThreshold(tolerance);
                solution = solver.solve(Eigen::Map< VectorX<ScalarT> const >(b, num_objectives));
                has_solution = true;
                break;
              }

              case StdLinearSolver::Method::BDCSVD:
              {
                Eigen::BDCSVD<typename MatrixT::PlainObject> solver(a);
                if (tolerance >= 0) solver.setThreshold(tolerance);
                solution = solver.solve(Eigen::Map< VectorX<ScalarT> const >(b, num_objectives));
                has_solution = true;
                break;
              }

              default:
                throw Error("Unsupported method for unconstrained dense least squares problems");
            }

            break;
          }

          default:
            throw Error("Unsupported constraint");
        }
      }
      THEA_STANDARD_CATCH_BLOCKS(return false;, ERROR, "%s",
                                 "StdLinearSolver: Error solving dense linear least-squares system")

      return has_solution;
    }

    // Solve the linear system Ax = b for a sparse ScalarT-precision matrix A.
    template < typename MatrixT, typename ScalarT,
               typename std::enable_if< std::is_same<typename MatrixT::value_type, ScalarT>::value, int >::type = 0 >
    bool solve(Eigen::SparseMatrixBase<MatrixT> const & a, ScalarT const * b, IOptions const * options = nullptr)
    {
      if (a.rows() < a.cols())
        THEA_WARNING << "StdLinearSolver: Fewer objectives than dimensions -- the solution will not be unique";

      has_solution = false;
      try
      {
        if (a.rows() <= 0 || a.cols() <= 0)
          throw Error("Empty coefficient matrix");

        ndims = a.cols();

        switch (constraint)
        {
          case StdLinearSolver::Constraint::UNCONSTRAINED:
          {
            // Return true if a matching solver was found
            if (solveSparseFactorize(a, b)) break;
            if (solveIterative(a, b)) break;

            throw Error("Unsupported method for unconstrained sparse least squares problems");
          }

          default:
            throw Error("Unsupported constraint");
        }
      }
      THEA_STANDARD_CATCH_BLOCKS(return false;, ERROR, "%s",
                                 "StdLinearSolver: Error solving sparse linear least-squares system")

      return has_solution;
    }

  private:
    // Use one of the iterative solvers to solve the dense or sparse problem. Returns true if a suitable solver was found, NOT
    // if the problem was successfully solved.
    template <typename MatrixT, typename ScalarT> bool solveIterative(MatrixT const & a, ScalarT const * b)
    {
      switch (method)
      {
        case StdLinearSolver::Method::CONJUGATE_GRADIENT:
        {
          Eigen::ConjugateGradient<typename MatrixT::PlainObject> solver;
          if (tolerance >= 0) solver.setTolerance((ScalarT)tolerance);
          if (max_iters > 0) solver.setMaxIterations(max_iters);
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        case StdLinearSolver::Method::DEFAULT:
        case StdLinearSolver::Method::LEAST_SQUARES_CONJUGATE_GRADIENT:
        {
          Eigen::LeastSquaresConjugateGradient<typename MatrixT::PlainObject> solver;
          if (tolerance >= 0) solver.setTolerance((ScalarT)tolerance);
          if (max_iters > 0) solver.setMaxIterations(max_iters);
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        case StdLinearSolver::Method::BICGSTAB:
        {
          Eigen::BiCGSTAB<typename MatrixT::PlainObject> solver;
          if (tolerance >= 0) solver.setTolerance((ScalarT)tolerance);
          if (max_iters > 0) solver.setMaxIterations(max_iters);
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        default: return false;
      }

      return true;
    }

    // Use a solver based on sparse factorization, for column-major matrices. Returns true if a suitable solver was found, NOT
    // if the problem was successfully solved.
    template < typename MatrixT, typename ScalarT,
               typename std::enable_if< !(MatrixT::Flags & Eigen::RowMajorBit), int >::type = 0 >
    bool solveSparseFactorize(MatrixT const & a, ScalarT const * b)
    {
      switch (method)
      {
        case StdLinearSolver::Method::SIMPLICIALT_LLT:
        {
          Eigen::SimplicialLLT<typename MatrixT::PlainObject> solver;
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        case StdLinearSolver::Method::SIMPLICIALT_LDLT:
        {
          Eigen::SimplicialLDLT<typename MatrixT::PlainObject> solver;
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        case StdLinearSolver::Method::SPARSE_LU:
        {
          Eigen::SparseLU<typename MatrixT::PlainObject> solver;
          if (tolerance >= 0) solver.setPivotThreshold((ScalarT)tolerance);
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        case StdLinearSolver::Method::DEFAULT:
        case StdLinearSolver::Method::SPARSE_QR:
        {
          Eigen::SparseQR<typename MatrixT::PlainObject, Eigen::AMDOrdering<typename MatrixT::StorageIndex> > solver;
          if (tolerance >= 0) solver.setPivotThreshold((ScalarT)tolerance);
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        default: return false;
      }

      return true;
    }

    // Use a solver based on sparse factorization, for row-major matrices. Eigen does not provide any such solvers, so this
    // method is empty. Returns false to indicate no solver was found.
    template < typename MatrixT, typename ScalarT,
               typename std::enable_if< (MatrixT::Flags & Eigen::RowMajorBit), int >::type = 0 >
    bool solveSparseFactorize(MatrixT const & a, ScalarT const * b)
    {
      return false;
    }

  public:  // no need for accessor fns for this internal class, and friend declarations are complicated by namespaces
    StdLinearSolver::Method method;          // Solution method.
    StdLinearSolver::Constraint constraint;  // Solution constraint.
    double tolerance;                        // Solution tolerance/threshold.
    intx max_iters;                          // Maximum number of solver iterations, if solver is iterative.
    intx ndims;                              // Solution dimensions.
    bool has_solution;                       // Was a solution computed by the last call to solve()? */
    VectorX<float64> solution;               // The solution vector <b>x</b>.
};

// Shorthand
template <MatrixLayout::Value L, typename StorageIndex> using SM = Eigen::Map< SparseMatrix<double, L, StorageIndex> >;

// Given a storage index type, dispatch a call to StdLinearSolverImpl::solve() with the correct pointer conversions
template <MatrixLayout::Value L, typename ScalarT>
bool
implSolve(StdLinearSolverImpl * impl, int storage_type, intx nr, intx nc, intx nnz, void const * in, void const * out,
          ScalarT const * val, void const * nzc, ScalarT const * b, IOptions const * opt)
{
  void    * in2  = const_cast<void *>(in);
  void    * out2 = const_cast<void *>(out);
  ScalarT * val2 = const_cast<ScalarT *>(val);
  void    * nzc2 = const_cast<void *>(nzc);

  switch (storage_type)
  {
// A casting bug in the current Eigen prevents us using StorageIndex shorter than an int.
// Specifically, OrderingMethods/Amd.h, L104: dense = (std::min)(n-2, dense);
// Here n and dense are of type StorageIndex, but n - 2 is of type int for StorageIndex shorter than an int, so std::min fails
// because the arguments are not of the same type.
//
//     case NumericType::INT8:
//       return impl->solve(SM<L, int8  >(nr, nc, nnz, (int8   *)in2, (int8   *)out2, val2, (int8   *)nzc2), b, opt);
//     case NumericType::INT16:
//       return impl->solve(SM<L, int16 >(nr, nc, nnz, (int16  *)in2, (int16  *)out2, val2, (int16  *)nzc2), b, opt);
    case NumericType::INT32:
      return impl->solve(SM<L, int32 >(nr, nc, nnz, (int32  *)in2, (int32  *)out2, val2, (int32  *)nzc2), b, opt);
    case NumericType::INT64:
      return impl->solve(SM<L, int64 >(nr, nc, nnz, (int64  *)in2, (int64  *)out2, val2, (int64  *)nzc2), b, opt);
    default: THEA_ERROR << "StdLinearSolver: Unsupported index type";
  }

  return false;
}

} // namespace StdLinearSolverInternal

StdLinearSolver::StdLinearSolver(Method method_, Constraint constraint_)
: NamedObject("StdLinearSolver"), impl(new StdLinearSolverInternal::StdLinearSolverImpl(method_, constraint_))
{
}

StdLinearSolver::~StdLinearSolver()
{
  delete impl;
}

StdLinearSolver::Method
StdLinearSolver::getMethod() const
{
  return impl->method;
}

StdLinearSolver::Constraint
StdLinearSolver::getConstraint() const
{
  return impl->constraint;
}

double
StdLinearSolver::getTolerance() const
{
  return impl->tolerance;
}

intx
StdLinearSolver::maxIterations() const
{
  return impl->max_iters;
}

void
StdLinearSolver::setMethod(StdLinearSolver::Method method_)
{
  impl->method = method_;
}

void
StdLinearSolver::setConstraint(StdLinearSolver::Constraint constraint_)
{
  impl->constraint = constraint_;
}

void
StdLinearSolver::setTolerance(double tol)
{
  impl->tolerance = tol;
}

void
StdLinearSolver::setMaxIterations(intx max_iters_)
{
  impl->max_iters = max_iters_;
}

bool
StdLinearSolver::solve(Eigen::Ref< MatrixXd > const & a, float64 const * b, IOptions const * options)
{
  return impl->solve(a, b, options);
}

bool
StdLinearSolver::solve(Eigen::Ref< SparseMatrix<double> > const & a, float64 const * b, IOptions const * options)
{
  return impl->solve(a, b, options);
}

int8
StdLinearSolver::solve(IMatrix<float64> const * a, float64 const * b, IOptions const * options)
{
  alwaysAssertM(a, "StdLinearSolver: Coefficient matrix is null");
  alwaysAssertM(b, "StdLinearSolver: Constant matrix is null");

  if (a->asAddressable() && a->asAddressable()->asDense())
  {
    IDenseMatrix<float64> const & dm = *a->asAddressable()->asDense();
    if (dm.isRowMajor())
    {
      Eigen::Map< MatrixX<float64, MatrixLayout::ROW_MAJOR> const > wrapped(dm.data(), dm.rows(), dm.cols());
      return impl->solve(wrapped, b, options);
    }
    else  // col-major
    {
      Eigen::Map< MatrixX<float64, MatrixLayout::COLUMN_MAJOR> const > wrapped(dm.data(), dm.rows(), dm.cols());
      return impl->solve(wrapped, b, options);
    }
  }
  else if (a->asSparse() && a->asSparse()->asCompressed())
  {
    ICompressedSparseMatrix<float64> const & sm = *a->asSparse()->asCompressed();
    int storage_type = sm.getInnerIndexType();
    if (storage_type != sm.getOuterIndexType() || storage_type != sm.getNonZeroCountType())
    {
      // TODO: Convert to integer arrays of consistent type to work around this problem

      THEA_ERROR << "StdLinearSolver: Different indices have different storage types -- cannot convert to SparseMatrix";
      return false;
    }

    if (sm.isRowMajor())
      return StdLinearSolverInternal::implSolve<MatrixLayout::ROW_MAJOR>(impl, storage_type, sm.rows(), sm.cols(),
                                                                         sm.numStoredElements(), sm.getOuterIndices(),
                                                                         sm.getInnerIndices(), sm.getValues(),
                                                                         sm.getNonZeroCounts(), b, options);
    else
      return StdLinearSolverInternal::implSolve<MatrixLayout::COLUMN_MAJOR>(impl, storage_type, sm.rows(), sm.cols(),
                                                                            sm.numStoredElements(), sm.getOuterIndices(),
                                                                            sm.getInnerIndices(), sm.getValues(),
                                                                            sm.getNonZeroCounts(), b, options);
  }
  else
  {
    THEA_ERROR << "StdLinearSolver: Unsupported matrix type";
    return false;
  }
}

int64
StdLinearSolver::dims() const
{
  return (int64)impl->ndims;
}

int8
StdLinearSolver::hasSolution() const
{
  return impl->has_solution;
}

float64 const *
StdLinearSolver::getSolution() const
{
  return impl->solution.data();
}

int8
StdLinearSolver::getSquaredError(float64 * err) const
{
  return false;
}

} // namespace Algorithms
} // namespace Thea


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/LinearMeasurementModel.h>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

std::pair<bool, bfl::Data> LinearMeasurementModel::predictedMeasure(const Eigen::Ref<const Eigen::MatrixXd>& cur_states) const
{
    MatrixXd prediction = getMeasurementMatrix() * cur_states;

    return std::make_pair(true, std::move(prediction));
}

std::pair<bool, bfl::Data> LinearMeasurementModel::innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const
{
    MatrixXd innovation = -(any::any_cast<MatrixXd>(predicted_measurements).colwise() - any::any_cast<MatrixXd>(measurements).col(0));

    return std::make_pair(true, std::move(innovation));
}


#include <iostream>
#include <cstdio>
#include <ctime>

#define EIGEN_NO_DEBUG // turn off assertions

#include <Eigen/Dense>
#include "boost/program_options.hpp"

using namespace std;
using namespace Eigen;
namespace po = boost::program_options;

// m: numRows, n: numCols
inline double simpleDenseTest_Eigen(const int m, const int n, const int num_trials) {
  MatrixXd A = MatrixXd::Random(m, n);
  MatrixXd B = MatrixXd::Random(m, n);
  MatrixXd C = MatrixXd::Random(m, n);
  MatrixXd D = MatrixXd::Random(m, n);
  MatrixXd E = MatrixXd::Random(m, n);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    MatrixXd res = ((A + B).cwiseQuotient(C) - D).cwiseProduct(E);

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;

}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double gemmSanityTest_Eigen(const int m, const int n, const int k, const int num_trials) {

  MatrixXd A = MatrixXd::Random(m, n);
  MatrixXd C = MatrixXd::Random(n, k);
  MatrixXd E = MatrixXd::Random(m, k);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    E.noalias() += A * C;

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;

}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double gemmDenseTest_Eigen(const int m, const int n, const int k, const int num_trials) {

  MatrixXd A = MatrixXd::Random(m, n);
  MatrixXd B = MatrixXd::Random(m, n);
  MatrixXd C = MatrixXd::Random(n, k);
  MatrixXd D = MatrixXd::Random(n, k);
  MatrixXd E = MatrixXd::Random(m, k);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    E.noalias() += (A + B) * (C - D);

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;

}

inline double mulDenseTest_Eigen(const int a, const int b, const int c, const int d, const int num_trials) {

  MatrixXd A = MatrixXd::Random(a, a);
  MatrixXd B = MatrixXd::Random(a, b);
  MatrixXd C = MatrixXd::Random(b, c);
  MatrixXd D = MatrixXd::Random(c, d);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    MatrixXd res = A * B * C * D;

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;

}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double denseVectorTest_Eigen(int l, int num_trials) {

  VectorXd a = VectorXd::Random(l);
  VectorXd b = VectorXd::Random(l);
  VectorXd c = VectorXd::Random(l);
  VectorXd d = VectorXd::Random(l);
  VectorXd res(l);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    res = a + b + c + d;

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;

  return duration / num_trials;
}

void runEigenTests(int num_trials, int l, int m, int n, int k, int a, int b, int c, int d,
    bool skip_vec, bool skip_simple, bool skip_gemm, bool skip_mult) {

  if (!skip_vec)
    cout << "Eigen Vectors Test:\t" << denseVectorTest_Eigen(l, num_trials) << endl;
  if (!skip_simple)
    cout << "Eigen Simple Test:\t" << simpleDenseTest_Eigen(m, n, num_trials) << endl;
  if (!skip_gemm) {
    cout << "Eigen gemmSanity Test:\t" << gemmDenseTest_Eigen(m, n, k, num_trials) << endl;
    cout << "Eigen gemm Test:\t" << gemmDenseTest_Eigen(m, n, k, num_trials) << endl;
  }

  if (!skip_mult)
    cout << "Eigen mulDense Test:\t" << mulDenseTest_Eigen(a, b, c, d, num_trials) << endl;

}

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

    int l, m, n, k, a, b, c, d, trials;
    bool skip_vec, skip_simple, skip_gemm, skip_mult;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("l", po::value<int>(&l)->default_value(1048576),
                    "length of vectors in vector addition test")
        ("m", po::value<int>(&m)->default_value(1024),
            "numRows of matrices in Simple Test, and gemm Test")
        ("n", po::value<int>(&n)->default_value(1024),
            "numCols of matrices in Simple Test, and gemm Test")
        ("k", po::value<int>(&k)->default_value(1024),
            "numCols of B in gemm Test")
        ("trials", po::value<int>(&trials)->default_value(10), "number of trials")
        ("a", po::value<int>(&a)->default_value(1024),
            "size matrix A in mulDense Test")
        ("b", po::value<int>(&b)->default_value(512),
            "size matrix B in mulDense Test")
        ("c", po::value<int>(&c)->default_value(256),
            "size matrix C in mulDense Test")
        ("d", po::value<int>(&d)->default_value(128),
            "size matrix D in mulDense Test")
        ("skip-vec", po::value<bool>(&skip_vec)->default_value(false),
            "skip vectors Test")
        ("skip-simple", po::value<bool>(&skip_simple)->default_value(false),
            "skip simple Test")
        ("skip-gemm", po::value<bool>(&skip_gemm)->default_value(false),
            "skip gemm Tests")
        ("skip-mult", po::value<bool>(&skip_mult)->default_value(false),
            "skip mulDense Test")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    runEigenTests(trials, l, m, n, k, a, b, c, d, skip_vec, skip_simple, skip_gemm, skip_mult);

    return 0;
}


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

#include <cmath>
#include <stdexcept>

#include <armadillo>

#include <ergodic_exploration/numerics.hpp>

namespace ergodic_exploration
{
namespace models
{
using arma::mat;
using arma::vec;

/**
 * @brief Kinematic model of 2 wheel differential drive robot
 * @details The state is [x, y, theta] and controls are the velocities of each wheel [u0,
 * u1] corresponding to the left and right wheels
 */
struct Cart
{
  /**
   * @brief Constructor
   * @param wheel_radius - radius of wheel
   * @param wheel_base - distance from point at the center in between both wheels to the
   * center of a wheel
   */
  Cart(double wheel_radius, double wheel_base)
    : wheel_radius(wheel_radius), wheel_base(wheel_base), state_space(3)
  {
  }

  /**
   * @brief Convert wheel velocities to a body frame twist
   * @param u - control [u0, u1]
   * @return twist in body frame Vb = [vx, vy, w]
   */
  vec wheels2Twist(const vec u) const
  {
    const double vx = wheel_radius / 2.0 * (u(0) + u(1));
    const double w = wheel_radius / (2.0 * wheel_base) * (u(1) - u(0));

    return { vx, 0.0, w };
  }

  /**
   * @brief Kinematic model of a 2 wheel differential drive robot
   * @param x - state [x, y, theta]
   * @param u - control [uL, uR]
   * @return [xdot, ydot, thetadot] = f(x,u)
   */
  vec operator()(const vec x, const vec u) const
  {
    vec xdot(3);
    xdot(0) = (u(0) + u(1)) * std::cos(x(2));
    xdot(1) = (u(0) + u(1)) * std::sin(x(2));
    xdot(2) = (u(1) - u(0)) / wheel_base;

    return (wheel_radius / 2.0) * xdot;
  }

  /**
   * @brief Jacobian of the model with respect to the state
   * @param x - state [x, y, theta]
   * @param u - control [uL, uR]
   * @return A = D1(f(x,u)) of shape (3x3)
   */
  mat fdx(const vec x, const vec u) const
  {
    mat A(3, 3, arma::fill::zeros);

    const auto df0dth = -(wheel_radius / 2.0) * (u(0) + u(1)) * std::sin(x(2));
    const auto df1dth = (wheel_radius / 2.0) * (u(0) + u(1)) * std::cos(x(2));

    A(0, 2) = df0dth;
    A(1, 2) = df1dth;

    return A;
  }

  /**
   * @brief Jacobian of the model with respect to the control
   * @param x - state [x, y, theta]
   * @return B = D2(f(x,u)) of shape (3x2)
   */
  mat fdu(const vec x) const
  {
    mat B(3, 2);

    B(0, 0) = std::cos(x(2));
    B(0, 1) = std::cos(x(2));

    B(1, 0) = std::sin(x(2));
    B(1, 1) = std::sin(x(2));

    B(2, 0) = -1.0 / wheel_base;
    B(2, 1) = 1.0 / wheel_base;

    return (wheel_radius / 2.0) * B;
  }

  double wheel_radius;       // radius of wheel
  double wheel_base;         // distance from robot center to wheel center
  unsigned int state_space;  // states space dimension
};

/**
 * @brief Kinematic model of a wheeled differential drive robot
 * @details The state is [x, y, theta] and controls are the linear and
 * angular velocities [vx, vy, w] (body twist)
 */
struct SimpleCart
{
  /** @brief Constructor */
  SimpleCart() : state_space(3)
  {
  }

  /**
   * @brief Kinematic model of a 2 wheel differential drive robot
   * @param x - state [x, y, theta]
   * @param u - body twist control [vx, vy, w]
   * @return xdot = f(x,u)
   */
  vec operator()(const vec x, const vec u) const
  {
    if (!almost_equal(u(1), 0.0))
    {
      throw std::invalid_argument("Invalid twist y-velocity must be 0.");
    }

    return { u(0) * std::cos(x(2)), u(0) * std::sin(x(2)), u(2) };
  }

  /**
   * @brief Jacobian of the model with respect to the state
   * @param x - state [x, y, theta]
   * @param u - body twist control [vx, vy, w]
   * @return A = D1(f(x,u)) of shape (3x3)
   */
  mat fdx(const vec x, const vec u) const
  {
    mat A(3, 3, arma::fill::zeros);
    A(0, 2) = -u(0) * std::sin(x(2));
    A(1, 2) = u(0) * std::cos(x(2));
    return A;
  }

  /**
   * @brief Jacobian of the model with respect to the control
   * @param x - state [x, y, theta]
   * @return B = D2(f(x,u)) of shape (3x3)
   */
  mat fdu(const vec x) const
  {
    mat B(3, 3, arma::fill::zeros);

    B(0, 0) = std::cos(x(2));
    B(1, 0) = std::sin(x(2));
    B(2, 2) = 1.0;

    return B;
  }

  unsigned int state_space;  // states space dimension
};
}  // namespace models
}  // namespace ergodic_exploration
#endif


//  Copyright John Maddock 2009.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include "required_defines.hpp"

#include "performance_measure.hpp"

#include <boost/math/special_functions/bessel.hpp>
#include <boost/array.hpp>

#define T double
#include "../test/bessel_j_data.ipp"
#include "../test/bessel_j_int_data.ipp"
#include "../test/bessel_j_large_data.ipp"
#include "../test/bessel_y01_data.ipp"
#include "../test/bessel_yn_data.ipp"
#include "../test/bessel_yv_data.ipp"
#include "../test/bessel_k_int_data.ipp"
#include "../test/bessel_k_data.ipp"
#include "../test/bessel_i_int_data.ipp"
#include "../test/bessel_i_data.ipp"
#include "../test/sph_bessel_data.ipp"
#include "../test/sph_neumann_data.ipp"

template <std::size_t N>
double bessel_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::cyl_bessel_j(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_bessel_j")
{
   double result= bessel_evaluate2(bessel_j_data);
   result += bessel_evaluate2(bessel_j_int_data);
   result += bessel_evaluate2(bessel_j_large_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_j_data)
      + sizeof(bessel_j_int_data)
      + sizeof(bessel_j_large_data)) / sizeof(bessel_j_data[0]));
}

template <std::size_t N>
double bessel_y_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::cyl_neumann(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_neumann")
{
   double result= bessel_y_evaluate2(bessel_y01_data);
   result += bessel_y_evaluate2(bessel_yn_data);
   result += bessel_y_evaluate2(bessel_yv_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_y01_data)
      + sizeof(bessel_yn_data)
      + sizeof(bessel_yv_data)) / sizeof(bessel_j_data[0]));
}

template <std::size_t N>
double bessel_i_evaluate(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::cyl_bessel_i(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_bessel_i")
{
   double result= bessel_i_evaluate(bessel_i_int_data);
   result += bessel_i_evaluate(bessel_i_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_i_data)
      + sizeof(bessel_i_int_data)) / sizeof(bessel_i_data[0]));
}

template <std::size_t N>
double bessel_k_evaluate(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::cyl_bessel_k(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_bessel_k")
{
   double result= bessel_k_evaluate(bessel_k_data);
   result += bessel_k_evaluate(bessel_k_int_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_k_data)
      + sizeof(bessel_k_int_data)) / sizeof(bessel_k_data[0]));
}

template <std::size_t N>
double bessel_sph_j_evaluate(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::sph_bessel(static_cast<unsigned>(data[i][0]), data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "sph_bessel")
{
   double result= bessel_sph_j_evaluate(sph_bessel_data);

   consume_result(result);
   set_call_count(
      sizeof(sph_bessel_data) / sizeof(sph_bessel_data[0]));
}

template <std::size_t N>
double bessel_sph_y_evaluate(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::sph_neumann(static_cast<unsigned>(data[i][0]), data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "sph_neumann")
{
   double result= bessel_sph_y_evaluate(sph_neumann_data);

   consume_result(result);
   set_call_count(
      sizeof(sph_neumann_data) / sizeof(sph_neumann_data[0]));
}

#ifdef TEST_DCDFLIB

#endif

#ifdef TEST_CEPHES

extern "C" double jv(double, double);
extern "C" double yv(double, double);
extern "C" double iv(double, double);

template <std::size_t N>
double bessel_evaluate2_cephes(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += jv(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_bessel_j-cephes")
{
   double result = bessel_evaluate2_cephes(bessel_j_data);
   result += bessel_evaluate2_cephes(bessel_j_int_data);
   result += bessel_evaluate2_cephes(bessel_j_large_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_j_data)
      + sizeof(bessel_j_int_data)
      + sizeof(bessel_j_large_data)) / sizeof(bessel_j_data[0]));
}

template <std::size_t N>
double bessel_y_evaluate2_cephes(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += yv(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_neumann-cephes")
{
   double result= bessel_y_evaluate2_cephes(bessel_y01_data);
   result += bessel_y_evaluate2_cephes(bessel_yn_data);
   result += bessel_y_evaluate2_cephes(bessel_yv_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_y01_data)
      + sizeof(bessel_yn_data)
      + sizeof(bessel_yv_data)) / sizeof(bessel_j_data[0]));
}

template <std::size_t N>
double bessel_i_evaluate_cephes(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += iv(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_bessel_i-cephes")
{
   double result= bessel_i_evaluate_cephes(bessel_i_int_data);
   result += bessel_i_evaluate_cephes(bessel_i_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_i_data)
      + sizeof(bessel_i_int_data)) / sizeof(bessel_i_data[0]));
}

#endif

#ifdef TEST_GSL

#include <gsl/gsl_sf_bessel.h>

template <std::size_t N>
double bessel_evaluate2_gsl(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += gsl_sf_bessel_Jnu(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_bessel_j-gsl")
{
   double result = bessel_evaluate2_gsl(bessel_j_data);
   result += bessel_evaluate2_gsl(bessel_j_int_data);
   result += bessel_evaluate2_gsl(bessel_j_large_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_j_data)
      + sizeof(bessel_j_int_data)
      + sizeof(bessel_j_large_data)) / sizeof(bessel_j_data[0]));
}

template <std::size_t N>
double bessel_y_evaluate2_gsl(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += gsl_sf_bessel_Ynu(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_neumann-gsl")
{
   double result= bessel_y_evaluate2_gsl(bessel_y01_data);
   result += bessel_y_evaluate2_gsl(bessel_yn_data);
   result += bessel_y_evaluate2_gsl(bessel_yv_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_y01_data)
      + sizeof(bessel_yn_data)
      + sizeof(bessel_yv_data)) / sizeof(bessel_j_data[0]));
}

template <std::size_t N>
double bessel_i_evaluate_gsl(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += gsl_sf_bessel_Inu(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_bessel_i-gsl")
{
   double result= bessel_i_evaluate_gsl(bessel_i_int_data);
   result += bessel_i_evaluate_gsl(bessel_i_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_i_data)
      + sizeof(bessel_i_int_data)) / sizeof(bessel_i_data[0]));
}

template <std::size_t N>
double bessel_k_evaluate_gsl(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += gsl_sf_bessel_Knu(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(bessel_test, "cyl_bessel_k-gsl")
{
   double result= bessel_k_evaluate_gsl(bessel_k_data);
   result += bessel_k_evaluate_gsl(bessel_k_int_data);

   consume_result(result);
   set_call_count(
      (sizeof(bessel_k_data)
      + sizeof(bessel_k_int_data)) / sizeof(bessel_k_data[0]));
}

#endif


#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include <NTL/lzz_pXFactoring.h>

#include <cassert>
#include <cstdio>
#include <iostream>
using namespace std;

Ctxt FHE_Add(Ctxt Ea, Ctxt Eb)
{
	Ctxt ctSum = Ea;
	ctSum += Eb;
	return ctSum;
}

Ctxt FHE_Mul(Ctxt Ea, Ctxt Eb)
{
	Ctxt ctMul = Ea;
	ctMul *= Eb;
	return ctMul;
}

Ctxt FHE_Sub(Ctxt Ea, Ctxt Eb, const FHEPubKey& publicKey)
{
	Ctxt minus1(publicKey);
	publicKey.Encrypt(minus1, to_ZZX(-1));
	Ctxt ctSub = Eb;
	ctSub *= minus1;
	ctSub += Ea;
	return ctSub;
}

Ctxt FHE_Div(Ctxt Ea, Ctxt Eb, long p,
			 const FHEPubKey& publicKey, const FHESecKey& secretKey)
{
	int quotient = 0;
	bool flag = true;
	while (flag)
	{
		Ctxt ctSub = FHE_Sub(Ea, Eb, publicKey);
		ZZX ptSub;
		secretKey.Decrypt(ptSub, ctSub);
		long sub;
		conv(sub, ptSub[0]);
		if (sub <= p/2)
		{
			Ea = ctSub;
			quotient ++;
		}
		if (sub >= p/2)
			flag = false;
	}
	Ctxt ctDiv(publicKey);
	publicKey.Encrypt(ctDiv, to_ZZX(quotient));
	return ctDiv;
}

int main()
{
	long m = 0;    // 确定系数
	long p = 1021; // 2^64
	long r = 1;
	long L = 16;
	long c = 3;
	long w = 64;
	long d = 0;
	long k = 128;
	long s = 0;

	m = FindM(k, L, c, p, d, s, 0);

	FHEcontext context(m, p, r);
	buildModChain(context, L, c);

	ZZX G = context.alMod.getFactorsOverZZ()[0];

	FHESecKey secretKey(context);
	const FHEPubKey& publicKey = secretKey;
	secretKey.GenSecKey(w);

	Ctxt Ea(publicKey);
	Ctxt Eb(publicKey);

	Vec<ZZ> h;
	h.SetLength(4);
	h[0]=2;
	h[1]=2;
	h[2]=0;
	h[3]=0;

	publicKey.Encrypt(Ea, to_ZZX(h));
	//publicKey.Encrypt(Eb, to_ZZX(2));

	ZZX ptEa;
	secretKey.Decrypt(ptEa, Ea);
	cout << "ptEa : " << ptEa <<endl;

/*
	ZZX ptSum;
	Ctxt ctSum = FHE_Add(Ea, Eb);
	secretKey.Decrypt(ptSum, ctSum);
	cout << "ptSum : " << ptSum <<endl;

	ZZX ptMul;
	Ctxt ctMul = FHE_Mul(Ea, Eb);
	secretKey.Decrypt(ptMul, ctMul);
	cout << "ptMul : " << ptMul <<endl;

	ZZX ptSub;
	secretKey.Decrypt(ptSub, FHE_Sub(Ea, Eb, publicKey));
	cout << "ptSub : " << ptSub <<endl;

	ZZX ptDiv;
	secretKey.Decrypt(ptDiv, FHE_Div(Ea, Eb, p, publicKey, secretKey));
	cout << "ptDiv : " << ptDiv <<endl;
*/
	return 0;
}


//
// Created by bobin on 17-11-9.
//

#include "PoseEstimate.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>
#include "opencv2/calib3d/calib3d.hpp"

using namespace cv;
using namespace std;

PoseEstimate::PoseEstimate(float _max_trans, float _max_rot) {
    last_r = cv::Mat::zeros(1, 3, CV_64F);
    last_t = cv::Mat::zeros(1, 3, CV_64F);
    max_trans = _max_trans;
    max_rot = _max_rot;
}

float PoseEstimate::pose_distance(const cv::Mat &r, const cv::Mat &t, float &delta_rot, float &delta_trans) {
    delta_trans = cv::norm(t - last_t);
    delta_rot = cv::norm(r - last_r);

}

void PoseEstimate::object_points(int numGridX, int numGridY,
                   float GridWidth, float GridHeight,
                   float GridX, float GridY,
                   int num_ids) {
    int cnt = 0;;
    GridPointXY.resize(num_ids);
    for (int i = 0; i < numGridY; ++i) {
        vector<Point3f> rowGridXY;
        rowGridXY.resize(4);
        for (int j = 0; j < numGridX; ++j) {
            rowGridXY[0] = Point3f(j * GridX, i * GridY, 0);
            rowGridXY[1] = Point3f(j * GridX + GridWidth, i * GridY, 0);
            rowGridXY[2] = Point3f(j * GridX, i * GridY + GridHeight, 0);
            rowGridXY[3] = Point3f(j * GridX + GridWidth, i * GridY + GridHeight, 0);
            GridPointXY[cnt] = rowGridXY;
            cnt++;
            if(cnt >= num_ids)
                break;
        }

    }
}

void PoseEstimate::set_pose(const cv::Mat &r, const cv::Mat &t) {
    unique_lock<mutex> lock(rotMutex);
    last_r = r;
    last_t = t;
}

void PoseEstimate::get_pose(cv::Mat &pose) {
    unique_lock<mutex> lock(rotMutex);
    cv::Mat R, t;
    cv::Rodrigues(last_r, R);
    t = last_t;

    R.copyTo(pose.rowRange(0, 3).colRange(0, 3));
    t.copyTo(pose.rowRange(0, 3).col(3));
}

void PoseEstimate::estimate(vector<Point3f> pts_3d,
                            vector<Point2f> pts_2d,
                            bool check_last
                            ) {
    cv::Mat r, t, R;
    solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法

//    unique_lock<mutex> lock(rotMutex);
    float delta_r, delta_t;
    if (check_last){
        pose_distance(r, t, delta_r, delta_t);
        if ((delta_r > max_rot) || (delta_t > max_trans)) {
            r = last_r;
            t = last_t;
        }
    }
    cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
//    cout << "R=" << endl << r << endl;
//    cout << "t=" << endl << t << endl;
    PoseEstimate::bundleAdjustment(pts_3d, pts_2d, K, R, t);
    cv::Rodrigues(R, r); // r为旋转向量形式，用Rodrigues公式转换为矩阵
    set_pose(r, t);

}

void PoseEstimate::bundleAdjustment(
        const vector<Point3f> points_3d,
        const vector<Point2f> points_2d,
        const Mat &K,
        Mat &R, Mat &t) {
    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> > Block;  // pose维度为 6, landmark 维度为 3
    Block::LinearSolverType *linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block *solver_ptr = new Block(linearSolver);     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // vertex
    g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
            R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
            R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2);
    pose->setId(0);
    pose->setEstimate(g2o::SE3Quat(
            R_mat,
            Eigen::Vector3d(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0))
    ));
    optimizer.addVertex(pose);

    int index = 1;
    for (const Point3f p:points_3d)   // landmarks
    {
        g2o::VertexSBAPointXYZ *point = new g2o::VertexSBAPointXYZ();
        point->setId(index++);
        point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));
        point->setMarginalized(true);
        optimizer.addVertex(point);
    }

    // parameter: camera intrinsics
    g2o::CameraParameters *camera = new g2o::CameraParameters(
            K.at<double>(0, 0), Eigen::Vector2d(K.at<double>(0, 2), K.at<double>(1, 2)), 0
    );
    camera->setId(0);
    optimizer.addParameter(camera);

    // edges
    index = 1;
    for (const Point2f p:points_2d) {
        g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId(index);
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *> ( optimizer.vertex(index)));
        edge->setVertex(1, pose);
        edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
        edge->setParameterId(0, 0);
        edge->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(edge);
        index++;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
//    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
//    cout << "optimization costs time: " << time_used.count() << " seconds." << endl;

//    cout << endl << "after optimization:" << endl;
//    cout << "T=" << endl << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
}


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_MATRIX_SVD_H__
#define __SOT_MATRIX_SVD_H__

/* --- Matrix --- */
#include <Eigen/SVD>
#include <dynamic-graph/linear-algebra.h>

namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace Eigen {

void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
                   const double threshold = 1e-6);

void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix,
                   const double threshold = 1e-6);

void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
                   dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref,
                   const double threshold = 1e-6);

void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
                   const double threshold = 1e-6);

} // namespace Eigen

#endif /* #ifndef __SOT_MATRIX_SVD_H__ */


/**TODO:  Add copyright*/

#define BOOST_TEST_MODULE ModelInterpreterFile test suite
#include <boost/test/included/unit_test.hpp>
#include <EvoNet/io/ModelInterpreterFileDefaultDevice.h>
#include <EvoNet/ml/ModelInterpreterDefaultDevice.h>

using namespace EvoNet;
using namespace std;

BOOST_AUTO_TEST_SUITE(ModelInterpreterFile1)

Model<float> makeModel1()
{
	/**
	* Directed Acyclic Graph Toy Network Model
	*/
	Node<float> i1, i2, h1, h2, o1, o2, b1, b2;
	Link l1, l2, l3, l4, lb1, lb2, l5, l6, l7, l8, lb3, lb4;
	Weight<float> w1, w2, w3, w4, wb1, wb2, w5, w6, w7, w8, wb3, wb4;
	Model<float> model1;

	// Toy network: 1 hidden layer, fully connected, DAG
	i1 = Node<float>("0", NodeType::input, NodeStatus::activated, std::make_shared<LinearOp<float>>(LinearOp<float>()), std::make_shared<LinearGradOp<float>>(LinearGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	i2 = Node<float>("1", NodeType::input, NodeStatus::activated, std::make_shared<LinearOp<float>>(LinearOp<float>()), std::make_shared<LinearGradOp<float>>(LinearGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	h1 = Node<float>("2", NodeType::hidden, NodeStatus::deactivated, std::make_shared<ReLUOp<float>>(ReLUOp<float>()), std::make_shared<ReLUGradOp<float>>(ReLUGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	h2 = Node<float>("3", NodeType::hidden, NodeStatus::deactivated, std::make_shared<ReLUOp<float>>(ReLUOp<float>()), std::make_shared<ReLUGradOp<float>>(ReLUGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	o1 = Node<float>("4", NodeType::output, NodeStatus::deactivated, std::make_shared<ReLUOp<float>>(ReLUOp<float>()), std::make_shared<ReLUGradOp<float>>(ReLUGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	o2 = Node<float>("5", NodeType::output, NodeStatus::deactivated, std::make_shared<ReLUOp<float>>(ReLUOp<float>()), std::make_shared<ReLUGradOp<float>>(ReLUGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	b1 = Node<float>("6", NodeType::bias, NodeStatus::activated, std::make_shared<LinearOp<float>>(LinearOp<float>()), std::make_shared<LinearGradOp<float>>(LinearGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	b2 = Node<float>("7", NodeType::bias, NodeStatus::activated, std::make_shared<LinearOp<float>>(LinearOp<float>()), std::make_shared<LinearGradOp<float>>(LinearGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));

	// weights
	std::shared_ptr<WeightInitOp<float>> weight_init;
	std::shared_ptr<SolverOp<float>> solver;
	// weight_init.reset(new RandWeightInitOp(1.0)); // No random init for testing
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w1 = Weight<float>("0", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w2 = Weight<float>("1", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w3 = Weight<float>("2", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w4 = Weight<float>("3", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	wb1 = Weight<float>("4", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	wb2 = Weight<float>("5", weight_init, solver);
	// input layer + bias
	l1 = Link("0", "0", "2", "0");
	l2 = Link("1", "0", "3", "1");
	l3 = Link("2", "1", "2", "2");
	l4 = Link("3", "1", "3", "3");
	lb1 = Link("4", "6", "2", "4");
	lb2 = Link("5", "6", "3", "5");
	// weights
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w5 = Weight<float>("6", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w6 = Weight<float>("7", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w7 = Weight<float>("8", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w8 = Weight<float>("9", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	wb3 = Weight<float>("10", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	wb4 = Weight<float>("11", weight_init, solver);
	// hidden layer + bias
	l5 = Link("6", "2", "4", "6");
	l6 = Link("7", "2", "5", "7");
	l7 = Link("8", "3", "4", "8");
	l8 = Link("9", "3", "5", "9");
	lb3 = Link("10", "7", "4", "10");
	lb4 = Link("11", "7", "5", "11");
	model1.setId(1);
	model1.setName("1");
	model1.addNodes({ i1, i2, h1, h2, o1, o2, b1, b2 });
	model1.addWeights({ w1, w2, w3, w4, wb1, wb2, w5, w6, w7, w8, wb3, wb4 });
	model1.addLinks({ l1, l2, l3, l4, lb1, lb2, l5, l6, l7, l8, lb3, lb4 });
  model1.setInputAndOutputNodes();
	return model1;
}

BOOST_AUTO_TEST_CASE(constructor)
{
  ModelInterpreterFileDefaultDevice<float>* ptr = nullptr;
	ModelInterpreterFileDefaultDevice<float>* nullPointer = nullptr;
  ptr = new ModelInterpreterFileDefaultDevice<float>();
  BOOST_CHECK_NE(ptr, nullPointer);
}

BOOST_AUTO_TEST_CASE(destructor)
{
	ModelInterpreterFileDefaultDevice<float>* ptr = nullptr;
	ptr = new ModelInterpreterFileDefaultDevice<float>();
  delete ptr;
}

Model<float> model1 = makeModel1();
BOOST_AUTO_TEST_CASE(loadModelBinary1)
{
	ModelInterpreterFileDefaultDevice<float> data;

	// START: model_interpreter test taken from ModelinterpreterCpu_test
	ModelInterpreterDefaultDevice<float> model_interpreter;
	const int batch_size = 4;
	const int memory_size = 1;
	const bool train = true;

	// compile the graph into a set of operations and allocate all tensors
	model_interpreter.getForwardPropogationOperations(model1, batch_size, memory_size, train, false, true, true);

	// Store the model interpreter
	std::string filename = "ModelInterpreterFileTest.binary";
	data.storeModelInterpreterBinary(filename, model_interpreter);

	// Read in the test model_interpreter
	ModelInterpreterDefaultDevice<float> model_interpreter_test;
	data.loadModelInterpreterBinary(filename, model_interpreter_test);

	BOOST_CHECK(model_interpreter_test.getTensorOpsSteps() == model_interpreter.getTensorOpsSteps());
	BOOST_CHECK_EQUAL(model_interpreter_test.getModelResources().size(), model_interpreter.getModelResources().size());
	//BOOST_CHECK(model_interpreter_test.getModelResources() == model_interpreter.getModelResources());
}

Model<float> model2 = makeModel1();
BOOST_AUTO_TEST_CASE(loadModelBinary2)
{
	ModelInterpreterFileDefaultDevice<float> data;

	// START: model_interpreter test taken from ModelinterpreterCpu_test
	ModelInterpreterDefaultDevice<float> model_interpreter;
	const int batch_size = 4;
	const int memory_size = 1;
	const bool train = true;

	// update the model solver
	std::shared_ptr<SolverOp<float>> solver(new AdamOp<float>(0.001, 0.9, 0.999, 1e-8));
	for (auto& weight_map : model2.getWeightsMap()) {
		if (weight_map.second->getSolverOpShared()->getName() == "SGDOp")
			weight_map.second->setSolverOp(solver);
	}

	// compile the graph into a set of operations and allocate all tensors
	model_interpreter.getForwardPropogationOperations(model2, batch_size, memory_size, train, false, true, true);
	model_interpreter.allocateModelErrorTensor(batch_size, memory_size, 0);

	// create the input
	const std::vector<std::string> node_ids = { "0", "1" };
	Eigen::Tensor<float, 3> input(batch_size, memory_size, (int)node_ids.size());
	input.setValues({
		{{1, 5}},
		{{2, 6}},
		{{3, 7}},
		{{4, 8}} });

	// create the expected output
	std::vector<std::string> output_nodes = { "4", "5" };
	Eigen::Tensor<float, 2> expected(batch_size, (int)output_nodes.size());
	expected.setValues({ {0, 1}, {0, 1}, {0, 1}, {0, 1} });
	LossFunctionTensorOp<float, Eigen::DefaultDevice>* loss_function = new MSELossTensorOp<float, Eigen::DefaultDevice>();
	LossFunctionGradTensorOp<float, Eigen::DefaultDevice>* loss_function_grad = new MSELossGradTensorOp<float, Eigen::DefaultDevice>();
	const int layer_id = model2.getNode("4").getTensorIndex().first;

	// iterate until we find the optimal values
	const int max_iter = 20;
	for (int iter = 0; iter < max_iter; ++iter)
	{
		// assign the input data
		model_interpreter.mapValuesToLayers(model2, input, node_ids, "output");
		model_interpreter.initBiases(model2); // create the bias

		model_interpreter.executeForwardPropogationOperations(0); //FP

		// calculate the model error and node output error
		model_interpreter.executeModelErrorOperations(expected, layer_id, loss_function, loss_function_grad, 0);
		std::cout << "Error at iteration: " << iter << " is " << model_interpreter.getModelError()->getError().sum() << std::endl;

		model_interpreter.executeBackwardPropogationOperations(0); // BP
		model_interpreter.executeWeightErrorOperations(); // Weight error
		model_interpreter.executeWeightUpdateOperations(0); // Weight update

		// reinitialize the model
		if (iter != max_iter - 1) {
			model_interpreter.reInitNodes();
			model_interpreter.reInitModelError();
		}
	}
	const Eigen::Tensor<float, 0> total_error = model_interpreter.getModelError()->getError().sum();
	BOOST_CHECK(total_error(0) <= 757.0);
	// END: model_interpreter test taken from ModelinterpreterCpu_test

	// Store the model interpreter
	std::string filename = "ModelInterpreterFileTest.binary";
	data.storeModelInterpreterBinary(filename, model_interpreter);

	// Read in the test model_interpreter
	ModelInterpreterDefaultDevice<float> model_interpreter_test;
	data.loadModelInterpreterBinary(filename, model_interpreter_test);

	// Test for the expected model_interpreter operations
	model_interpreter.getModelResults(model2, true, true, true, true);
	model_interpreter.clear_cache();

	// Compile the graph into a set of operations and allocate all tensors
	model_interpreter_test.getForwardPropogationOperations(model2, batch_size, memory_size, train, false, true, true);
	model_interpreter_test.allocateModelErrorTensor(batch_size, memory_size, 0);

	BOOST_CHECK(model_interpreter_test == model_interpreter);  // Trivial comparison; instead we use the following from `ModelInterpreterCpu_test.cpp`

	// RE-START: model_interpreter test taken from ModelinterpreterCpu_test
	// iterate until we find the optimal values
	for (int iter = 0; iter < max_iter; ++iter)
	{
		// assign the input data
		model_interpreter_test.mapValuesToLayers(model2, input, node_ids, "output");
		model_interpreter_test.initBiases(model2); // create the bias

		model_interpreter_test.executeForwardPropogationOperations(0); //FP

		// calculate the model error and node output error
		model_interpreter_test.executeModelErrorOperations(expected, layer_id, loss_function, loss_function_grad, 0);
		std::cout << "Error at iteration: " << iter << " is " << model_interpreter_test.getModelError()->getError().sum() << std::endl;

		model_interpreter_test.executeBackwardPropogationOperations(0); // BP
		model_interpreter_test.executeWeightErrorOperations(); // Weight error
		model_interpreter_test.executeWeightUpdateOperations(0); // Weight update

		// reinitialize the model
		if (iter != max_iter - 1) {
			model_interpreter_test.reInitNodes();
			model_interpreter_test.reInitModelError();
		}
	}

	const Eigen::Tensor<float, 0> total_error_test = model_interpreter_test.getModelError()->getError().sum();
	BOOST_CHECK(total_error_test(0) <= 757.0);
	// END RE-START: model_interpreter test taken from ModelinterpreterCpu_test
}

Model<float> model3 = makeModel1();
BOOST_AUTO_TEST_CASE(loadModelCsv1)
{
  ModelInterpreterFileDefaultDevice<float> data;

  // START: model_interpreter test taken from ModelinterpreterCpu_test
  ModelInterpreterDefaultDevice<float> model_interpreter;
  const int batch_size = 4;
  const int memory_size = 1;
  const bool train = true;

  // compile the graph into a set of operations and allocate all tensors
  model_interpreter.getForwardPropogationOperations(model3, batch_size, memory_size, train, false, true, true);

  // Store the model interpreter
  std::string filename = "ModelInterpreterFileTest.csv";
  data.storeModelInterpreterCsv(filename, model_interpreter);

  // NO TESTS
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <ctime>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <imgproc/derivative_gradient.hpp>
#define DISABLE_DC_ZERO_FIX
#include <imgproc/quadratureG2.hpp>
#include <imgproc/quadratureS.hpp>
#include <imgproc/laplace.hpp>
#include <dlib/optimization.h>

using namespace std;
using namespace lsfm;
using namespace cv;

void showGradient(const std::string &name,const cv::Mat &mag, double mul = 1) {
    double vmin,vmax;
    cv::minMaxIdx(mag,&vmin,&vmax);
    mag -= vmin;
    mag /= vmax - vmin;
    mag *= mul;
    imshow("gradient " + name,mag);
}

double d_lower = 0.1, d_upper = 10, start = 1;
cv::Mat white(18, 18, CV_64F, 1);

template <
    class GRAD,
    class search_strategy_type = dlib::bfgs_search_strategy,
    class stop_strategy_type = dlib::objective_delta_stop_strategy
>
double optimizeGradKernel(GRAD& grad,
    double derivative_prec = 1e-7, search_strategy_type search = dlib::bfgs_search_strategy(),
    stop_strategy_type stop = dlib::objective_delta_stop_strategy(1e-7)) {

    typedef dlib::matrix<double, 0, 1> column_vector;

    auto eval = [&](const column_vector& v) -> double {
        grad.kernelSpacing(v(0));
        return std::abs(cv::sum(grad.kernel())[0]);
    };

    grad.kernelSpacing(start);
    column_vector starting_point(1), lower(1), upper(1);
    starting_point = start;
    lower = d_lower;
    upper = d_upper;
    return dlib::find_min_box_constrained(search, stop,
        eval, dlib::derivative(eval, derivative_prec), starting_point, lower, upper);

}

template <
    class GRAD,
    class search_strategy_type = dlib::bfgs_search_strategy,
    class stop_strategy_type = dlib::objective_delta_stop_strategy
>
double optimizeGradKernel2( GRAD& grad,
    double derivative_prec = 1e-7, search_strategy_type search = dlib::bfgs_search_strategy(),
    stop_strategy_type stop = dlib::objective_delta_stop_strategy(1e-7)) {

    typedef dlib::matrix<double, 0, 1> column_vector;

    auto eval = [&](const column_vector& v) -> double {
        grad.kernelSpacing(v(0));
        grad.process(white);
        cv::Mat mag = grad.even();
        return std::abs(mag.at<double>(0, 0));
    };

    grad.kernelSpacing(start);
    column_vector starting_point(1), lower(1), upper(1);
    starting_point = start;
    lower = d_lower;
    upper = d_upper;
    return dlib::find_min_box_constrained(search, stop,
        eval, dlib::derivative(eval, derivative_prec), starting_point, lower, upper);

}

int main(int argc, char** argv)
{
    const char* filename = argc >= 2 ? argv[1] : "../../images/circle2.png";
    //const char* filename = argc >= 2 ? argv[1] : "../../images/bike.png";
    //const char* filename = argc >= 2 ? argv[1] : "../../images/office1_low.JPG";

    cv::Mat src = cv::imread(filename, IMREAD_GRAYSCALE);
    if (src.empty())
    {
        cout << "Can not open " << filename << endl;
        return -1;
    }

    GaussianBlur(src, src, cv::Size(3, 3),0.6);
    typedef double FT;

    QuadratureG2<uchar, FT> quad3(3, 1.240080);
	QuadratureG2<uchar, FT> quad5(5, 1.008000);
    QuadratureG2<uchar, FT> quad7(7, 0.873226);
    QuadratureG2<uchar, FT> quad9(9, 0.781854);

    quad3.process(src);
    showGradient("Quad3 -", Mat(abs(quad3.even())));

    quad5.process(src);
    showGradient("Quad5 -", Mat(abs(quad5.even())));

    quad7.process(src);
    showGradient("Quad7 -", Mat(abs(quad7.even())));

    quad9.process(src);
    showGradient("Quad9 -", Mat(abs(quad9.even())));

    cv::waitKey();

    QuadratureS<uchar, FT, FT> quadS3(1, 2, 3, 1);
    QuadratureS<uchar, FT, FT> quadS5(1, 2, 5, 1);
    QuadratureS<uchar, FT, FT> quadS7(1, 2, 7, 1);
    QuadratureS<uchar, FT, FT> quadS9(1, 2, 9, 1);

    LoG<uchar, FT> log3(3, 1);
    LoG<uchar, FT> log5(5, 1);
    LoG<uchar, FT> log7(7, 1);
    LoG<uchar, FT> log9(9, 1);
    LoG<uchar, FT> log11(11, 1);
    LoG<uchar, FT> log15(15, 1);
    LoG<uchar, FT> log25(25, 1);
    LoG<uchar, FT> log75(75, 1);
    LoG<uchar, FT> log125(125, 1);

    double e;
    e = optimizeGradKernel(log3);
    std::cout << "LoG3 - error: " << e << ", spacing: " << log3.kernelSpacing() << std::endl;
    //std::cout << log3.kernel() << std::endl;

    e = optimizeGradKernel(log5);
    std::cout << "LoG5 - error: " << e << ", spacing: " << log5.kernelSpacing() << std::endl;
    //std::cout << log5.kernel() << std::endl;

    e = optimizeGradKernel(log7);
    std::cout << "LoG7 - error: " << e << ", spacing: " << log7.kernelSpacing() << std::endl;
    //std::cout << log7.kernel() << std::endl;

    e = optimizeGradKernel(log9);
    std::cout << "LoG9 - error: " << e << ", spacing: " << log9.kernelSpacing() << std::endl;
    //std::cout << log9.kernel() << std::endl;

    e = optimizeGradKernel2(log3);
    std::cout << "LoG3 - error2: " << e << ", spacing: " << log3.kernelSpacing() << std::endl;
    //std::cout << log3.kernel() << std::endl;

    e = optimizeGradKernel2(log5);
    std::cout << "LoG5 - error2: " << e << ", spacing: " << log5.kernelSpacing() << std::endl;
    //std::cout << log5.kernel() << std::endl;

    e = optimizeGradKernel2(log7);
    std::cout << "LoG7 - error2: " << e << ", spacing: " << log7.kernelSpacing() << std::endl;
    //std::cout << log7.kernel() << std::endl;

    e = optimizeGradKernel2(log9);
    std::cout << "LoG9 - error2: " << e << ", spacing: " << log9.kernelSpacing() << std::endl;
    //std::cout << log9.kernel() << std::endl;

    e = optimizeGradKernel2(log11);
    std::cout << "LoG11 - error2: " << e << ", spacing: " << log11.kernelSpacing() << std::endl;
    //std::cout << log11.kernel() << std::endl;

    e = optimizeGradKernel2(log15);
    std::cout << "LoG15 - error2: " << e << ", spacing: " << log15.kernelSpacing() << std::endl;
    //std::cout << log15.kernel() << std::endl;

    e = optimizeGradKernel2(log25);
    std::cout << "LoG25 - error2: " << e << ", spacing: " << log25.kernelSpacing() << std::endl;
    //std::cout << log25.kernel() << std::endl;

    e = optimizeGradKernel2(log75);
    std::cout << "LoG75 - error2: " << e << ", spacing: " << log75.kernelSpacing() << std::endl;
    //std::cout << log75.kernel() << std::endl;

    e = optimizeGradKernel2(log125);
    std::cout << "LoG125 - error2: " << e << ", spacing: " << log125.kernelSpacing() << std::endl;
    //std::cout << log25.kernel() << std::endl;

    /*log3.process(src);
    showGradient("LoG3 -", log3.laplace());

    log5.process(src);
    showGradient("LoG5 -", log5.laplace());

    log7.process(src);
    showGradient("LoG7 -", log7.laplace());

    log9.process(src);
    showGradient("LoG9 -", log9.laplace());

    cv::waitKey();*/

    e = optimizeGradKernel2(quad3);
    std::cout << "Quad3 - error: " << e << ", spacing: " << quad3.kernelSpacing() << std::endl;

    e = optimizeGradKernel2(quad5);
    std::cout << "Quad5 - error: " << e << ", spacing: " << quad5.kernelSpacing() << std::endl;

    e = optimizeGradKernel2(quad7);
    std::cout << "Quad7 - error: " << e << ", spacing: " << quad7.kernelSpacing() << std::endl;

    e = optimizeGradKernel2(quad9);
    std::cout << "Quad9 - error: " << e << ", spacing: " << quad9.kernelSpacing() << std::endl;

    quad3.process(src);
    showGradient("Quad3 -", Mat(abs(quad3.even())));

    quad5.process(src);
    showGradient("Quad5 -", Mat(abs(quad5.even())));

    quad7.process(src);
    showGradient("Quad7 -", Mat(abs(quad7.even())));

    quad9.process(src);
    showGradient("Quad9 -", Mat(abs(quad9.even())));

    cv::waitKey();

    e = optimizeGradKernel(quadS3);
    std::cout << "QuadS3 - error: " << e << ", spacing: " << quadS3.kernelSpacing() << ", scale: " << quadS3.scale() << ", muls: " << quadS3.muls() << std::endl;
    //std::cout << quadS3.kernel() << std::endl;

    e = optimizeGradKernel(quadS5);
    std::cout << "QuadS5 - error: " << e << ", spacing: " << quadS5.kernelSpacing() << ", scale: " << quadS5.scale() << ", muls: " << quadS5.muls() << std::endl;
    //std::cout << quadS5.kernel() << std::endl;

    e = optimizeGradKernel(quadS7);
    std::cout << "QuadS7 - error: " << e << ", spacing: " << quadS7.kernelSpacing() << ", scale: " << quadS7.scale() << ", muls: " << quadS7.muls() << std::endl;
    //std::cout << quadS7.kernel() << std::endl;

    e = optimizeGradKernel(quadS9);
    std::cout << "QuadS9 - error: " << e << ", spacing: " << quadS9.kernelSpacing() << ", scale: " << quadS9.scale() << ", muls: " << quadS9.muls() << std::endl;
    //std::cout << quadS9.kernel() << std::endl;

    e = optimizeGradKernel2(quadS3);
    std::cout << "QuadS3 - error2: " << e << ", spacing: " << quadS3.kernelSpacing() << ", scale: " << quadS3.scale() << ", muls: " << quadS3.muls() << std::endl;
    //std::cout << quadS3.kernel() << std::endl;

    e = optimizeGradKernel2(quadS5);
    std::cout << "QuadS5 - error2: " << e << ", spacing: " << quadS5.kernelSpacing() << ", scale: " << quadS5.scale() << ", muls: " << quadS5.muls() << std::endl;
    //std::cout << quadS5.kernel() << std::endl;

    e = optimizeGradKernel2(quadS7);
    std::cout << "QuadS7 - error2: " << e << ", spacing: " << quadS7.kernelSpacing() << ", scale: " << quadS7.scale() << ", muls: " << quadS7.muls() << std::endl;
    //std::cout << quadS7.kernel() << std::endl;

    e = optimizeGradKernel2(quadS9);
    std::cout << "QuadS9 - error2: " << e << ", spacing: " << quadS9.kernelSpacing() << ", scale: " << quadS9.scale() << ", muls: " << quadS9.muls() << std::endl;
    //std::cout << quadS9.kernel() << std::endl;

    /*quadS3.process(src);
    showGradient("QuadS3 -", quadS3.even());

    quadS5.process(src);
    showGradient("QuadS5 -", quadS5.even());

    quadS7.process(src);
    showGradient("QuadS7 -", quadS7.even());

    quadS9.process(src);
    showGradient("QuadS9 -", quadS9.even());

    cv::waitKey();*/

    /*QuadratureS<uchar, FT, FT> quadS5(1.0, 2.0, 5, 1);
    QuadratureS<uchar, FT, FT> quadS7(1.0, 2.0, 7, 1);
    QuadratureS<uchar, FT, FT> quadS9(1.0, 2.0, 9, 0.1);

    quadS5.process(src);
    quadS7.process(src);
    quadS9.process(src);

    //showGradient("QuadS5 -", quadS5.laplace(),5);
    //showGradient("QuadS7 -", quadS7.laplace(),5);
    showGradient("QuadS9 -", quadS9.laplace(),5);

    //showGradient("QuadS5 m-", quadS5.magnitude());
    //showGradient("QuadS7 m-", quadS7.magnitude());
    showGradient("QuadS9 m-", quadS9.magnitude());

    //showGradient("QuadS5 lm-", quadS5.localMagnitude());
    //showGradient("QuadS7 lm-", quadS7.localMagnitude());
    showGradient("QuadS9 lm-", quadS9.localMagnitude());

    //showGradient("QuadS5 e-", quadS5.even());
    //showGradient("QuadS7 e-", quadS7.even());
    showGradient("QuadS9 e-", quadS9.even());
    cv::waitKey();*/

    return 0;
}


#pragma once
#include "math.h"
#include "math.h"
#include <tuple>
#include "integration.hpp"
//#include "datatypes.hpp"
#include <boost/python.hpp>
#include <boost/math/special_functions.hpp>
#include "gsl/gsl_sf_bessel.h"

namespace gd {
using namespace std;

void py_export_profile();

class Density {
public:
	virtual double densityr(double R) = 0;
};

class Profile : public Density {
public:
	virtual double densityr(double r) = 0;
	virtual double densityR(double R) = 0;
	virtual double I(double r, double I0=1.0) = 0;
	virtual double dphidr(double r) = 0;
	virtual double potentialr(double r) = 0;
	double dphidx2(double x, double y) {
		double r = sqrt(x*x+y*y);
		return dphidr(r) * x / r;
	}
	double dphidy2(double x, double y) {
		double r = sqrt(x*x+y*y);
		return dphidr(r) * y / r;
	}
	std::tuple<double,double> dphidxy(double x, double y) {
		double r = sqrt(x*x+y*y);
		double F = dphidr(r);
		return std::make_tuple(F*x/r, F*y/r);
	};

	double enclosed_mass(double r) {
		auto dmass = [this](double rp){ return this->densityr(rp) * rp*rp * 4 * M_PI; };
		IntegratorGSL<> integratorGSL(dmass); // the integrator
		double integral = integratorGSL.integrate(0,r);
		return integral;
	}
	double total_mass() {
		auto dmass = [this](double rp){ return this->densityr(rp) * rp*rp * 4 * M_PI; };
		IntegratorGSL<> integratorGSL(dmass); // the integrator
		double integral = integratorGSL.integrate_to_inf(0);
		return integral;
	}
};

class ProfileModel {
public:
	virtual double densityr(double r) = 0;
	virtual double densityR(double r) = 0;
	virtual double dphidr(double r) = 0;
	virtual double potentialr(double r) = 0;
	boost::python::tuple get_apo_peri(double E, double L, double rmin, double rcirc, double rmax);
	boost::python::tuple Lmax_and_rcirc_at_E_(double E);
	void Lmax_and_rcirc_at_E(double E, double& Lmax, double& rcirc);
	double Lmax_at_E(double E);
	double rcirc_at_E(double E);
	double rmax_at_E(double E, double rcirc);
};

class ProfileModel1C : public ProfileModel {
public:
	ProfileModel1C(Profile* p) : p(p) {}
	virtual double densityr(double r) { return p->densityr(r); }
	virtual double densityR(double r)  { return p->densityR(r); }
	virtual double dphidr(double r) { return p->dphidr(r); }
	virtual double potentialr(double r) { return p->potentialr(r); }
	Profile* p;
};

class ProfileModel2C : public ProfileModel {
public:
	ProfileModel2C(Profile* p1, Profile* p2) : p1(p1), p2(p2) {}
	virtual double densityr(double r) { return p1->densityr(r) + p2->densityr(r); }
	virtual double densityR(double r)  { return p1->densityR(r); }
	virtual double dphidr(double r) { return p1->dphidr(r) + p2->dphidr(r); }
	virtual double potentialr(double r) { return p1->potentialr(r) + p2->potentialr(r); }
	Profile *p1, *p2;
};

class Plummer : public Profile {
public:
	Plummer(double mass, double scale, double G) : mass(mass), scale(scale), G(G) {
	}
	double densityr(double r) {
		return 3 * mass * scale * scale / (4*M_PI) / pow((r*r + scale*scale), (5./2));
	}
	double densityR(double R) {
		double a = (scale*scale+R*R);
		return mass * scale*scale / (M_PI * a*a);
	}
	double I(double R, double I0=1.0) {
		double a = (scale*scale+R*R);
		return I0 * scale*scale / (M_PI * a*a);
	}
	double dphidr(double r) {
		return G * mass * r / pow((r*r + scale*scale), (3./2));
	}
	double potentialr(double r) {
		return - G * mass / sqrt(r*r + scale*scale);
	}
	/*double_vector dphidr2(double_vector r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		return r * 2;
	}*/
	double mass, scale, G;
};
/*
class ProjectedExponential(Potential):
	def __init__(self, M, scale, G=G):
		self.M = M
		self.scale = scale
		self.rho0 = 1.
		print self.scale
		Mcurrent = self.enclosed_mass(inf)
		self.rho0 = M/Mcurrent
		self._fast = mab.gdfast.ProjectedExponential(M, scale, G)

		#2 \[Pi] Rs (Rs - E^(-(r/Rs)) (r + Rs))

	def densityR(self, r):
		return exp(-r/self.scale) * self.rho0

	def densityr(self, r):
		# kn is the (modified) bessel of second kind (integer order = 0)
		return self.rho0 * scipy.special.kn(0., r) / (self.scale * pi)

	def potentialr(self, r):
		return 0

	def dphidr(self, r):
		return 0
*/
class ProjectedExponential : public Profile {
public:
	ProjectedExponential(double mass, double scale, double G) : mass(mass), scale(scale), G(G) {
		rho0 = 1;
		double current_mass = this->total_mass();
		rho0 *= mass/current_mass;
	}
	double densityr(double r) {
		return rho0 * boost::math::cyl_bessel_k(0, r/scale)  / (scale * M_PI);
	}
	double densityR(double R) {
		return rho0 * exp(-R/scale);
	}
	double I(double R, double I0=1.0) {
		return 0;
	}
	double dphidr(double r) {
		return 0;
	}
	double potentialr(double r) {
		return 0;
	}
	/*double_vector dphidr2(double_vector r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		return r * 2;
	}*/
	double mass, scale, G, rho0;
};

class TestCase : public Profile {
public:
	TestCase(double mass, double scale, double G) : mass(mass), scale(scale), G(G) {
	}
	double densityr(double r) {
		//return 3 * mass * scale * scale / (4*M_PI) / pow((r*r + scale*scale), (5./2));
		return 3 * pow(1+pow(r/20,2), -5./2);
	}
	double densityR(double R) {
		double a = (scale*scale+R*R);
		return mass * scale*scale / (M_PI * a*a);
	}
	double I(double R, double I0=1.0) {
		double a = (scale*scale+R*R);
		return I0 * scale*scale / (M_PI * a*a);
	}
	double dphidr(double r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		return 1/(4*M_PI*2)*(8000*pow(400+r*r, -3./2)*2*r);
	}
	double potentialr(double r) {
		return -1/(4*M_PI)*(8000/sqrt(400+r*r)-178.88);
	}
	/*double_vector dphidr2(double_vector r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		return r * 2;
	}*/
	double mass, scale, G;
};

class Isochrone : public Profile {
public:
	Isochrone(double mass, double scale, double G) : mass(mass), scale(scale), G(G) {
	}
	double densityr(double) {
		return 0;
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double) {
		return 0;
	}
	double potentialr(double) {
		return 0;
	}
	/*double_vector dphidr2(double_vector r) {
		return r * 2;
	}*/
	double mass, scale, G;
};

class LogarithmicProfile : public Profile {
public:
	LogarithmicProfile(double vcirc, double G) : vcirc(vcirc), G(G) {
	}
	double densityr(double) {
		return 0;
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double) {
		return 0;
	}
	double potentialr(double) {
		return 0;
	}
	/*double_vector dphidr2(double_vector r) {
		return r * 2;
	}*/
	double vcirc, G;
};

class NullProfile : public Profile {
public:
	NullProfile() {
	}
	double densityr(double) {
		return 0;
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double) {
		return 0;
	}
	double potentialr(double) {
		return 0;
	}
	/*double_vector dphidr2(double_vector r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		return r*0;
	}*/
};

class Hernquist : public Profile {
public:
	Hernquist(double mass, double scale, double G) : mass(mass), scale(scale), G(G) {
		rho0 = mass / (2*M_PI*scale*scale*scale);
	}
	double potentialr(double r) {
		return - 4 * M_PI * G * rho0 * scale*scale /(2*(1+r/scale));
	}
	double densityr(double r) {
		double m = r/scale;
		double a = (1+m);
		return rho0 / (m * a*a*a);
	}
	double _ddensityR(double r, double R) {
		return 2 * r * densityr(r) / sqrt(r*r-R*R);
	}
	double densityR(double R) {
		/*return -pow(scale,4) * rho0 * (3 * scale * (scale-R)*(scale+R)+sqrt(R*R-scale*scale)*(2*scale*scale+R*R)*acos(scale/R)) / pow(scale*scale-R*R,3);-*/
		//std::tr1::function<double(double)> f(bind(&Hernquist::_ddensityR, this, _1, R));
		auto ddensity = [&R,this](double r){ return 2 * r * this->densityr(r) / sqrt(r*r-R*R); };
		IntegratorGSL<> integratorGSL(ddensity); // the integrator
		double integral = integratorGSL.integrate_to_inf(R);
		return integral;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		double a = (1+r/scale);
		return 4 * M_PI * G * rho0 * scale / (2*a*a);
	}
	double mass, rho0, scale, G;
};

class NFW : public Profile {
public:
	NFW(double mass200, double rs, double G, double rho_crit) : mass200(mass200), rs(rs), G(G) {
		r200 = pow(mass200/200*3/(4*M_PI)/rho_crit, 1./3);
		c = r200/rs;
		double x = r200 / rs;
		rho0 = mass200 / (4*M_PI*pow(rs,3)*(log(1+x)-x/(1+x)));
	}
	double potentialr(double r) {
		double x = r/rs;
		return - 4 * M_PI * G * rho0 * rs*rs * log(1+x)/x;
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (x * pow(1+x, 2));
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double r) {
		double x = r/rs;
		double f = -4.0*M_PI*G*rho0*pow(rs, 3)/pow(r,2);
		double t1 = log(1.0 + x);
		double t2 = x/(1.0+x);
		//cout << x << ", " << f << ", " << t1 << ", " << t2 << endl;
		return -f*(t1 - t2);
	}
	double mass200, rho0, rs, r200, c, G;
};

class Jaffe : public Profile {
public:
	Jaffe(double rho0, double rs, double G) : rho0(rho0), rs(rs), G(G) {
	}
	double potentialr(double r) {
		return - 4 * M_PI * G * rho0 * rs*rs * log(1+rs/r);
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (pow(x, 2) * pow(1+x, 2));
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double r) {
		/*double x = r/rs;
		double f = -4.0*M_PI*G*rho0*pow(rs, 3)/pow(r,2);
		double t1 = log(1.0 + x);
		double t2 = x/(1.0+x);
		//cout << x << ", " << f << ", " << t1 << ", " << t2 << endl;
		return -f*(t1 - t2);*/
		return - 4 * M_PI * G * rho0 * rs*rs * 1./(1.+rs/r) * -rs/(r*r);
	}
	double rho0, rs, G;
};

class NFWCut : public Density {
public:
	NFWCut(double rho0, double rs, double rte) : rho0(rho0), rs(rs), rte(rte) {
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (x * pow(1+x,2)) / (1+pow(r/rte, 3));
	}
	double rho0, rs, rte;
};

class TwoSlopeDensity : public Density {
public:
	TwoSlopeDensity(double rho0, double alpha, double beta, double rs, double gamma=1.) : rho0(rho0), alpha(alpha), beta(beta), rs(rs), gamma(gamma) {
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (pow(x, -alpha) * pow(1+pow(x, gamma), (-beta+alpha)/gamma));
	}
	double rho0, alpha, beta, rs, gamma;
};

class TwoSlopeDensityCut : public Density {
public:
	TwoSlopeDensityCut(double rho0, double alpha, double beta, double rs, double gamma, double rte) : rho0(rho0), alpha(alpha), beta(beta), rs(rs), gamma(gamma), rte(rte) {
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (pow(x, -alpha) * pow(1+pow(x, gamma), (-beta+alpha)/gamma)) / (1+pow(r/rte, 3));
	}
	double rho0, alpha, beta, rs, gamma, rte;
};

class BrokenPowerLawDensitySoft3 : public Density {
public:
	BrokenPowerLawDensitySoft3(double rho0, double s1, double s2, double s3, double gamma1, double gamma2, double rs1, double rs2) : rho0(rho0), s1(s1), s2(s2), s3(s3), gamma1(gamma1), gamma2(gamma2), rs1(rs1), rs2(rs2) {
	}
	double densityr(double r) {
		double x1 = r/rs1;
		double x2 = r/rs2;
		return rho0 * pow(x1, s1) * pow(1+pow(x1, gamma1), (s2-s1)/gamma1) * pow(1+pow(x2, gamma2), (s3-s2)/gamma2);
	}
	double rho0, s1, s2, s3, gamma1, gamma2, rs1, rs2;
};

class Einasto : public Profile {
public:
	Einasto(double rho_2, double rs_2, double alpha, double G) : rho_2(rho_2), rs_2(rs_2), alpha(alpha), G(G) {
	}
	double potentialr(double) {
		return 0;
	}
	double densityr(double r) {
		double x = r/rs_2;
		return rho_2 * exp((-2/alpha)*(pow(x, alpha)-1));
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double) {
		return 0;
	}
	double rho_2, rs_2, alpha, G;
	/*double M(double r) {
		double x = r/rs_2;
		double a = alpha;
		double Rg;
		gsl_sf_result R1, R2;
		gsl_sf_gamma_inc_P_e(3./a, (2*pow(x,a))/a, &Rg)
		//gsl_sf_gamma_inc_P_e(3./a, (2*pow(x,a))/a, &R1)
		double R = Rg.val * gsl_sf_gamma(3./a);
		return 0;
		//return pow(2, (2.-3./a)) * exp(2./a) * M_PI * pow(pow(1/self.r_2,a)/a, -3./a) * rho_2 * R / a;
	}*/
};

class Burkert : public Profile {
public:
	Burkert(double rho, double rs, double G) : rho(rho), rs(rs), G(G) {
	}
	double potentialr(double) {
		return 0;
	}
	double densityr(double r) {
		double x = r/rs;
		return rho / ((1+x) * (1+x*x));
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double) {
		return 0;
	}
	double rho, rs, G;
};

}


//
// Created by Hamza El-Kebir on 4/17/21.
//

#include "Lodestar/analysis/LinearSystemInverse.hpp"
#include "Lodestar/systems/StateSpace.hpp"
#include <Eigen/Dense>

ls::systems::StateSpace<> linearSystemInverseExample()
{
    Eigen::MatrixXd A(3, 3), B(3, 2), C(2, 3), D(2, 2);
    A.block(0, 0, 3, 3) << 1, 2, 0, 4, -1, 0, 0, 0, 1;
    B.block(0, 0, 3, 2) << 1, 0, 0, 1, 1, 0;
    C.block(0, 0, 2, 3) << 0, 1, -1, 0, 0, 1;
    D.block(0, 0, 2, 2) << 4, 0, 0, 1;

    ls::systems::StateSpace<> lti = ls::systems::StateSpace<>(A, B, C, D);
    return lti;
//    ls::systems::StateSpace<> ilti = ls::analysis::LinearSystemInverse::inverse(lti);
//
//    return ilti;
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int a, b = 0, ans = 0; cin >> a;
    while(a > 1) {
        a /= 2, b++;
    }
    for (int i = 0; i <= b; i++) ans += (cpp_int)pow(2, i);
    cout << ans << endl;
}


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>
#include <Eigen/StdVector>

const float MTR_PI = 3.1415926535f;

typedef double scalar;
typedef unsigned long long uint64;
typedef unsigned char uchar;

typedef Eigen::Matrix<scalar, 2, 1> vec2s;
typedef Eigen::Matrix<float, 2, 1> vec2f;
typedef Eigen::Matrix<int, 2, 1> vec2i;
typedef Eigen::Matrix<scalar, 3, 1> vec3s;
typedef Eigen::Matrix<float, 3, 1> vec3f;
typedef Eigen::Matrix<int, 3, 1> vec3i;
typedef Eigen::Matrix<scalar, 4, 1> vec4s;
typedef Eigen::Matrix<float, 4, 1> vec4f;
typedef Eigen::Matrix<int, 4, 1> vec4i;
typedef Eigen::Matrix<float, Eigen::Dynamic, 1> vecNf;

typedef Eigen::Matrix<scalar, 4, 4> mat4s;
typedef Eigen::Matrix<float, 4, 4> mat4f;
typedef Eigen::Matrix<int, 4, 4> mat4i;

float clamp(float min, float max, float a);

float to_degrees(float radian);
float to_radians(float degree);

mat4f perspective(float fovy, float aspect, float near, float far);
mat4f ortho(float left, float right, float bottom, float top, float near,
            float far);
mat4f look_at(const vec3f &eye, const vec3f &center, const vec3f &up);

mat4f translate_mat(float offset_x, float offset_y, float offset_z);
mat4f rotate_mat(const vec3f &pivot, float angle);
mat4f scale_mat(float s_x, float s_y, float s_z);

vec3f barycentric2D(float x, float y, vec4f *v);