#pragma once

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

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// @brief Compute the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param origin The origin of the plane.
/// @param normal The normal of the plane.
/// @return The distance between the point and plane.
template <typename DerivedP, typename DerivedOrigin, typename DerivedNormal>
auto point_plane_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedOrigin>& origin,
    const Eigen::MatrixBase<DerivedNormal>& normal)
{
    auto point_to_plane = (p - origin).dot(normal);
    return point_to_plane * point_to_plane / normal.squaredNorm();
}

/// @brief Compute the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param t0 The first vertex of the triangle.
/// @param t1 The second vertex of the triangle.
/// @param t2 The third vertex of the triangle.
/// @return The distance between the point and plane.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2>
auto point_plane_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    auto normal = cross(t1 - t0, t2 - t0);
    return point_plane_distance(p, t0, normal);
}

// Symbolically generated derivatives;
namespace autogen {
    void point_plane_distance_gradient(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double g[12]);

    void point_plane_distance_hessian(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double H[144]);
} // namespace autogen

/// @brief Compute the gradient of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] origin The origin of the plane.
/// @param[in] normal The normal of the plane.
/// @param[out] grad The gradient of the distance wrt p.
template <
    typename DerivedP,
    typename DerivedOrigin,
    typename DerivedNormal,
    typename DerivedGrad>
void point_plane_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedOrigin>& origin,
    const Eigen::MatrixBase<DerivedNormal>& normal,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    grad = (2 * (p - origin).dot(normal)) / normal.squaredNorm() * normal;
}

/// @brief Compute the gradient of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] t0 The first vertex of the triangle.
/// @param[in] t1 The second vertex of the triangle.
/// @param[in] t2 The third vertex of the triangle.
/// @param[out] grad The gradient of the distance wrt p, t0, t1, and t2.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename DerivedGrad>
void point_plane_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    grad.resize(p.size() + t0.size() + t1.size() + t2.size());
    autogen::point_plane_distance_gradient(
        p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0],
        t2[1], t2[2], grad.data());
}

/// @brief Compute the hessian of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] origin The origin of the plane.
/// @param[in] normal The normal of the plane.
/// @param[out] hess The hessian of the distance wrt p.
template <
    typename DerivedP,
    typename DerivedOrigin,
    typename DerivedNormal,
    typename DerivedHess>
void point_plane_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedOrigin>& origin,
    const Eigen::MatrixBase<DerivedNormal>& normal,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    if (normal.cols() == 1) {
        // (n×1)(n×1)ᵀ = (n×n)
        hess = 2 / normal.squaredNorm() * normal * normal.transpose();
    } else {
        assert(normal.rows() == 1);
        // (1×n)ᵀ(1×n) = (n×n)
        hess = 2 / normal.squaredNorm() * normal.transpose() * normal;
    }
}

/// @brief Compute the hessian of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] t0 The first vertex of the triangle.
/// @param[in] t1 The second vertex of the triangle.
/// @param[in] t2 The third vertex of the triangle.
/// @param[out] hess The hessian of the distance wrt p, t0, t1, and t2.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename DerivedHess>
void point_plane_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    hess.resize(
        p.size() + t0.size() + t1.size() + t2.size(),
        p.size() + t0.size() + t1.size() + t2.size());
    autogen::point_plane_distance_hessian(
        p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0],
        t2[1], t2[2], hess.data());
}

} // namespace ipc


/*
 * File: fast_hankel_transform.cc
 * Created Date: 2019-09-11
 * Author: Lei Pan
 * Contact: <panlei7@gmail.com>
 *
 * Last Modified: Wednesday September 25th 2019 11:37:38 am
 *
 * MIT License
 *
 * Copyright (c) 2019 Lei Pan
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the 'Software'), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 * -----
 * HISTORY:
 * Date      	 By	Comments
 * ----------	---
 * ----------------------------------------------------------
 */

#include "fast_hankel_transform.hpp"

#include <Eigen/Dense>
#include <boost/math/special_functions/bessel.hpp>
#include <cmath>
#include <complex>
#include <fftw3.h>
#include <fmt/format.h>
#include <fmt/ostream.h>
#include <iostream>

using namespace Eigen;
using std::abs;
using std::exp;
using std::log;
using std::pow;

FastHankelTransform::FastHankelTransform(int num_sample, double ux, double uy)
    : num_sample_(num_sample), ux_(ux), uy_(uy),
      x_(VectorXd::Zero(num_sample_)), f_(VectorXd::Zero(num_sample_ + 1)),
      phi_(new std::complex<double>[num_sample_ * 2]),
      j1_(new std::complex<double>[num_sample_ * 2]) {
  alpha_ = evaluate_alpha();
  k0_ = evaluate_k0(alpha_);
}

FastHankelTransform::~FastHankelTransform() {
  delete[] phi_;
  delete[] j1_;
}

double FastHankelTransform::evaluate_alpha() {
  auto func = [&](auto a) { return -log(1.0 - exp(-a)) / (num_sample_ - 1); };

  const int maxiter = 100;
  double alpha = 1.0;
  for (auto i = 0; i < maxiter; ++i) {
    alpha = func(alpha);
  }
  return alpha;
}

double FastHankelTransform::evaluate_k0(double alpha) {
  double k0 = (2.0 * exp(alpha) + exp(2.0 * alpha)) /
              (pow(1 + exp(alpha), 2) * (1 - exp(-2.0 * alpha)));
  return k0;
}

VectorXd FastHankelTransform::sampling() {
  x_updated_ = true;
  x_(0) = (1.0 + exp(alpha_)) * exp(-alpha_ * num_sample_) / 2.0;
  for (auto i = 1; i < num_sample_; ++i) {
    x_(i) = x_(0) * exp(alpha_ * i);
  }
  return x_;
}

void FastHankelTransform::set_feval(const Ref<const VectorXd> &feval) {
  assert(x_updated_ == true);
  f_updated_ = true;
  f_.head(num_sample_) = feval;
}

void FastHankelTransform::evaluate_phi() {
  phi_[0] = k0_ * (f_(0) - f_(1)) * exp(alpha_ * (1 - num_sample_));
  for (auto i = 1; i < num_sample_; ++i) {
    phi_[i] = (f_(i) - f_(i + 1)) * exp(alpha_ * (i + 1 - num_sample_));
  }
  for (auto i = num_sample_; i < num_sample_ * 2; ++i) {
    phi_[i] = 0.0;
  }
}

void FastHankelTransform::evaluate_j1() {
  for (auto i = 0; i < num_sample_ * 2; ++i) {
    double x = ux_ * uy_ * x_(0) * exp(alpha_ * (i + 1 - num_sample_));
    j1_[i] = boost::math::cyl_bessel_j(1, x);
  }
}

