#include <iostream>
#include <cstdio>
#include <ctime>
#include <armadillo>
#include "boost/program_options.hpp"

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

// m: numRows, n: numCols
inline double simpleDenseTest_Arma(int m, int n, int num_trials) {

  mat A = randu<mat>(m, n);
  mat B = randu<mat>(m, n);
  mat C = randu<mat>(m, n);
  mat D = randu<mat>(m, n);
  mat E = randu<mat>(m, n);

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

    mat res = ((A + B) / C - D) % E;

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

}

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

  mat A = randu<mat>(m, n);
  mat C = randu<mat>(n, k);
  mat E = randu<mat>(m, k);

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

    E += A * C;

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

  return duration / num_trials;

}

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

  mat A = randu<mat>(m, n);
  mat B = randu<mat>(m, n);
  mat C = randu<mat>(n, k);
  mat D = randu<mat>(n, k);
  mat E = randu<mat>(m, k);

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

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

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

  return duration / num_trials;

}

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

  mat A = randu<mat>(a, a);
  mat B = randu<mat>(a, b);
  mat C = randu<mat>(b, c);
  mat D = randu<mat>(c, d);

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

    mat res = A * B * C * D;

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

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

  vec a = randu<vec>(l);
  vec b = randu<vec>(l);
  vec c = randu<vec>(l);
  vec d = randu<vec>(l);
  vec e = randu<vec>(l);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {
    e = a + b + c + d;
  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;

  return duration / num_trials;
}

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

  if (!skip_vec)
    cout << "Armadillo Vectors Test:\t" << denseVectorTest_Arma(l, num_trials) << endl;
  if (!skip_simple)
    cout << "Armadillo Simple Test:\t" << simpleDenseTest_Arma(m, n, num_trials) << endl;
  if (!skip_gemm) {
    cout << "Armadillo gemmSanity Test:\t" << gemmDenseTest_Arma(m, n, k, num_trials) << endl;
    cout << "Armadillo gemm Test:\t" << gemmDenseTest_Arma(m, n, k, num_trials) << endl;
  }
  if (!skip_mult)
    cout << "Armadillo mulDense Test:\t" << mulDenseTest_Arma(a, b, c, d, num_trials) << endl;

}

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

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

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

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

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

    return 0;
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix4d X = Matrix4d::Random(4,4);
Matrix4d A = X + X.transpose();
cout << "Here is a random symmetric 4x4 matrix:" << endl << A << endl;
Tridiagonalization<Matrix4d> triOfA(A);
Matrix4d pm = triOfA.packedMatrix();
cout << "The packed matrix M is:" << endl << pm << endl;
cout << "The diagonal and subdiagonal corresponds to the matrix T, which is:"
     << endl << triOfA.matrixT() << endl;

  return 0;
}


#include "include/MMVII_all.h"
#include <boost/math/special_functions/fpclassify.hpp>

namespace MMVII
{

// return the variance of  exponential distribution of parameter "a" ( i.e proportiona to  "a^|x|")
double Sigma2FromFactExp(double a)
{
   return (2*a) / Square(1-a);
}

// return the value of "a" such that  exponential distribution of parameter "a" has variance S2
// i.e. this the "inverse" function of Sigma2FromFactExp

double FactExpFromSigma2(double aS2)
{
    return (aS2+1 - sqrt(Square(aS2+1)-Square(aS2))  ) / aS2 ;
}

/* *********************************************** */
/*                                                 */
/*        cComputeStdDev<Dim>                     */
/*                                                 */
/* *********************************************** */

#if (0)
///  Class to compute non biased variance from a statisic

/** Class to compute non biased variance from a statisic
    This generalise the standard formula
            EstimVar = N/(N-1) EmpirVar
    To the case where there is a weighting.

    It can be uses with N variable to factorize the computation on Weight
 */

template <const int Dim> class cComputeStdDev
{
    public :
        typedef  double tTab[Dim];

        cComputeStdDev();

        void Add(const  double * aVal,const double & aPds);
        const double  *  ComputeUnBiasedVar() ;
        const double  *  ComputeBiasedVar() ;
        double  DeBiasFactor() const;

    private :
        double    mSomW; ///< Sum of Weight
        double    mSomWW; ///< Sum of Weight ^2
        tTab      mSomWV;  ///< Weighted som of vals
        tTab      mSomWVV; ///< Weighted som of vals ^2
        tTab      mVar;   ///< Buffer to compute the unbiased variance
        tTab      mBVar;   ///< Buffer to compute the empirical variance
};
#endif

template  <const int Dim> cComputeStdDev<Dim>::cComputeStdDev() :
    mSomW   (0.0),
    mSomWW  (0.0)
{
    for (int aD=0 ; aD<Dim ; aD++)
    {
        mSomWV[aD] = 0.0;
        mSomWVV[aD] = 0.0;
    }
}

template  <const int Dim> void cComputeStdDev<Dim>::Add(const  double *  aVal,const double & aPds)
{
    mSomW += aPds;
    mSomWW += Square(aPds);
    for (int aD=0 ; aD<Dim ; aD++)
    {
        mSomWV[aD] += aPds * aVal[aD];
        mSomWVV[aD] += aPds * Square(aVal[aD]);
    }
}
template  <const int Dim>  double cComputeStdDev<Dim>::DeBiasFactor() const
{
    MMVII_INTERNAL_ASSERT_strong(mSomW>0,"No value in DeBiasFactor");
    return   1 - mSomWW/Square(mSomW);
}

template  <const int Dim> bool    cComputeStdDev<Dim>::OkForUnBiasedVar() const
{
   return (mSomW>0)  && (DeBiasFactor()!=0);
}

template  <const int Dim> const double *   cComputeStdDev<Dim>::ComputeUnBiasedVar()
{
    /* At least, this formula is correct :
         - when all weight are equal => 1-1/N
         - when all but one weight are 0 => 0
         - and finally if all are equal or 0
    */
    double aDebias = DeBiasFactor();
    MMVII_INTERNAL_ASSERT_strong(aDebias>0,"No var can be computed in ComputeVar");

    for (int aD=0 ; aD<Dim ; aD++)
    {
        double aAver = mSomWV[aD] / mSomW;
        double aVar  = mSomWVV[aD] / mSomW;
        aVar = aVar - Square(aAver);
        mVar[aD] = std::max(0.0,aVar) /aDebias;
    }

   return mVar;
}

template  <const int Dim> const double *   cComputeStdDev<Dim>::ComputeBiasedVar()
{
    for (int aD=0 ; aD<Dim ; aD++)
    {
        double aAver = mSomWV[aD] / mSomW;
        double aVar  = mSomWVV[aD] / mSomW;
        aVar = aVar - Square(aAver);
        mBVar[aD] = std::max(0.0,aVar);
    }

   return mBVar;
}

template class cComputeStdDev<1>;

void BenchUnbiasedStdDev()
{
    for (int aNbTest = 0 ; aNbTest<1000 ; aNbTest++)
    {
         int aNbVar = 1 + RandUnif_N(3);
         int aNbTir = aNbVar;
         int aNbComb = pow(aNbVar,aNbTir);
         std::vector<double> aVecVals = VRandUnif_0_1(aNbVar);
         std::vector<double> aVecWeight = VRandUnif_0_1(aNbVar);
// aVecWeight =  std::vector<double> (aNbVar,1.0);

         // compute the average of all empirical variance
         double aMoyVar=0;
         for (int aFlag=0 ; aFlag < aNbComb ; aFlag++) // Explore all combinaison
         {
             cComputeStdDev<1> aUBS;
             for (int aVar=0 ; aVar < aNbVar ; aVar++) // All Variable of this realization
             {
                 int aNumVar = (aFlag / round_ni(pow(aNbVar,aVar))) % aNbVar; // "Majic" formula to exdtrac p-adic decomp
                 aUBS.Add(&(aVecVals.at(aNumVar)),aVecWeight.at(aNumVar));
             }
             aMoyVar += aUBS.ComputeBiasedVar()[0];
         }
         aMoyVar /= aNbComb;

         cComputeStdDev<1> aUBS;
StdOut() << "WWW=" ;
         for (int aK=0 ; aK<aNbVar ; aK++)
         {
StdOut() << " " << aVecWeight.at(aK) ;
            aUBS.Add(&(aVecVals.at(aK)),aVecWeight.at(aK));
         }
StdOut() << "\n" ;
         StdOut() << "MOYVAR " << aMoyVar <<  " " <<  aUBS.DeBiasFactor() *  aUBS.ComputeBiasedVar()[0]
                  << " DBF=" <<  aUBS.DeBiasFactor() << "\n";
getchar();
    }
}

/* *********************************************** */
/*                                                 */
/*            Test Exp Filtre                      */
/*                                                 */
/* *********************************************** */

// Test Sigma2FromFactExp
template <class Type> void TestVarFilterExp(cPt2di aP0,cPt2di aP1,const Type & aV0,double aFx,double aFy,int aNbIt)
{
   cPt2di aPMil = (aP0+aP1) / 2;
   cRect2 aRect2(aP0,aP1);

   cIm2D<Type> aIm(aP0,aP1,nullptr,eModeInitImage::eMIA_Null);
   cDataIm2D<Type> & aDIm = aIm.DIm();

   // 1- Make 1 iteration of expon filter on a Direc, check variance and aver
   aDIm.InitDirac(aPMil,aV0);

   ExponentialFilter(true,aDIm,aNbIt,aRect2,aFx,aFy);
   cMatIner2Var<double>  aMat = StatFromImageDist(aDIm);

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S1() - aPMil.x()) < 1e-5  ,"Average Exp")
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S2() - aPMil.y()) < 1e-5  ,"Average Exp")

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S11()-Sigma2FromFactExp(aFx) * aNbIt)<1e-5,"Var Exp");
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S22()-Sigma2FromFactExp(aFy) * aNbIt)<1e-5,"Var Exp");
}

template <class Type> void TestVarFilterExp(cPt2di aSz,double aStdDev,int aNbIter,double aEps)
{
   cIm2D<Type> aIm(cPt2di(0,0),aSz,nullptr,eModeInitImage::eMIA_Null);
   cDataIm2D<Type> & aDIm = aIm.DIm();
   cPt2di aPMil = aSz / 2;
   double aV0 = 2.0;

   // 1- Make 1 iteration of expon filter on a Direc, check variance and aver
   aDIm.InitDirac(aPMil,aV0);

   ExpFilterOfStdDev(aDIm,aNbIter,aStdDev);
   cMatIner2Var<double>  aMat = StatFromImageDist(aDIm);

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S11()-Square(aStdDev))<aEps,"Std dev");
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S22()-Square(aStdDev))<aEps,"Std dev");
}

void BenchStat()
{
   TestVarFilterExp<double>(cPt2di(-2,2),cPt2di(400,375),2.0,0.6,0.67,1);
   TestVarFilterExp<double>(cPt2di(-2,2),cPt2di(400,375),2.0,0.6,0.67,3);

   // Test Sigma2FromFactExp
   for (int aK=1 ; aK<100 ; aK++)
   {
        double aS2 = aK / 5.0;
        double aF = FactExpFromSigma2(aS2);
        {
           // Check both formula are inverse of each others
           double aCheckS2 = Sigma2FromFactExp(aF);
           MMVII_INTERNAL_ASSERT_bench(std::abs( aS2 - aCheckS2 ) < 1e-5  ,"Sigma2FromFactExp")

        }
        {
           // Check formula Sigma2FromFactExp on exponential filters
           TestVarFilterExp<double>(cPt2di(0,0),cPt2di(4000,3),2.0,aF,0,1);
        }
   }

   TestVarFilterExp<double>(cPt2di(300,300),2.0,2,1e-6);
   TestVarFilterExp<float>(cPt2di(300,300),5.0,3,1e-3);
}

};


#define _wassert wassert_awf
#include <cassert>
#define _USE_MATH_DEFINES
#include <cmath>

#include <iostream>
#include <iomanip>

#include <Eigen/Eigen>
#include <Eigen/SparseQR>

#include "eigen_extras.h"

#include <unsupported/Eigen/LevenbergMarquardt>
#include <unsupported/Eigen/SparseExtra>
#include "unsupported/Eigen/src/SparseExtra/BlockSparseQR.h"
#include "unsupported/Eigen/src/SparseExtra/BlockDiagonalSparseQR.h"

#include "MeshTopology.h"
#include "SubdivEvaluator.h"
#include "log3d.h"

using namespace Eigen;

struct Subdiv3D_Functor : Eigen::SparseFunctor<Scalar>
{
  typedef Eigen::SparseFunctor<Scalar> Base;
  typedef typename Base::JacobianType JacobianType;

  // Input data
  Matrix3X data_points;

  // Topology (faces as vertex indices, fixed during shape optimization)
  MeshTopology mesh;

  SubdivEvaluator evaluator;

  // Functor constructor
  Subdiv3D_Functor(const Matrix3X& data_points, const MeshTopology& mesh) :
    Base(mesh.num_vertices*3 + data_points.cols()*2,   /* number of parameters */
         data_points.cols()*3),                        /* number of residuals */
    data_points(data_points),
    mesh(mesh),
    evaluator(mesh)
  {
    initWorkspace();
  }

  // Variables for optimization live in InputType
  struct InputType {
    Matrix3X control_vertices;
    std::vector<SurfacePoint> us;

    Index nVertices() const { return control_vertices.cols();  }
  };