VectorXd FastHankelTransform::calculate() {
  assert(f_updated_ == true);
  evaluate_phi();
  evaluate_j1();

  int nsample = num_sample_ * 2;
  fftw_complex *fft_phi =
      (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * nsample);
  fftw_complex *phi = reinterpret_cast<fftw_complex *>(phi_);
  fftw_plan p1 =
      fftw_plan_dft_1d(nsample, phi, fft_phi, FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_execute(p1);

  fftw_complex *fft_j1 =
      (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * nsample);
  fftw_complex *j1 = reinterpret_cast<fftw_complex *>(j1_);
  fftw_plan p2 =
      fftw_plan_dft_1d(nsample, j1, fft_j1, FFTW_BACKWARD, FFTW_ESTIMATE);
  fftw_execute(p2);

  fftw_complex *in =
      (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * nsample);

  for (auto i = 0; i < nsample; ++i) {
    in[i][0] = fft_phi[i][0] * fft_j1[i][0] - fft_phi[i][1] * fft_j1[i][1];
    in[i][1] = fft_phi[i][0] * fft_j1[i][1] + fft_phi[i][1] * fft_j1[i][0];
  }

  fftw_complex *out =
      (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * nsample);

  fftw_plan p3 =
      fftw_plan_dft_1d(nsample, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_execute(p3);

  VectorXd ret(num_sample_);
  for (auto i = 0; i < num_sample_; ++i) {
    ret(i) = 2.0 / (x_(i) * pow(ux_, 2) * uy_) * out[i][0] / nsample;
  }

  fftw_destroy_plan(p1);
  fftw_destroy_plan(p2);
  fftw_destroy_plan(p3);
  fftw_free(fft_phi);
  fftw_free(fft_j1);
  fftw_free(in);
  fftw_free(out);

  return ret;
}


#ifndef Interpolators__2D_BicubicInterpolator_hpp
#define Interpolators__2D_BicubicInterpolator_hpp

/** @file BicubicInterpolator.hpp
  * @brief
  * @author C.D. Clark III
  * @date 12/27/16
  */

#include "InterpolatorBase.hpp"
#include <boost/range/algorithm/lower_bound.hpp>
#include <boost/range/adaptor/strided.hpp>

/** @class
  * @brief Cubic spline interpolation for for 2D functions.
  * @author C.D. Clark III, Aaron Hoffman
  *
  * This class implements the bicubic spline interpolation method.
  * It is essentially the 2D equivalent of cubic splines for 1D.
  *
  */

namespace _2D {

template<class Real>
class BicubicInterpolator : public InterpolatorBase<BicubicInterpolator<Real>>
{
  public:
    using BASE = InterpolatorBase<BicubicInterpolator<Real>>;
    using VectorType = typename BASE::VectorType;
    using MapType = typename BASE::MapType;

    // types used to view data as 2D coordinates
    using MatrixType    = typename BASE::MatrixType;
    using _2DVectorView = typename BASE::_2DVectorView;
    using _2DMatrixView = typename BASE::_2DMatrixView;

    // types used for 4x4 matrix algebra
    using Matrix44 = Eigen::Matrix<Real,4,4 >;
    using Matrix44Array = Eigen::Array< Matrix44, Eigen::Dynamic, Eigen::Dynamic >;
    using ColVector4 = Eigen::Matrix<Real,4,1 >;
    using RowVector4 = Eigen::Matrix<Real,1,4 >;

  protected:
    using BASE::xView;
    using BASE::yView;
    using BASE::zView;
    using BASE::X;
    using BASE::Y;
    using BASE::Z;

    Matrix44Array a; // naming convention used by wikipedia article (see Wikipedia https://en.wikipedia.org/wiki/Bicubic_interpolation)

  public:

    template<typename I>
    BicubicInterpolator( I n, Real *x, Real *y, Real *z ) {this->setData(n,x,y,z);}

    template<typename X, typename Y, typename Z>
    BicubicInterpolator( X &x, Y &y, Z &z ) {this->setData(x,y,z);}

    BicubicInterpolator():BASE()
    { }

    BicubicInterpolator(const BicubicInterpolator& rhs)
    :BASE(rhs)
    ,a(rhs.a)
    {}

    // copy-swap idiom
    friend void swap( BicubicInterpolator& lhs, BicubicInterpolator& rhs)
    {
      lhs.a.swap(rhs.a);
      swap( static_cast<BASE&>(lhs), static_cast<BASE&>(rhs) );
    }

    BicubicInterpolator& operator=(BicubicInterpolator rhs)
    {
      swap(*this,rhs);
      return *this;
    }

    // methods required by the interface
    Real operator()( Real x, Real y ) const;

  protected:

    void setupInterpolator();
    friend BASE;

};

template<class Real>
void
BicubicInterpolator<Real>::setupInterpolator()
{
  // Interpolation will be done by multiplying the coordinates by coefficients.

  a = Matrix44Array( X->size()-1, Y->size()-1 );

  // We are going to precompute the interpolation coefficients so
  // that we can interpolate quickly. This requires a 4x4 matrix for each "patch".
  Matrix44 Left, Right;

  Left <<  1,  0,  0,  0,
           0,  0,  1,  0,
          -3,  3, -2, -1,
           2, -2,  1,  1;

  Right<<  1,  0, -3,  2,
           0,  0,  3, -2,
           0,  1, -2,  1,
           0,  0, -1,  1;

  for(int i = 0; i < X->size() - 1; i++)
  {
    for( int j = 0; j < Y->size() - 1; j++)
    {
      Matrix44 F;

      Real f00,   f01,   f10,   f11;
      Real fx00,  fx01,  fx10,  fx11;
      Real fy00,  fy01,  fy10,  fy11;
      Real fxy00, fxy01, fxy10, fxy11;

      Real fm, fp;
      int im, ip, jm, jp;

      int iN = X->size();
      int jN = Y->size();

      // function values
      f00 = (*Z)(i    ,j    ); // <<<<<<
      f01 = (*Z)(i    ,j + 1); // <<<<<<
      f10 = (*Z)(i + 1,j    ); // <<<<<<
      f11 = (*Z)(i + 1,j + 1); // <<<<<<

      // need to calculate function values and derivatives
      // at each corner.
      //
      // note: interpolation algorithm is derived for the unit square.
      // so we need to take the derivatives assuming X(i+1) - X(i) = Y(j+1) - Y(j) = 1

      Real xL = (*X)(i+1) - (*X)(i);
      Real yL = (*Y)(j+1) - (*Y)(j);
      Real dx, dy;

      // x derivatives

      im = std::max(i-1,0);
      ip = std::min(i+1,iN-1);

      dx = ((*X)(ip) - (*X)(im))/xL;

      fp = (*Z)(ip,j);
      fm = (*Z)(im,j);
      fx00 = (fp - fm) / dx; // <<<<<<

      fp = (*Z)(ip,j+1);
      fm = (*Z)(im,j+1);
      fx01 = (fp - fm) / dx; // <<<<<<

      im = std::max(i,0);
      ip = std::min(i+2,iN-1);

      dx = ((*X)(ip) - (*X)(im))/xL;

      fp = (*Z)(ip,j);
      fm = (*Z)(im,j);
      fx10 = (fp - fm) / dx; // <<<<<<

      fp = (*Z)(ip,j+1);
      fm = (*Z)(im,j+1);
      fx11 = (fp - fm) / dx; // <<<<<<

      // y derivatives

      jm = std::max(j-1,0);
      jp = std::min(j+1,jN-1);

      dy = ((*Y)(jp) - (*Y)(jm))/yL;

      fp = (*Z)(i,jp);
      fm = (*Z)(i,jm);
      fy00 = (fp - fm) / dy; // <<<<<<

      fp = (*Z)(i+1,jp);
      fm = (*Z)(i+1,jm);
      fy10 = (fp - fm) / dy; // <<<<<<

      jm = std::max(j,0);
      jp = std::min(j+2,jN-1);

      dy = ((*Y)(jp) - (*Y)(jm))/yL;

      fp = (*Z)(i,jp);
      fm = (*Z)(i,jm);
      fy01 = (fp - fm) / yL; // <<<<<<

      fp = (*Z)(i+1,jp);
      fm = (*Z)(i+1,jm);
      fy11 = (fp - fm) / yL; // <<<<<<

      // xy derivatives

      im = std::max(i-1,0);
      ip = std::min(i+1,iN-1);
      jm = std::max(j-1,0);
      jp = std::min(j+1,jN-1);

      dx = ((*X)(ip) - (*X)(im)) / xL;

      dy = ((*Y)(jp) - (*Y)(jm)) / yL;

      fp = ((*Z)(ip,jp) - (*Z)(im,jp))/dx;
      fm = ((*Z)(ip,jm) - (*Z)(im,jm))/dx;
      fxy00 = (fp - fm) / dy; // <<<<<<

      jm = std::max(j,0);
      jp = std::min(j+2,jN-1);

      dy = ((*Y)(jp) - (*Y)(jm))/yL;

      fp = ((*Z)(ip,jp) - (*Z)(im,jp))/dx;
      fm = ((*Z)(ip,jm) - (*Z)(im,jm))/dx;
      fxy01 = (fp - fm) / dy; // <<<<<<

      im = std::max(i,0);
      ip = std::min(i+2,iN-1);
      jm = std::max(j-1,0);
      jp = std::min(j+1,jN-1);

      dx = ((*X)(ip) - (*X)(im)) / xL;

      dy = ((*Y)(jp) - (*Y)(jm)) / yL;

      fp = ((*Z)(ip,jp) - (*Z)(im,jp))/dx;
      fm = ((*Z)(ip,jm) - (*Z)(im,jm))/dx;
      fxy10 = (fp - fm) / dy; // <<<<<<

      jm = std::max(j,0);
      jp = std::min(j+2,jN-1);

      dy = ((*Y)(jp) - (*Y)(jm)) / yL;

      fp = ((*Z)(ip,jp) - (*Z)(im,jp))/dx;
      fm = ((*Z)(ip,jm) - (*Z)(im,jm))/dx;
      fxy11 = (fp - fm) / dy; // <<<<<<

      F <<  f00,  f01,  fy00,  fy01,
            f10,  f11,  fy10,  fy11,
           fx00, fx01, fxy00, fxy01,
           fx10, fx11, fxy10, fxy11;

      a(i,j) = Left * F * Right;
    }
  }
}

template<class Real>
Real
BicubicInterpolator<Real>::operator()( Real x, Real y ) const
{
  BASE::checkData();

  // no extrapolation...
  if( x < this->xView->minCoeff()
   || x > this->xView->maxCoeff()
   || y < this->yView->minCoeff()
   || y > this->yView->maxCoeff() )
  {
    return 0;
  }

  // find the x index that is just to the LEFT of x
  //int i  = Utils::index__last_lt( x, *X, X->size() );
  // NOTE: X data is strided.
  auto xrng = std::make_pair( X->data(), X->data()+X->size()*X->innerStride() ) | boost::adaptors::strided(X->innerStride());
  int i = boost::lower_bound( xrng, x) - boost::begin(xrng) - 1;
  if(i < 0)
    i = 0;

  // find the y index that is just BELOW y
  //int j  = Utils::index__last_lt( y, *Y, Y->size() );
  // NOTE: Y data is NOT strided
  auto yrng = std::make_pair( Y->data(), Y->data()+Y->size() );
  int j = boost::lower_bound( yrng, y) - boost::begin(yrng) - 1;
  if(j < 0)
    j = 0;

  Real xL = (*X)(i+1) - (*X)(i);
  Real yL = (*Y)(j+1) - (*Y)(j);

  // now, create the coordinate vectors (see Wikipedia https://en.wikipedia.org/wiki/Bicubic_interpolation)
  RowVector4 vx;
  vx[0] = 1;                                   // x^0
  vx[1] = (x - (*X)(i))/xL;                    // x^1
  vx[2] = vx[1] * vx[1];                       // x^2
  vx[3] = vx[2] * vx[1];                       // x^3

  ColVector4 vy;
  vy[0] = 1;                                   // y^0
  vy[1] = (y - (*Y)(j))/yL;                    // y^1
  vy[2] = vy[1] * vy[1];                       // y^2
  vy[3] = vy[2] * vy[1];                       // y^3

  // interpolation is just x*a*y

  return vx*a(i,j)*vy;

}

}

#endif // include protector


    #include <Eigen/Dense>
    #include <Eigen/Sparse>
    #include <unsupported/Eigen/MatrixFunctions>
    #include <unsupported/Eigen/KroneckerProduct>

    #include <iostream>
    #include <cmath>

    #define TVMTL_MATRIX_UTILS_DEBUG

    #include <mtvmtl/core/matrix_utils.hpp>

    int main(){

    using namespace Eigen;

    Matrix3d R = Matrix3d::Random();
    Matrix3d M = R + R.transpose() + R.rows()*Matrix3d::Identity();

    std::cout << "Matrix M :\n" << M << std::endl;
    std::cout << "\nlog(M) :\n" << M.log() << std::endl;

    for (int i = 0; i < 3; i++) {
    	for (int j = 0; j < 3; j++) {
	    Matrix3d E = Matrix3d::Zero();
	    MatrixXd D = MatrixXd::Zero(3,3);
	    E(i,j)=1.0;
	    tvmtl::MatrixLogarithmFrechetDerivative(M, E, D);
	    std::cout << "\ndlog(M,E_" << i+1 << j+1 << ") :\n" << D << std::endl;
	    Map<VectorXd> V(D.data(), D.size());
	    std::cout << "\nRowwise Vectorized:\n" << V << std::endl;
    	}
    }

    Matrix<double, 9 ,9>  Result;
    tvmtl::KroneckerDLog(M, Result);
    std::cout << "Full Kronecker Representation of DLog:\n" << Result << std::endl;

    std::cout << "\n\nPermutation Matrix Test:\n" << std::endl;

    PermutationMatrix<9, 9, int> P;
    P.setIdentity();
    for(int i=0; i<3; i++)
	for(int j=0; j<i; j++)
	    P.applyTranspositionOnTheRight(j*3+i,i*3+j);

    std::cout << P*Matrix<double, 9,9>::Identity() << std::endl;

    std::cout << "\n\n Simple Rotation Matrix Test\n" << std::endl;

    Matrix3d s1 = Matrix3d::Identity();
    Matrix3d s2;
    s2 << 0, -1, 0, 1, 0, 0, 0, 0, 1;
    Matrix3d m2 = s1.transpose()*s2;

    std::cout << "\nMatrix 1\n" << s1 << std::endl;
    std::cout << "\nMatrix 2\n" << s2 << std::endl;
    std::cout << "\nDlog Argument\n" << m2 << std::endl;

    tvmtl::KroneckerDLog(m2, Result);
    std::cout << "\n\nFull Kronecker Representation of DLog:\n" << Result << std::endl;

    Matrix3d m3;
    m3 << 4102350, -2446420, 2209640, -2446420,  1458920, -1317710, 2209640, -1317710, 1190180;

    tvmtl::KroneckerDLog(m3, Result);
    std::cout << "\n\nPathological Matrix test:\n" << Result << std::endl;

    return 0;
    }


#include <iostream>
#include <cmath>
#include <math.h>
#include <iostream>
#include <vector>
#include <array>
#include "heatEquation.hpp"
#include "AdaptiveHeat.hpp"
#include "TriDiagMatrix.hpp"
#include "MassMatrix.hpp"
#include "StiffnessMatrix.hpp"
#include <fstream>
#include <string>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;

void AdaptiveHeatEquation::AdaptiveSolver()
{
AnalyticSolutionVec();
mpPreviousSolution = mpAnalyticSolution;
BuildSystemAtTimeStep();
oldmesh.CopySpaceMesh(mpsmesh);
mpsmesh.PrintSpaceNodes();

int m = mptmesh.NumberOfTimeSteps();
for(int j = 0; j<m; j++)
{
    mpcurrenTimeStep = j+1;
    mpcurrentMeshIndex = j;

    BuildSystemAtTimeStep();
    SystemSolver();
    mpPreviousSolution = mpx;

    SaveIntervalsForRefinement();
    SaveIntervalsForCoarsening();
    mpsmesh.BisectIntervals(intervalsForRefinement);
    mpsmesh.CoarsenIntervals(NodesForRemoval);
    std::cout<<mpsmesh.meshsize() <<"\n";
    std::cout<<"\n";
    PrintVector(mpErrorMesh);
}
    std::cout<<mpsmesh.meshsize() <<"\n";
    std::cout<<"\n";
}

void AdaptiveHeatEquation::SaveIntervalsForCoarsening()
{
    BuildErrorMesh();
    NodesForRemoval.clear();
    for(int i=0; i<mpErrorMesh.size()-1; i++)
    {
        if (sqrt(mpErrorMesh.at(i)+mpErrorMesh.at(i+1))<coarseningtol)
        {
            NodesForRemoval.push_back(i+1);
        }
    }

}

void AdaptiveHeatEquation::BuildSystemAtTimeStep()
{
stiff.BuildStiffnessMatrix( mpsmesh );
stiff.MultiplyByScalar( mptmesh.ReadTimeMesh(mpcurrentMeshIndex) );
mass.BuildMassMatrix(mpsmesh);
LHS.AddTwoMatrices( mass, stiff );
}

void AdaptiveHeatEquation::SystemSolver()
{
    BuildRHS();
    LHS.MatrixSolver( mpRHS, mpx );
    oldmesh.CopySpaceMesh(mpsmesh);
}

void AdaptiveHeatEquation::SaveIntervalsForRefinement()
{
    BuildErrorMesh();
    intervalsForRefinement.clear();
    for(int i=0; i<mpErrorMesh.size(); i++)
    {
        if (sqrt(mpErrorMesh.at(i))>tolerance)
        {
            intervalsForRefinement.push_back(i);
        }
    }
}

double AdaptiveHeatEquation::IntegrateBasisWithU( int NodeIndex, double lowerlimit,
                              double upperlimit )
{
    auto SolutionWithBasis = [&](double x)
        { return mpsmesh.TestFunctions( NodeIndex, x)*PiecewiseU(x, oldmesh, mpPreviousSolution); };

    return gauss<double, 7>::integrate(SolutionWithBasis, lowerlimit, upperlimit);
}

    //this function works for refinement and coarsening
void AdaptiveHeatEquation::BuildRHS()
{
mpRHS.clear();
std::vector<double> intervals;

refinedsmesh.CommonMesh(mpsmesh, oldmesh);

double integral;
for (int i = 1; i<mpsmesh.meshsize(); i++)
{
    integral=0;
    refinedsmesh.Range(mpsmesh.ReadSpaceNode(i-1), mpsmesh.ReadSpaceNode(i+1), intervals);

    for(int j = 0; j<intervals.size()-1; j++)
    {
        integral = integral + IntegrateBasisWithU(i, intervals.at(j),
                    intervals.at(j+1));
    }
    mpRHS.push_back(integral);
}
}

Sover class with method to adapt in space and time/AdaptiveHeat.cpp


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

#include "mahalanobis_meas_model.h"
#include <boost/math/distributions/chi_squared.hpp>
#include <assert.h>

MahalanobisMeasModel::MahalanobisMeasModel(double cov_scale)
{
  cov_scale_ = cov_scale;
}

double MahalanobisMeasModel::getProbability(const Ref<const VectorXd>& curr_state, const Ref<const VectorXd>& meas, const Ref<const MatrixXd>& meas_cov, double dt)
{
  // simple measurement model, position directly observable (by detector)
  // state = [x, y, vx, vy]^T
  // z = [x, y]^T
  // C = [1, 0, 0, 0; 0, 1, 0, 0]

  // calculate mahalanobis distance from predicted measurement to actual measurement
  // note: consider current state as deterministic state with no covariance
  // only measurement provides covariance
  // mahalanobis distance is chi_square distributed

  double dist = sqrt((curr_state.block<2, 1>(0, 0) - meas).transpose() *
                    ((meas_cov.block<2, 2>(0, 0) * cov_scale_).inverse()) *
                    (curr_state.block<2, 1>(0, 0) - meas));

  //boost::math::normal norm = boost::math::normal(0, sigma_ * sigma_);
  boost::math::chi_squared chi(2);

  return boost::math::pdf(chi, dist);
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef STENCILS_HPP
#define STENCILS_HPP

#include <functional>

#include "Config.hpp"

#include <Eigen/Core>

/*! \typedef DifferentiableFunction
 *  \brief sort of a function pointer to a function of a pair of vectors that can be numerically differentiated
 */
typedef pcm::function<double(const Eigen::Vector3d &, const Eigen::Vector3d &)> DifferentiableFunction;

/*! \brief Calculate directional derivative using a three-point stencil
 *  \param[in] func function to be differentiated
 *  \param[in] arg_1 first point, the directional derivative is calculated with respect to this point
 *  \param[in] arg_2 second point
 *  \param[in] direction the direction in which the directional derivative is to be evaluated
 *  \param[in] step finite difference value for the stencil
 */
inline double threePointStencil(const DifferentiableFunction & func,
        const Eigen::Vector3d & arg_1, const Eigen::Vector3d & arg_2,
        const Eigen::Vector3d & direction, double step = 1.0e-04)
{
    // f(x-h)
    Eigen::Vector3d delta_m1 = arg_1 - direction * step / direction.norm();
    // f(x+h)
    Eigen::Vector3d delta_1  = arg_1 + direction * step / direction.norm();

    Eigen::Vector2d stencil;
    stencil << -0.5, 0.5;
    Eigen::Vector2d function_values;
    function_values << func(delta_m1, arg_2), func(delta_1,  arg_2);

    return (function_values.dot(stencil) / step);
}

/*! \brief Calculate directional derivative using a five-point stencil
 *  \param[in] func function to be differentiated
 *  \param[in] arg_1 first point, the directional derivative is calculated with respect to this point
 *  \param[in] arg_2 second point
 *  \param[in] direction the direction in which the directional derivative is to be evaluated
 *  \param[in] step finite difference value for the stencil
 */
inline double fivePointStencil(const DifferentiableFunction & func,
        const Eigen::Vector3d & arg_1, const Eigen::Vector3d & arg_2,
        const Eigen::Vector3d & direction, double step = 1.0e-04)
{
    // f(x-2h)
    Eigen::Vector3d delta_m2 = arg_1 - 2.0 * direction * step / direction.norm();
    // f(x-h)
    Eigen::Vector3d delta_m1 = arg_1 - direction * step / direction.norm();
    // f(x+h)
    Eigen::Vector3d delta_1  = arg_1 + direction * step / direction.norm();
    // f(x+2h)
    Eigen::Vector3d delta_2  = arg_1 + 2.0 * direction * step / direction.norm();

    Eigen::Vector4d stencil;
    stencil << 1.0/12.0, -2.0/3.0, 2.0/3.0, -1.0/12.0;
    Eigen::Vector4d function_values;
    function_values << func(delta_m2, arg_2), func(delta_m1, arg_2),
                       func(delta_1,  arg_2), func(delta_2,  arg_2);

    return (function_values.dot(stencil) / step);
}

/*! \brief Calculate directional derivative using a seven-point stencil
 *  \param[in] func function to be differentiated
 *  \param[in] arg_1 first point, the directional derivative is calculated with respect to this point
 *  \param[in] arg_2 second point
 *  \param[in] direction the direction in which the directional derivative is to be evaluated
 *  \param[in] step finite difference value for the stencil
 */
inline double sevenPointStencil(const DifferentiableFunction & func,
        const Eigen::Vector3d & arg_1, const Eigen::Vector3d & arg_2,
        const Eigen::Vector3d & direction, double step = 1.0e-04)
{
    // f(x-3h)
    Eigen::Vector3d delta_m3 = arg_1 - 3.0 * direction * step / direction.norm();
    // f(x-2h)
    Eigen::Vector3d delta_m2 = arg_1 - 2.0 * direction * step / direction.norm();
    // f(x-h)
    Eigen::Vector3d delta_m1 = arg_1 - direction * step / direction.norm();
    // f(x+h)
    Eigen::Vector3d delta_1  = arg_1 + direction * step / direction.norm();
    // f(x+2h)
    Eigen::Vector3d delta_2  = arg_1 + 2.0 * direction * step / direction.norm();
    // f(x+3h)
    Eigen::Vector3d delta_3  = arg_1 + 3.0 * direction * step / direction.norm();

    Eigen::Matrix<double, 6, 1> stencil;
    stencil << -1.0/60.0, 3.0/20.0, -3.0/4.0, 3.0/4.0, -3.0/20.0, 1.0/60.0;
    Eigen::Matrix<double, 6, 1> function_values;
    function_values << func(delta_m3, arg_2), func(delta_m2, arg_2), func(delta_m1, arg_2),
                       func(delta_1,  arg_2), func(delta_2,  arg_2), func(delta_3, arg_2);

    return (function_values.dot(stencil) / step);
}

#endif // STENCILS_HPP


// This example consists of a single static target which sits in one
// place and does not move; in effect it has a "GPS" which measures
// its position

#include <Eigen/StdVector>
#include <iostream>
#include <stdint.h>

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/stuff/sampler.h>

#include "targetTypes3D.hpp"

using namespace Eigen;
using namespace std;
using namespace g2o;

int main()
{
  // Set up the optimiser
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  // Create the block solver - the dimensions are specified because
  // 3D observations marginalise to a 3D estimate
  typedef BlockSolver<BlockSolverTraits<3, 3> > BlockSolver_3_3;
  BlockSolver_3_3::LinearSolverType* linearSolver
      = new LinearSolverCholmod<BlockSolver_3_3::PoseMatrixType>();
  BlockSolver_3_3* blockSolver
      = new BlockSolver_3_3(linearSolver);
  OptimizationAlgorithmGaussNewton* solver
    = new OptimizationAlgorithmGaussNewton(blockSolver);
  optimizer.setAlgorithm(solver);

  // Sample the actual location of the target
  Vector3d truePoint(sampleUniform(-500, 500),
                     sampleUniform(-500, 500),
                     sampleUniform(-500, 500));

  // Construct vertex which corresponds to the actual point of the target
  VertexPosition3D* position = new VertexPosition3D();
  position->setId(0);
  optimizer.addVertex(position);

  // Now generate some noise corrupted measurements; for simplicity
  // these are uniformly distributed about the true target. These are
  // modelled as a unary edge because they do not like to, say,
  // another node in the map.
  int numMeasurements = 10;
  double noiseLimit = sqrt(12.);
  double noiseSigma = noiseLimit*noiseLimit / 12.0;

  for (int i = 0; i < numMeasurements; i++)
    {
      Vector3d measurement = truePoint +
        Vector3d(sampleUniform(-0.5, 0.5) * noiseLimit,
                 sampleUniform(-0.5, 0.5) * noiseLimit,
                 sampleUniform(-0.5, 0.5) * noiseLimit);
      GPSObservationPosition3DEdge* goe = new GPSObservationPosition3DEdge();
      goe->setVertex(0, position);
      goe->setMeasurement(measurement);
      goe->setInformation(Matrix3d::Identity() / noiseSigma);
      optimizer.addEdge(goe);
    }

  // Configure and set things going
  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(5);

  cout << "truePoint=\n" << truePoint << endl;

  cerr <<  "computed estimate=\n" << dynamic_cast<VertexPosition3D*>(optimizer.vertices().find(0)->second)->estimate() << endl;

  //position->setMarginalized(true);

  SparseBlockMatrix<MatrixXd> spinv;

  optimizer.computeMarginals(spinv, position);

  //optimizer.solver()->computeMarginals();

  // covariance
  //
  cout << "covariance\n" << spinv << endl;

  cout << spinv.block(0,0) << endl;

}


#include <NTL/MatPrime.h>

NTL_START_IMPL

MatPrimeTablesType MatPrimeTables;
// a truly GLOBAL variable, shared among all threads

// For now, we use the same logic as for IsFFTPrime,
// which is good enough

static
long IsMatPrime(long n)
{
   long  m, x, y, z;
   long j, k;

   if (n <= 1 || n >= NTL_SP_BOUND) return 0;

   if (n % 2 == 0) return 0;

   if (n % 3 == 0) return 0;

   if (n % 5 == 0) return 0;

   if (n % 7 == 0) return 0;

   m = n - 1;
   k = 0;
   while ((m & 1) == 0) {
      m = m >> 1;
      k++;
   }

   for (;;) {
      x = RandomBnd(n);

      if (x == 0) continue;
      z = PowerMod(x, m, n);
      if (z == 1) continue;

      x = z;
      j = 0;
      do {
         y = z;
         z = MulMod(y, y, n);
         j++;
      } while (j != k && z != 1);

      if (z != 1 || y !=  n-1) return 0;

      if (j == k)
         break;
   }

   /* x^{2^k} = 1 mod n, x^{2^{k-1}} = -1 mod n */

   long TrialBound;

   TrialBound = m >> k;
   if (TrialBound > 0) {
      if (!ProbPrime(n, 5)) return 0;

      /* we have to do trial division by special numbers */

      TrialBound = SqrRoot(TrialBound);

      long a, b;

      for (a = 1; a <= TrialBound; a++) {
         b = (a << k) + 1;
         if (n % b == 0) return 0;
      }
   }

   return 1;
}

static
void NextMatPrime(long& q, long index)
{
   static long m = NTL_MatPrime_NBITS-1;
   static long k = 0;
   // m and k are truly GLOBAL variables, shared among
   // all threads.  Access is protected by a critical section
   // guarding MatPrimeTables

   static long last_index = -1;
   static long last_m = 0;
   static long last_k = 0;

   if (index == last_index) {
      // roll back m and k...part of a simple error recovery
      // strategy if an exception was thrown in the last
      // invocation of UseMatPrime...probably of academic
      // interest only

      m = last_m;
      k = last_k;
   }
   else {
      last_index = index;
      last_m = m;
      last_k = k;
   }

   long cand;

   for (;;) {
      if (k == 0) {
         m--;
         if (m < 3) ResourceError("ran out of matrix primes");
         k = 1L << (NTL_MatPrime_NBITS-m-2);
      }

      k--;

      cand = (1L << (NTL_MatPrime_NBITS-1)) + (k << (m+1)) + (1L << m) + 1;

      if (!IsMatPrime(cand)) continue;
      q = cand;
      return;
   }
}

void InitMatPrimeInfo(MatPrimeInfo& info, long q)
{
   info.q = q;
   info.context = zz_pContext(q);
}

void UseMatPrime(long index)
{
   if (index < 0) LogicError("invalid matrix prime index");
   if (index >= NTL_MAX_MATPRIMES) ResourceError("matrix prime index too large");

   if (index+1 >= NTL_NSP_BOUND) ResourceError("matrix prime index too large");
   // largely acacedemic, but it is a convenient assumption

   do {  // NOTE: thread safe lazy init
      MatPrimeTablesType::Builder bld(MatPrimeTables, index+1);
      long amt = bld.amt();
      if (!amt) break;

      long first = index+1-amt;
      // initialize entries first..index

      long i;
      for (i = first; i <= index; i++) {
         UniquePtr<MatPrimeInfo> info;
         info.make();

         long q, w;
         NextMatPrime(q, i);

         InitMatPrimeInfo(*info, q);
         bld.move(info);
      }

   } while (0);
}

#ifndef NTL_MatPrime_HALF_SIZE_STRATEGY

void build(MatPrime_crt_helper& H, const ZZ& P)
{
   ZZ B, M, M1, M2, M3;
   long n, i;
   long q, t;
   mulmod_t qinv;

   sqr(B, P);
   mul(B, B, NTL_MatPrimeLimit);
   LeftShift(B, B, NTL_MatPrimeFudge);

   set(M);
   n = 0;
   while (M <= B) {
      UseMatPrime(n);
      q = GetMatPrime(n);
      n++;
      mul(M, M, q);
   }

   double fn = double(n);

   if (8.0*fn*(fn+48) > NTL_FDOUBLE_PRECISION)
      ResourceError("modulus too big");

   H.NumPrimes = n;
   H.sz = P.size();
   H.prime.SetLength(n);
   H.prime_recip.SetLength(n);
   H.u.SetLength(n);
   H.uqinv.SetLength(n);
   H.ZZ_red_struct.SetLength(n);

   H.coeff.SetSize(n, P.size());

   H.montgomery_struct.init(P, ZZ(n) << NTL_MatPrime_NBITS);

   ZZ qq, rr;

   DivRem(qq, rr, M, P);

   NegateMod(H.MinusMModP, rr, P);

   H.montgomery_struct.adjust(H.MinusMModP);

   for (i = 0; i < n; i++) {
      q = GetMatPrime(i);
      qinv = MatPrimeTables[i]->context.ModulusInverse();

      long tt = rem(qq, q);

      mul(M2, P, tt);
      add(M2, M2, rr);
      div(M2, M2, q);  // = (M/q) rem p

      div(M1, M, q);
      t = rem(M1, q);
      t = InvMod(t, q);

      // montgomery
      H.montgomery_struct.adjust(M2);

      H.prime[i] = q;
      H.prime_recip[i] = 1/double(q);
      H.u[i] = t;
      H.uqinv[i] = PrepMulModPrecon(H.u[i], q, qinv);
      H.ZZ_red_struct[i] = &MatPrimeTables[i]->context.ZZ_red_struct();
      H.coeff[i] = M2;
   }

   H.cost = double(H.sz)*double(n);

}

void reduce(const MatPrime_crt_helper& H, const ZZ& value, MatPrime_residue_t *remainders,
            MatPrime_crt_helper_scratch& scratch)
{
   long n = H.NumPrimes;
   const sp_ZZ_reduce_struct *const *red_struct = H.ZZ_red_struct.elts();

   for (long i = 0; i < n; i++)
      remainders[i] = red_struct[i]->rem(value);
}

void reconstruct(const MatPrime_crt_helper& H, ZZ& value, const MatPrime_residue_t *remainders,
                 MatPrime_crt_helper_scratch& scratch)
{
   ZZ& t = scratch.t;

   long nprimes = H.NumPrimes;
   const long *u = H.u.elts();
   const long *prime = H.prime.elts();
   const mulmod_precon_t  *uqinv = H.uqinv.elts();
   const double *prime_recip = H.prime_recip.elts();

   double y = 0.0;

   QuickAccumBegin(t, H.sz);
   for (long i = 0; i < nprimes; i++) {
      long r = MulModPrecon(remainders[i], u[i], prime[i], uqinv[i]);
      y += double(r)*prime_recip[i];
      QuickAccumMulAdd(t, H.coeff[i], r);
   }

   long q = long(y + 0.5);
   QuickAccumMulAdd(t, H.MinusMModP, q);

   QuickAccumEnd(t);

   // montgomery
   H.montgomery_struct.eval(value, t);

}

#else

void build(MatPrime_crt_helper& H, const ZZ& P)
{
   ZZ B, M, M1, M2, M3;
   long n, i, j;
   long q, t;
   mulmod_t qinv;

   sqr(B, P);
   mul(B, B, NTL_MatPrimeLimit);
   LeftShift(B, B, NTL_MatPrimeFudge);

   set(M);
   n = 0;
   while (M <= B) {
      UseMatPrime(n);
      q = GetMatPrime(n);
      n++;
      mul(M, M, q);
   }

   double fn = double(n);

   if (8.0*fn*(fn+48) > NTL_FDOUBLE_PRECISION)
      ResourceError("modulus too big");

   long n_half_ceil = (n+1)/2;

   H.NumPrimes = n;
   H.sz = P.size();
   H.prime.SetLength(n);
   H.prime_recip.SetLength(n);
   H.u.SetLength(n);
   H.uqinv.SetLength(n);
   H.red_struct.SetLength(n);

   H.ZZ_red_struct.SetLength(n_half_ceil);
   H.coeff.SetSize(n_half_ceil, P.size());

   H.montgomery_struct.init(P, ZZ(n) << (2*NTL_MatPrime_NBITS));

   for (i = 0; i < n; i++) {
      q = GetMatPrime(i);
      qinv = MatPrimeTables[i]->context.ModulusInverse();

      div(M1, M, q);
      t = rem(M1, q);
      t = InvMod(t, q); // = (M/q)^{-1} rem q

      H.prime[i] = q;
      H.prime_recip[i] = 1/double(q);
      H.u[i] = t;
      H.uqinv[i] = PrepMulModPrecon(H.u[i], q, qinv);
      H.red_struct[i] = MatPrimeTables[i]->context.red_struct();

   }

   ZZ qq, rr;
   DivRem(qq, rr, M, P);
   NegateMod(H.MinusMModP, rr, P);
   H.montgomery_struct.adjust(H.MinusMModP);

   for (i = 0, j = 0; i < n; i += 2, j++) {
      q = GetMatPrime(i);
      if (i+1 < n) q *= GetMatPrime(i+1);

      long tt = rem(qq, q);

      mul(M2, P, tt);
      add(M2, M2, rr);
      div(M2, M2, q);  // = (M/q) rem p

      // montgomery
      H.montgomery_struct.adjust(M2);

      H.ZZ_red_struct[j].build(q);
      H.coeff[j] = M2;
   }

   H.cost = double(H.sz)*double(n_half_ceil);
}

void reduce(const MatPrime_crt_helper& H, const ZZ& value, MatPrime_residue_t *remainders,
            MatPrime_crt_helper_scratch& scratch)
{
   long n = H.NumPrimes;
   const sp_ZZ_reduce_struct *ZZ_red_struct = H.ZZ_red_struct.elts();
   const sp_reduce_struct *red_struct = H.red_struct.elts();
   const long *prime = H.prime.elts();

   long i = 0, j = 0;
   for (; i <= n-2; i += 2, j++) {
      unsigned long t = ZZ_red_struct[j].rem(value);
      remainders[i] = rem(t, prime[i], red_struct[i]);
      remainders[i+1] = rem(t, prime[i+1], red_struct[i+1]);
   }
   if (i < n) {
      remainders[i] = ZZ_red_struct[j].rem(value);
   }
}

void reconstruct(const MatPrime_crt_helper& H, ZZ& value, const MatPrime_residue_t *remainders,
                 MatPrime_crt_helper_scratch& scratch)
{
   ZZ& t = scratch.t;

   long nprimes = H.NumPrimes;
   const long *u = H.u.elts();
   const long *prime = H.prime.elts();
   const mulmod_precon_t  *uqinv = H.uqinv.elts();
   const double *prime_recip = H.prime_recip.elts();

   double y = 0.0;

   QuickAccumBegin(t, H.sz);

   long i = 0, j = 0;
   for (; i <= nprimes-2; i += 2, j++) {
      long r0 = MulModPrecon(remainders[i], u[i], prime[i], uqinv[i]);
      long r1 = MulModPrecon(remainders[i+1], u[i+1], prime[i+1], uqinv[i+1]);
      y += double(r0)*prime_recip[i] + double(r1)*prime_recip[i+1];
      long r = r0*prime[i+1] + r1*prime[i];
      QuickAccumMulAdd(t, H.coeff[j], r);
   }

   if (i < nprimes) {
      long r = MulModPrecon(remainders[i], u[i], prime[i], uqinv[i]);
      y += double(r)*prime_recip[i];
      QuickAccumMulAdd(t, H.coeff[j], r);
   }

   long q = long(y + 0.5);
   QuickAccumMulAdd(t, H.MinusMModP, q);

   QuickAccumEnd(t);

   // montgomery
   H.montgomery_struct.eval(value, t);

}

#endif

// Facillitates PIMPL in ZZ_p.h
void MatPrime_crt_helper_deleter(MatPrime_crt_helper* p)
{
   delete p;
}

NTL_END_IMPL

#if 0

NTL_CLIENT

int main()
{
   ZZ P;
   RandomLen(P, 8000);

   MatPrime_crt_helper H;
   build(H, P);

   MatPrime_crt_helper_scratch scratch;

   long nprimes = H.GetNumPrimes();

   cerr << nprimes << "\n";

   ZZ a, b, c, d, e;
   RandomBnd(a, P);
   RandomBnd(b, P);
   RandomBnd(c, P);
   RandomBnd(d, P);

   e = (a*b + c*d) % P;

   Vec<MatPrime_residue_t> avec, bvec, cvec, dvec, evec;

   avec.SetLength(nprimes);
   bvec.SetLength(nprimes);
   cvec.SetLength(nprimes);
   dvec.SetLength(nprimes);
   evec.SetLength(nprimes);

   reduce(H, a, avec.elts(), scratch);
   reduce(H, b, bvec.elts(), scratch);
   reduce(H, c, cvec.elts(), scratch);
   reduce(H, d, dvec.elts(), scratch);

   for (long i = 0; i < nprimes; i++) {
      long q = GetMatPrime(i);
      evec[i] = AddMod(MulMod(avec[i], bvec[i], q), MulMod(cvec[i], dvec[i], q), q);
   }

   ZZ e1;

   reconstruct(H, e1, evec.elts(), scratch);

   if (e == e1)
      cerr << "PASS\n";
   else
      cerr << "FAIL\n";
}

#endif


/*
 * LinearMixedModel.hpp
 *
 *  Created on: Mar 17, 2016
 *      Author: Aditya Gautam (agautam1@andrew.cmu.edu)
 */

#ifndef SRC_MODEL_LINEARMIXEDMODEL_HPP_
#define SRC_MODEL_LINEARMIXEDMODEL_HPP_

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>
#include <vector>
#include <math.h>
#include <cstdlib>
#include <iostream>

#ifdef BAZEL
#include "Math/Math.hpp"
#include "Model.hpp"
#else
#include "../Math/Math.hpp"
#include "../Models/Model.hpp"
#endif

using namespace std;
using namespace Eigen;

class LinearMixedModel : public Model {
protected:

    // Training data
//    MatrixXf X;
//    MatrixXf Y;

    // Dimensions of the data
    long n; // Number of samples
    long d; // Number of input features

    //Similary matrix and SVD
    MatrixXf K;
    MatrixXf S;
    MatrixXf U;
    void decomposition();

//    MatrixXf beta; // d*1
    MatrixXf mau;  // Coeff matrix of similarity matrix.
    float lambda_optimized; // Value at which log likelihood is max
    float sigma;
    bool initFlag;

public:

    // Constructor
    LinearMixedModel();
    LinearMixedModel(const unordered_map<string, string>& options);

    //Setters and Getters
    long get_num_samples();
    long get_X_features();
    float get_lambda();
    float getSigma();
    void set_lambda(float);
    void set_S(MatrixXf);
    void set_U(MatrixXf);
    void setX(const MatrixXf&);
    void setXY(MatrixXf, MatrixXf);
    void setXYK(MatrixXf, MatrixXf, MatrixXf);
    void setUS(MatrixXf, MatrixXf);
    MatrixXf getBeta();
    //Supporting functions
    // Final Objective of the LLM : Obtain beta matrix.
    void calculate_beta(float);
    void calculate_sigma(float);
    float get_log_likelihood_value(float);
    void find_max_log_likelihood();

    void set_num_samples(int num_samples);
    // Search objective functions
    float f(float);
    void assertReadyToRun();
    void init();
};

#endif /* SRC_MODEL_LINEARMIXEDMODEL_HPP_ */


#include <Eigen/Core>
#include <Eigen/SparseCholesky>
#include <Euclid/MeshUtil/CGALMesh.h>
#include <Euclid/MeshUtil/EigenMesh.h>
#include <Euclid/Topology/MeshTopology.h>
#include <igl/cotmatrix.h>
#include <igl/vector_area_matrix.h>
#include <igl/repdiag.h>

namespace Euclid
{

namespace _impl
{

template<typename Mesh, typename T>
void boundary_matrix(const Mesh& mesh,
                     Eigen::SparseMatrix<T>& B,
                     Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& E)
{
    using Triplet = Eigen::Triplet<T>;
    std::vector<Triplet> triplets;
    auto vimap = get(CGAL::vertex_index, mesh);

    auto nv = mesh.number_of_vertices();
    auto sz = nv * 2;
    B.resize(sz, sz);
    E.setZero(sz, 2);

    auto bnds = boundary_components(mesh);
    int nb = 0;
    for (auto h : bnds) {
        auto hi = h;
        do {
            auto v = target(hi, mesh);
            auto vi = get(vimap, v);
            hi = next(hi, mesh);
            triplets.emplace_back(vi, vi, 1.0);
            triplets.emplace_back(nv + vi, nv + vi, 1.0);
            E(vi, 0) = 1.0;
            E(nv + vi, 1) = 1.0;
            ++nb;
        } while (hi != h);
        E /= std::sqrt(nb + 0.0);
    }
    B.setFromTriplets(triplets.begin(), triplets.end());
    B.makeCompressed();
}

} // namespace _impl

template<typename Mesh, typename VertexUVMap>
void spectral_conformal_parameterization(Mesh& mesh, VertexUVMap uvm)
{
    using Point_2 = typename boost::property_traits<VertexUVMap>::value_type;
    using FT = typename CGAL::Kernel_traits<Point_2>::Kernel::FT;
    using SpMat = Eigen::SparseMatrix<FT>;
    using Mat = Eigen::Matrix<FT, Eigen::Dynamic, Eigen::Dynamic>;
    using Vec = Eigen::Matrix<FT, Eigen::Dynamic, 1>;
    constexpr const int MAX_ITERS = 30;

    // convert mesh
    std::vector<FT> positions;
    std::vector<int> indices;
    extract_mesh<3>(mesh, positions, indices);
    Mat V;
    Eigen::MatrixXi F;
    make_mesh<3>(V, F, positions, indices);

    // assemble matrices
    SpMat L, LD, A, LC, B;
    Mat E;
    igl::cotmatrix(V, F, L);
    igl::repdiag(L, 2, LD);
    igl::vector_area_matrix(F, A);
    LC = -LD + 2.0 * A;
    _impl::boundary_matrix(mesh, B, E);

    // inverse power iterations
    Vec fidler = Vec::Random(LC.rows());
    Eigen::SimplicialLDLT<SpMat> solver(LC);
    for (int i = 0; i < MAX_ITERS; ++i) {
        fidler = (B - E * E.transpose()) * fidler;
        fidler = solver.solve(fidler);
        fidler.normalize();
    }

    // assemble results
    auto nv = mesh.number_of_vertices();
    auto vimap = get(CGAL::vertex_index, mesh);
    for (auto v : vertices(mesh)) {
        auto i = get(vimap, v);
        auto x = fidler(i);
        auto y = fidler(nv + i);
        put(uvm, v, Point_2(x, y));
    }
}

template<typename Mesh>
template<typename VertexUVMap,
         typename VertexIndexMap,
         typename VertexParameterizedMap>
CGAL::Surface_mesh_parameterization::Error_code
SCP_parameterizer_3<Mesh>::parameterize(TriangleMesh& mesh,
                                        halfedge_descriptor bhd,
                                        VertexUVMap uvmap,
                                        VertexIndexMap vimap,
                                        VertexParameterizedMap vpmap)
{
    spectral_conformal_parameterization(mesh, uvmap);
    return CGAL::Surface_mesh_parameterization::OK;
}

} // namespace Euclid


#include "util/number_util.h"

#include <boost/math/constants/constants.hpp>

namespace metternich::number {

double degree_to_radian(const double degree)
{
	return degree * boost::math::constants::pi<double>() / 180.;
}

}


#ifndef _POSE2DRANSAC_H
#define _POSE2DRANSAC_H

#include <opencv2/core/core.hpp>

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

namespace pose2d
{
   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation);

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<cv::Point3d>& train_img_pts,
                         const std::vector<cv::Point3d>& query_img_pts,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation);

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                         const double depth, const Eigen::Matrix3d& R, Eigen::Vector3d& translation);

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<cv::Point3d>& train_img_pts,
                         const std::vector<cv::Point3d>& query_img_pts, const double depth,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation);
};