  // And the optimization steps are computed using VectorType.
  // For subdivs (see xx), the correspondences are of type (int, Vec2) while the updates are of type (Vec2).
  // The iteractions between InputType and VectorType are restricted to:
  //   The Jacobian computation takeas an InputType, and its worows must easily convert to VectorType
  //   The increment_in_place operation takes InputType and StepType.
  typedef VectorX VectorType;

  // Workspace variables for evaluation
  Matrix3X S;
  Matrix3X dSdu;
  Matrix3X dSdv;
  SubdivEvaluator::triplets_t dSdX, dSudX, dSvdX;
  void initWorkspace()
  {
    Index nPoints = data_points.cols();
    S.resize(3, nPoints);
    dSdu.resize(3, nPoints);
    dSdv.resize(3, nPoints);
  }

  // Functor functions
  // 1. Evaluate the residuals at x
  int operator()(const InputType& x, ValueType& fvec) {
    evaluator.evaluateSubdivSurface(x.control_vertices, x.us, &S);

    // Fill residuals
    for (int i = 0; i < data_points.cols(); i++)
      fvec.segment(i * 3, 3) = S.col(i) - data_points.col(i);

    return 0;
  }

  // 2. Evaluate jacobian at x
  int df(const InputType& x, JacobianType& fjac)
  {
    // Evaluate surface at x
    evaluator.evaluateSubdivSurface(x.control_vertices, x.us, &S, &dSdX, &dSudX, &dSvdX, &dSdu, &dSdv);

    Index nPoints = data_points.cols();
    Index X_base = nPoints * 2;
    Index ubase = 0;

    // Fill Jacobian columns.
    // 1. Derivatives wrt control vertices.
    Eigen::TripletArray<Scalar, typename JacobianType::Index> jvals(nPoints * 3 * 3);
    for (int i = 0; i < dSdX.size(); ++i) {
      auto const& triplet = dSdX[i];
      assert(0 <= triplet.row() && triplet.row() < nPoints);
      assert(0 <= triplet.col() && triplet.col() < x.nVertices());
      jvals.add(triplet.row() * 3 + 0, X_base + triplet.col() * 3 + 0, triplet.value());
      jvals.add(triplet.row() * 3 + 1, X_base + triplet.col() * 3 + 1, triplet.value());
      jvals.add(triplet.row() * 3 + 2, X_base + triplet.col() * 3 + 2, triplet.value());
    }

    // 2. Derivatives wrt correspondences
    for (int i = 0; i < nPoints; i++) {
      jvals.add(3 * i + 0, ubase + 2 * i + 0, dSdu(0, i));
      jvals.add(3 * i + 1, ubase + 2 * i + 0, dSdu(1, i));
      jvals.add(3 * i + 2, ubase + 2 * i + 0, dSdu(2, i));

      jvals.add(3 * i + 0, ubase + 2 * i + 1, dSdv(0, i));
      jvals.add(3 * i + 1, ubase + 2 * i + 1, dSdv(1, i));
      jvals.add(3 * i + 2, ubase + 2 * i + 1, dSdv(2, i));
    }

    fjac.resize(3 * nPoints, 2 * nPoints + 3 * x.nVertices());
    fjac.setFromTriplets(jvals.begin(), jvals.end());
    fjac.makeCompressed();

    return 0;
  }

  void increment_in_place(InputType* x, StepType const& p)
  {
    Index nPoints = data_points.cols();
    Index X_base = nPoints * 2;
    Index ubase = 0;

    // Increment control vertices
    Index nVertices = x->nVertices();

    assert(p.size() == nVertices * 3 + nPoints * 2);
    assert(x->us.size() == nPoints);

    Map<VectorX>(x->control_vertices.data(), nVertices * 3) += p.tail(nVertices * 3);

    // Increment surface correspondences
    int loopers = 0;
    int totalhops = 0;
    for (int i = 0; i < nPoints; ++i) {
      Vector2 du = p.segment<2>(ubase + 2 * i);
      int nhops = increment_u_crossing_edges(x->control_vertices, x->us[i].face, x->us[i].u, du, &x->us[i].face, &x->us[i].u);
      if (nhops < 0)
        ++loopers;
      totalhops += std::abs(nhops);
    }
    if (loopers > 0)
      std::cerr << "[" << totalhops / Scalar(nPoints) << " hops, " << loopers << " points looped]";
    else if (totalhops > 0)
      std::cerr << "[" << totalhops << "/"  << Scalar(nPoints) << " hops]";
  }

  // "Mesh walking" to update correspondences, as in Fig 3, Taylor et al, CVPR 2014, "Hand shape.."
  int increment_u_crossing_edges(Matrix3X const& X, int face, const Vector2& u, const Vector2& du, int* new_face_out, Vector2* new_u_out)
  {
    const int MAX_HOPS = 7;

    Scalar u1_old = u[0];
    Scalar u2_old = u[1];
    Scalar du1 = du[0];
    Scalar du2 = du[1];
    Scalar u1_new = u1_old + du1;
    Scalar u2_new = u2_old + du2;

    for (int count = 0; ; ++count) {
      bool crossing = (u1_new < 0.f) || (u1_new > 1.f) || (u2_new < 0.f) || (u2_new > 1.f);

      if (!crossing) {
        *new_face_out = face;
        *new_u_out << u1_new, u2_new;
        return count;
      }

      //Find the new face	and the coordinates of the crossing point within the old face and the new face
      int face_new;

      bool face_found = false;

      Scalar dif, aux, u1_cross, u2_cross;

      if (u1_new < 0.f)
      {
        dif = u1_old;
        const Scalar u2t = u2_old - du2*dif / du1;
        if ((u2t >= 0.f) && (u2t <= 1.f))
        {
          face_new = mesh.face_adj(3, face); aux = u2t; face_found = true;
          u1_cross = 0.f; u2_cross = u2t;
        }
      }
      if ((u1_new > 1.f) && (!face_found))
      {
        dif = 1.f - u1_old;
        const Scalar u2t = u2_old + du2*dif / du1;
        if ((u2t >= 0.f) && (u2t <= 1.f))
        {
          face_new = mesh.face_adj(1, face); aux = 1.f - u2t; face_found = true;
          u1_cross = 1.f; u2_cross = u2t;
        }
      }
      if ((u2_new < 0.f) && (!face_found))
      {
        dif = u2_old;
        const Scalar u1t = u1_old - du1*dif / du2;
        if ((u1t >= 0.f) && (u1t <= 1.f))
        {
          face_new = mesh.face_adj(0, face); aux = 1.f - u1t; face_found = true;
          u1_cross = u1t; u2_cross = 0.f;
        }
      }
      if ((u2_new > 1.f) && (!face_found))
      {
        dif = 1.f - u2_old;
        const Scalar u1t = u1_old + du1*dif / du2;
        if ((u1t >= 0.f) && (u1t <= 1.f))
        {
          face_new = mesh.face_adj(2, face); aux = u1t; face_found = true;
          u1_cross = u1t; u2_cross = 1.f;
        }
      }
      assert(face_found);

      // Find the coordinates of the crossing point as part of the new face, and update u_old (as that will be new u in next iter).
      unsigned int conf;
      for (unsigned int f = 0; f < 4; f++)
        if (mesh.face_adj(f, face_new) == face) { conf = f; }

      switch (conf)
      {
      case 0: u1_old = aux; u2_old = 0.f; break;
      case 1: u1_old = 1.f; u2_old = aux; break;
      case 2:	u1_old = 1.f - aux; u2_old = 1.f; break;
      case 3:	u1_old = 0.f; u2_old = 1.f - aux; break;
      }

      // Evaluate the subdivision surface at the edge (with respect to the original face)
      std::vector<SurfacePoint> pts;
      pts.push_back({ face,{ u1_cross, u2_cross } });
      pts.push_back({ face_new, { u1_old, u2_old } });
      Matrix3X S(3, 2);
      Matrix3X Su(3, 2);
      Matrix3X Sv(3, 2);
      evaluator.evaluateSubdivSurface(X, pts, &S, 0, 0, 0, &Su, &Sv);

      Matrix<Scalar, 3, 2> J_Sa;
      J_Sa.col(0) = Su.col(0);
      J_Sa.col(1) = Sv.col(0);

      Matrix<Scalar, 3, 2> J_Sb;
      J_Sb.col(0) = Su.col(1);
      J_Sb.col(1) = Sv.col(1);

      //Compute the new u increments
      Vector2 du_remaining;
      du_remaining << u1_new - u1_cross, u2_new - u2_cross;
      Vector3 prod = J_Sa*du_remaining;
      Matrix22 AtA = J_Sb.transpose()*J_Sb;
      Vector2 AtB = J_Sb.transpose()*prod;

      //Vector2 du_new = AtA.ldlt().solve(AtB);
      Vector2  u_incr = AtA.inverse()*AtB;

      du1 = u_incr[0];
      du2 = u_incr[1];

      if (count == MAX_HOPS) {
        //std::cerr << "Problem!!! Many jumps between the mesh faces for the update of one correspondence. I remove the remaining u_increment!\n";
        auto dmax = std::max(du1, du2);
        Scalar scale = Scalar(0.5 / dmax);
        *new_face_out = face;
        //*new_u_out << u1_old + du1 * scale, u2_old + du2 * scale;
        *new_u_out << 0.5, 0.5;

        assert((*new_u_out)[0] >= 0 && (*new_u_out)[1] <= 1.0 && (*new_u_out)[1] >= 0 && (*new_u_out)[1] <= 1.0);
        return -count;
      }

      u1_new = u1_old + du1;
      u2_new = u2_old + du2;
      face = face_new;
    }
  }

  Scalar estimateNorm(InputType const& x, StepType const& diag)
  {
    Index nVertices = x.nVertices();
    Map<VectorX> xtop{ (Scalar*)x.control_vertices.data(), nVertices * 3 };
    double total = xtop.cwiseProduct(diag.tail(nVertices*3)).stableNorm();
    total = total*total;
    for (int i = 0; i < x.us.size(); ++i) {
      Vector2 const& u = x.us[i].u;
      Vector2 di = diag.segment<2>(2 * i);
      total += u.cwiseProduct(di).squaredNorm();
    }
    return Scalar(sqrt(total));
  }

  // 5. Describe the QR solvers
  // For generic Jacobian, one might use this Dense QR solver.
  typedef SparseQR<JacobianType, COLAMDOrdering<int> > GeneralQRSolver;

  // But for optimal performance, declare QRSolver that understands the sparsity structure.
  // Here it's block-diagonal LHS with dense RHS
  //
  // J1 = [J11   0   0 ... 0
  //         0 J12   0 ... 0
  //                   ...
  //         0   0   0 ... J1N];
  // And
  // J = [J1 J2];

  // QR for J1 subblocks is 2x1
  typedef ColPivHouseholderQR<Matrix<Scalar, 3, 2> > DenseQRSolver3x2;

  // QR for J1 is block diagonal
  typedef BlockDiagonalSparseQR<JacobianType, DenseQRSolver3x2> LeftSuperBlockSolver;

  // QR for J1'J2 is general dense (faster than general sparse by about 1.5x for n=500K)
  typedef ColPivHouseholderQR<Matrix<Scalar, Dynamic, Dynamic> > RightSuperBlockSolver;

  // QR for J is concatenation of the above.
  typedef BlockSparseQR<JacobianType, LeftSuperBlockSolver, RightSuperBlockSolver> SchurlikeQRSolver;

  typedef SchurlikeQRSolver QRSolver;

  // And tell the algorithm how to set the QR parameters.
  void initQRSolver(SchurlikeQRSolver &qr) {
    // set block size
    qr.setBlockParams(data_points.cols() * 2);
    qr.getLeftSolver().setSparseBlockParams(3, 2);
  }
};

void logmesh(log3d& log, MeshTopology const& mesh, Matrix3X const& vertices)
{
  Matrix3Xi tris(3, mesh.quads.cols() * 2);
  tris.block(0, 0, 1, mesh.quads.cols()) = mesh.quads.row(0);
  tris.block(1, 0, 1, mesh.quads.cols()) = mesh.quads.row(2);
  tris.block(2, 0, 1, mesh.quads.cols()) = mesh.quads.row(1);
  tris.block(0, mesh.quads.cols(), 1, mesh.quads.cols()) = mesh.quads.row(0);
  tris.block(1, mesh.quads.cols(), 1, mesh.quads.cols()) = mesh.quads.row(3);
  tris.block(2, mesh.quads.cols(), 1, mesh.quads.cols()) = mesh.quads.row(2);
  log.mesh(tris, vertices);
}

void logsubdivmesh(log3d& log, MeshTopology const& mesh, Matrix3X const& vertices)
{
  log.wiremesh(mesh.quads, vertices);
  SubdivEvaluator evaluator(mesh);
  MeshTopology refined_mesh;
  Matrix3X refined_verts;
  evaluator.generate_refined_mesh(vertices, 3, &refined_mesh, &refined_verts);
  logmesh(log, refined_mesh, refined_verts);
}