namespace pose3d
{
   void pose_translation(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& query_image_pts,
                         const Eigen::Matrix3d& KI, const Eigen::Quaterniond& Q, Eigen::Vector3d& translation);

   void pose_translation(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& query_image_pts,
                         const Eigen::Matrix3d& KI, const Eigen::Matrix3d& R, Eigen::Vector3d& translation);
};

struct GravPoseRansacModel
//=========================
{
   GravPoseRansacModel() : rotation(0, 0, 0, 0), translation(0, 0, 0) {}
   GravPoseRansacModel(const Eigen::Quaterniond& rotation_, const Eigen::Vector3d& translation_) : rotation(rotation_),
                                                                                translation(translation_) {}
   GravPoseRansacModel(const GravPoseRansacModel& other) = default;

   GravPoseRansacModel& operator=(const GravPoseRansacModel &other) = default;

   Eigen::Quaterniond rotation;
   Eigen::Vector3d translation;
};

#ifdef USE_THEIA_RANSAC
#include "sample_consensus_estimator.h"
#include "create_and_initialize_ransac_variant.h"

struct Grav2DRansacEstimator : public theia::Estimator<std::pair<cv::Point3d, cv::Point3d>, GravPoseRansacModel>
//=================================================================================================================
{
   Grav2DRansacEstimator(const cv::Mat& K_, const Eigen::Quaterniond& Q_, const double depth_ =0, int samples =3) :
      Q(Q_), R(Q_.toRotationMatrix()), depth(depth_), sample_size(samples)
   //--------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> EK_((double *) K_.data);
      EK = EK_;
      KI = EK.inverse();
   }

   Grav2DRansacEstimator(Eigen::Matrix3d _K, const Eigen::Quaterniond& Q_, const double depth_ =0, int samples =3) :
         EK(std::move(_K)), Q(Q_), R(Q_.toRotationMatrix()), depth(depth_), sample_size(samples) { KI = EK.inverse(); }

   virtual double SampleSize() const override { return sample_size; }

   bool EstimateModel(const std::vector<std::pair<cv::Point3d, cv::Point3d>> &matches,
                      std::vector<GravPoseRansacModel> *models) const override;

   double Error(const std::pair<cv::Point3d, cv::Point3d> &match, const GravPoseRansacModel &model) const override;

   Eigen::Matrix3d EK;
   const Eigen::Quaterniond Q;
   const Eigen::Matrix3d R;
   Eigen::Matrix3d KI;
   const double depth;
   int sample_size;
};