int main()
{
  std::cout << "Go\n";
  log3d log("log3d.html", "fit-subdiv-to-3d-points");
  log.ArcRotateCamera();
  log.axes();

  // CREATE DATA SAMPLES
  int nDataPoints = 200;
  Matrix3X data(3, nDataPoints);
  for (int i = 0; i < nDataPoints; i++) {
    if (0) {
      float t = float(i) / float(nDataPoints);
      data(0, i) = 0.1f + 1.3f*cos(80*t);
      data(1, i) = -0.2f + 0.7f*sin(80*t);
      data(2, i) = t;
    }
    else {
      Scalar t = rand() / Scalar(RAND_MAX);
      Scalar s = rand() / Scalar(RAND_MAX);

      auto u = Scalar(2 * EIGEN_PI * t);
      auto v = Scalar(EIGEN_PI * (s - 0.5));
      data(0, i) = 0.1f + 1.3f*cos(u)*cos(v);
      data(1, i) = -0.2f + 0.7f*sin(u)*cos(v);
      data(2, i) = sin(v);
    }
    if (1)
      log.position(log.CreateSphere(0, 0.02), data(0, i), data(1, i), data(2, i));
    else
      log.star(data.col(i));
  }

  MeshTopology mesh;
  Matrix3X control_vertices_gt;
  makeCube(&mesh, &control_vertices_gt);
  int nFaces = int(mesh.quads.cols());

  // INITIAL PARAMS
  typedef Subdiv3D_Functor Functor;

  Functor::InputType params;
  params.control_vertices = control_vertices_gt + 0.1 * MatrixXX::Random(3, control_vertices_gt.cols());
  params.us.resize(nDataPoints);

  // Initialize uvs.
  {
    // 1. Make a list of test points, e.g. centre point of each face
    Matrix3X test_points(3, nFaces);
    std::vector<SurfacePoint> uvs{ size_t(nFaces),{ 0,{ 0.5, 0.5 } } };
    for (int i = 0; i < nFaces; ++i)
      uvs[i].face = i;

    SubdivEvaluator evaluator(mesh);
    evaluator.evaluateSubdivSurface(params.control_vertices, uvs, &test_points);

    for (int i = 0; i < nDataPoints; i++) {
      // Closest test point
      Eigen::Index test_pt_index;
      (test_points.colwise() - data.col(i)).colwise().squaredNorm().minCoeff(&test_pt_index);
      params.us[i] = uvs[test_pt_index];
    }
  }

  logsubdivmesh(log, mesh, params.control_vertices);

  Functor functor(data, mesh);

  // Check Jacobian
  if (0)
  for (float eps = 1e-8f; eps < 1.1e-3f; eps*=10.f) {
    NumericalDiff<Functor> fd{ functor, Functor::Scalar(eps) };
    Functor::JacobianType J;
    Functor::JacobianType J_fd;
    functor.df(params, J);
    fd.df(params, J_fd);
    double diff = (J - J_fd).norm();
    if (diff > 0) {
      std::cerr << "Jacobian diff(eps=" << eps <<"), = " << diff << std::endl;
      write(J, "c:\\tmp\\J.txt");
      write(J_fd, "c:\\tmp\\J_fd.txt");
    }
  }

  Eigen::LevenbergMarquardt< Functor > lm(functor);
  lm.setVerbose(true);
  lm.setMaxfev(10);

  Eigen::LevenbergMarquardtSpace::Status info = lm.minimize(params);
  log.color(0, 1, 0);
  logsubdivmesh(log, mesh, params.control_vertices);

  std::cerr << "Done: err = "<< lm.fnorm() <<"\n";

  // Now, on a refined mesh.
  if (1) {
    log.color(.5, .8, 0);

    MeshTopology mesh1;
    Matrix3X verts1;
    SubdivEvaluator evaluator(mesh);
    evaluator.generate_refined_mesh(params.control_vertices, 1, &mesh1, &verts1);

    {
      log3d log2("log2.html");
      log2.ArcRotateCamera();
      log2.axes();
      log2.wiremesh(mesh1.quads, verts1);
    }

    params.control_vertices = verts1;

    // Initialize uvs.
    {
      // 1. Make a list of test points, e.g. centre point of each face
      int nFaces = mesh1.num_faces();
      Matrix3X test_points(3, nFaces);
      std::vector<SurfacePoint> uvs{ size_t(nFaces),{ 0,{ 0.5, 0.5 } } };
      for (int i = 0; i < nFaces; ++i)
        uvs[i].face = i;

      SubdivEvaluator evaluator(mesh1);
      evaluator.evaluateSubdivSurface(params.control_vertices, uvs, &test_points);

      for (int i = 0; i < nDataPoints; i++) {
        // Closest test point
        Eigen::Index test_pt_index;
        (test_points.colwise() - data.col(i)).colwise().squaredNorm().minCoeff(&test_pt_index);
        params.us[i] = uvs[test_pt_index];
      }
    }

    Functor functor1(data, mesh1);

    Eigen::LevenbergMarquardt< Functor > lm(functor1);
    lm.setVerbose(true);
    lm.setMaxfev(40);

    Eigen::LevenbergMarquardtSpace::Status info = lm.minimize(params);
    logsubdivmesh(log, mesh1, params.control_vertices);

    std::cerr << "Done: err = " << lm.fnorm() << "\n";
  }
}

// Override system assert so one can set a breakpoint in it rather than clicking "Retry" and "Break"
void __cdecl _wassert(_In_z_ wchar_t const* _Message,_In_z_ wchar_t const* _File, _In_ unsigned _Line)
{
  std::wcerr << _File << "(" << _Line << "): ASSERT FAILED [" << _Message << "]\n";

  abort();
}


/*
 * This example was gathered from Stack Overflow. Only small modification were introduced.
 *
 * Check here: https://stackoverflow.com/questions/35656237/dynamic-eigen-vectors-in-boostodeint
 *
 * Credit: Sjonnie (user nickname)
 */

#include <iostream>
#include <Eigen/Core>
#include <cstdlib>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_algebra.hpp>

using namespace boost::numeric::odeint;

typedef Eigen::VectorXd state_type;

Eigen::MatrixXd A;

void ODE_function (const state_type &x, state_type &dxdt, const double &t)
{
    dxdt = A * x;
}

void write_states(const state_type &x, const double &t)
{
    std::cout << t << "\t";
    for (int i = 0; i < x.size(); i++)
    {
        std::cout << *(x.data()+i) << "\t";
    }
    std::cout << std::endl;
}

int main()
{
    int nr_of_states = 10;
    state_type x;

    std::cout << "Simulation with " << nr_of_states << " states\n";

    x = state_type(nr_of_states);
    A = Eigen::MatrixXd(nr_of_states, nr_of_states);

    srand(365);

    for (int i = 0; i < A.size(); i++)
    {
        *(A.data()+i) = ((double)rand()/(double)RAND_MAX);
    }

    for (int i = 0; i < x.size(); i++)
    {
        *(x.data()+i) = i;
    }

    typedef runge_kutta_dopri5<state_type, double, state_type, double, vector_space_algebra> stepper;
    integrate_adaptive(stepper(), ODE_function, x, 0.0, 25.0, 0.1, write_states);

    std::cout <<std::endl << "final state vector: " << std::endl << x << std::endl;

    return 0;
}


#include <Eigen/Dense>
#include <cmath>
#include <vector>
#include "Adam.hpp"

#define EIGEN_MPL2_ONLY
using namespace Eigen;
using namespace std;

Adam::Adam(int _N,int _d,MatrixXd _x,VectorXd _label,double _C,double _alpha,double _beta1,double _beta2,double _epsilon,double _lambda,int iter):
  N(_N),
  d(_d),
  alpha(_alpha),
  beta1(_beta1),
  beta2(_beta2),
  epsilon(_epsilon),
  lambda(_lambda),
  C(_C),
  X(_x),
  m(VectorXd::Zero(d)),
  v(VectorXd::Zero(d)),
  label(_label),
  iteration(iter),
  w(VectorXd::Zero(d))
{}

double Adam::Acc(vector<double>& pred,VectorXd &l){
  int t =0;
  double loss =0;
  for(int i = 0; i< pred.size();i++){
    loss += (l(i) - pred[i]) * (l(i) - pred[i]);
    int s = pred[i] > 0.5 ? 1 : 0;
    if(s == l(i)){
      t++;
    }
  }
  return (double)t/pred.size();
}

void Adam::train(){
  for(int iter = 0; iter< iteration; iter++){
    for(int i = 0; i < N; i++){
      double pred = sigma(X,i);
      for(int idx = 0; idx < d; idx++){
        if(X(i,idx) != 0){
          double tbeta = 1-(1-beta1) * pow(lambda,iter);
          double grad = (pred - label(i)) * X(i,idx)+ C * w(idx);
          m[idx] = tbeta * grad + (1-tbeta) * m[idx];
          v[idx] = beta2*pow(grad,2) + (1-beta2) * v[idx];
          double hat_m = m[idx]/(1-pow((1-beta1),iter+1));
          double hat_v = v[idx]/(1-pow((1-beta2),iter+1));
          w(idx) -= alpha*hat_m/(sqrt(hat_v) + epsilon);
        }
      }
    }
    vector<double> ret;
    predict(X,label,ret);
    iterscores.push_back(Acc(ret,label));
  }
}

double Adam::sigma(const MatrixXd& _x, int i){
  return sigmoid(_x.row(i).dot(w));
}

double Adam::sigmoid(double z){
  return 1.0 / (1.0 + exp(-z));
}

void Adam::predict(MatrixXd& _x,VectorXd& _l,vector<double>& ret){
  for(int i = 0; i < _x.rows(); i++){
    ret.push_back(sigma(_x,i));
  }
}


#include <iostream>
#include <Eigen/Dense>

int main()
{
  Eigen::MatrixXd xn;
  Eigen::MatrixXd mu;
  Eigen::VectorXi gk;
  Eigen::VectorXi numk;
  Eigen::MatrixXd::Index minIndex;
  int ni;
  int nn;
  int kk;
  int i;
  int j;
  int l;
  int sum_gk;
  int bef_sum_gk;

  // after :: from file input
  ni = 10;
  kk = 3;

  nn = ni*kk;
  std::cout << "nn " <<  nn << std::endl;

  xn = Eigen::MatrixXd::Random(2, nn);
  gk = Eigen::VectorXi(nn);
  numk = Eigen::VectorXi::Zero(kk);
  std::cout << "xn =" << std::endl << xn << std::endl;

  xn.block(0, nn/kk, 1, nn/kk).array() += 2.0;
  xn.block(0, 2*nn/kk, 1, nn/kk).array() += 4.0;

  mu = xn.block(0, 0, 2, kk);

  std::cout << "mu =" << std::endl << mu << std::endl;

  bef_sum_gk = 0;
  for(i = 0; i < 100; i++){
    std::cout << "loop =" << i << std::endl;
    for(j = 0; j < nn; j++){

// Is which better?
      (mu.colwise() - xn.col(j)).colwise().squaredNorm().minCoeff(&minIndex);
//      (xn.col(j).replicate(1, 3) - mu).colwise().squaredNorm().minCoeff(&minIndex);

      gk(j) = minIndex;
    }
    numk.array() = 0;
    for(l = 0; l < kk; l++){
      mu.col(l).array() = 0.0;
      for(j = 0; j < nn; j++){
	if(gk(j) == l){
	  numk(l) += 1;
	  mu.col(l) += xn.col(j);
	}
      }
// Is which better?
      mu.col(l).array() /= static_cast<double> (numk(l));
//      mu.col(l) /= static_cast<double> (numk(l));
    }
    sum_gk = gk.sum();
    if(bef_sum_gk == sum_gk) break;
    bef_sum_gk = sum_gk;
  }

  std::cout << "numk =" << std::endl << numk << std::endl;
  std::cout << "gk =" << std::endl << gk << std::endl;
  std::cout << "mu =" << std::endl << mu << std::endl;
}


#include <Eigen/Jacobi>
#include <Eigen/SVD>

#include <sphericalsfm/so3.h>
#include <sphericalsfm/spherical_utils.h>

namespace sphericalsfm {
    void make_spherical_essential_matrix( const Eigen::Matrix3d &R, bool inward, Eigen::Matrix3d &E )
    {
        Eigen::Vector3d t( R(0,2), R(1,2), R(2,2)-1 );
        if ( inward ) t = -t;
        E = skew3(t)*R;
    }

    void decompose_spherical_essential_matrix( const Eigen::Matrix3d &E, bool inward, Eigen::Vector3d &r, Eigen::Vector3d &t )
    {
        Eigen::JacobiSVD<Eigen::Matrix3d> svdE(E,Eigen::ComputeFullU|Eigen::ComputeFullV);

        Eigen::Matrix3d U = svdE.matrixU();
        Eigen::Matrix3d V = svdE.matrixV();

        // from theia sfm
        if (U.determinant() < 0) {
            U.col(2) *= -1.0;
        }

        if (V.determinant() < 0) {
            V.col(2) *= -1.0;
        }

        Eigen::Matrix3d D;
        D <<
        0,1,0,
        -1,0,0,
        0,0,1;

        Eigen::Matrix3d DT;
        DT <<
        0,-1,0,
        1,0,0,
        0,0,1;

        Eigen::Matrix3d VT = V.transpose().eval();

        Eigen::Vector3d tu = U.col(2);

        Eigen::Matrix3d R1 = U*D*VT;
        Eigen::Matrix3d R2 = U*DT*VT;

        Eigen::Vector3d t1( R1(0,2), R1(1,2), R1(2,2)-1 );
        Eigen::Vector3d t2( R2(0,2), R2(1,2), R2(2,2)-1 );

        if ( inward ) { t1 = -t1; t2 = -t2; }

        Eigen::Vector3d myt1 = t1/t1.norm();
        Eigen::Vector3d myt2 = t2/t2.norm();

        Eigen::Vector3d r1 = so3ln(R1);
        Eigen::Vector3d r2 = so3ln(R2);

        double score1 = fabs(myt1.dot(tu));
        double score2 = fabs(myt2.dot(tu));

        if ( score1 > score2 ) { r = r1; t = t1; }
        else { r = r2; t = t2; }
    }