struct Grav3DRansacEstimator : public theia::Estimator<std::pair<cv::Point3d, cv::Point2d>, GravPoseRansacModel>
//=================================================================================================================
{
   Grav3DRansacEstimator(const Eigen::Matrix3d& KI_, const Eigen::Quaterniond& Q_, int samples =3) :
      KI(KI_), K(KI_.inverse()), Q(Q_), R(Q_.toRotationMatrix()), sample_size(samples) { }

   double SampleSize() const override { return sample_size; }

   bool EstimateModel(const std::vector<std::pair<cv::Point3d, cv::Point2d>> &matches,
                      std::vector<GravPoseRansacModel> *models) const override;

   double Error(const std::pair<cv::Point3d, cv::Point2d> &match, const GravPoseRansacModel &model) const override;

   Eigen::Matrix3d KI, K;
   const Eigen::Quaterniond Q;
   const Eigen::Matrix3d R;
   int sample_size;
};
#else
#include "Ransac.hh"

struct Grav2DRansacData
//=====================
{
   Grav2DRansacData(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts) : pts(pts) {}

   const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts;
};

struct Grav3DRansacData
//=====================
{
   Grav3DRansacData(const std::vector<std::pair<cv::Point3d, cv::Point2d>>& pts) : pts(pts) {}

   const std::vector<std::pair<cv::Point3d, cv::Point2d>>& pts;
};

template <typename Pt>
static inline size_t RANSAC_copy_points(const Grav2DRansacData& samples, const std::vector<size_t>& sampleIndices,
                                        std::vector<Pt>& train_pts, std::vector<Pt>& query_pts)
//-----------------------------------------------------------------------------------------------------
{
   const size_t no = sampleIndices.size();
   for (size_t i=0; i<no; i++)
   {
      size_t index = sampleIndices[i];
      train_pts.emplace_back(samples.pts[index].first);
      query_pts.emplace_back(samples.pts[index].second);
   }
   return no;
}

static inline size_t copy_points3d(const Grav3DRansacData& samples, const std::vector<size_t>& sampleIndices,
                                   std::vector<cv::Point3d>& world_pts, std::vector<cv::Point2d>& query_pts)
//-----------------------------------------------------------------------------------------------------
{
   const size_t no = sampleIndices.size();
   for (size_t i=0; i<no; i++)
   {
      size_t index = sampleIndices[i];
      const cv::Point3d& wpt = samples.pts[index].first;
      const cv::Point2d& ipt = samples.pts[index].second;
      world_pts.emplace_back(wpt.x, wpt.y, wpt.z);
      query_pts.emplace_back(ipt.x, ipt.y);
   }
   return no;
}

struct PlanarRansacEstimator
//==========================
{
   PlanarRansacEstimator(const cv::Mat& K_, const Eigen::Quaterniond& Q_) : Q(Q_), R(Q_.toRotationMatrix())
   //-------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> EK_((double *) K_.data);
      EK = EK_;
      KI = EK.inverse();
   }

   PlanarRansacEstimator(Eigen::Matrix3d K_, const Eigen::Quaterniond& Q_) : EK(std::move(K_)), Q(Q_), R(Q_.toRotationMatrix())
   //-------------------------------------------------------------------------------------------
   {
      KI = EK.inverse();
   }

   Eigen::Matrix3d EK;
   const Eigen::Quaterniond Q;
   const Eigen::Matrix3d R;
   Eigen::Matrix3d KI;

};