    static Eigen::Vector3d triangulateMidpoint( const Eigen::Matrix4d &rel_pose, const Eigen::Vector3d &u, const Eigen::Vector3d &v )
    {
        Eigen::Vector3d cu( 0, 0, 0 );
        Eigen::Vector3d cv( -rel_pose.block<3,3>(0,0).transpose() * rel_pose.block<3,1>(0,3) );

        Eigen::Matrix3d A;
        A <<
        u(0), -v(0), cu(0) - cv(0),
        u(1), -v(1), cu(1) - cv(1),
        u(2), -v(2), cu(2) - cv(2);

        const Eigen::Vector3d soln = A.jacobiSvd( Eigen::ComputeFullV ).matrixV().col(2);
        const double du = soln(0)/soln(2);
        const double dv = soln(1)/soln(2);

        const Eigen::Vector3d Xu = cu + u*du;
        const Eigen::Vector3d Xv = cv + v*dv;

        return (Xu+Xv)*0.5;
    }

    void decompose_spherical_essential_matrix( const Eigen::Matrix3d &E, bool inward,
                     RayPairList::iterator begin, RayPairList::iterator end, const std::vector<bool> &inliers,
                     Eigen::Vector3d &r, Eigen::Vector3d &t )
    {
        Eigen::JacobiSVD<Eigen::Matrix3d> svdE(E,Eigen::ComputeFullU|Eigen::ComputeFullV);

        Eigen::Matrix3d U = svdE.matrixU();
        Eigen::Matrix3d V = svdE.matrixV();

        // from theia sfm
        if (U.determinant() < 0) {
            U.col(2) *= -1.0;
        }

        if (V.determinant() < 0) {
            V.col(2) *= -1.0;
        }

        Eigen::Matrix3d D;
        D <<
        0,1,0,
        -1,0,0,
        0,0,1;

        Eigen::Matrix3d DT;
        DT <<
        0,-1,0,
        1,0,0,
        0,0,1;

        Eigen::Matrix3d VT = V.transpose().eval();

        Eigen::Matrix3d R1 = U*D*VT;
        Eigen::Matrix3d R2 = U*DT*VT;

        Eigen::Vector3d t1( R1(0,2), R1(1,2), (inward) ? R1(2,2)+1 : R1(2,2)-1 );
        Eigen::Vector3d t2( R2(0,2), R2(1,2), (inward) ? R2(2,2)+1 : R2(2,2)-1 );

        Eigen::Vector3d r1 = so3ln(R1);
        Eigen::Vector3d r2 = so3ln(R2);

        double r1test = r1.norm();
        double r2test = r2.norm();

        if ( r2test > M_PI/2 && r1test < M_PI/2 ) { r = r1; t = t1; return; }
        if ( r1test > M_PI/2 && r2test < M_PI/2 ) { r = r2; t = t2; return; }

        Eigen::Matrix4d P1( Eigen::Matrix4d::Identity() );
        Eigen::Matrix4d P2( Eigen::Matrix4d::Identity() );

        P1.block(0,0,3,3) = R1;
        P1.block(0,3,3,1) = t1;

        P2.block(0,0,3,3) = R2;
        P2.block(0,3,3,1) = t2;

        int ninfront1 = 0;
        int ninfront2 = 0;

        int i = 0;
        for ( RayPairList::iterator it = begin; it != end; it++,i++ )
        {
            if ( !inliers[i] ) continue;

            Eigen::Vector3d u = it->first.head(3);
            Eigen::Vector3d v = it->second.head(3);

            Eigen::Vector3d X1 = triangulateMidpoint(P1, u, v);
            Eigen::Vector3d X2 = triangulateMidpoint(P2, u, v);

            if ( X1(2) > 0 ) ninfront1++;
            if ( X2(2) > 0 ) ninfront2++;
        }

        if ( ninfront1 > ninfront2 )
        {
            r = so3ln(R1);
            t = t1;
        }
        else
        {
            r = so3ln(R2);
            t = t2;
        }
    }
}


/*
 * polynomial_splines.hpp
 *
 *  Created on: Mar 7, 2017
 *      Author: Dario Bellicoso
 */

#pragma once

// stl
#include <vector>
#include <iostream>

// eigen
#include <Eigen/Core>
#include <Eigen/QR>

// boost
#include <boost/math/special_functions/pow.hpp>

namespace curves {

struct SplineOptions {

  SplineOptions()
      : tf_(0.0),
        pos0_(0.0), posT_(0.0),
        vel0_(0.0), velT_(0.0),
        acc0_(0.0), accT_(0.0)
  {

  }

  constexpr SplineOptions(double tf, double pos0, double posT, double vel0,
                          double velT, double acc0, double accT)
      : tf_(tf),
        pos0_(pos0), posT_(posT),
        vel0_(vel0), velT_(velT),
        acc0_(acc0), accT_(accT)
  {

  }

  //! The total duration of the spline in seconds.
  double tf_;

  //! The scalar position at time 0.
  double pos0_;

  //! The scalar position at time tf.
  double posT_;

  //! The scalar velocity at time 0.
  double vel0_;

  //! The scalar velocity at time tf.
  double velT_;

  //! The scalar acceleration at time 0.
  double acc0_;

  //! The scalar acceleration at time tf.
  double accT_;
};

namespace spline_traits {

// Main struct template.
template<typename Core_, int SplineOrder_>
struct spline_rep {

  static constexpr unsigned int splineOrder = SplineOrder_;
  static constexpr unsigned int numCoefficients = SplineOrder_+1;

  using TimeVectorType = std::array<Core_, numCoefficients>;
  using SplineCoefficients = std::array<Core_, numCoefficients>;

  static inline TimeVectorType tau(Core_ tk) noexcept;
  static inline TimeVectorType dtau(Core_ tk) noexcept;
  static inline TimeVectorType ddtau(Core_ tk) noexcept;

  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients);
};

// Specialization for third order splines.
template<>
struct spline_rep<double, 3> {

  static constexpr unsigned int splineOrder = 3;
  static constexpr unsigned int numCoefficients = splineOrder+1;

  using TimeVectorType = std::array<double, numCoefficients>;
  using SplineCoefficients = std::array<double, numCoefficients>;

  static inline TimeVectorType tau(double tk) noexcept {
    return { boost::math::pow<3>(tk), boost::math::pow<2>(tk), tk, 1.0 };
  }

  static inline TimeVectorType dtau(double tk) noexcept {
    return { 3.0*boost::math::pow<2>(tk), 2.0*tk, 1.0, 0.0 };
  }

  static inline TimeVectorType ddtau(double tk) noexcept {
    return { 6.0*tk, 2.0, 0.0, 0.0 };
  }

  static constexpr TimeVectorType   tauZero{{ 0.0, 0.0, 0.0, 1.0 }};
  static constexpr TimeVectorType  dtauZero{{ 0.0, 0.0, 1.0, 0.0 }};
  static constexpr TimeVectorType ddtauZero{{ 0.0, 2.0, 0.0, 0.0 }};

  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients) {

    Eigen::Matrix<double, numCoefficients, 1> b;
    b << opts.pos0_, opts.vel0_, opts.posT_, opts.velT_;

    Eigen::Matrix<double, numCoefficients, numCoefficients> A;
    A << Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(tauZero.data()),
         Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(dtauZero.data()),
         Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((tau(opts.tf_)).data()),
         Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((dtau(opts.tf_)).data());

    Eigen::Map<Eigen::VectorXd>(coefficients.data(), numCoefficients, 1) = A.colPivHouseholderQr().solve(b);

    return true;
  }
};

// Specialization for fifth order splines.
template<>
struct spline_rep<double, 5> {

  static constexpr unsigned int splineOrder = 5;
  static constexpr unsigned int numCoefficients = splineOrder+1;

  using TimeVectorType = std::array<double, numCoefficients>;
  using SplineCoefficients = std::array<double, numCoefficients>;

  static inline TimeVectorType tau(double tk) noexcept {
    return { boost::math::pow<5>(tk), boost::math::pow<4>(tk), boost::math::pow<3>(tk),
             boost::math::pow<2>(tk), tk, 1.0};
  }

  static inline TimeVectorType dtau(double tk) noexcept {
    return { 5.0*boost::math::pow<4>(tk), 4.0*boost::math::pow<3>(tk), 3.0*boost::math::pow<2>(tk),
             2.0*tk, 1.0, 0.0};
  }

  static inline TimeVectorType ddtau(double tk) noexcept {
    return { 20.0*boost::math::pow<3>(tk), 12.0*boost::math::pow<2>(tk), 6.0*tk,
              2.0, 0.0, 0.0};
  }

  static constexpr TimeVectorType   tauZero{{ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 }};
  static constexpr TimeVectorType  dtauZero{{ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 }};
  static constexpr TimeVectorType ddtauZero{{ 0.0, 0.0, 0.0, 2.0, 0.0, 0.0 }};

  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients) {
    Eigen::Matrix<double, numCoefficients, 1> b;
    b << opts.pos0_, opts.vel0_, opts.acc0_, opts.posT_, opts.velT_, opts.accT_;

    Eigen::Matrix<double, numCoefficients, numCoefficients> A;
    A << Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(tauZero.data()),
         Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(dtauZero.data()),
         Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(ddtauZero.data()),
         Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((tau(opts.tf_)).data()),
         Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((dtau(opts.tf_)).data()),
         Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((ddtau(opts.tf_)).data());

    Eigen::Map<Eigen::VectorXd>(coefficients.data(), numCoefficients, 1) = A.colPivHouseholderQr().solve(b);

    return true;
  }

};

}

}


#pragma once
#include <Eigen/Dense>
#include <autoppl/math/autocorrelation.hpp>

namespace ppl {
namespace math {
namespace details {

template <class T>
using vec_cref_t = std::vector<
    std::reference_wrapper<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>> >;

} // namespace details

/**
 * Computes the effective sample size (ESS) for a given vector of samples (matrices).
 * Every element of the vector is a matrix of samples for each chain.
 * Every matrix contains the samples as rows, i.e.
 * every row is a sample of an p-dimensional vector, where p
 * is the number of columns of the matrix (number of parameters).
 *
 * The algorithm assumes that every sample matrix has the same dimensions.
 *
 * @tparam  T           underlying Eigen expression type
 * @param   samples     vector of samples
 *
 * @return  a vector of ESS for each component
 *          If number of samples is 1 or less, or there are 0 components,
 *          or number of chains is 0, return an empty vector.
 *          In either case, the dimension of the return vector is same
 *          as the number of components.
 */
template <class T>
inline auto ess(const details::vec_cref_t<T>& samples)
{
    using value_t = T;
    using vec_t = Eigen::Matrix<value_t, Eigen::Dynamic, 1>;
    using mat_t = Eigen::Matrix<value_t, Eigen::Dynamic, Eigen::Dynamic>;

    vec_t tau_hat;

    size_t M = samples.size();      // number of chains

    if (M == 0) return tau_hat;

    size_t dim = samples[0].get().cols();    // sample dimension
    size_t N = samples[0].get().rows();      // number of samples

    if (N <= 1 || dim == 0) return tau_hat;

    tau_hat = vec_t::Zero(dim);

    auto var = [N](const auto& mat) {
        return (mat.rowwise() -
                mat.colwise().mean()).colwise()
                                     .squaredNorm() / (N-1);
    };

    // use N-1 scaling to compute variance
    // each col is the sample variance per chain
    mat_t sample_vars(dim, M);
    for (size_t i = 0; i < M; ++i) {
        sample_vars.col(i) = var(samples[i].get());
    }

    // column vector of average of sample variances
    vec_t W = sample_vars.rowwise().mean();

    // compute variance estimator
    vec_t var_est = static_cast<T>(N-1) / N * W;

    // if there is more than 1 chain, then update by N * B
    // where B is the between-chain variance
    if (M > 1) {
        mat_t sample_mean(dim, M);
        for (size_t i = 0; i < M; ++i) {
            sample_mean.col(i) = samples[i].get().colwise().mean();
        }
        var_est += var(sample_mean.transpose());
    }

    // compute autocorrelation vector for each component
    // every column vector (every component) is average AC over chains
    mat_t acov_mean(N, dim);
    acov_mean.setZero();

    for (size_t m = 1; m <= M; ++m) {
        mat_t next_acov = autocorrelation(samples[m-1].get());
        for (int j = 0; j < next_acov.cols(); ++j) {
            next_acov.col(j) *= sample_vars(j,m-1);
        }
        value_t m_inv = 1./m;
        acov_mean = m_inv * next_acov + (m-1) * m_inv * acov_mean;
    }

    // compute rho-hat at lag t for dimension d
    auto rho_hat = [&](size_t t, size_t d) {
        return 1. - (W(d) - acov_mean(t,d))/var_est(d);
    };

    // compute tau-hat directly to save memory
    for (size_t d = 0; d < dim; ++d) {

        // first two should not be corrected for positive and monotoneness
        value_t curr_rho_hat_even = rho_hat(0,d);
        value_t curr_p_hat = curr_rho_hat_even + rho_hat(1,d);  // current P_hat(t)
        value_t curr_min = curr_p_hat;                          // current min of P_hat(t)
        tau_hat(d) = curr_min;                                  // update with P_hat(0)

        // only estimate up to 3 samples before the end
        // and Geyer's positive condition holds
        size_t t = 2;
        for (; t < (N-3) && curr_p_hat > 0; t += 2) {
            curr_rho_hat_even = rho_hat(t,d);
            curr_p_hat = curr_rho_hat_even + rho_hat(t+1,d);

            // if positive condition holds, take the min
            // of current P_hat(t) with the min of previous P_hat's
            // to create a monotone sequence and accumulate to tau_hat
            if (curr_p_hat >= 0) {
                curr_min = std::min(curr_min, curr_p_hat);
                tau_hat(d) += curr_min;
            }
        }

        // correct to improve estimate (see STAN's implementation)
        value_t correction = (curr_rho_hat_even > 0) ?
                curr_rho_hat_even : rho_hat(t,d);

        tau_hat(d) *= 2.; // 2 * sum of adjusted P_hat(t)
        tau_hat(d) -= 1.; // -1 + 2 * sum of adjusted P_hat(t)
        tau_hat(d) += correction;
    }

    vec_t n_eff = N*M*(1./tau_hat.array()).min(std::log10(N)).matrix();

    return n_eff;
}

/**
 * Computes the effective sample size (ESS) for a given sample matrix.
 * This is an overload for when there is only chain and can supply
 * a single matrix instead.
 * See above overload for more details.
 *
 * @tparam  T           underlying Eigen expression type
 * @param   samples     sample matrix (1 chain)
 *
 * @return  a vector of ESS for each component
 */
template <class T>
inline auto ess(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& samples)
{
    details::vec_cref_t<T> v;
    v.emplace_back(samples);
    return ess(v);
}

} // namespace math
} // namespace ppl


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef META_MATH_IS_PRIME_INCLUDE
#define META_MATH_IS_PRIME_INCLUDE

#include <boost/mpl/bool.hpp>
#include <boost/numeric/meta_math/sqrt.hpp>
// #include <boost/config/concept_macros.hpp>
#ifdef __GXX_CONCEPTS__
#   include <concepts>
#endif

namespace meta_math {

namespace mpl = boost::mpl;

namespace impl {

    // Checks if 'x' is divisible by some odd number >= max_odd, checking decreasingly
    template <long int x, long int max_odd>
    struct is_prime_to_max_odd
    {
	static bool const value = x % max_odd != 0
                                  && is_prime_to_max_odd<x, max_odd-2>::value;
    };

    // Once we reach 1, it's prime
    template <long int x> struct is_prime_to_max_odd<x, 1> : mpl::true_ {};

    // Returns the largest number that x is tried to divided by.
    // This is a odd number slightly larger than the approximated square root.
    template <long int x>
    struct max_prime_compare
    {
	static long int const tmp = sqrt<x>::value,
                              value = tmp % 2 == 0 ? tmp + 1 : tmp + 2;
    };

    // Checks if there is an odd number between 3 and sqrt(x)+1 that divides x
    // if there is no divisor found then x is prime (otherwise not)
    // must be disabled when x is even
    template <long int x, bool Disable>
    struct check_odd
    {
	static bool const value = is_prime_to_max_odd<x, max_prime_compare<x>::value>::value;
    };

    // Intended for even numbers (> 2) which are always prime
    template <long int x>
    struct check_odd<x, true>
    {
	static bool const value = false;
    };

}

template <long int x>
struct is_prime
{
  static bool const value = impl::check_odd<x, x % 2 == 0>::value;
};

template <> struct is_prime<0> : mpl::false_ {};
template <> struct is_prime<1> : mpl::false_ {};
template <> struct is_prime<2> : mpl::true_ {};
template <> struct is_prime<3> : mpl::true_ {};
template <> struct is_prime<5> : mpl::true_ {};

#ifdef __GXX_CONCEPTS__
    concept Prime<long int N> {}

    template <long int N>
        where std::True<is_prime<N>::value>
    concept_map Prime<N> {}
#endif

} // namespace meta_math

#endif // META_MATH_IS_PRIME_INCLUDE


#pragma once
#include "PolynomialRootSolver.h"
#include <opencv2/core/core.hpp>
#include <Eigenvalues>
#include <opencv2/core/eigen.hpp>

PolynomialRootSolver::PolynomialRootSolver()
{

}

PolynomialRootSolver::~PolynomialRootSolver()
{

}

Eigen::MatrixXd computeCompanionMatrix(const cv::Mat rowOfCoefficientsInAscendingDegreeOrder)
{
    int numberOfColumns  = rowOfCoefficientsInAscendingDegreeOrder.size().width  -1 ;
    Eigen::MatrixXd eigenCoefficients(1,numberOfColumns+1);
    Eigen::MatrixXd companionMatrix(numberOfColumns,numberOfColumns);
    companionMatrix.setZero();
    Eigen::MatrixXd identitySubmatrix = Eigen::MatrixXd::Identity(numberOfColumns-1,numberOfColumns-1);
    cv::cv2eigen(rowOfCoefficientsInAscendingDegreeOrder, eigenCoefficients);
    eigenCoefficients /= eigenCoefficients(numberOfColumns);

    companionMatrix.block(0,1,numberOfColumns-1,numberOfColumns-1) = identitySubmatrix;
    companionMatrix.block(numberOfColumns-1,0,1,numberOfColumns) -= eigenCoefficients.block(0,0,1,numberOfColumns);
    return companionMatrix;
}

cv::Mat PolynomialRootSolver::computeRoots(const cv::Mat& rowOfCoefficientsInAscendingDegreeOrder)
{
    Eigen::MatrixXd eigenCompanionMatrix = computeCompanionMatrix(rowOfCoefficientsInAscendingDegreeOrder);
    Eigen::EigenSolver<Eigen::MatrixXd> eigenSolver(eigenCompanionMatrix);
    Eigen::MatrixXd realEigenvalues = eigenSolver.eigenvalues().real();
    realEigenvalues.transposeInPlace();
    cv::Mat roots;
    cv::eigen2cv(realEigenvalues, roots);
    return roots;
}


// (C) Copyright Andrew Sutton 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

//[mean_geodesic_example
#include <iostream>
#include <iomanip>

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/geodesic_distance.hpp>
#include "helper.hpp"

using namespace std;
using namespace boost;

// The Actor type stores the name of each vertex in the graph.
struct Actor
{
    string name;
};

// Declare the graph type and its vertex and edge types.
typedef undirected_graph<Actor> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

// The name map provides an abstract accessor for the names of
// each vertex. This is used during graph creation.
typedef property_map<Graph, string Actor::*>::type NameMap;

// Declare a matrix type and its corresponding property map that
// will contain the distances between each pair of vertices.
typedef exterior_vertex_property<Graph, int> DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;

// Declare the weight map so that each edge returns the same value.
typedef constant_property_map<Edge, int> WeightMap;

// Declare a container and its corresponding property map that
// will contain the resulting mean geodesic distances of each
// vertex in the graph.
typedef exterior_vertex_property<Graph, float> GeodesicProperty;
typedef GeodesicProperty::container_type GeodesicContainer;
typedef GeodesicProperty::map_type GeodesicMap;

int
main(int argc, char *argv[])
{
    // Create the graph and a property map that provides access
    // to the actor names.
    Graph g;
    NameMap nm(get(&Actor::name, g));

    // Read the graph from standad input.
    read_graph(g, nm, cin);

    // Compute the distances between all pairs of vertices using
    // the Floyd-Warshall algorithm. Note that the weight map is
    // created so that every edge has a weight of 1.
    DistanceMatrix distances(num_vertices(g));
    DistanceMatrixMap dm(distances, g);
    WeightMap wm(1);
    floyd_warshall_all_pairs_shortest_paths(g, dm, weight_map(wm));

    // Compute the mean geodesic distances for each vertex in
    // the graph and get the average mean geodesic distace (the
    // so-called small-world distance) as a result.
    GeodesicContainer geodesics(num_vertices(g));
    GeodesicMap gm(geodesics, g);
    float sw = all_mean_geodesics(g, dm, gm);

    // Print the mean geodesic distance of each vertex and finally,
    // the graph itself.
    graph_traits<Graph>::vertex_iterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        cout << setw(12) << setiosflags(ios::left)
             << g[*i].name << get(gm, *i) << endl;
    }
    cout << "small world distance: " << sw << endl;

    return 0;
}
//]


// This file is part of PoseEstimation.
// Copyright (c) 2021, Eijiro Shibusawa <phd_kimberlite@yahoo.co.jp>
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// 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.

#ifndef THREE_POINT_HPP_
#define THREE_POINT_HPP_

#include <Eigen/Dense>

class ThreePointTest;

namespace ThreePoint
{
template <typename FloatType>
class ThreePoint
{
	friend ::ThreePointTest;

private:
	typedef Eigen::Matrix<FloatType, Eigen::Dynamic, 1> Matrix_Dx1;
	typedef Eigen::Matrix<FloatType, 3, 1> Matrix_3x1;
	typedef Eigen::Matrix<FloatType, 3, 3> Matrix_3x3;
	typedef Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> Matrix_3x3R;
	typedef Eigen::Matrix<FloatType, 3, Eigen::Dynamic> Matrix_3xD;
	typedef Eigen::Matrix<FloatType, 4, 4> Matrix_4x4;
	typedef Eigen::Matrix<FloatType, 4, 1> Matrix_4x1;

public:
	// q = [x, y, z, w] = w + x*i + y*j + z*k
	static inline void getRotation(const FloatType *q, FloatType *R)
	{
		const FloatType q0_2 = 2 * q[0], q1_2 = 2 * q[1], q2_2 = 2 * q[2];
		const FloatType q03_2 = q0_2 * q[3], q13_2 = q1_2 * q[3], q23_2 = q2_2 * q[3];
		const FloatType q00_2 = q0_2 * q[0], q01_2 = q0_2 * q[1], q02_2 = q0_2 * q[2];
		const FloatType q11_2 = q1_2 * q[1], q12_2 = q1_2 * q[2], q22_2 = q2_2 * q[2];
		R[0] = 1 - (q11_2 + q22_2);
		R[1] = q01_2 - q23_2;
		R[2] = q02_2 + q13_2;
		R[3] = q01_2 + q23_2;
		R[4] = 1 - (q00_2 + q22_2);
		R[5] = q12_2 - q03_2;
		R[6] = q02_2 - q13_2;
		R[7] = q12_2 + q03_2;
		R[8] = 1 - (q00_2 + q11_2);
	}

	static inline void transformPoints(int n, FloatType s, const FloatType *R, const FloatType *t, const FloatType *X1, FloatType *X2)
	{
		Eigen::Map<const Matrix_3x3R> mR(R);
		Eigen::Map<const Matrix_3x1> mt(t);
		Eigen::Map<const Matrix_3xD> mX1(X1, 3, n);
		Eigen::Map<Matrix_3xD> mX2(X2, 3, n);
		mX2 = (s * mR * mX1).colwise() + mt;
	}

	// X1 = [X11 Y11 Z11 X12 Y12 ... X1n Y1n Z1n]
	// X2 = [X21 Y21 Z21 X22 Y22 ... X2n Y2n Z2n]
	// X2 = s*R*X1 + t
	static void getRT(int n, const FloatType *X1, const FloatType *X2, FloatType *R, FloatType *t, FloatType &s)
	{
		Eigen::Map<const Matrix_3xD> mX1(X1, 3, n);
		Eigen::Map<const Matrix_3xD> mX2(X2, 3, n);
		// centroid
		Matrix_3x1 mc1 = mX1.rowwise().sum() / mX1.cols();
		Matrix_3x1 mc2 = mX2.rowwise().sum() / mX2.cols();
		// translate origin to centroid
		Matrix_3xD mX1c = mX1.colwise() - mc1;
		Matrix_3xD mX2c = mX2.colwise() - mc2;

		// estimate scale
		Matrix_Dx1 mX1n = mX1c.colwise().norm();
		Matrix_Dx1 mX2n = mX2c.colwise().norm();
		s = (mX1n.array() * mX2n.array()).sum() / (mX1n.array() * mX1n.array()).sum();
		mX2c *= s;

		// corrrelation matrix
		Matrix_3x3 mC = mX1c * mX2c.transpose();
		// system matrix
		Matrix_4x4 A;
		A(3, 3) = mC(0, 0) + mC(1, 1) + mC(2, 2);
		A(3, 0) = A(0, 3) = mC(1, 2) - mC(2, 1);
		A(3, 1) = A(1, 3) = mC(2, 0) - mC(0, 2);
		A(3, 2) = A(2, 3) = mC(0, 1) - mC(1, 0);
		A(0, 0) = mC(0, 0) - mC(1, 1) - mC(2, 2);
		A(0, 1) = A(1, 0) = mC(0, 1) + mC(1, 0);
		A(0, 2) = A(2, 0) = mC(2, 0) + mC(0, 2);
		A(1, 1) = -mC(0, 0) + mC(1, 1) - mC(2, 2);
		A(1, 2) = A(2, 1) = mC(1, 2) + mC(2, 1);
		A(2, 2) = -mC(0, 0) - mC(1, 1) + mC(2, 2);

		// estimate quaternion from eiven vector
		Eigen::SelfAdjointEigenSolver<Matrix_4x4> eig(A, Eigen::ComputeEigenvectors);
		getRotation(eig.eigenvectors().col(3).data(), R);
		Eigen::Map<Matrix_3x3R> mR(R);
		Eigen::Map<Matrix_3x1> mt(t);
		mt = -mR * mc1 * s + mc2;
	}
};

}