struct Grav2DRansacEstimator : public PlanarRansacEstimator
//==============================================================
{
   Grav2DRansacEstimator(const cv::Mat& K_, const Eigen::Quaterniond& Q_) : PlanarRansacEstimator(K_, Q_) {}

   Grav2DRansacEstimator(const Eigen::Matrix3d K_, const Eigen::Quaterniond& Q_) : PlanarRansacEstimator(K_, Q_) {}

   const int estimate(const Grav2DRansacData &samples, const std::vector<size_t> &sampleIndices,
                      templransac::RANSACParams &parameters,
                      std::vector<GravPoseRansacModel> &models) const;

   const void error(const Grav2DRansacData &samples, const std::vector<size_t> &sampleIndices,
                    GravPoseRansacModel &model, std::vector<size_t> &inlier_indices,
                    std::vector<size_t> &outlier_indices, double error_threshold) const;
};

struct Grav2DDepthRansacEstimator : public PlanarRansacEstimator
//==============================================================
{
   Grav2DDepthRansacEstimator(const cv::Mat& K_, const Eigen::Quaterniond& Q_, const double depth_) :
         PlanarRansacEstimator(K_, Q_), depth(depth_) {}

   Grav2DDepthRansacEstimator(const Eigen::Matrix3d K_, const Eigen::Quaterniond& Q_, const double depth_) :
         PlanarRansacEstimator(K_, Q_), depth(depth_) {}

   const int estimate(const Grav2DRansacData& samples, const std::vector<size_t>& sampleIndices,
                      templransac::RANSACParams& parameters,
                      std::vector<GravPoseRansacModel>& models) const;

   const void error(const Grav2DRansacData& samples, const std::vector<size_t>& sampleIndices,
                    GravPoseRansacModel& model, std::vector<size_t>& inlier_indices,
                    std::vector<size_t>& outlier_indices, double error_threshold) const;

   const double depth;
};

struct Grav3DRansacEstimator
//==========================
{
   Grav3DRansacEstimator(const Eigen::Matrix<double, 3, 3>& KI_, const Eigen::Quaterniond& Q_)
         : Q(Q_), KI(KI_), K(KI.inverse()), R(Q_.toRotationMatrix()) { }

   const int estimate(const Grav3DRansacData& samples, const std::vector<size_t>& sampleIndices,
                      templransac::RANSACParams& parameters,
                      std::vector<GravPoseRansacModel>& models) const;

   const void error(const Grav3DRansacData& samples, const std::vector<size_t>& sampleIndices,
                    GravPoseRansacModel& model, std::vector<size_t>& inlier_indices,
                    std::vector<size_t>& outlier_indices, double error_threshold) const;

   const Eigen::Quaterniond Q;
   const Eigen::Matrix3d& KI;
   const Eigen::Matrix3d K, R;
};

#endif // USE_THEIA_RANSAC

#endif //_POSE2DRANSAC_H


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  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 MPI-IS 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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Andreas Orthey */

#include <ompl/base/spaces/TorusStateSpace.h>
#include <ompl/tools/config/MagicConstants.h>
#include <cstring>

#include <boost/math/constants/constants.hpp>
using namespace boost::math::double_constants;  // pi
using namespace ompl::base;

TorusStateSampler::TorusStateSampler(const StateSpace *space) : StateSampler(space)
{
}

void TorusStateSampler::sampleUniform(State *state)
{
    // https://stackoverflow.com/questions/26300510/generating-random-points-on-a-surface-of-an-n-dimensional-torus
    // Based on publication "Random selection of points distributed on curved surfaces."
    // Link: https://iopscience.iop.org/article/10.1088/0031-9155/32/10/009/pdf
    const auto *T = static_cast<const TorusStateSpace *>(space_);

    bool acceptedSampleFound = false;
    while (!acceptedSampleFound)
    {
        double u = rng_.uniformReal(-pi, pi);
        double v = rng_.uniformReal(-pi, pi);

        const double &R = T->getMajorRadius();
        const double &r = T->getMinorRadius();

        double vprime = (R + r * cos(v)) / (R + r);

        double mu = rng_.uniformReal(0, 1);
        if (mu <= vprime)
        {
            TorusStateSpace::StateType *T = state->as<TorusStateSpace::StateType>();
            T->setS1S2(u, v);
            acceptedSampleFound = true;
        }
    }
}

void TorusStateSampler::sampleUniformNear(State *state, const State *near, double distance)
{
    TorusStateSpace::StateType *T = state->as<TorusStateSpace::StateType>();
    const TorusStateSpace::StateType *Tnear = near->as<TorusStateSpace::StateType>();
    T->setS1(rng_.uniformReal(Tnear->getS1() - distance, Tnear->getS1() + distance));
    T->setS2(rng_.uniformReal(Tnear->getS2() - distance, Tnear->getS2() + distance));
    space_->enforceBounds(state);
}

void TorusStateSampler::sampleGaussian(State *state, const State *mean, double stdDev)
{
    TorusStateSpace::StateType *T = state->as<TorusStateSpace::StateType>();
    const TorusStateSpace::StateType *Tmean = mean->as<TorusStateSpace::StateType>();
    T->setS1(rng_.gaussian(Tmean->getS1(), stdDev));
    T->setS2(rng_.gaussian(Tmean->getS2(), stdDev));

    space_->enforceBounds(state);
}

TorusStateSpace::TorusStateSpace(double majorRadius, double minorRadius)
  : majorRadius_(majorRadius), minorRadius_(minorRadius)
{
    setName("Torus" + getName());
    type_ = STATE_SPACE_TORUS;
    addSubspace(std::make_shared<SO2StateSpace>(), 1.0);
    addSubspace(std::make_shared<SO2StateSpace>(), 1.0);
    lock();
}

StateSamplerPtr TorusStateSpace::allocDefaultStateSampler() const
{
    return std::make_shared<TorusStateSampler>(this);
}

double TorusStateSpace::distance(const State *state1, const State *state2) const
{
    const auto *cstate1 = static_cast<const CompoundState *>(state1);
    const auto *cstate2 = static_cast<const CompoundState *>(state2);
    double x = components_[0]->distance(cstate1->components[0], cstate2->components[0]);
    double y = components_[1]->distance(cstate1->components[1], cstate2->components[1]);
    return sqrtf(x * x + y * y);
}

State *TorusStateSpace::allocState() const
{
    auto *state = new StateType();
    allocStateComponents(state);
    return state;
}

double TorusStateSpace::getMajorRadius() const
{
    return majorRadius_;
}

double TorusStateSpace::getMinorRadius() const
{
    return minorRadius_;
}

Eigen::Vector3f TorusStateSpace::toVector(const State *state) const
{
    Eigen::Vector3f v;

    const TorusStateSpace::StateType *s = state->as<TorusStateSpace::StateType>();
    float theta = s->getS1();
    float phi = s->getS2();

    const double &R = majorRadius_;
    const double &r = minorRadius_;

    v[0] = (R + r*cos(phi))*cos(theta);
    v[1] = (R + r*cos(phi))*sin(theta);
    v[2] = r*sin(phi);

    return v;
}


/**
 * Copyright (c) 2018, The Akatsuki(Jacob.lsx). All rights reserved.
 */

#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>

#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <chrono>

using namespace std;
using namespace cv;

/* find the two photo feature matches points */
void find_feature_matches(const cv::Mat&, const cv::Mat&, std::vector<cv::KeyPoint>&, std::vector<cv::KeyPoint>&, std::vector<cv::DMatch>&);

/* Converts the pixel coordinate system to the normalized imaging plane coordinate system */
cv::Point2d pixel2cam(const cv::Point2d&, const cv::Mat&);

/*  */
void pose_estimation_3d3d(const std::vector<cv::Point3f>&, const std::vector<cv::Point3f>&, cv::Mat&, cv::Mat&);

/*  */
void bundleAdjustment(const std::vector<cv::Point3f>&, const std::vector<cv::Point3f>&, cv::Mat&, cv::Mat&);

int main(int argc, char** argv)
{
    google::InitGoogleLogging(argv[0]);
    if (argc != 5) {
        cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2." << endl;
        return 1;
    }

    //-- 1st, read photo
    cv::Mat img_1 = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread(argv[2], CV_LOAD_IMAGE_COLOR);
    if (img_1.empty() || img_2.empty()) {
        cerr << "img1 or img2 is empty." << endl;
        return 1;
    }

    //-- 2nd, find the two photo feature matches points
    std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
    std::vector<cv::DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "feature matches points total is " << matches.size() << endl;

    //-- 3rd, create 3 dimensions point correspondences
    cv::Mat depth1 = cv::imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);      // 深度图为16位无符号数，单通道图像
    cv::Mat depth2 = cv::imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);
    if (depth1.empty() || depth2.empty()) {
        cerr << "depth1 or depth2 is empty." << endl;
        return 1;
    }

    cv::Mat K = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    std::vector<cv::Point3f> pts1, pts2;

    for (cv::DMatch m : matches) {
        ushort d1 = depth1.ptr<unsigned short>((int)keypoints_1[m.queryIdx].pt.y)[(int)keypoints_1[m.queryIdx].pt.x];
        ushort d2 = depth2.ptr<unsigned short>((int)keypoints_2[m.trainIdx].pt.y)[(int)keypoints_2[m.trainIdx].pt.x];
        if (d1 == 0 || d2 == 0) {
            continue;
        }
        cv::Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        cv::Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        float dd1 = float(d1) / 5000.0;
        float dd2 = float(d2) / 5000.0;
        pts1.push_back(cv::Point3f(p1.x * dd1, p1.y * dd1, dd1));
        pts2.push_back(cv::Point3f(p2.x * dd2, p2.y * dd2, dd2));
    }
    cout << "3d-3d pairs: " << pts1.size() << endl;

    cv::Mat R, t;
    pose_estimation_3d3d(pts1, pts2, R, t);
    cout << "ICP vid SVD results: " << endl;
    cout << "R = " << R << endl;
    cout << "t = " << t << endl;
    cout << "R_inv = " << R.t() << endl;
    cout << "t_inv = " << -R.t() * t << endl;

    cout << "calling bundle adjustment. \r\n";

    bundleAdjustment(pts1, pts2, R, t);

    // verify p1 = R*p2 + t
    for (int i = 0; i < 5; i ++) {
        cout << "p1 = " << pts1[i] << endl;
        cout << "p2 = " << pts2[i] << endl;
        cout << "R*p2 + t = " << R * (Mat_<double>(3, 1) << pts2[i].x, pts2[i].y, pts2[i].z) + t << endl;
        cout << endl;
    }

    return 0;
}

/**
 * find the two photo feature matches points
 */
void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2, std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2, std::vector<cv::DMatch>& matches)
{
    //-- Initialize
    cv::Mat descriptors_1, descriptors_2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    //-- 1st: detect Oriented FAST KeyPoint
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 2nd: Calculates BRIEF descriptors with KeyPoint's coordinate
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 3rd: 对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    std::vector<cv::DMatch> match;
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 4th: 匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    /* 找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 */
    for (int i = 0; i < descriptors_1.rows; i++) {
        double dist = match[i].distance;
        if (dist < min_dist) {
            min_dist = dist;
        }
        if (dist > max_dist) {
            max_dist = dist;
        }
    }

    printf("-- Max distance : %f \r\n", max_dist);
    printf("-- Min distance : %f \r\n", min_dist);

    // 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_2.rows; i ++) {
        if (match[i].distance <= max(2*min_dist, 30.0)) {
            matches.push_back(match[i]);
        }
    }
}

/**
 * Converts the pixel coordinate system to the normalized imaging plane coordinate system
 */