#endif // THREE_POINT_HPP_


#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <math.h>
#include <complex>
#include <boost/math/special_functions/jacobi_elliptic.hpp>
#include <boost/math/special_functions/ellint_1.hpp>

double am(double x, double m) {
	if (m == 0) {
		return x;
	} else if (x == 0) {
		return 0;
	} else {
		// function to calculate jacobi amplitude for real input
		// "borrowed" from sagemath
		double sn, cn, dn;
		if (fabs(m) == 1) {
			double tanhx = tanh(x);
			return asin(tanhx);
		} else if (fabs(m) > 1){
			gsl_sf_elljac_e(x, m, &sn, &cn, &dn);
			return atan2(sn, cn);
		}
		else{
			double K = gsl_sf_ellint_Kcomp(sqrt(m), GSL_PREC_DOUBLE);
			if (fabs(x) <= K) {
				gsl_sf_elljac_e(x, m, &sn, &cn, &dn);
				return atan2(sn, cn);
			} else {
				// Do argument reduction on x to end up with z = x - 2nK, with
				// abs(z) <= K
				double tK = 2 * K;
				int n = floor(x / tK);
				double tnK = n * tK;
				double npi = n * M_PI;
				double z = x - tnK;

				gsl_sf_elljac_e(z, m, &sn, &cn, &dn);
				return atan2(sn, cn) + npi;
			}
		}
	}
}

double calc_radius(double psi, double slr, double ecc) {
	double r = slr / (1 + ecc * cos(psi));
	// printf("slr = %.17g \n", slr);
	// printf("ecc = %.17g \n", ecc);
	// printf("psi = %.17g \n", psi);
	return r;
}

double calc_lambda_r(double r, double r1, double r2, double r3,
	double r4, double En) {

	// compute function to change from t[lambda] -> t[psi] with psi an angle
	// associated with radial motion.

	double yr, F_asin, kr;
	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));
	// there is some problem here because of the high precision part of the
	// code. r can be less than r2 (peribothron).
	// TODO: fix the code, but for now a temporary fix:
	if (r < r2) {
		r = r2;
		yr = sqrt(((r - r2)*(r1 - r3))/((r1 - r2)*(r - r3)));
	}
	else {
		yr = sqrt(((r - r2)*(r1 - r3))/((r1 - r2)*(r - r3)));
	}
	// printf("r = %.17g \n", r);
	// printf("r2 = %.17g \n", r2);
	F_asin = gsl_sf_ellint_F(asin(yr), sqrt(kr), GSL_PREC_DOUBLE);
	return (2*F_asin)/(sqrt(1 - En * En)*sqrt((r1 - r3)*(r2 - r4)));
}

double calc_lambda_psi(double psi, double ups_r, double r1,
	double r2, double r3, double r4, double En, double slr, double ecc) {

	double lam_r, lam_r1, r, turns, res, kr;
	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));
	r = calc_radius(psi, slr, ecc);
	lam_r = 2 * M_PI / ups_r;  // radial period
	lam_r1 = calc_lambda_r(r2, r1, r2, r3, r4, En);
	turns = floor(psi / (2 * M_PI));
	if (fmod(psi, 2 * M_PI) <= M_PI) {
		// printf("psi = %.17g\n", psi);
		res = calc_lambda_r(r, r1, r2, r3, r4, En) - lam_r1;
		// printf("res = %.17g\n", res);
	} else {
		res = lam_r1 - calc_lambda_r(r, r1, r2, r3, r4, En);
	}
	// printf("two\n");

	return lam_r * turns + res;
}

double calc_lambda_chi(double chi, double zp, double zm, double En, double aa) {
	// compute function to change from t[lambda] -> t[chi] with chi an angle
	// associated with polar motion.
	double beta, k, k2, prefactor, ellipticK_k, ellipticF;
	beta = aa * aa * (1 - En * En);
	k = sqrt(zm / zp);
	k2 = k * k;
	prefactor = 1 / sqrt(beta * zp);
	ellipticK_k = gsl_sf_ellint_Kcomp(sqrt(k2), GSL_PREC_DOUBLE);
	ellipticF = gsl_sf_ellint_F(M_PI / 2 - chi, sqrt(k2), GSL_PREC_DOUBLE);

	return prefactor * (ellipticK_k - ellipticF);
}

// routines used to find t, r, theta, phi

double calc_theta_chi(double chi, double zm) {
	return acos(zm * cos(chi));
}

double calc_rq(double qr, double r1, double r2,
	double r3, double r4) {

	double arg, sn2, sn, cn, dn, kr;

	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));

	arg = (qr*gsl_sf_ellint_Kcomp(sqrt(kr), GSL_PREC_DOUBLE))/M_PI;
	gsl_sf_elljac_e(arg, kr, &sn, &cn, &dn);
	sn2 = sn * sn;

	return ((-(r2*(r1 - r3)) + (r1 - r2)*r3*sn2)/ (-r1 + r3 + (r1 - r2)*sn2));
}

double calc_zq(double qz, double zp, double zm, double En, double aa) {
	double ktheta, sn, dn, cn, ellipticK_k, arg1, arg2;
	ktheta = (aa*aa *(1 - En*En)*zm*zm)/zp*zp;
	ellipticK_k = gsl_sf_ellint_Kcomp(sqrt(ktheta), GSL_PREC_DOUBLE);
	arg1 = (2*(M_PI/2. + qz)*ellipticK_k)/M_PI;
	arg2 = ktheta;
	gsl_sf_elljac_e(arg1, arg2, &sn, &cn, &dn);

	return zm * sn;
}

double calc_psi_r(double qr, double r1, double r2, double r3, double r4) {
	double kr, amplitude;
	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));
	amplitude = am((qr * gsl_sf_ellint_Kcomp(sqrt(kr), GSL_PREC_DOUBLE))/M_PI,
				   kr);
	return amplitude;
}

double calc_t_r(double qr, double r1, double r2, double r3, double r4,
	double En, double Lz, double aa, double M=1) {
	double psi_r, kr, rp, rm, hr, hp, hm, aa2, En2, sin_psi_r, ellipe_k, ince_k;
	double ellippi_hm, ellippi_hr, ellippi_hp, inc_pi_hm, inc_pi_hr, inc_pi_hp;
	aa2 = aa * aa;
	En2 = En * En;
	psi_r = calc_psi_r(qr, r1, r2, r3, r4);

	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));

	rp = M + sqrt(-aa*aa + M*M);
	rm = M - sqrt(-aa*aa + M*M);

	hr = (r1 - r2)/(r1 - r3);
	hp = ((r1 - r2)*(r3 - rp))/((r1 - r3)*(r2 - rp));
	hm = ((r1 - r2)*(r3 - rm))/((r1 - r3)*(r2 - rm));

	sin_psi_r = pow(sin(psi_r), 2);
	ince_k = gsl_sf_ellint_E(psi_r, sqrt(kr), GSL_PREC_DOUBLE);
	ellipe_k = gsl_sf_ellint_Ecomp(sqrt(kr), GSL_PREC_DOUBLE);
	ellippi_hm = gsl_sf_ellint_Pcomp(sqrt(kr), -hm, GSL_PREC_DOUBLE);
	ellippi_hp = gsl_sf_ellint_Pcomp(sqrt(kr), -hp, GSL_PREC_DOUBLE);
	ellippi_hr = gsl_sf_ellint_Pcomp(sqrt(kr), -hr, GSL_PREC_DOUBLE);

	// printf("ellippi_hm = %.17g \n", ellippi_hm);

	inc_pi_hm = gsl_sf_ellint_P(psi_r, sqrt(kr), -hm, GSL_PREC_DOUBLE);
	inc_pi_hp = gsl_sf_ellint_P(psi_r, sqrt(kr), -hp, GSL_PREC_DOUBLE);
	inc_pi_hr = gsl_sf_ellint_P(psi_r, sqrt(kr), -hr, GSL_PREC_DOUBLE);

	return  (-((En * ((-4*(r2 - r3)*(-(((-2*aa2 + (4 - (aa*Lz)/En)*rm)*
			((qr*ellippi_hm)/M_PI - inc_pi_hm))/
			((r2 - rm)*(r3 - rm))) +
			((-2*aa2 + (4 - (aa*Lz)/En)*rp)*
			((qr*ellippi_hp)/M_PI - inc_pi_hp))/
			((r2 - rp)*(r3 - rp))))/(-rm + rp) +
			4*(r2 - r3)*((qr*ellippi_hr)/M_PI - inc_pi_hr) +
			(r2 - r3)*(r1 + r2 + r3 + r4)*
			((qr*ellippi_hr)/M_PI - inc_pi_hr) +
			(r1 - r3)*(r2 - r4)*((qr*ellipe_k)/M_PI - ince_k +
			(hr*cos(psi_r)*sin(psi_r)*sqrt(1 - kr*sin_psi_r))/
			(1 - hr*sin_psi_r))))/sqrt((1 - En2)*(r1 - r3)*(r2 - r4))));

}

double calc_phi_r(double qr, double r1, double r2, double r3, double r4,
	double En, double Lz, double aa, double M=1){

	double psi_r, kr, rp, rm, hp, hm, aa2, En2;
	double ellpi_hp, ellpi_hm, incpi_hp, incpi_hm;
	aa2 = aa * aa;
	En2 = En * En;
	psi_r = calc_psi_r(qr, r1, r2, r3, r4);
	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));
	rp = M + sqrt(-aa2 + M*M);
	rm = M - sqrt(-aa2 + M*M);

	hp = ((r1 - r2)*(r3 - rp))/((r1 - r3)*(r2 - rp));
	hm = ((r1 - r2)*(r3 - rm))/((r1 - r3)*(r2 - rm));

	ellpi_hp = gsl_sf_ellint_Pcomp(sqrt(kr), -hp, GSL_PREC_DOUBLE);
	ellpi_hm = gsl_sf_ellint_Pcomp(sqrt(kr), -hm, GSL_PREC_DOUBLE);
	incpi_hp = gsl_sf_ellint_P(psi_r, sqrt(kr), -hp, GSL_PREC_DOUBLE);
	incpi_hm = gsl_sf_ellint_P(psi_r, sqrt(kr), -hm, GSL_PREC_DOUBLE);

	return ((2*aa*En*(-(((r2 - r3)*(-((aa*Lz)/En) + 2*rm)*
			((qr*ellpi_hm)/M_PI - incpi_hm))/
			((r2 - rm)*(r3 - rm))) +
			((r2 - r3)*(-((aa*Lz)/En) + 2*rp)*
			((qr*ellpi_hp)/M_PI - incpi_hp))/
			((r2 - rp)*(r3 - rp))))/
			(sqrt((1 - En2)*(r1 - r3)*(r2 - r4))*(-rm + rp)));

}

double calc_psi_z(double qz, double zp, double zm, double En, double aa){
	double ktheta, ellK_k;
	ktheta = (aa*aa *(1 - En*En)*zm*zm)/zp*zp;
	ellK_k = gsl_sf_ellint_Kcomp(sqrt(ktheta), GSL_PREC_DOUBLE);
	return am((2*(M_PI/2. + qz) * ellK_k)/M_PI, ktheta);
}

double calc_t_z(double qz, double zp, double zm, double En, double aa){
	double psi_z, ktheta, elle_k, ince_k;
	psi_z = calc_psi_z(qz, zp, zm, En, aa);
	ktheta = (aa*aa *(1 - En*En)*zm*zm)/zp*zp;
	elle_k = gsl_sf_ellint_Ecomp(sqrt(ktheta), GSL_PREC_DOUBLE);
	ince_k = gsl_sf_ellint_E(psi_z, sqrt(ktheta), GSL_PREC_DOUBLE);
	return ((En*zp*((2*(M_PI/2. + qz)*elle_k)/M_PI - ince_k))/
			(1 - En*En));
}

double calc_phi_z(double qz, double zp, double zm, double En, double Lz,
	double aa){

	double psi_z, ktheta, ellpi_k, incpi_k;
	psi_z = calc_psi_z(qz, zp, zm, En, aa);
	ktheta = (aa*aa * (1 - En*En)*zm*zm)/zp*zp;
	ellpi_k = gsl_sf_ellint_Pcomp(sqrt(ktheta), -zm*zm, GSL_PREC_DOUBLE);
	incpi_k = gsl_sf_ellint_P(psi_z, sqrt(ktheta), -zm*zm, GSL_PREC_DOUBLE);
	return (-((Lz*((2*(M_PI/2. + qz)*ellpi_k)/M_PI -
			incpi_k))/zp));
}

double calc_Ct(double qr0, double qz0, double r1, double r2, double r3,
	double r4, double zp, double zm, double En, double Lz, double aa) {

	double t_r, t_z;
	t_r = calc_t_r(qr0, r1, r2, r3, r4, En, Lz, aa);
	t_z = calc_t_z(qz0, zp, zm, En, aa);
	return t_r + t_z;
}

double calc_Cz(double qr0, double qz0, double r1, double r2, double r3, double r4,
	double zp, double zm, double En, double Lz, double aa) {

	double phi_r, phi_z;
	phi_r = calc_phi_r(qr0, r1, r2, r3, r4, En, Lz, aa);
	phi_z = calc_phi_z(qz0, zp, zm, En, Lz, aa);
	return phi_r + phi_z;
}

double calc_t(double mino_t, double ups_r, double ups_theta, double gamma,
	double qt0, double qr0, double qz0, double r1, double r2, double r3,
	double r4, double zp, double zm, double En, double Lz, double aa){

	double eta_t, eta_r, eta_z, t_r, t_z, Ct;
	eta_t = qt0 + gamma * mino_t;
	eta_r = qr0 + ups_r * mino_t;
	eta_z = qz0 + ups_theta * mino_t;
	if (r1 == r2) {
		t_r = 0;
	} else {
		t_r = calc_t_r(eta_r, r1, r2, r3, r4, En, Lz, aa);
		// printf("t_r = %.17g \n", t_r);
	}
	if (zm == 0){
		t_z = 0;
	} else {
		t_z = calc_t_z(eta_z, zp, zm, En, aa);
	}
	if (qr0 == 0 && qz0 == 0) {
		Ct = 0;
	} else {
		Ct = calc_Ct(qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
	}
	return eta_t + t_r + t_z - Ct;
}

double calc_r(double mino_t, double ups_r, double qr0, double r1, double r2,
	double r3, double r4){

	double eta;
	eta = ups_r * mino_t + qr0;
	return calc_rq(eta, r1, r2, r3, r4);
}

double calc_theta(double mino_t, double ups_theta, double qz0, double zp,
	double zm, double En, double aa){

	double eta;
	eta = ups_theta * mino_t + qz0;
	return acos(calc_zq(eta, zp, zm, En, aa));
}

double calc_phi(double mino_t, double ups_r, double ups_theta, double ups_phi,
	double qphi0, double qr0, double qz0, double r1, double r2, double r3,
	double r4, double zp, double zm, double En, double Lz, double aa){

	double eta_phi, eta_r, eta_theta, phi_r, phi_z, Cz;
	eta_phi = ups_phi * mino_t + qphi0;
	eta_r = ups_r * mino_t + qr0;
	eta_theta = ups_theta * mino_t + qz0;
	if (r1 == r2) {
		phi_r = 0;
	} else {
		phi_r = calc_phi_r(eta_r, r1, r2, r3, r4, En, Lz, aa);
	}

	if (zm == 0) {
		phi_z = 0;
	}
	else {
		phi_z = calc_phi_z(eta_theta, zp, zm, En, Lz, aa);
	}

	if (qr0 == 0 && qz0 == 0){
		Cz = 0;
	} else {
		Cz = calc_Cz(qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
	}
	return eta_phi + phi_r + phi_z - Cz;
}

double calc_J(double chi, double En, double Lz, double Q, double aa, double slr, double ecc) {
    // """
    // Schmidt's J function.

    // Parameters:
    //     chi (float): radial angle
    //     En (float): energy
    //     Lz (float): angular momentum
    //     Q (float): Carter constant
    //     aa (float): spin
    //     slr (float): semi-latus rectum
    //     ecc (float): eccentricity

    // Returns:
    //     J (float)
    // """
    double En2 = En * En;
    double ecc2 = ecc * ecc;
    double aa2 = aa * aa;
    double Lz2 = Lz * Lz;
    double slr2 = slr * slr;

    double eta = 1 + ecc * cos(chi);
    double eta2 = eta * eta;

    double J = (
        (1 - ecc2) * (1 - En2)
        + 2 * (1 - En2 - (1 - ecc2) / slr) * eta
        + (
            ((3 + ecc2) * (1 - En2)) / (1 - ecc2)
            + ((1 - ecc2) * (aa2 * (1 - En2) + Lz2 + Q)) / slr2
            - 4 / slr
        )
        * eta2
    );

    return J;
}

// double calc_wr(double psi, double ups_r, double En, double Lz,
// 			   double Q, double aa, double slr, double ecc, double x) {
// 	// note that this has only been checked for psi in [0, pi]
// 	double aa2 = aa * aa;
// 	double slr2 = slr * slr;
// 	double ecc2 = ecc * ecc;
// 	double En2 = En * En;
// 	double Lz2 = Lz * Lz;
// 	double a1 = (1 - ecc2) * (1 - En2);
//     double b1 = 2 * (1 - En2 - (1 - ecc2) / slr);
//     double c1 = (((3 + ecc2)*(1 - En2))/(1 - ecc2) - 4/slr +
//           		 ((1 - ecc2) * (aa2*(1 - En2) + Lz2 + Q)) / slr2);

// 	double b12 = b1 * b1;
// 	if (psi == M_PI) {
// 		return M_PI;
// 	} else {
// 		complex<double> phi {0, asinh(sqrt((a1 - (-1 + ecc)*(b1 + c1 - c1*ecc))/
//        						 (a1 + b1 + c1 - c1*ecc2 + sqrt(b12 - 4*a1*c1)*ecc2)))* tan(psi/2.))};

// 	 	double m = ((a1 + b1 + c1 - c1*ecc2 + sqrt((b12 - 4*a1*c1)*ecc2))/
//   				    (a1 + b1 + c1 - c1*ecc2 - sqrt((b12 - 4*a1*c1)*ecc2)));
// 		double k = sqrt(m);
// 		complex<double> ellint_f = ellint_1(k, phi);
// 		complex<double> res {0, ((-2*(1 - ecc2) * ellint_f * ups_r * power(cos(psi/2.),2)*
// 				sqrt(2 + (2*(a1 - (-1 + ecc)*(b1 + c1 - c1*ecc))*power(tan(psi/2.),2))/
// 					(a1 + b1 + c1 - c1*ecc2 - sqrt(b12 - 4*a1*c1)*ecc2)))*
// 				sqrt(1 + ((a1 - (-1 + ecc)*(b1 + c1 - c1*ecc))*power(tan(psi/2.),2))/
// 					(a1 + b1 + c1 - c1*ecc2 + sqrt((b12 - 4*a1*c1)*ecc2))))/
// 			(sqrt((a1 - (-1 + ecc)*(b1 + c1 - c1*ecc))/
// 				(a1 + b1 + c1 - c1*ecc2 + sqrt((b12 - 4*a1*c1)*ecc2)))*
// 				slr*sqrt(2*a1 + 2*b1 + 2*c1 + c1*ecc2 + 2*(b1 + 2*c1)*ecc*cos(psi) +
// 				c1*ecc2*cos(2*psi))))};
// 		double re_res = real(res);

// 		return re_res;
// 	}
// }

double calc_dwr_dpsi(double psi, double ups_r, double En, double Lz,
					 double Q, double aa, double slr, double ecc) {
    double J = calc_J(psi, En, Lz, Q, aa, slr, ecc);
	double ecc2 = ecc * ecc;
    return (1 - ecc2) / slr * ups_r / sqrt(J);
}

// double calc_wtheta(double chi, double ups_theta, double zp, double zm,
// 				   double En, double Lz, double aa, double slr, double x) {
//     // """
//     // w_theta = ups_theta * lambda as a function of polar angle chi

//     // Parameters:
//     //     chi (float): polar angle
//     //     ups_theta (float): theta mino frequency
//     //     En (float): energy
//     //     Lz (float): angular momentum
//     //     aa (float): spin
//     //     slr (float): semi-latus rectum
//     //     x (float): inclination

//     // Returns:
//     //     w_theta (float)
//     // """
//     double pi = M_PI;
//     if (chi >= 0 && chi <= pi / 2) {
//         return ups_theta * calc_lambda_chi(chi, zp, zm, En, Lz, aa, slr, x);
// 	} else if (chi > pi / 2 && chi <= pi) {
//         return pi - ups_theta * calc_lambda_chi(pi - chi, zp, zm, En, Lz, aa, slr, x);
// 	} else if (chi > pi && chi <= 3 * pi / 2) {
//         return pi + ups_theta * calc_lambda_chi(chi - pi, zp, zm, En, Lz, aa, slr, x);
// 	} else if (chi > 3 * pi / 2 && chi <= 2 * pi) {
//         return 2 * pi - ups_theta * calc_lambda_chi(2 * pi - chi, zp, zm, En, Lz, aa, slr, x);
// 	} else {
//         // print("Something went wrong in calc_wtheta!")
//         return 0.0;  // this case should not occur, but is required by C++
// 	}
// }

double calc_dwtheta_dchi(double chi, double zp, double zm) {
    // """
    // derivative of w_theta

    // Parameters:
    //     chi (float): polar angle
    //     zp (float): polar root
    //     zm (float): polar root

    // Returns:
    //     dw_dtheta (float)
    // """
    double k = sqrt(zm / zp);
    double ellipticK_k = gsl_sf_ellint_Kcomp(k, GSL_PREC_DOUBLE);
    return M_PI / (2 * ellipticK_k) * (1 / (1 - k * k * pow(cos(chi),2)));
}

void calc_circular_eq_coords(double &t, double &r, double &theta, double &phi,
							 double psi, double En, double Lz, double aa, double slr, double M) {
    // lam_psi is used to cross check with circular orbits in BHPTK
    // lam_psi = (psi / (sqrt(1 - En**2 + 3*(1 - En**2) + 2*(1 - En**2 - 1/slr) +
    //            (aa**2*(1 - En**2) + Lz**2)/slr**2 - 4/slr)*slr))
    // print(lam_psi)

	double x, Vr, Vphi, Vt, J, denom;
	double aa2 = aa * aa;
	double slr2 = slr * slr;
	x = Lz - aa * En;
	double x2 = x * x;

    Vr = aa2 + x2 + 2 * aa * x * En - 6 * M * x2 / slr;
    Vphi = x + aa * En - 2 * M * x / slr;
    Vt = aa2 * En - 2 * aa * M * x / slr + En * slr2;
    J = 1 - 2 * M / slr + aa2 / slr2;
    denom = J * sqrt(Vr);
	t = Vt / denom * psi;
    phi = Vphi / denom * psi;
    r = slr;
	theta = M_PI / 2;
}

void calc_equatorial_coords(double &t, double &r, double &theta, double &phi,
	double psi, double ups_r, double ups_theta,
	double ups_phi, double gamma, double r1, double r2, double r3,
	double r4, double zp, double zm, double En, double Lz, double aa,
	double slr, double ecc, double qt0, double qr0, double qz0,
	double qphi0) {

	double lam_psi;
	if (zm != 0){
		printf("The orbit specified is not equatorial.\n");
	}
	r = calc_radius(psi, slr, ecc);
	lam_psi = calc_lambda_psi(psi, ups_r, r1, r2, r3, r4, En, slr, ecc);
	t = calc_t(lam_psi, ups_r, ups_theta, gamma, qt0, qr0, qz0, r1, r2, r3, r4,
			   zp, zm, En, Lz, aa);
	theta = M_PI / 2;  // TODO(aaron): check generic theta
	phi = calc_phi(lam_psi, ups_r, ups_theta, ups_phi, qphi0, qr0, qz0, r1, r2,
				   r3, r4, zp, zm, En, Lz, aa);

}

void calc_gen_coords_mino(double &t, double &r, double &theta, double &phi,
	double mino_t, double ups_r, double ups_theta, double ups_phi, double gamma,
    double r1, double r2, double r3, double r4, double zp, double zm, double En,
    double Lz, double aa, double qphi0, double qr0, double qz0, double qt0) {

	t = calc_t(mino_t, ups_r, ups_theta, gamma, qt0, qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
	r = calc_r(mino_t, ups_r, qr0, r1, r2, r3, r4);
	theta = calc_theta(mino_t, ups_theta, qz0, zp, zm, En, aa);
	phi = calc_phi(mino_t, ups_r, ups_theta, ups_phi, qphi0, qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
}

// void calc_gen_coords_psi(double &t, double &r, double &theta, double &phi,
// 	double psi, double ups_r, double ups_theta, double ups_phi, double gamma,
//     double r1, double r2, double r3, double r4, double zp, double zm, double En,
// 	double Lz, double Q, double aa, double slr, double ecc, double x,
// 	double qphi0, double qr0, double qz0, double qt0) {

// 	double wr = calc_wr(psi, ups_r, En, Lz, Q, aa, slr, ecc, x);
// 	double mino_t = wr / ups_r;
// 	t = calc_t(mino_t, ups_r, ups_theta, gamma, qt0, qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
// 	r = calc_r(mino_t, ups_r, qr0, r1, r2, r3, r4);
// 	theta = calc_theta(mino_t, ups_theta, qz0, zp, zm, En, aa);
// 	phi = calc_phi(mino_t, ups_r, ups_theta, ups_phi, qphi0, qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
// }


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2006 - 2016 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, 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 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Ivan Christov, Wolfgang Bangerth, Texas A&M University, 2006
 */
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <fstream>
#include <iostream>
namespace Step25
{
  using namespace dealii;
  template <int dim>
  class SineGordonProblem
  {
  public:
    SineGordonProblem();
    void run();
  private:
    void         make_grid_and_dofs();
    void         assemble_system();
    void         compute_nl_term(const Vector<double> &old_data,
                                 const Vector<double> &new_data,
                                 Vector<double> &      nl_term) const;
    void         compute_nl_matrix(const Vector<double> &old_data,
                                   const Vector<double> &new_data,
                                   SparseMatrix<double> &nl_matrix) const;
    unsigned int solve();
    void         output_results(const unsigned int timestep_number) const;
    Triangulation<dim> triangulation;
    FE_Q<dim>          fe;
    DoFHandler<dim>    dof_handler;
    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;
    const unsigned int n_global_refinements;
    double       time;
    const double final_time, time_step;
    const double theta;
    Vector<double> solution, solution_update, old_solution;
    Vector<double> M_x_velocity;
    Vector<double> system_rhs;
    const unsigned int output_timestep_skip;
  };
  template <int dim>
  class ExactSolution : public Function<dim>
  {
  public:
    ExactSolution(const unsigned int n_components = 1, const double time = 0.)
      : Function<dim>(n_components, time)
    {}
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;
  };
  template <int dim>
  double ExactSolution<dim>::value(const Point<dim> &p,
                                   const unsigned int /*component*/) const
  {
    double t = this->get_time();
    switch (dim)
      {
        case 1:
          {
            const double m  = 0.5;
            const double c1 = 0.;
            const double c2 = 0.;
            return -4. * std::atan(m / std::sqrt(1. - m * m) *
                                   std::sin(std::sqrt(1. - m * m) * t + c2) /
                                   std::cosh(m * p[0] + c1));
          }
        case 2:
          {
            const double theta  = numbers::PI / 4.;
            const double lambda = 1.;
            const double a0     = 1.;
            const double s      = 1.;
            const double arg    = p[0] * std::cos(theta) +
                               std::sin(theta) * (p[1] * std::cosh(lambda) +
                                                  t * std::sinh(lambda));
            return 4. * std::atan(a0 * std::exp(s * arg));
          }
        case 3:
          {
            const double theta = numbers::PI / 4;
            const double phi   = numbers::PI / 4;
            const double tau   = 1.;
            const double c0    = 1.;
            const double s     = 1.;
            const double arg   = p[0] * std::cos(theta) +
                               p[1] * std::sin(theta) * std::cos(phi) +
                               std::sin(theta) * std::sin(phi) *
                                 (p[2] * std::cosh(tau) + t * std::sinh(tau));
            return 4. * std::atan(c0 * std::exp(s * arg));
          }
        default:
          Assert(false, ExcNotImplemented());
          return -1e8;
      }
  }
  template <int dim>
  class InitialValues : public Function<dim>
  {
  public:
    InitialValues(const unsigned int n_components = 1, const double time = 0.)
      : Function<dim>(n_components, time)
    {}
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;
  };
  template <int dim>
  double InitialValues<dim>::value(const Point<dim> & p,
                                   const unsigned int component) const
  {
    return ExactSolution<dim>(1, this->get_time()).value(p, component);
  }
  template <int dim>
  SineGordonProblem<dim>::SineGordonProblem()
    : fe(1)
    , dof_handler(triangulation)
    , n_global_refinements(6)
    , time(-5.4414)
    , final_time(2.7207)
    , time_step(10 * 1. / std::pow(2., 1. * n_global_refinements))
    , theta(0.5)
    , output_timestep_skip(1)
  {}
  template <int dim>
  void SineGordonProblem<dim>::make_grid_and_dofs()
  {
    GridGenerator::hyper_cube(triangulation, -10, 10);
    triangulation.refine_global(n_global_refinements);
    std::cout << "   Number of active cells: " << triangulation.n_active_cells()
              << std::endl
              << "   Total number of cells: " << triangulation.n_cells()
              << std::endl;
    dof_handler.distribute_dofs(fe);
    std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
              << std::endl;
    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(sparsity_pattern);
    mass_matrix.reinit(sparsity_pattern);
    laplace_matrix.reinit(sparsity_pattern);
    MatrixCreator::create_mass_matrix(dof_handler,
                                      QGauss<dim>(fe.degree + 1),
                                      mass_matrix);
    MatrixCreator::create_laplace_matrix(dof_handler,
                                         QGauss<dim>(fe.degree + 1),
                                         laplace_matrix);
    solution.reinit(dof_handler.n_dofs());
    solution_update.reinit(dof_handler.n_dofs());
    old_solution.reinit(dof_handler.n_dofs());
    M_x_velocity.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }
  template <int dim>
  void SineGordonProblem<dim>::assemble_system()
  {
    system_matrix.copy_from(mass_matrix);
    system_matrix.add(std::pow(time_step * theta, 2), laplace_matrix);
    SparseMatrix<double> tmp_matrix(sparsity_pattern);
    compute_nl_matrix(old_solution, solution, tmp_matrix);
    system_matrix.add(std::pow(time_step * theta, 2), tmp_matrix);
    system_rhs = 0.;
    Vector<double> tmp_vector(solution.size());
    mass_matrix.vmult(system_rhs, solution);
    laplace_matrix.vmult(tmp_vector, solution);
    system_rhs.add(std::pow(time_step * theta, 2), tmp_vector);
    mass_matrix.vmult(tmp_vector, old_solution);
    system_rhs.add(-1.0, tmp_vector);
    laplace_matrix.vmult(tmp_vector, old_solution);
    system_rhs.add(std::pow(time_step, 2) * theta * (1 - theta), tmp_vector);
    system_rhs.add(-time_step, M_x_velocity);
    compute_nl_term(old_solution, solution, tmp_vector);
    system_rhs.add(std::pow(time_step, 2) * theta, tmp_vector);
    system_rhs *= -1.;
  }
  template <int dim>
  void SineGordonProblem<dim>::compute_nl_term(const Vector<double> &old_data,
                                               const Vector<double> &new_data,
                                               Vector<double> &nl_term) const
  {
    nl_term = 0;
    const QGauss<dim> quadrature_formula(fe.degree + 1);
    FEValues<dim>     fe_values(fe,
                            quadrature_formula,
                            update_values | update_JxW_values |
                              update_quadrature_points);
    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points    = quadrature_formula.size();
    Vector<double>                       local_nl_term(dofs_per_cell);
    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
    std::vector<double>                  old_data_values(n_q_points);
    std::vector<double>                  new_data_values(n_q_points);
    typename DoFHandler<dim>::active_cell_iterator cell =
                                                     dof_handler.begin_active(),
                                                   endc = dof_handler.end();
    for (; cell != endc; ++cell)
      {
        local_nl_term = 0;
        fe_values.reinit(cell);
        fe_values.get_function_values(old_data, old_data_values);
        fe_values.get_function_values(new_data, new_data_values);
        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            local_nl_term(i) +=
              (std::sin(theta * new_data_values[q_point] +
                        (1 - theta) * old_data_values[q_point]) *
               fe_values.shape_value(i, q_point) * fe_values.JxW(q_point));
        cell->get_dof_indices(local_dof_indices);
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          nl_term(local_dof_indices[i]) += local_nl_term(i);
      }
  }
  template <int dim>
  void SineGordonProblem<dim>::compute_nl_matrix(
    const Vector<double> &old_data,
    const Vector<double> &new_data,
    SparseMatrix<double> &nl_matrix) const
  {
    QGauss<dim>   quadrature_formula(fe.degree + 1);
    FEValues<dim> fe_values(fe,
                            quadrature_formula,
                            update_values | update_JxW_values |
                              update_quadrature_points);
    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points    = quadrature_formula.size();
    FullMatrix<double> local_nl_matrix(dofs_per_cell, dofs_per_cell);
    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
    std::vector<double>                  old_data_values(n_q_points);
    std::vector<double>                  new_data_values(n_q_points);
    typename DoFHandler<dim>::active_cell_iterator cell =
                                                     dof_handler.begin_active(),
                                                   endc = dof_handler.end();
    for (; cell != endc; ++cell)
      {
        local_nl_matrix = 0;
        fe_values.reinit(cell);
        fe_values.get_function_values(old_data, old_data_values);
        fe_values.get_function_values(new_data, new_data_values);
        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              local_nl_matrix(i, j) +=
                (std::cos(theta * new_data_values[q_point] +
                          (1 - theta) * old_data_values[q_point]) *
                 fe_values.shape_value(i, q_point) *
                 fe_values.shape_value(j, q_point) * fe_values.JxW(q_point));
        cell->get_dof_indices(local_dof_indices);
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int j = 0; j < dofs_per_cell; ++j)
            nl_matrix.add(local_dof_indices[i],
                          local_dof_indices[j],
                          local_nl_matrix(i, j));
      }
  }
  template <int dim>
  unsigned int SineGordonProblem<dim>::solve()
  {
    SolverControl solver_control(1000, 1e-12 * system_rhs.l2_norm());
    SolverCG<>    cg(solver_control);
    PreconditionSSOR<> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);
    cg.solve(system_matrix, solution_update, system_rhs, preconditioner);
    return solver_control.last_step();
  }
  template <int dim>
  void SineGordonProblem<dim>::output_results(
    const unsigned int timestep_number) const
  {
    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "u");
    data_out.build_patches();
    const std::string filename =
      "solution-" + Utilities::int_to_string(timestep_number, 3) + ".vtk";
    std::ofstream output(filename);
    data_out.write_vtk(output);
  }
  template <int dim>
  void SineGordonProblem<dim>::run()
  {
    make_grid_and_dofs();
    {
      AffineConstraints<double> constraints;
      constraints.close();
      VectorTools::project(dof_handler,
                           constraints,
                           QGauss<dim>(fe.degree + 1),
                           InitialValues<dim>(1, time),
                           solution);
    }
    output_results(0);
    unsigned int timestep_number = 1;
    for (time += time_step; time <= final_time;
         time += time_step, ++timestep_number)
      {
        old_solution = solution;
        std::cout << std::endl
                  << "Time step #" << timestep_number << "; "
                  << "advancing to t = " << time << "." << std::endl;
        double initial_rhs_norm = 0.;
        bool   first_iteration  = true;
        do
          {
            assemble_system();
            if (first_iteration == true)
              initial_rhs_norm = system_rhs.l2_norm();
            const unsigned int n_iterations = solve();
            solution += solution_update;
            if (first_iteration == true)
              std::cout << "    " << n_iterations;
            else
              std::cout << '+' << n_iterations;
            first_iteration = false;
          }
        while (system_rhs.l2_norm() > 1e-6 * initial_rhs_norm);
        std::cout << " CG iterations per nonlinear step." << std::endl;
        Vector<double> tmp_vector(solution.size());
        laplace_matrix.vmult(tmp_vector, solution);
        M_x_velocity.add(-time_step * theta, tmp_vector);
        laplace_matrix.vmult(tmp_vector, old_solution);
        M_x_velocity.add(-time_step * (1 - theta), tmp_vector);
        compute_nl_term(old_solution, solution, tmp_vector);
        M_x_velocity.add(-time_step, tmp_vector);
        if (timestep_number % output_timestep_skip == 0)
          output_results(timestep_number);
      }
  }
} // namespace Step25
int main()
{
  try
    {
      using namespace dealii;
      using namespace Step25;
      SineGordonProblem<1> sg_problem;
      sg_problem.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  return 0;
}


#include <cap_gait/contrib/KalmanFilter.h>
#include <Eigen/LU>

using namespace margait_contrib;

KalmanFilter::KalmanFilter()
{
	lastX = 0;
	lastV = 0;
	smoothing = 0.1;
	timeStep = 0.010;
	x = 0;
	v = 0;
	a = 0;
	px = 0;
	pv = 0;
	pa = 0;

	// When the gain is adaptive, the smoothing config parameters don't make any difference.
	adaptiveGain = true;

	init();
}

// Prepares (resets) the noise matrices and the kalman gain, but preserves the current state.
// This is good for changing the smoothing on the fly without messing with the state estimate.
void KalmanFilter::init()
{
	// Process noise.
	Q = Eigen::Matrix3d::Identity();

	// Measurement noise.
	R = (0.001 + smoothing*100.0) * Eigen::Matrix3d::Identity();

	// Passive linear system dynamics matrix A.
	A << 1.0, timeStep,   0.0   ,
	     0.0,   1.0   , timeStep,
	     0.0,   0.0   ,   1.0   ;

	// State and measurement vectors.
	X << x, v, a;
	Z.setZero();

	// Other matrices
	H = I = K = Eigen::Matrix3d::Identity();
	P.setZero();
}

// Sets the smoothing parameter.
void KalmanFilter::setSmoothing(double s)
{
	smoothing = s;
	init();
}

// Sets the time interval between the updates. It's set to 10 milliseconds by default.
void KalmanFilter::setTimeStep(double t)
{
	timeStep = t;
	init();
}

// Resets the state to the provided parameters, but doesn't change the noise and the kalman gain.
// This is good to set the state estimate on the fly without messing with the Kalman gain.
void KalmanFilter::reset(double xx, double vv, double aa)
{
	x = xx;
	v = vv;
	a = aa;

	X << x, v, a;

	lastX = x;
	lastV = v;
}

// Updates the process. Provide a position measurement z.
void KalmanFilter::update(double z)
{
	double Zv = (z - lastX)/timeStep;
	Z << z, Zv, (Zv - lastV)/timeStep;
	lastX = Z(0);
	lastV = Z(1);

	if (adaptiveGain)
	{
		X = A*X;
		P = A*P*A.transpose() + Q;
		K = P*H.transpose() * (H*P*H.transpose() + R).inverse();
		X = X + K*(Z - H*X);
		P = (I - K*H)*P;
	}
	else
	{
		X = A*X;
		K = Q*H.transpose() * (H*Q*H.transpose() + R).inverse();
		X = X + K*(Z - H*X);
	}

	x = X(0);
	v = X(1);
	a = X(2);

	px = P(0,0);
	pv = P(1,1);
	pa = P(2,2);
}