cv::Point2d pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
    return cv::Point2d
        (
            (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
            (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
        );
}

void pose_estimation_3d3d(const std::vector<cv::Point3f>& pts1, const std::vector<cv::Point3f>& pts2, cv::Mat& R, cv::Mat& t)
{
    // center of mass
    cv::Point3f p1, p2;
    int N = pts1.size();
    for (int i = 0; i < N; i ++) {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = cv::Point3f(Vec3f(p1) / N);
    p2 = cv::Point3f(Vec3f(p2) / N);

    // remove the center
    std::vector<cv::Point3f> q1(N), q2(N);
    for (int i = 0; i < N; i ++) {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i = 0; i < N; i ++) {
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
    }
    cout << "W = " << W << endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    // 利用SVD求解3D-3D变换，需要U和V的行列式同号。换言之，旋转矩阵的行列式只能为1，不能为-1
    if (U.determinant() * V.determinant() < 0) {
        for (int x = 0; x < 3; ++x) {
            U(x, 2) *= -1;
        }
    }
    cout << "U = " << U << endl;
    cout << "V = " << V << endl;

    Eigen::Matrix3d R_ = U * (V.transpose());
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

    // convert to cv::mat
    R = (cv::Mat_<double>(3, 3) <<
        R_(0, 0), R_(0, 1), R_(0, 2),
        R_(1, 0), R_(1, 1), R_(1, 2),
        R_(2, 0), R_(2, 1), R_(2, 2)
    );
    t = (cv::Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

class ReprojectionError {
public:
    ReprojectionError(const cv::Point3f &point1, const cv::Point3f &point2) : pt1_(point1), pt2_(point2) {}

    template<typename T>
    bool operator() (const T* const r, const T* const t, T* residuals) const
    {
        T p[3];
        T pt2[3] = {(T)pt2_.x, (T)pt2_.y, (T)pt2_.z};

        ceres::AngleAxisRotatePoint(r, pt2, p);

        p[0] += t[0];
        p[1] += t[1];
        p[2] += t[2];

        residuals[0] = p[0] - (T)pt1_.x;
        residuals[1] = p[1] - (T)pt1_.y;
        residuals[2] = p[2] - (T)pt1_.z;

        return true;
    }

    static ceres::CostFunction* create(const cv::Point3f &point1, const cv::Point3f &point2)
    {
        return new ceres::AutoDiffCostFunction<ReprojectionError, 3, 3, 3>(new ReprojectionError(point1, point2));
    }

private:
    cv::Point3f pt1_, pt2_;
};

void bundleAdjustment(const std::vector<cv::Point3f>& pts1, const std::vector<cv::Point3f>& pts2, cv::Mat& R, cv::Mat& t)
{
    cv::Mat r;
    cv::Rodrigues(R, r);

    double rotation[3] = {0.f};
    double translation[3] = {0.f};

#if 1
    for (int i = 0; i < 3; i++) {
        rotation[i] = r.at<double>(i, 0);
        translation[i] = r.at<double>(i, 0);
    }
#endif

    ceres::Problem problem;
    for (int i = 0; i < pts1.size(); i ++) {
        ceres::CostFunction *costFunction = ReprojectionError::create(pts1[i], pts2[i]);
        problem.AddResidualBlock(costFunction, nullptr, rotation, translation);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    ceres::Solve(options, &problem, &summary);

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2 - t1);

    cout << summary.BriefReport() << endl;

    cout << "optimization costs time: " << time_used.count() << " seconds." << "\r\n";

    cout << endl << "after optimization: " << endl;

    cv::Mat R_vec = (cv::Mat_<double>(3, 1) << rotation[0], rotation[1], rotation[2]);
    cv::Mat Rotation;
    cv::Rodrigues(R_vec, Rotation);

    cout << "R = \r\n" << Rotation << endl;
    cout << "t = \r\n" << translation[0] << ", " << translation[1] << ", " << translation[2] << endl;

    R = Rotation;
    t = (cv::Mat_<double>(3,1) << translation[0], translation[1], translation[2]);
}


/**
 * Multidimensional Static Matrix C++11
 * Copyright Emanuele Ruffaldi (2015) at Scuola Superiore Sant'Anna Pisa
 *
 * AKA extreme parameter packs
 *
 * Core functionalities ... the rest is "trivial"
 *
 * Under Apache License
 */
#pragma once
#include <Eigen/Dense>
#include <initializer_list>
#include <iostream>
#include <type_traits>
#include "multidim_details.hpp"

namespace multidim
{
	template <class T, class TS>
	class MultiDimNView;

	/**
	 * Base class of Multidimensional Static matrix of elements of type T
	 *
	 * TS is a sequence of types, where each type is (size,step)
	 *
	 */
	template <class T, class TS>
	class MultiDimNBase
	{
	public:
		using value_t = T;
		static constexpr int Ncount = TS::size;
		static constexpr int Ntot = details::productseq<TS>::value;
		using indexvector_t = Eigen::Matrix<int,Ncount,1>; // instead of std::vector<int>

		template <int i>
		using getsizetype = std::integral_constant<int,TS::template pick<i>::xsize>;

		template <int i>
		using getsteptype = std::integral_constant<int,TS::template pick<i>::xstep>;

		template <int i>
		constexpr int getssize() const { return details::saccessorseq<i,TS>::xsize; }

		/// can't move in base
		template <int i>
		constexpr int getsstep() const { return details::saccessorseq<i,TS>::xstep; }

		constexpr int getsize(int i) const { return details::daccessorseq<TS>::type::size(i); }

		constexpr int getstep(int i) const { return details::daccessorseq<TS>::type::step(i); }

		constexpr int ndims() const { return Ncount; }

		constexpr int numel() const { return Ntot; }

		/// compile type via pack: offsetvalue<1,2,3,4>::value
		template<int... I>
		using offsetvalue = typename details::offsetcompute<TS,I...>::type;

		/// returns offset from variadic via initializer list
		template <class...X>
		constexpr int offset(X... I) const
		{
			static_assert(sizeof...(I) == Ncount,"wrong number of dimensions");
			return offset({I...});
		}

		template <class dummy=void>
		auto squeeze() -> MultiDimNView<T, typename TS::template removeif<singletondim> >;

		template <int...neworder>
		auto permutedim() -> MultiDimNView<T, typename TS::template permuted<neworder...> >;

		// TODO: let last dimension free to match
		template <int...N>
		auto reshapeR() -> MultiDimNView<T, typename details::rowmajorstepper<N...> >;

		template <int...N>
		auto reshapeC() -> MultiDimNView<T, typename details::colmajorstepper<N...> >;

		/// via initializer list (bit ugly...)
		/// we loop over i Ncount because is compile time length, while for(int x: L) IS NOT
		int offset(const std::initializer_list<int> & L) const
		{
			assert(L.size() == Ncount); //,"wrong number of dims");
			auto x = L.begin(); /// C++14 constexpr
			int o = 0;
			for(int i = 0; i < Ncount; x++, i++)
				o += *x *getstep(i);
			return o;
		}
	};

	/**
	 * View over a TS (typesequence of sizes)
	 */
	template <class T, class TS>
	class MultiDimNView: public MultiDimNBase<T,TS>
	{
	public:
		using base_t = MultiDimNBase<T,TS>;
		using data_t = T*;
		using map_t = Eigen::Map<Eigen::Matrix<T,base_t::Ntot,1> >;

		MultiDimNView(data_t x):  data_(x)
		{
		}

		constexpr const T * data() const { return data_; }

		T * data() { return data_; }

		void setOnes()
		{
			map_t(data_).setOnes();
		}

		void setZero()
		{
			map_t(data_).setZero();
		}

		/// COMMON ACROSS MultiDimNView and MultiDimN
		/// TODO: replace with curiously recursive pattern
		/// limit by dimension
		/// FUTURE: variant with index compile time
		template<int dim>
	 	auto limit1(int index) -> MultiDimNView<T, typename TS::template drop<dim> >
		{
			return (data()+index*  MultiDimNBase<T,TS>::template getsteptype<dim>::value ); // via implicit construction
		}

		/// for the dimension dim takes from the given index1 up to newsize elements. This is not reducing the number of dimensions
		/// FUTURE: variant with index compile time
		template<int dim, int newsize>
		auto limit1block(int index1) ->
			MultiDimNView<T, typename TS::template replacetype<dim,sspair<newsize,  MultiDimNBase<T,TS>::template getsteptype<dim>::value   > > >
		{
			static_assert(newsize <= MultiDimNBase<T,TS>::template getsizetype<dim>::value,"sub-size cannot be larger than original");

			return data() + index1*MultiDimNBase<T,TS>::template getsteptype<dim>::value;
		}
		template <int ...neworder>
		auto permutedim() -> MultiDimNView<T, typename TS::template permuted<neworder...> >
		{
			static_assert(sizeof...(neworder) == TS::size,"permutation requires same order");
			return data();
		}

		// TODO: let last dimension free to match
		template <int...N>
		auto reshapeR() -> MultiDimNView<T, typename details::rowmajorstepper<N...> >
		{
			using RT = MultiDimNView<T, typename details::rowmajorstepper<N...> >;

			static_assert(RT::Ntot == MultiDimNBase<T,TS>::Ntot,"result requires same number of elements");
			return data();
		}

		template <int...N>
		auto reshapeC() -> MultiDimNView<T, typename details::colmajorstepper<N...> >
		{
			using RT = MultiDimNView<T, typename details::colmajorstepper<N...> >;

			static_assert(RT::Ntot == MultiDimNBase<T,TS>::Ntot,"result requires same number of elements");
			return data();
		}

		auto squeeze() -> MultiDimNView<T, typename TS::template removeif<singletondim> >
		{
			return data();
		}

	private:
		data_t data_;
	};

	// column wise
	template <class T, class TS>
	class MultiDimN: public MultiDimNBase<T,TS>
	{
	public:
		using data_t = Eigen::Matrix<T,MultiDimNBase<T,TS>::Ntot,1>;

		MultiDimN()
		{

		}

		const T * data() const { return data_.data(); }

		T * data() { return data_.data(); }

		void setOnes()
		{
			data_.setOnes();
		}

		void setZero()
		{
			data_.setZero();
		}

		/// COMMON ACROSS MultiDimNView and MultiDimN
		/// TODO: replace with curiously recursive pattern
		template<int dim>
		auto limit1(int index) -> MultiDimNView<T, typename TS::template drop<dim> >
		{
			return (data()+index*  MultiDimNBase<T,TS>::template getsteptype<dim>::value ); // via implicit construction
		}

		/// for the dimension dim takes from the given index1 up to newsize elements. This is not reducing the number of dimensions
		/// FUTURE: variant with index compile time
		template<int dim, int newsize>
		auto limit1block(int index1) ->
			MultiDimNView<T, typename TS::template replacetype<dim,sspair<newsize,  MultiDimNBase<T,TS>::template getsteptype<dim>::value   > > >
		{
			static_assert(newsize <= MultiDimNBase<T,TS>::template getsizetype<dim>::value,"sub-size cannot be larger than original");

			return data() + index1*MultiDimNBase<T,TS>::template getsteptype<dim>::value;
		}

		template <int ...neworder>
		auto permutedim() -> MultiDimNView<T, typename TS::template permuted<neworder...> >
		{
			static_assert(sizeof...(neworder) == TS::size,"permutation requires same order");
			return data();
		}

		// TODO: let last dimension free to match
		template <int...N>
		auto reshapeR() -> MultiDimNView<T, typename details::rowmajorstepper<N...> >
		{
			using RT = MultiDimNView<T, typename details::rowmajorstepper<N...> >;

			static_assert(RT::Ntot == MultiDimNBase<T,TS>::Ntot,"result requires same number of elements");
			return data();
		}

		template <int...N>
		auto reshapeC() -> MultiDimNView<T, typename details::colmajorstepper<N...> >
		{
			using RT = MultiDimNView<T, typename details::colmajorstepper<N...> >;

			static_assert(RT::Ntot == MultiDimNBase<T,TS>::Ntot,"result requires same number of elements");
			return data();
		}

		auto squeeze() -> MultiDimNView<T, typename TS::template removeif<singletondim> >
		{
			return data();
		}

	private:
		data_t data_;
	};

	/// declares a multidim with type T and given dimensions in row-major
	template <class T, int...N>
	using MultiDimNRow = MultiDimN<T, typename details::rowmajorstepper<N...> >;

	/// declares a multidim with type T and given dimensions in col-major
	template <class T, int...N>
	using MultiDimNCol = MultiDimN<T, typename details::colmajorstepper<N...> >;
}


#include "utils.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/integer.hpp>
#include <memory>
using namespace std;
using Bint = boost::multiprecision::cpp_int;

Bint f(Bint const& x, Bint const& c, Bint const& n);

class LinkedList{
    public:
        LinkedList(Bint value_);
        ~LinkedList();
        Bint getValue();
        LinkedList* getAfter();
        void setAfter(LinkedList* after_);
    private:
        Bint value;
        LinkedList* after;
};

LinkedList::LinkedList(Bint value_){
    value = value_;
    after = nullptr;
}

LinkedList::~LinkedList(){
    ;
}

Bint LinkedList::getValue(){
    return value;
}

LinkedList* LinkedList::getAfter(){
    return after;
}

void LinkedList::setAfter(LinkedList* after_){
    after = after_;
}

string PollardsRhoFactorizer_cppfunc(string s, long c_){
    Bint n(s);
    Bint c(c_);
    LinkedList* x = new LinkedList(Bint(2));
    LinkedList* y = new LinkedList(f(x->getValue(), c, n));
    x->setAfter(y);
    Bint d = gcd(abs(x->getValue()-y->getValue()), n);
    while(d==1){
        LinkedList* z1 = new LinkedList(f(y->getValue(), c, n));
        LinkedList* z2 = new LinkedList(f(z1->getValue(), c, n));
        y->setAfter(z1);
        z1->setAfter(z2);
        y = z2;

        LinkedList* tmp;
        tmp = x;
        x = x->getAfter();
        delete tmp;
        d = gcd(abs(x->getValue()-y->getValue()), n);
    }

    if(d<n){
        return d.str();
    }
    else{
        return "1";
    }
}

Bint f(Bint const& x, Bint const& c, Bint const& n){
    return (x*x+c)%n;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/
// http://csclab.murraystate.edu/~bob.pilgrim/445/munkres.html

#pragma once

#include "AlgorithmUtils.hpp"
#include "FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class Munkres
{
public:
  using intPair = std::pair<int, int>;
  void init(index rows, index cols)
  {
    using namespace Eigen;
    index N = std::max(rows, cols);
    mCost = ArrayXXd::Zero(N, N);
    mRowMin = ArrayXd::Zero(N);
    mColMin = ArrayXd::Zero(N);
    mMask = ArrayXXi::Zero(N, N);
    mRowCover = ArrayXi::Zero(N);
    mColCover = ArrayXi::Zero(N);
    mPath = ArrayXXi::Zero(2 * N + 1, 2);
  }

  void reset()
  {
    mCost.setZero();
    mRowMin.setZero();
    mColMin.setZero();
    mMask.setZero();
    mRowCover.setZero();
    mColCover.setZero();
    mPath.setZero();
  }

  void process(Eigen::Ref<const Eigen::ArrayXXd> costMatrix,
               Eigen::Ref<Eigen::ArrayXi>        result)
  {
    bool done;
    reset();
    double maxCost = costMatrix.maxCoeff();
    mCost = Eigen::ArrayXXd::Ones(mCost.rows(), mCost.cols());
    mCost = mCost * (10.0 * maxCost);
    mCost.block(0, 0, costMatrix.rows(), costMatrix.cols()) = costMatrix;
    step1();
    step2();
    done = step3();
    while (!done)
    {
      intPair Z = step4();
      while (Z.first < 0)
      {
        step6();
        Z = step4();
      }
      step5(Z);
      done = step3();
    }
    for (int i = 0; i < result.size(); i++)
    { mMask.row(i).maxCoeff(&result(i)); }
  }

  void step1()
  {
    mRowMin = mCost.rowwise().minCoeff();
    mColMin = (mCost.colwise() - mRowMin).colwise().minCoeff();
    for (int i = 0; i < mCost.rows(); i++)
    {
      double min = mCost.row(i).minCoeff();
      mCost.row(i) -= min;
    }
  }

  void step2()
  {
    for (int i = 0; i < mCost.rows(); i++)
    {
      for (int j = 0; j < mCost.cols(); j++)
      {
        if (mCost(i, j) == 0 && mRowCover(i) == 0 && mColCover(j) == 0)
        {
          mMask(i, j) = 1;
          mRowCover(i) = 1;
          mColCover(j) = 1;
        }
      }
    }
    mRowCover.setZero();
    mColCover.setZero();
  }

  bool step3()
  {
    for (int i = 0; i < mMask.rows(); i++)
    {
      for (int j = 0; j < mMask.cols(); j++)
      {
        if (mMask(i, j) == 1) { mColCover(j) = 1; }
      }
    }
    Eigen::Index nCovered = (mColCover == 1).count();
    return nCovered >= mMask.rows() || nCovered >= mMask.cols();
  }

  intPair step4()
  {
    int     row = -1, col = -1;
    intPair result = std::make_pair(row, col);
    while (true)
    {
      intPair pos = findZero();
      row = pos.first;
      col = pos.second;
      if (row < 0) { break; }
      mMask(row, col) = 2;
      Eigen::ArrayXi r = mMask.row(row);
      int            colStar = findValue(r, 1);
      if (colStar >= 0)
      {
        col = colStar;
        mRowCover(row) = 1;
        mColCover(col) = 0;
      }
      else
      {
        result = std::make_pair(row, col);
        break;
      }
    }
    return result;
  }

  void step5(const intPair Z)
  {
    int row = -1, col = -1;
    int pathCount = 0;
    mPath(pathCount, 0) = Z.first;
    mPath(pathCount, 1) = Z.second;
    while (true)
    {
      int            tmp = mPath(pathCount, 1);
      Eigen::ArrayXi tmpCol = mMask.col(tmp);
      row = findValue(tmpCol, 1);
      if (row == -1) break;
      pathCount++;
      mPath(pathCount, 0) = row;
      mPath(pathCount, 1) = mPath(pathCount - 1, 1);
      Eigen::ArrayXi r = mMask.row(mPath(pathCount, 0));
      col = findValue(r, 2);
      pathCount++;
      mPath(pathCount, 0) = mPath(pathCount - 1, 0);
      mPath(pathCount, 1) = col;
    }
    augmentPath(pathCount);
    mRowCover.setZero();
    mColCover.setZero();
    erasePrimes();
  }

  void step6()
  {
    double m = minCost();
    for (int i = 0; i < mCost.rows(); i++)
    {
      for (int j = 0; j < mCost.cols(); j++)
      {
        if (mRowCover(i) == 1) mCost(i, j) += m;
        if (mColCover(j) == 0) mCost(i, j) -= m;
      }
    }
  }

  double minCost()
  {
    double minVal = std::numeric_limits<double>::max();
    for (int i = 0; i < mCost.rows(); i++)
    {
      for (int j = 0; j < mCost.cols(); j++)
      {
        if (mRowCover(i) == 0 && mColCover(j) == 0)
        {
          double v = mCost(i, j);
          minVal = (minVal > v) ? v : minVal;
        }
      }
    }
    return minVal;
  }

  void erasePrimes()
  {
    for (int i = 0; i < mMask.rows(); i++)
    {
      for (int j = 0; j < mMask.cols(); j++)
      {
        if (mMask(i, j) == 2) { mMask(i, j) = 0; }
      }
    }
  }

  intPair findZero()
  {
    for (int i = 0; i < mCost.rows(); i++)
      for (int j = 0; j < mCost.cols(); j++)
      {
        if (mCost(i, j) == 0 && mRowCover(i) == 0 && mColCover(j) == 0)
          return std::make_pair(i, j);
      }
    return std::make_pair(-1, -1);
  }

  int findValue(Eigen::Ref<const Eigen::ArrayXi> vector, const int val)
  {
    for (int i = 0; i < vector.size(); i++)
    {
      if (vector(i) == val) return i;
    }
    return -1;
  }

  void augmentPath(const int count)
  {
    for (int i = 0; i < count + 1; i++)
    {
      int c = mPath(i, 0), r = mPath(i, 1);
      mMask(c, r) = mMask(c, r) == 1 ? 0 : 1;
    }
  }

private:
  Eigen::ArrayXXd mCost;
  Eigen::ArrayXd  mRowMin;
  Eigen::ArrayXd  mColMin;
  Eigen::ArrayXXi mMask;
  Eigen::ArrayXXi mPath;
  Eigen::ArrayXi  mRowCover;
  Eigen::ArrayXi  mColCover;
};
} // namespace algorithm
} // namespace fluid


#include <boost/numeric/odeint.hpp>

typedef std::vector< double > state_type;

const double gam = 0.15;

void harmonic_oscillator( const state_type &x , state_type &dxdt , const double /* t */ )
{
    dxdt[0] = x[1];
    dxdt[1] = -x[0] - gam*x[1];
}

int main()
{
  state_type x(2);
  x[0] = 1.0; // start at x=1.0, p=0.0
  x[1] = 0.0;

  using namespace boost::numeric::odeint;
  size_t steps = integrate( harmonic_oscillator, x, 0.0, 10.0, 0.1 );
  if(steps == 0)
    return 1;

  return 0;
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <boost/numeric/odeint.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/feedback/mpc.hpp>

#include <chrono>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

using namespace std::chrono_literals;
using namespace boost::numeric::odeint;

using Time = std::chrono::duration<double>;

template<typename T>
using G = Eigen::Vector2<T>;
template<typename T>
using U = Eigen::Matrix<T, 1, 1>;

using Gd = G<double>;
using Ud = U<double>;

int main()
{
  using std::sin;
  std::srand(5);

  // system variables
  Gd g = Gd::Random();
  Ud u;

  // dynamics
  auto f = []<typename T>(Time, const G<T> & x, const U<T> u) -> smooth::Tangent<G<T>> {
    return {x(1), u(0)};
  };

  // parameters
  smooth::feedback::MPCParams<Gd, Ud> prm{
    .T = 5,
    .K = 20,
    .weights =
      {
        .Q  = Eigen::Matrix2d::Identity(),
        .QT = 0.1 * Eigen::Matrix2d::Identity(),
        .R  = Eigen::Matrix<double, 1, 1>::Constant(0.1),
      },
    .ulim =
      smooth::feedback::ManifoldBounds<Ud>{
        .A = Eigen::Matrix<double, 1, 1>(1),
        .c = Ud::Zero(),
        .l = Eigen::Matrix<double, 1, 1>(-0.5),
        .u = Eigen::Matrix<double, 1, 1>(0.5),
      },
  };

  // create MPC object and set input bounds, and desired trajectories
  smooth::feedback::MPC<Time, Gd, Ud, decltype(f)> mpc(f, prm);
  mpc.set_xdes([]<typename T>(T t) -> G<T> { return G<T>{-0.5 * sin(0.3 * t), 0}; });
  mpc.set_udes([]<typename T>(T) -> U<T> { return U<T>::Zero(); });

  // prepare for integrating the closed-loop system
  runge_kutta4<Gd, double, smooth::Tangent<Gd>, double, vector_space_algebra> stepper{};
  const auto ode = [&f, &u](const Gd & x, smooth::Tangent<Gd> & d, double t) -> void {
    d = f(Time(t), x, u);
  };
  std::vector<double> tvec, xvec, vvec, uvec;

  // integrate closed-loop system
  for (std::chrono::milliseconds t = 0s; t < 60s; t += 50ms) {
    // compute MPC input
    auto [u_mpc, code] = mpc(t, g);
    u                  = u_mpc;
    if (code != smooth::feedback::QPSolutionStatus::Optimal) {
      std::cerr << "Solver failed with code " << static_cast<int>(code) << std::endl;
    }

    // store data
    tvec.push_back(duration_cast<Time>(t).count());
    xvec.push_back(g.x());
    vvec.push_back(g.y());
    uvec.push_back(u(0));

    // step dynamics
    stepper.do_step(ode, g, 0, 0.05);
  }

#ifdef ENABLE_PLOTTING
  matplot::figure();
  matplot::hold(matplot::on);

  matplot::plot(tvec, xvec)->line_width(2);
  matplot::plot(tvec, matplot::transform(tvec, [](auto t) { return -0.5 * sin(0.3 * t); }), "k--")
    ->line_width(2);
  matplot::plot(tvec, vvec)->line_width(2);
  matplot::plot(tvec, uvec)->line_width(2);
  matplot::legend({"x", "x_{des}", "v", "u"});

  matplot::show();
#else
  std::cout << "TRAJECTORY:" << std::endl;
  for (auto i = 0u; i != tvec.size(); ++i) {
    std::cout << "t=" << tvec[i] << ": x=" << xvec[i] << ", v=" << vvec[i] << std::endl;
  }
#endif
}