#include <Eigen/Dense>
#include <iostream>
#include <math.h>
#define PI  3.141592
//LEG LINK LENGTHS
#define L1 0.12
#define L2 0.303
#define L3 0.303

#define FL 0
#define FR 1
#define BL 2
#define BR 3
#define MASS 11.168
/*inertia  = 0.178402 -0.0458692 0.0180358
-0.0458692 0.470056 -8.95236e-08
0.0180358 -8.95236e-08 0.446392
*/
//LEG-FL

int bound_end_eff_pos(double &x, double &y, double &z)
{
    if(x<= 0.4 && x>= - 0.4)
    {
        if(y <= 0.22 && y>= 0)
        {
            if(z<= -0.15 && z>= -0.4)
            {
                return 1;
            }
        }
    }
    return 0;
}
void _forward_kinematics_FL(Eigen::Vector3d &end_effector_pos, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
    end_effector_pos[0] = (l2*sin(thetas[1]) + l3*sin(thetas[1] + thetas[2]));
    end_effector_pos[1] = l1*cos(thetas[0])+(l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*sin(thetas[0]);
    end_effector_pos[2] = l1*sin(thetas[0])-((l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*cos(thetas[0]));
}

void _inverse_kinematics_FL(Eigen::Vector3d &thetas, Eigen::Vector3d &end_effector_pos, float l1 = L1, float l2 = L2, float l3 = L3)
{
    double x = end_effector_pos[0];
    double y = end_effector_pos[1];
    double z = end_effector_pos[2];
    int isValid = bound_end_eff_pos(x,y,z);
    if(!isValid)
    {
       // std::cout<<"invalid point: "<<x<<","<<y<<","<<z<<std::endl;
    }
    double r, th23, th1, th2, th3,t;
    r = sqrt(y*y + z*z -l1*l1);
    th1 = atan2(y*r + z*l1, y*l1 - z*r);
    t = (2*l2*x + sqrt(4*pow(l2,2)*pow(r,2) - pow(r,4) + 4*pow(l2,2)*pow(x,2) - 2*pow(r,2)*pow(x,2) - pow(x,4)))/(2*l2*r + pow(r,2) + pow(x,2));
    th23 = atan2(2*t, 1-t*t);
    th2 = atan2(x - l2*sin(th23), r - l2*cos(th23));
    th3 = th23 - th2;
    thetas[0]=th1;
    thetas[1]=th2;
    thetas[2]=th3;
}

void _jacobian_FL(Eigen::Matrix<double, 3, 3> &jacob, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
    jacob << 0,l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]),l3*cos(thetas[1] + thetas[2]),
           (l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*cos(thetas[0]) - l1*sin(thetas[0]),(-l2*sin(thetas[1]) - l3*sin(thetas[1] + thetas[2]))*sin(thetas[0]),-l3*sin(thetas[1] + thetas[2])*sin(thetas[0]),
           ((l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*sin(thetas[0])) +l1*cos(thetas[0]),(l2*sin(thetas[1]) + l3*sin(thetas[1] + thetas[2]))*cos(thetas[0]),l3*sin(thetas[1] + thetas[2])*cos(thetas[0]);

}

void _inverse_dynamics_FL(Eigen::Vector3d &torques, Eigen::Vector3d &force, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
    /*
    Finds Joint torques for desired force at end effector
    torques =  [abduction, hip, knee]
    force = [x,y,z]
    thetas = [abduction, hip, knee] angles in radians
    l1, l2, l3 = link lengths
    */
    Eigen::Matrix<double,3,3> jacob;
    _jacobian_FL(jacob, thetas);
    torques = jacob.transpose()*force;
}

//LEG-FR
void _forward_kinematics_FR(Eigen::Vector3d &end_effector_pos, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
    end_effector_pos[0] = (l2*sin(thetas[1]) + l3*sin(thetas[1] + thetas[2]));
    end_effector_pos[1] = -1*(l1*cos(thetas[0])+(l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*sin(thetas[0]));
    end_effector_pos[2] = l1*sin(thetas[0])-((l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*cos(thetas[0]));
}

void _inverse_kinematics_FR(Eigen::Vector3d &thetas, Eigen::Vector3d &end_effector_pos, float l1 = L1, float l2 = L2, float l3 = L3)
{
    double x = end_effector_pos[0];
    double y = -1*end_effector_pos[1];
    double z = end_effector_pos[2];
    int isValid = bound_end_eff_pos(x,y,z);
    if(!isValid)
    {
       // std::cout<<"invalid point: "<<x<<","<<y<<","<<z<<std::endl;
    }
    double r, th23, th1, th2, th3,t;
    r = sqrt(y*y + z*z -l1*l1);
    th1 = atan2(y*r + z*l1, y*l1 - z*r);
    t = (2*l2*x + sqrt(4*pow(l2,2)*pow(r,2) - pow(r,4) + 4*pow(l2,2)*pow(x,2) - 2*pow(r,2)*pow(x,2) - pow(x,4)))/(2*l2*r + pow(r,2) + pow(x,2));
    th23 = atan2(2*t, 1-t*t);
    th2 = atan2(x - l2*sin(th23), r - l2*cos(th23));
    th3 = th23 - th2;
    thetas[0]=th1;
    thetas[1]=th2;
    thetas[2]=th3;
}

void _jacobian_FR(Eigen::Matrix<double, 3, 3> &jacob, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
   jacob << 0,l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]),l3*cos(thetas[1] + thetas[2]),
           (l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*cos(thetas[0]) - l1*sin(thetas[0]),(l2*sin(thetas[1]) + l3*sin(thetas[1] + thetas[2]))*sin(thetas[0]),l3*sin(thetas[1] + thetas[2])*sin(thetas[0]),
           ((l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*sin(thetas[0])) +l1*cos(thetas[0]),(l2*sin(thetas[1]) + l3*sin(thetas[1] + thetas[2]))*cos(thetas[0]),l3*sin(thetas[1] + thetas[2])*cos(thetas[0]);
}

void _inverse_dynamics_FR(Eigen::Vector3d &torques, Eigen::Vector3d &force, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
    /*
    Finds Joint torques for desired force at end effector
    torques =  [abduction, hip, knee]
    force = [x,y,z]
    thetas = [abduction, hip, knee] angles in radians
    l1, l2, l3 = link lengths
    */
    Eigen::Matrix<double,3,3> jacob;
    _jacobian_FR(jacob, thetas);
    torques = jacob.transpose()*force;
    torques[0]=-1*torques[0];
}

//LEG-BL
void _forward_kinematics_BL(Eigen::Vector3d &end_effector_pos, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
    end_effector_pos[0] = (l2*sin(thetas[1]) + l3*sin(thetas[1] + thetas[2]));
    end_effector_pos[1] = l1*cos(thetas[0])+(l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*sin(thetas[0]);
    end_effector_pos[2] = l1*sin(thetas[0])-((l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*cos(thetas[0]));
}

void _inverse_kinematics_BL(Eigen::Vector3d &thetas, Eigen::Vector3d &end_effector_pos, float l1 = L1, float l2 = L2, float l3 = L3)
{
    double x = end_effector_pos[0];
    double y = end_effector_pos[1];
    double z = end_effector_pos[2];
    int isValid = bound_end_eff_pos(x,y,z);
    if(!isValid)
    {
        //std::cout<<"invalid point: "<<x<<","<<y<<","<<z<<std::endl;
    }
    double r, th23, th1, th2, th3,t;
    r = sqrt(y*y + z*z -l1*l1);
    th1 = atan2(y*r + z*l1, y*l1 - z*r);
    t = (2*l2*x + sqrt(4*pow(l2,2)*pow(r,2) - pow(r,4) + 4*pow(l2,2)*pow(x,2) - 2*pow(r,2)*pow(x,2) - pow(x,4)))/(2*l2*r + pow(r,2) + pow(x,2));
    th23 = atan2(2*t, 1-t*t);
    th2 = atan2(x - l2*sin(th23), r - l2*cos(th23));
    th3 = th23 - th2;
    thetas[0]=th1;
    thetas[1]=th2;
    thetas[2]=th3;
}

void _jacobian_BL(Eigen::Matrix<double, 3, 3> &jacob, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
   jacob << 0,l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]),l3*cos(thetas[1] + thetas[2]),
           (l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*cos(thetas[0]) - l1*sin(thetas[0]),(-l2*sin(thetas[1]) - l3*sin(thetas[1] + thetas[2]))*sin(thetas[0]),-l3*sin(thetas[1] + thetas[2])*sin(thetas[0]),
           ((l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*sin(thetas[0])) +l1*cos(thetas[0]),(l2*sin(thetas[1]) + l3*sin(thetas[1] + thetas[2]))*cos(thetas[0]),l3*sin(thetas[1] + thetas[2])*cos(thetas[0]);

}

void _inverse_dynamics_BL(Eigen::Vector3d &torques, Eigen::Vector3d &force, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
    /*
    Finds Joint torques for desired force at end effector
    torques =  [abduction, hip, knee]
    force = [x,y,z]
    thetas = [abduction, hip, knee] angles in radians
    l1, l2, l3 = link lengths
    */
    Eigen::Matrix<double,3,3> jacob;
    _jacobian_BL(jacob, thetas);
    torques = jacob.transpose()*force;
}

//Leg - BR
void _forward_kinematics_BR(Eigen::Vector3d &end_effector_pos, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
    end_effector_pos[0] = (l2*sin(thetas[1]) + l3*sin(thetas[1] + thetas[2]));
    end_effector_pos[1] = -1*(l1*cos(thetas[0])+(l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*sin(thetas[0]));
    end_effector_pos[2] = l1*sin(thetas[0])-((l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*cos(thetas[0]));
}

void _inverse_kinematics_BR(Eigen::Vector3d &thetas, Eigen::Vector3d &end_effector_pos, float l1 = L1, float l2 = L2, float l3 = L3)
{
    double x = end_effector_pos[0];
    double y = -1*end_effector_pos[1];
    double z = end_effector_pos[2];
    int isValid = bound_end_eff_pos(x,y,z);
    if(!isValid)
    {
       // std::cout<<"invalid point: "<<x<<","<<y<<","<<z<<std::endl;
    }
    double r, th23, th1, th2, th3,t;
    r = sqrt(y*y + z*z -l1*l1);
    th1 = atan2(y*r + z*l1, y*l1 - z*r);
    t = (2*l2*x + sqrt(4*pow(l2,2)*pow(r,2) - pow(r,4) + 4*pow(l2,2)*pow(x,2) - 2*pow(r,2)*pow(x,2) - pow(x,4)))/(2*l2*r + pow(r,2) + pow(x,2));
    th23 = atan2(2*t, 1-t*t);
    th2 = atan2(x - l2*sin(th23), r - l2*cos(th23));
    th3 = th23 - th2;
    thetas[0]=th1;
    thetas[1]=th2;
    thetas[2]=th3;
}

void _jacobian_BR(Eigen::Matrix<double, 3, 3> &jacob, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
   jacob << 0,l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]),l3*cos(thetas[1] + thetas[2]),
           (l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*cos(thetas[0]) - l1*sin(thetas[0]),(l2*sin(thetas[1]) + l3*sin(thetas[1] + thetas[2]))*sin(thetas[0]),l3*sin(thetas[1] + thetas[2])*sin(thetas[0]),
           ((l2*cos(thetas[1]) + l3*cos(thetas[1] + thetas[2]))*sin(thetas[0])) +l1*cos(thetas[0]),(l2*sin(thetas[1]) + l3*sin(thetas[1] + thetas[2]))*cos(thetas[0]),l3*sin(thetas[1] + thetas[2])*cos(thetas[0]);

}

void _inverse_dynamics_BR(Eigen::Vector3d &torques, Eigen::Vector3d &force, Eigen::Vector3d &thetas, float l1 = L1, float l2 = L2, float l3 = L3)
{
    /*
    Finds Joint torques for desired force at end effector
    torques =  [abduction, hip, knee]
    force = [x,y,z]
    thetas = [abduction, hip, knee] angles in radians
    l1, l2, l3 = link lengths
    */
    Eigen::Matrix<double,3,3> jacob;
    _jacobian_BR(jacob, thetas);
    torques = jacob.transpose()*force;
    torques[0]=-1*torques[0];
}

void forward_kinematics(Eigen::Vector3d &thetas, Eigen::Vector3d &end_effector_pos, int leg, float l1 = L1, float l2 = L2, float l3 = L3)
{
    /*
    Performs forward kinematics for a 3R spatial manipulator
    thetas = [abduction, hip, knee] angles in radians
    end_effector_pos = [x,y,z] in base frame
    l1, l2, l3 = link lengths
    FL=0
    FR=1
    BL=2
    BR=3
    */
   if(leg == FL)
   {
       _forward_kinematics_FL(thetas, end_effector_pos);
   }
   else if(leg == FR)
   {
       _forward_kinematics_FR(thetas, end_effector_pos);
   }
   else if(leg == BL)
   {
       _forward_kinematics_BL(thetas, end_effector_pos);
   }
   else if(leg == BR)
   {
       _forward_kinematics_BR(thetas, end_effector_pos);
   }
   else
   {
       std::cout<<"Invalid leg number, forward kinematics"<<std::endl;
   }

}

void inverse_kinematics(Eigen::Vector3d &thetas, Eigen::Vector3d &end_effector_pos, int leg, float l1 = L1, float l2 = L2, float l3 = L3)
{
    /*
    Performs forward kinematics for a 3R spatial manipulator
    thetas = [abduction, hip, knee] angles in radians
    end_effector_pos = [x,y,z] in base frame
    l1, l2, l3 = link lengths
    FL=0
    FR=1
    BL=2
    BR=3
    */
   if(leg == FL)
   {
       _inverse_kinematics_FL(thetas, end_effector_pos);
   }
   else if(leg == FR)
   {
       _inverse_kinematics_FR(thetas, end_effector_pos);
   }
   else if(leg == BL)
   {
       _inverse_kinematics_BL(thetas, end_effector_pos);
   }
   else if(leg == BR)
   {
       _inverse_kinematics_BR(thetas, end_effector_pos);
   }
   else
   {
       std::cout<<"Invalid leg number, inverse kinematics"<<std::endl;
   }

}

// int main()
// {

//     // Eigen::Vector3d end_eff(0.15, 0.15, 0.2);
//     // Eigen::Vector3d fwd_kin(0.0, 0.0, 0.0);
//     // Eigen::Vector3d jpos(0,0,0);
//     // Eigen::Matrix<double,3,3> jaco;
//     // // _inverse_kinematics_BR(jpos, end_eff);
//     // _forward_kinematics_FL(fwd_kin, jpos);
//     // _jacobian_FL(jaco, jpos);
//     // std::cout<<jaco<<std::endl;
//     Eigen::Vector3d thetas(1.2, 0.3, 0.66);
//     Eigen::Vector3d fwd_kin(0.0, 0.0, 0.0);
//     Eigen::Vector3d inv_kin(0,0,0);
//     Eigen::Vector3d fwd_kin2(0,0,0);
//     _forward_kinematics_FL(fwd_kin, thetas);
//     _inverse_kinematics_FL(inv_kin, fwd_kin);
//     _forward_kinematics_FL(fwd_kin2, inv_kin);
//     std::cout<<fwd_kin[0]<<","<<fwd_kin[1]<<","<<fwd_kin[2]<<std::endl;
//     std::cout<<fwd_kin2[0]<<","<<fwd_kin2[1]<<","<<fwd_kin2[2]<<std::endl;

//     return 0;
// }


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2015 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "biharmonic_coordinates.h"
#include "cotmatrix.h"
#include "massmatrix.h"
#include "min_quad_with_fixed.h"
#include "normal_derivative.h"
#include "on_boundary.h"
#include <Eigen/Sparse>

template <
  typename DerivedV,
  typename DerivedT,
  typename SType,
  typename DerivedW>
IGL_INLINE bool igl::biharmonic_coordinates(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedT> & T,
  const std::vector<std::vector<SType> > & S,
  Eigen::PlainObjectBase<DerivedW> & W)
{
  return biharmonic_coordinates(V,T,S,2,W);
}

template <
  typename DerivedV,
  typename DerivedT,
  typename SType,
  typename DerivedW>
IGL_INLINE bool igl::biharmonic_coordinates(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedT> & T,
  const std::vector<std::vector<SType> > & S,
  const int k,
  Eigen::PlainObjectBase<DerivedW> & W)
{
  using namespace Eigen;
  using namespace std;
  // This is not the most efficient way to build A, but follows "Linear
  // Subspace Design for Real-Time Shape Deformation" [Wang et al. 2015].
  SparseMatrix<double> A;
  {
    SparseMatrix<double> N,Z,L,K,M;
    normal_derivative(V,T,N);
    Array<bool,Dynamic,1> I;
    Array<bool,Dynamic,Dynamic> C;
    on_boundary(T,I,C);
    {
      std::vector<Triplet<double> >ZIJV;
      for(int t =0;t<T.rows();t++)
      {
        for(int f =0;f<T.cols();f++)
        {
          if(C(t,f))
          {
            const int i = t+f*T.rows();
            for(int c = 1;c<T.cols();c++)
            {
              ZIJV.emplace_back(T(t,(f+c)%T.cols()),i,1);
            }
          }
        }
      }
      Z.resize(V.rows(),N.rows());
      Z.setFromTriplets(ZIJV.begin(),ZIJV.end());
      N = (Z*N).eval();
    }
    cotmatrix(V,T,L);
    K = N+L;
    massmatrix(V,T,MASSMATRIX_TYPE_DEFAULT,M);
    // normalize
    M /= ((VectorXd)M.diagonal()).array().abs().maxCoeff();
    DiagonalMatrix<double,Dynamic> Minv =
      ((VectorXd)M.diagonal().array().inverse()).asDiagonal();
    switch(k)
    {
      default:
        assert(false && "unsupported");
      case 2:
        // For C1 smoothness in 2D, one should use bi-harmonic
        A = K.transpose() * (Minv * K);
        break;
      case 3:
        // For C1 smoothness in 3D, one should use tri-harmonic
        A = K.transpose() * (Minv * (-L * (Minv * K)));
        break;
    }
  }
  // Vertices in point handles
  const size_t mp =
    count_if(S.begin(),S.end(),[](const vector<int> & h){return h.size()==1;});
  // number of region handles
  const size_t r = S.size()-mp;
  // Vertices in region handles
  size_t mr = 0;
  for(const auto & h : S)
  {
    if(h.size() > 1)
    {
      mr += h.size();
    }
  }
  const size_t dim = T.cols()-1;
  // Might as well be dense... I think...
  MatrixXd J = MatrixXd::Zero(mp+mr,mp+r*(dim+1));
  VectorXi b(mp+mr);
  MatrixXd H(mp+r*(dim+1),dim);
  {
    int v = 0;
    int c = 0;
    for(int h = 0;h<S.size();h++)
    {
      if(S[h].size()==1)
      {
        H.row(c) = V.block(S[h][0],0,1,dim);
        J(v,c++) = 1;
        b(v) = S[h][0];
        v++;
      }else
      {
        assert(S[h].size() >= dim+1);
        for(int p = 0;p<S[h].size();p++)
        {
          for(int d = 0;d<dim;d++)
          {
            J(v,c+d) = V(S[h][p],d);
          }
          J(v,c+dim) = 1;
          b(v) = S[h][p];
          v++;
        }
        H.block(c,0,dim+1,dim).setIdentity();
        c+=dim+1;
      }
    }
  }
  // minimize    ½ W' A W'
  // subject to  W(b,:) = J
  return min_quad_with_fixed(
    A,VectorXd::Zero(A.rows()).eval(),b,J,SparseMatrix<double>(),VectorXd(),true,W);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template bool igl::biharmonic_coordinates<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, int, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


//  ratio_test.cpp  ----------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#include <iostream>
#include <boost/ratio/ratio.hpp>
#include "duration.hpp"

namespace User1
{
// Example type-safe "physics" code interoperating with chrono::duration types
//  and taking advantage of the std::ratio infrastructure and design philosophy.

// length - mimics chrono::duration except restricts representation to double.
//    Uses boost::ratio facilities for length units conversions.

template <class Ratio>
class length
{
public:
    typedef Ratio ratio;
private:
    double len_;
public:

    length() : len_(1) {}
    length(const double& len) : len_(len) {}

    // conversions
    template <class R>
    length(const length<R>& d)
            : len_(d.count() * boost::ratio_divide<Ratio, R>::type::den /
                               boost::ratio_divide<Ratio, R>::type::num) {}

    // observer

    double count() const {return len_;}

    // arithmetic

    length& operator+=(const length& d) {len_ += d.count(); return *this;}
    length& operator-=(const length& d) {len_ -= d.count(); return *this;}

    length operator+() const {return *this;}
    length operator-() const {return length(-len_);}

    length& operator*=(double rhs) {len_ *= rhs; return *this;}
    length& operator/=(double rhs) {len_ /= rhs; return *this;}
};

// Sparse sampling of length units
typedef length<boost::ratio<1> >          meter;        // set meter as "unity"
typedef length<boost::centi>              centimeter;   // 1/100 meter
typedef length<boost::kilo>               kilometer;    // 1000  meters
typedef length<boost::ratio<254, 10000> > inch;         // 254/10000 meters
// length takes ratio instead of two integral types so that definitions can be made like so:
typedef length<boost::ratio_multiply<boost::ratio<12>, inch::ratio>::type>   foot;  // 12 inchs
typedef length<boost::ratio_multiply<boost::ratio<5280>, foot::ratio>::type> mile;  // 5280 feet

// Need a floating point definition of seconds
typedef boost_ex::chrono::duration<double> seconds;                         // unity
// Demo of (scientific) support for sub-nanosecond resolutions
typedef boost_ex::chrono::duration<double,  boost::pico> picosecond;  // 10^-12 seconds
typedef boost_ex::chrono::duration<double, boost::femto> femtosecond; // 10^-15 seconds
typedef boost_ex::chrono::duration<double,  boost::atto> attosecond;  // 10^-18 seconds

// A very brief proof-of-concept for SIUnits-like library
//  Hard-wired to floating point seconds and meters, but accepts other units (shown in testUser1())
template <class R1, class R2>
class quantity
{
    double q_;
public:
    typedef R1 time_dim;
    typedef R2 distance_dim;
    quantity() : q_(1) {}

    double get() const {return q_;}
    void set(double q) {q_ = q;}
};

template <>
class quantity<boost::ratio<1>, boost::ratio<0> >
{
    double q_;
public:
    quantity() : q_(1) {}
    quantity(seconds d) : q_(d.count()) {}  // note:  only User1::seconds needed here

    double get() const {return q_;}
    void set(double q) {q_ = q;}
};

template <>
class quantity<boost::ratio<0>, boost::ratio<1> >
{
    double q_;
public:
    quantity() : q_(1) {}
    quantity(meter d) : q_(d.count()) {}  // note:  only User1::meter needed here

    double get() const {return q_;}
    void set(double q) {q_ = q;}
};

template <>
class quantity<boost::ratio<0>, boost::ratio<0> >
{
    double q_;
public:
    quantity() : q_(1) {}
    quantity(double d) : q_(d) {}

    double get() const {return q_;}
    void set(double q) {q_ = q;}
};

// Example SI-Units
typedef quantity<boost::ratio<0>, boost::ratio<0> >  Scalar;
typedef quantity<boost::ratio<1>, boost::ratio<0> >  Time;         // second
typedef quantity<boost::ratio<0>, boost::ratio<1> >  Distance;     // meter
typedef quantity<boost::ratio<-1>, boost::ratio<1> > Speed;        // meter/second
typedef quantity<boost::ratio<-2>, boost::ratio<1> > Acceleration; // meter/second^2

template <class R1, class R2, class R3, class R4>
quantity<typename boost::ratio_subtract<R1, R3>::type, typename boost::ratio_subtract<R2, R4>::type>
operator/(const quantity<R1, R2>& x, const quantity<R3, R4>& y)
{
    typedef quantity<typename boost::ratio_subtract<R1, R3>::type, typename boost::ratio_subtract<R2, R4>::type> R;
    R r;
    r.set(x.get() / y.get());
    return r;
}

template <class R1, class R2, class R3, class R4>
quantity<typename boost::ratio_add<R1, R3>::type, typename boost::ratio_add<R2, R4>::type>
operator*(const quantity<R1, R2>& x, const quantity<R3, R4>& y)
{
    typedef quantity<typename boost::ratio_add<R1, R3>::type, typename boost::ratio_add<R2, R4>::type> R;
    R r;
    r.set(x.get() * y.get());
    return r;
}

template <class R1, class R2>
quantity<R1, R2>
operator+(const quantity<R1, R2>& x, const quantity<R1, R2>& y)
{
    typedef quantity<R1, R2> R;
    R r;
    r.set(x.get() + y.get());
    return r;
}

template <class R1, class R2>
quantity<R1, R2>
operator-(const quantity<R1, R2>& x, const quantity<R1, R2>& y)
{
    typedef quantity<R1, R2> R;
    R r;
    r.set(x.get() - y.get());
    return r;
}

// Example type-safe physics function
Distance
compute_distance(Speed v0, Time t, Acceleration a)
{
    return v0 * t + Scalar(.5) * a * t * t;  // if a units mistake is made here it won't compile
}

} // User1

// Exercise example type-safe physics function and show interoperation
// of custom time durations (User1::seconds) and standard time durations (std::hours).
// Though input can be arbitrary (but type-safe) units, output is always in SI-units
//   (a limitation of the simplified Units lib demoed here).

int main()
{
    //~ typedef boost::ratio<8, BOOST_INTMAX_C(0x7FFFFFFFD)> R1;
    //~ typedef boost::ratio<3, BOOST_INTMAX_C(0x7FFFFFFFD)> R2;
    typedef User1::quantity<boost::ratio_subtract<boost::ratio<0>, boost::ratio<1> >::type,
                             boost::ratio_subtract<boost::ratio<1>, boost::ratio<0> >::type > RR;
    //~ typedef boost::ratio_subtract<R1, R2>::type RS;
    //~ std::cout << RS::num << '/' << RS::den << '\n';

    std::cout << "*************\n";
    std::cout << "* testUser1 *\n";
    std::cout << "*************\n";
    User1::Distance d(( User1::mile(110) ));
    boost_ex::chrono::hours h((2));
    User1::Time t(( h ));
    //~ boost_ex::chrono::seconds sss=boost_ex::chrono::duration_cast<boost_ex::chrono::seconds>(h);
    //~ User1::seconds sss((120));
    //~ User1::Time t(( sss ));

    //typedef User1::quantity<boost::ratio_subtract<User1::Distance::time_dim, User1::Time::time_dim >::type,
    //                        boost::ratio_subtract<User1::Distance::distance_dim, User1::Time::distance_dim >::type > R;
    RR r=d / t;
    //r.set(d.get() / t.get());

    User1::Speed rc= r;

    User1::Speed s = d / t;
    std::cout << "Speed = " << s.get() << " meters/sec\n";
    User1::Acceleration a = User1::Distance( User1::foot(32.2) ) / User1::Time() / User1::Time();
    std::cout << "Acceleration = " << a.get() << " meters/sec^2\n";
    User1::Distance df = compute_distance(s, User1::Time( User1::seconds(0.5) ), a);
    std::cout << "Distance = " << df.get() << " meters\n";
    std::cout << "There are " << User1::mile::ratio::den << '/' << User1::mile::ratio::num << " miles/meter";
    User1::meter mt = 1;
    User1::mile mi = mt;
    std::cout << " which is approximately " << mi.count() << '\n';
    std::cout << "There are " << User1::mile::ratio::num << '/' << User1::mile::ratio::den << " meters/mile";
    mi = 1;
    mt = mi;
    std::cout << " which is approximately " << mt.count() << '\n';
    User1::attosecond as(1);
    User1::seconds sec = as;
    std::cout << "1 attosecond is " << sec.count() << " seconds\n";
    std::cout << "sec = as;  // compiles\n";
    sec = User1::seconds(1);
    as = sec;
    std::cout << "1 second is " << as.count() << " attoseconds\n";
    std::cout << "as = sec;  // compiles\n";
    std::cout << "\n";
  return 0;
}


/**
 * \file CachedCosinusGeneratorFilter.cpp
 */

#include "CachedCosinusGeneratorFilter.h"

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

#include <cmath>
#include <cstdint>
#include <cstring>

namespace ATK
{
  template<typename DataType_>
  CachedCosinusGeneratorFilter<DataType_>::CachedCosinusGeneratorFilter(int periods, int seconds)
  :Parent(0, 1), periods(periods), seconds(seconds)
  {
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::set_frequency(int periods, int seconds)
  {
    if(periods <= 0)
    {
      throw std::out_of_range("Periods must be strictly positive");
    }
    this->periods = periods;
    this->seconds = seconds;
    setup();
  }

  template<typename DataType_>
  std::pair<int, int> CachedCosinusGeneratorFilter<DataType_>::get_frequency() const
  {
    return std::make_pair(periods, seconds);
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::set_volume(DataType_ volume)
  {
    this->volume = volume;
  }

  template<typename DataType_>
  DataType_ CachedCosinusGeneratorFilter<DataType_>::get_volume() const
  {
    return volume;
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::set_offset(DataType_ offset)
  {
    this->offset = offset;
  }

  template<typename DataType_>
  DataType_ CachedCosinusGeneratorFilter<DataType_>::get_offset() const
  {
    return offset;
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::setup()
  {
    indice = 0;
    cache.resize(output_sampling_rate * seconds);
    for(gsl::index i = 0; i < cache.size(); ++i)
    {
      cache[i] = static_cast<DataType>(std::cos(2 * boost::math::constants::pi<double>() * (i+1) * periods / seconds / output_sampling_rate));
    }
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::process_impl(gsl::index size) const
  {
    DataType* ATK_RESTRICT output = outputs[0];
    gsl::index processed = 0;
    while(processed < size)
    {
      auto to_copy = std::min(size - processed, static_cast<gsl::index>(cache.size()) - indice);
      memcpy(reinterpret_cast<void*>(output + processed), reinterpret_cast<const void*>(cache.data() + indice), to_copy * sizeof(DataType_));
      indice += to_copy;
      processed += to_copy;
      if(indice >= cache.size())
      {
        indice = 0;
      }
    }
    for(gsl::index i = 0; i < size; ++i)
    {
      output[i] = static_cast<DataType>(offset + volume * output[i]);
    }
  }

#if ATK_ENABLE_INSTANTIATION
  template class CachedCosinusGeneratorFilter<float>;
#endif
  template class CachedCosinusGeneratorFilter<double>;
}


//////////////////////////////////////////////////////////////////////////////
// random::poisson::devroye::q::lemma1.hpp                        	        //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_Q_LEMMA1_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_Q_LEMMA1_HPP_ER_2010
#include <string>
#include <boost/format.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace q{
namespace lemma1{

    // Lemma 1, p.199
    // q(y) < upper_bound(y), if y >= -mean
    template<typename T,typename Int,typename IntT>
    T upper_bound(const Int& i_mean,const Int& i_y,const IntT& converter)
    {
        T z = IntT::convert( 0 );
        T y = IntT::convert( i_y );
        T yp1 = IntT::convert( i_y +1 );
        T pos_y = IntT::convert( i_y < 0 ? z : y );
        T num = ( - y * yp1 );
        T den = IntT::convert( 2 * i_mean ) + pos_y;
        return num / den;
    }

    template<typename T,typename Int,typename IntT>
    bool do_raise_error(const T& q,const T& tol,std::string& str,
        const Int& i_mean,const Int& i_y,const IntT& converter)
    {
        T ub1 = lemma1::upper_bound<T>(i_mean,i_y,converter);
        if( q > ( tol + ub1 ) ){
            std::string msg = " [ lemma1 : q = %1% < %2% has failed ]";
            boost::format f( msg ); f % q % ub1;
            str += f.str();
            return true;
        }else{
            return false;
        }
    }

}// lemma1
}// q
}// devroye
}// poisson
}// random
}// boost

#endif


#define DEBUG 1

#include <iostream>
#include <iomanip>
#include <string>
//#include <cmath>
#include "Hubbard2D.hpp"

#ifdef USE_SPECTRA
#include <MatOp/SparseGenMatProd.h>
#include <SymEigsSolver.h>
#endif

#include <boost/random.hpp>
#include <boost/limits.hpp>
#include <ietl/interface/eigen3.h>
#include <ietl/vectorspace.h>
#include <ietl/lanczos.h>

///check if a file exists
bool fileExists(const std::string& fname){
  std::ifstream f(fname.c_str());
  bool isGood = f.good();
  f.close();
  return isGood;
}

int main(int argc, const char * argv[]) {
  int ne; //# of electrons
  double U;

  if(argc == 1){
    std::cerr<<"Using all defaults"<<std::endl;
    ne=2; //# of electrons
    U=4;
  }
  else if(argc >5 || (argc-1)%2){
    std::cout<<"Wrong input format!"<<std::endl;
    std::cout<<"-ne [#num electrons] -U [# for U/t]"<<std::endl;
    return 1;
  }
  else{
    ne=2;
    U=4;
    for(int i=1;i<argc;i+=2){
     if(std::string(argv[i]) == "-ne")
       ne = std::atoi(argv[i+1]);
     if(std::string(argv[i]) == "-U")
       U = std::atof(argv[i+1]);
    }
  }
  std::cout << "2D "<<BASISSIZE/2<<" site Hubbard model with "
  << ne <<" up/down electrons and U/t="<<U<<std::endl;

  HubbardModel2D H(ne,ne,1,U);
  H.makeBasis();
  std::cout <<"Made Basis: " << H.getBasis()->size()<< std::endl;

  H.buildHubbard2D();
  std::cout <<"Built Matrix" << std::endl;
  SpMat Hmat = *(H.getH());
  std::cout << "# of non-zero elements:"<<(H.getH())->nonZeros() << std::endl;
  H.getH()->makeCompressed();

  std::ofstream outfile;
  if(fileExists("energies.dat")){
    outfile.open("energies.dat",std::ofstream::out |std::ofstream::app);
  }
  else{
     outfile.open("energies.dat",std::ofstream::out |std::ofstream::app);
     outfile<<"#ne U E0"<<std::endl;
  }
  outfile << std::setprecision(9);

  std::ofstream outfile_v;
  if(fileExists("ketvals.dat")){
    outfile_v.open("ketvals.dat",std::ofstream::out |std::ofstream::app);
  }
  else{
    outfile_v.open("ketvals.dat",std::ofstream::out |std::ofstream::app);
    outfile_v <<"#ne U dbl up|down antiferro"<<std::endl;
  }
  outfile_v << std::setprecision(9);

  //construct test states
  size_t spinOffset = BASISSIZE/2;

  lattice_t psi1;
  int i;
  for(i=0;i<ne;i++)
    psi1[i].flip();

  for(i=0;i<ne;i++)
    psi1[(i+spinOffset)%(BASISSIZE)].flip();

  lattice_t psi2;
  i=0;

  for(i=0;i<ne;i++)
    psi2[i].flip();

  for(i=0;i<ne;i++)
    psi2[(i+ne+spinOffset)%(BASISSIZE)].flip();

  lattice_t psi3;
  i=0;
  for(i=0;i<ne;i++)
    psi3[2*i].flip();
  for(i=0;i<ne;i++)
    psi3[(2*i+1+spinOffset)%(BASISSIZE)].flip();
  //std::cout<<psi1<<std::endl;
  //std::cout<<psi2<<std::endl;
  //std::cout<<psi3<<std::endl;

  std::string double_occ="|";
  for(i=0;i<ne;i++)
    double_occ+="↑↓|";
  std::string not_occ="|";
  for(i=0;i<ne;i++)
    not_occ+="↑|";
  for(i=0;i<ne;i++)
    not_occ+="↓|";
  std::string antiferro="|";
  for(i=0;i<ne;i++)
    antiferro+="↑|↓|";

  //**************************************
  //        generate eigenvalues
  //**************************************

  //Diag with specrta
#ifdef USE_SPECTRA
  // Construct matrix operation object using the wrapper class SparseGenMatProd
  using wrapper_t =Spectra::SparseGenMatProd<double,Eigen::RowMajor,long>;
  wrapper_t op(Hmat);

  // Construct eigen solver object, requesting the smallest eigenvalue
  Spectra::SymEigsSolver< double, Spectra::SMALLEST_ALGE , wrapper_t > eigs(&op, 1, 6);
  // Initialize and compute
  eigs.init();
  int nconv = eigs.compute();

#ifdef DEBUG
  if(eigs.info() != Spectra::SUCCESSFUL)
      std::cout <<"Warning something failed " <<nconv << std::endl;
#endif

  std::cout<< "E0="<<eigs.eigenvalues()[0]<<std::endl;

  std::cout<<double_occ<<" :"<<eigs.eigenvectors()(H.getState(psi1),0)<<std::endl; //psi1
  std::cout<<not_occ<<" :"<<eigs.eigenvectors()(H.getState(psi2),0)<<std::endl; //psi2
#else
  typedef boost::lagged_fibonacci607 Gen;
  Gen mygen;
  mygen.seed(0);

  typedef Eigen::SparseMatrix<double,Eigen::RowMajor,long> Matrix;
  typedef Eigen::VectorXd Vector;
  typedef ietl::vectorspace<Vector> Vecspace;

  // Creation of an iteration object:
  int max_iter = 10*H.getBasis()->size();
  double rel_tol = 500*std::numeric_limits<double>::epsilon();
  double abs_tol = std::pow(std::numeric_limits<double>::epsilon(),2./3);
  int n_lowest_eigenval = 1;
  std::vector<double> eigen;
  std::vector<double> err;
  std::vector<int> multiplicity;

  std::vector<double> groundStates;

  Vecspace vec(Hmat.cols());
  ietl::lanczos<Matrix,Vecspace> lanczos(Hmat,vec);
  ietl::lanczos_iteration_nlowest<double>
  iter(max_iter, n_lowest_eigenval, rel_tol, abs_tol);
  std::cout << "Running lanczos" << std::endl;
  try{
    lanczos.calculate_eigenvalues(iter,mygen);
    eigen = lanczos.eigenvalues();
    err = lanczos.errors();
    multiplicity = lanczos.multiplicities();
    std::cout<<"number of iterations: "<<iter.iterations()<<"\n";
    groundStates.push_back(eigen[0]);
  }
  catch (std::runtime_error& e) {
    std::cout << e.what() << "\n";
  }
  std::cout << "#        eigenvalue            error         multiplicity\n";
  for (int i=0;i<10;++i)
    std::cout << i+1 << "\t" << eigen[i] << "\t" << err[i] << "\t"
    << multiplicity[i] << "\n";

  outfile<<ne<<" " <<U<< " "<< groundStates[0]<<std::endl;
  //**************************************
  //        generate eigenvectors
  //**************************************
  // call of eigenvectors function follows:
  std::cout << "\nHead of Eigenvector for the lowest eigenvalue:\n\n";
  std::vector<double>::iterator start = eigen.begin();
  std::vector<double>::iterator end = eigen.begin()+1;
  std::vector<Vector> eigenvectors; // for storing the eigen vectors.
  ietl::Info<double> info; // (m1, m2, ma, eigenvalue, residualm, status).

  try {
    lanczos.eigenvectors(start,end,std::back_inserter(eigenvectors),info,mygen,max_iter);
  }
  catch (std::runtime_error& e) {
    std::cout << e.what() << "\n";
  }
  std::cout << eigenvectors[0].head(10)<<std::endl;
  std::cout << "Information about the eigenvector computations:\n\n";
  for(int i = 0; i < info.size(); i++) {
    std::cout << " m1(" << i+1 << "): " << info.m1(i) << ", m2(" << i+1 << "): "
    << info.m2(i) << ", ma(" << i+1 << "): " << info.ma(i) << " eigenvalue("
    << i+1 << "): " << info.eigenvalue(i) << " residual(" << i+1 << "): "
    << info.residual(i) << " error_info(" << i+1 << "): "
    << info.error_info(i) << "\n\n";
  }

  std::cout<<double_occ<<" :"<<eigenvectors[0](H.getState(psi1))<<std::endl; //psi1
  std::cout<<not_occ<<" :"<<eigenvectors[0](H.getState(psi2))<<std::endl; //psi2
  std::cout<<antiferro<<" :"<<eigenvectors[0](H.getState(psi3))<<std::endl; //psi3

  outfile_v << ne << " "<<U<<" " << eigenvectors[0](H.getState(psi1)) << " "
            <<eigenvectors[0](H.getState(psi2))<<" " <<eigenvectors[0](H.getState(psi3))<<std::endl;
#endif

  return 0;
}


#include "fast_particle_slam.h"
#include "robot_configuration.h"
#include "error_handling.h"
#include "occupancy_grid.inl"

#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/adaptor/transformed.hpp>

#include <opencv2/imgproc.hpp>
#include <iostream>
#include <random>
#include <future>

// Based on Grisetti, Stachniss, Burgard
// "Improving Grid-based SLAM with Rao-Blackwellized Particle Filters by Adaptive Proposals and Selective Resampling"
// and their implementation at https://openslam.org/gmapping.html
SFastSlamParticle::SFastSlamParticle() : m_pose(rbt::pose<double>::zero()) {}

void SFastSlamParticle::updatePose(SScanLine const& scanline) {
    // 1. Update particles with probabilistic motion model
    auto poseSampled = sample_motion_model(m_pose, scanline.translation(), scanline.rotation());

    // 2. If not first update (and optionally: enough distance traveled since last update)
    //    scan match and update particle pose
    m_pose = m_occgrid.fit(poseSampled, scanline);

    // 3. Compute likelihood of resulting match
    // gmapping computes log likelihood, also skips distanceTransform and searches
    // in small kernel around expected obstacle
    m_fLogWeight += log_likelihood_field(m_pose, scanline, m_occgrid);

    LOG("Update Particle: poseSampled = " << poseSampled << " m_pose = " << m_pose << " m_fLogWeight = " << m_fLogWeight << "\n");
}

void SFastSlamParticle::updateMap(SScanLine const& scanline) {
    boost::for_each(scanline.m_vecscan, [&](auto const& scan) {
        m_occgrid.update(m_pose, scan.m_fRadAngle, scan.m_nDistance);
    });
}

CFastParticleSlamBase::CFastParticleSlamBase(int cParticles)
    : m_vecparticle(cParticles), m_itparticleBest(m_vecparticle.begin()), m_fNEff(1.0)
{}

static std::random_device s_rd;
void CFastParticleSlamBase::receivedSensorData(SScanLine const& scanline) {
     LOG("=== Update === ");
     LOG("t = " << scanline.translation() << " phi = " << scanline.rotation());

    {
        std::vector<std::future<void>> vecfuture;
        boost::for_each(m_vecparticle, [&](auto& p) {
            vecfuture.emplace_back(
                std::async(std::launch::async,
                    [&] {
                        p.updatePose(scanline);
                    }
                ));
        });
    }

    // 4. Normalize weights (see GridSlamProcessor::normalize())
    {
        // TODO: m_obsSigmaGain
        double const fGain = 1. / ( 3 * /* = m_obsSigmaGain */ m_vecparticle.size());
        auto const fMax = *boost::max_element(boost::adaptors::transform(m_vecparticle, std::mem_fn(&SFastSlamParticle::m_fLogWeight)));

        double fWeightSum = 0;
        boost::for_each(m_vecparticle, [&](auto& p) {
            p.m_fWeight = std::exp(fGain * (p.m_fLogWeight - fMax));
            fWeightSum += p.m_fWeight;
        });

        m_fNEff = 0;
        boost::for_each(m_vecparticle, [&](auto& p) {
            p.m_fWeight /= fWeightSum;
            m_fNEff += p.m_fWeight*p.m_fWeight;
        });
        m_fNEff = 1.0 / m_fNEff;
    }

    // 5. If neff < threshold, resample
    if(m_fNEff<0.5 * m_vecparticle.size()) {
        LOG("============ Resample ============");
        // Resampling
        // Thrun, Probabilistic robotics, p. 110
        auto const fStepSize = 1.0/m_vecparticle.size();
        auto const r = std::uniform_real_distribution<double>(0.0, fStepSize)(s_rd);
        auto c = m_vecparticle.front().m_fWeight;

        std::vector<int> veciparticle;
        for(int i = 0, m = 0; m<m_vecparticle.size(); ++m) {
            auto const u = r + m * fStepSize;
            while(c<u) {
                ++i;
                c += m_vecparticle[i].m_fWeight;
            }
            veciparticle.emplace_back(i);
            LOG("Keep particle " << i);
        }

        std::vector<SFastSlamParticle> vecparticle(m_vecparticle.size());
        auto itparticleOut = vecparticle.begin();
        for(auto itn = veciparticle.begin(); itn!=veciparticle.end(); ++itn) {
            if(boost::next(itn)==veciparticle.end() || *itn!=*boost::next(itn)) {
                *itparticleOut = std::move(m_vecparticle[*itn]);
            } else {
                *itparticleOut = m_vecparticle[*itn];
            }
            itparticleOut->m_fLogWeight = 0.0;
            ++itparticleOut;
        }
        std::swap(m_vecparticle, vecparticle);
    }

    m_itparticleBest = boost::max_element(
        boost::adaptors::transform(m_vecparticle, std::mem_fn(&SFastSlamParticle::m_fWeight))
    ).base();
    m_vecpose.emplace_back(m_itparticleBest->m_pose);

    {
        std::vector<std::future<void>> vecfuture;
        boost::for_each(m_vecparticle, [&](auto& p) {
            vecfuture.emplace_back(
                std::async(std::launch::async,
                    [&] {
                        p.updateMap(scanline);
                    }
                ));
        });
    }
}

cv::Mat CFastParticleSlamBase::getMapWithPoses() const {
    ASSERT(m_itparticleBest!=m_vecparticle.end());
    return m_itparticleBest->m_occgrid.ObstacleMapWithPoses(m_vecpose);
}

cv::Mat CFastParticleSlamBase::getMap() const {
    ASSERT(m_itparticleBest!=m_vecparticle.end());
    return m_itparticleBest->m_occgrid.ObstacleMap();
}

cv::Mat CFastParticleSlamBase::getMapWithPose() const {
    ASSERT(m_itparticleBest!=m_vecparticle.end());
    cv::Mat mat = m_itparticleBest->m_occgrid.ObstacleMap();
    cv::Mat matColor;
    cvtColor(mat, matColor, CV_GRAY2RGB);
    RenderRobotPose(matColor, m_vecpose.back(), cv::Scalar(255, 0, 0));
    return matColor;
}


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

// Written by Cornelius Steinhardt

#ifndef ITL_QMR_INCLUDE
#define ITL_QMR_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/numeric/itl/krylov/base_solver.hpp>

namespace itl {

/// Quasi-Minimal Residual method
template < typename Matrix, typename Vector,typename LeftPreconditioner,
	   typename RightPreconditioner, typename Iteration >
int qmr(const Matrix& A, Vector& x, const Vector& b, LeftPreconditioner& L,
	const RightPreconditioner& R, Iteration& iter)
{
    mtl::vampir_trace<7008> tracer;
    using mtl::size;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");

    const Scalar         zero= math::zero(Scalar()), one= math::one(Scalar());
    Scalar               beta, gamma(one), gamma_1, delta, eta(-one), ep(one), rho_1, theta(zero), theta_1;
    Vector               r(b - A * x), v_tld(r), y(solve(L, v_tld)), w_tld(r), z(adjoint_solve(R,w_tld)), v(resource(x)), w(resource(x)),
                         y_tld(resource(x)), z_tld(resource(x)), p(resource(x)), q(resource(x)), p_tld(resource(x)), d(resource(x)), s(resource(x));

    if (iter.finished(r))
	return iter;

    Scalar rho = two_norm(y), xi = two_norm(z);
    while(! iter.finished(rho)) {
	++iter;
        if (rho == zero)
	    return iter.fail(1, "qmr breakdown #1, rho=0");
        if (xi == zero)
            return iter.fail(2, "qmr breakdown #2, xi=0");

        v= v_tld / rho;
        y/= rho;
        w= w_tld / xi;
        z/= xi;

        delta = dot(z,y);
        if (delta == zero)
            return iter.fail(3, "qmr breakdown, delta=0 #3");

        y_tld = solve(R,y);
        z_tld = adjoint_solve(L,z);

	if (iter.first()) {
            p = y_tld;
            q = z_tld;
	} else {
            p = y_tld - ((xi * delta) / ep) * p;
            q = z_tld - ((rho* delta) / ep) * q;
        }

        p_tld = A * p;
        ep = dot(q, p_tld);
        if (ep == zero)
            return iter.fail(4, "qmr breakdown ep=0 #4");
        beta= ep / delta;
        if (beta == zero)
            return iter.fail(5, "qmr breakdown beta=0 #5");
        v_tld = p_tld - beta * v;
        y = solve(L,v_tld);
        rho_1 = rho;
	rho = two_norm(y);
        w_tld= trans(A)*q  - beta*w;
        z = adjoint_solve(R, w_tld);
        xi = two_norm(z);
        gamma_1 = gamma;
        theta_1 = theta;
        theta = rho / (gamma_1 * beta);
        gamma = one / (sqrt(one + theta * theta));

        if (gamma == zero)
            return iter.fail(6, "qmr breakdown gamma=0 #6");

        eta= -eta * rho_1 * gamma * gamma / (beta * gamma_1 * gamma_1);
	if (iter.first()) {
           d= eta * p;
	   s= eta * p_tld;
	} else {
            d= eta * p + (theta_1 * theta_1 * gamma * gamma) * d;
            s= eta * p_tld + (theta_1 * theta_1 * gamma * gamma) * s;
        }
        x += d;
        r -= s;
    }
    return iter;
}

/// Solver class for Quasi-minimal residual method; right preconditioner ignored (prints warning if not identity)
/** Methods inherited from \ref base_solver. **/
template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator>,
	   typename RightPreconditioner= pc::identity<LinearOperator> >
class qmr_solver
  : public base_solver< qmr_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator >
{
    typedef base_solver< qmr_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator > base;
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit qmr_solver(const LinearOperator& A) : base(A), L(A), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    qmr_solver(const LinearOperator& A, const Preconditioner& L) : base(A), L(L), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    qmr_solver(const LinearOperator& A, const Preconditioner& L, const RightPreconditioner& R)
      : base(A), L(L), R(R) {}

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceX, typename HilbertSpaceB, typename Iteration >
    int solve(HilbertSpaceX& x, const HilbertSpaceB& b, Iteration& iter) const
    {
	return qmr(this->A, x, b, L, R, iter);
    }

  private:
    Preconditioner        L;
    RightPreconditioner   R;
};

} // namespace itl

#endif // ITL_QMR_INCLUDE


#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>

#include "simulator.hpp"
#include "spdlog/spdlog.h"

using namespace SnowSimulator;

Simulator::Simulator(MaterialPoints &materialPoints, Grid *grid,
                     SnowModel &snowModel,
                     std::vector<CollisionObject *> colliders)
    : m_materialPoints(materialPoints), m_grid(grid), m_snowModel(snowModel),
      m_colliders(colliders) {
  m_stepCount = 0;
  logger = spdlog::get("snowsim");
}

/**
 * The simulation proceeds as follows:
 *
 * 1. Rasterize particle data to the grid (rasterizeParticlesToGrid)
 * 2. Compute particle volumes and densities (setParticleVolumesAndDensities)
 *    NOTE: This is done only on the first timestep!
 * 3. Compute grid forces (computeGridForces)
 * 4. Update velocities on grid to v_i' (updateGridVelocities)
 * 5. Grid-based body collisions (detectGridCollisions)
 * 6. Solve linear system for semi-implicit integration (solveLinearSystem)
 * 7. Update deformation gradient (updateDeformationGradient)
 * 8. Update particle velocities (updateParticleVelocities)
 * 9. Particle-based body collisions (detectParticleCollisions)
 * 10. Update particle positions (updateParticlePostions)
 **/
void Simulator::advance(double delta_t) {
  logger->info("[Beginning step {} -> advancing by {}]", stepCount(), delta_t);

  rasterizeParticlesToGrid();

  // On the first timestep, we need to compute particle volumes and densities

  if (m_stepCount == 0) {
    setParticleVolumesAndDensities();
  }

  computeGridForces();
  updateGridVelocities(delta_t);

  detectGridCollisions(delta_t);

  explicitIntegration();
  // solveLinearSystem();

  updateDeformationGradient(delta_t);
  updateParticleVelocities(delta_t);
  detectParticleCollisions(delta_t);
  updateParticlePositions(delta_t);

  m_stepCount++;
}

void Simulator::rasterizeParticlesToGrid() {
  // Reset all node properties

  for (auto &node : m_grid->nodes()) {
    node->m_mass = 0;
    node->m_velocity.setZero();
    node->m_nextVelocity.setZero();
    node->m_velocityStar.setZero();
    node->m_force.setZero();
  }

  // Place all particles in their updated cells

  // TODO(kvchen): Comment this out once we fix this bug

  // for (auto &mp : m_materialPoints.particles()) {
  //   Vector3f idx = (mp->m_position.array() / m_grid->m_spacing).floor();
  //   int i = m_grid->vectorToIdx(idx.cast<int>());
  //
  //   if (i < 0 || i >= m_grid->m_gridCells.size()) {
  //     logger->error("Particle index {} exceeds grid boundaries", i);
  //     std::cout << *mp << std::endl;
  //
  //     logger->error("Other particles in the same cell as invalid particle:");
  //     for (auto &other : mp->cell()->particles()) {
  //       if (other == mp) {
  //         continue;
  //       }
  //       logger->error("Distance to invalid particle: {}",
  //                     (other->position() - mp->position()).norm());
  //       std::cout << *other << std::endl;
  //     }
  //   }
  // }

  for (auto &cell : m_grid->cells()) {
    cell->clear();
  }

  for (auto &mp : m_materialPoints.particles()) {
    int idx = m_grid->getParticleIdx(mp);
    if (idx < 0 || idx >= m_grid->m_gridCells.size()) {
      logger->error("Particle index {} exceeds grid boundaries", idx);
      std::cout << *mp << std::endl;
    }

    m_grid->cells()[idx]->addMaterialPoint(mp);
  }

  for (auto &mp : m_materialPoints.particles()) {
    m_grid->forEachNeighbor(mp, [&mp](GridNode *node) {
      double w = node->basisFunction(mp->position());
      node->m_mass += mp->mass() * w;
      node->m_velocity += mp->velocity() * mp->mass() * w;
    });
  }

  for (auto &node : m_grid->nodes()) {
    if (node->m_mass > 0) {
      node->m_velocity /= node->m_mass;
    }
  }
}

void Simulator::setParticleVolumesAndDensities() {
  logger->info("Setting initial particle volumes and densities");

  for (auto const &mp : m_materialPoints.particles()) {
    mp->m_volume = 0;
    mp->m_density = 0;

    m_grid->forEachNeighbor(mp, [&](GridNode *node) {
      mp->m_density += node->m_mass * node->basisFunction(mp->m_position) /
                       pow(m_grid->m_spacing, 3);
    });

    if (mp->m_density != 0) {
      mp->m_volume = mp->m_mass / mp->m_density;
    }
  }
}

void Simulator::computeGridForces() {
  for (auto &mp : m_materialPoints.particles()) {
    JacobiSVD<Matrix3f> svd(mp->m_defElastic, ComputeFullU | ComputeFullV);

    double Jp = mp->m_defPlastic.determinant();
    double Je = mp->m_defElastic.determinant();

    Matrix3f Re = svd.matrixU() * svd.matrixV().transpose();

    double epsilon =
        exp(fmin(m_snowModel.hardeningCoefficient * (1 - Jp), 1e3));

    // Compute the Cauchy stress
    // mu * 2 * (fe - re)_f

    Matrix3f stress =
        epsilon *
        (2 * m_snowModel.initialMu * (mp->m_defElastic - Re) *
             mp->m_defElastic.transpose() +
         Matrix3f::Identity() * (m_snowModel.initialLambda * (Je - 1) * Je));

    m_grid->forEachNeighbor(mp, [mp, &stress](GridNode *node) {
      node->m_force -=
          mp->m_volume * stress * node->gradBasisFunction(mp->m_position);
    });
  }
}

void Simulator::updateGridVelocities(double delta_t) {
  for (auto &node : m_grid->getAllNodes()) {
    if (node->m_mass > 0) {
      node->m_nextVelocity =
          node->m_velocity + delta_t * node->m_force / node->m_mass;
    }

    // Add gravitational forces

    node->m_nextVelocity += Vector3f(0, -9.8, 0) * delta_t;
  }
}

void Simulator::detectGridCollisions(double delta_t) {
  for (auto &node : m_grid->getAllNodes()) {
    node->m_velocityStar = node->m_nextVelocity;

    for (auto &collider : m_colliders) {
      Vector3f position = node->position();
      if (collider->phi(position) > 0) {
        continue;
      }

      Vector3f normal = collider->normal(position);
      Vector3f relVelocity = node->m_velocityStar - collider->velocity();
      double magnitude = relVelocity.dot(normal);

      if (magnitude >= 0) {
        continue;
      }

      Vector3f tangentialVelocity = relVelocity - magnitude * normal;

      // Only apply dynamic friction if the tangential velocity is large
      // compared to the normal

      double normComponent = collider->friction() * magnitude;
      double tangentNorm = tangentialVelocity.norm();

      if (collider->sticky() || tangentNorm <= -normComponent) {
        relVelocity.setZero();
      } else {
        relVelocity = tangentialVelocity * (1 + normComponent / tangentNorm);
      }

      node->m_velocityStar = relVelocity + collider->velocity();
    }
  }
}

void Simulator::explicitIntegration() {
  for (auto &node : m_grid->getAllNodes()) {
    node->m_nextVelocity = node->m_velocityStar;
    // std::cout << node->m_nextVelocity << std::endl;
  }
}

void Simulator::updateDeformationGradient(double delta_t) {
  for (auto &mp : m_materialPoints.particles()) {
    Matrix3f gradVelocity = Matrix3f::Identity();
    m_grid->forEachNeighbor(mp, [mp, delta_t, &gradVelocity](GridNode *node) {
      gradVelocity += delta_t * node->nextVelocity() *
                      node->gradBasisFunction(mp->m_position).transpose();
    });

    Matrix3f defNext = gradVelocity * mp->m_defElastic * mp->m_defPlastic;
    mp->m_defElastic = gradVelocity * mp->m_defElastic;

    // Push deformations exceeding critical deformation thresholds into Fp

    JacobiSVD<Matrix3f> svd(mp->m_defElastic, ComputeFullU | ComputeFullV);
    Matrix3f sigma = svd.singularValues()
                         .cwiseMax(1 - m_snowModel.criticalCompression)
                         .cwiseMin(1 + m_snowModel.criticalStretch)
                         .asDiagonal();

    mp->m_defPlastic =
        svd.matrixV() * sigma.inverse() * svd.matrixU().transpose() * defNext;
    mp->m_defElastic = svd.matrixU() * sigma * svd.matrixV().transpose();
    if (!defNext.isApprox(mp->m_defElastic * mp->m_defPlastic)) {
      logger->warn("Deformation update is incorrect");
    }
  }
}

void Simulator::updateParticleVelocities(double delta_t, float alpha) {
  for (auto &mp : m_materialPoints.particles()) {
    Vector3f velocityPIC = Vector3f::Zero();
    Vector3f velocityFLIP = mp->m_velocity;

    m_grid->forEachNeighbor(
        mp, [mp, &velocityPIC, &velocityFLIP](GridNode *node) {
          float weight = node->basisFunction(mp->m_position);

          velocityPIC += node->m_nextVelocity * weight;
          velocityFLIP += (node->m_nextVelocity - node->m_velocity) * weight;
        });

    mp->m_velocity = (1 - alpha) * velocityPIC + alpha * velocityFLIP;
  }
}

void Simulator::detectParticleCollisions(double delta_t) {
  for (auto &mp : m_materialPoints.particles()) {
    for (auto &collider : m_colliders) {
      Vector3f position = mp->m_position;
      if (collider->phi(position) > 0) {
        continue;
      }

      // std::cout << mp->position() << std::endl;

      Vector3f normal = collider->normal(position);
      Vector3f relVelocity = mp->velocity() - collider->velocity();

      // std::cout << collider->velocity() << std::endl;

      double magnitude = relVelocity.dot(normal);
      if (magnitude >= 0) {
        continue;
      }

      // std::cout << "GOT HERE" << std::endl;

      Vector3f tangentialVelocity = relVelocity - magnitude * normal;

      // Only apply dynamic friction if the tangential velocity is large
      // compared to the normal

      double normComponent = collider->friction() * magnitude;
      double tangentNorm = tangentialVelocity.norm();

      if (collider->sticky() || tangentNorm <= -normComponent) {
        relVelocity.setZero();
      } else {
        relVelocity = tangentialVelocity * (1 + normComponent / tangentNorm);
      }

      mp->m_velocity = relVelocity + collider->velocity();
    }
  }
}

/**
 * Uses backwards Euler integration to update the particle position for each
 * delta_t. At this point, the velocities should already have been updated
 * to the next delta_t.
 */
void Simulator::updateParticlePositions(double delta_t) {
  for (auto &mp : m_materialPoints.particles()) {
    mp->m_position += delta_t * mp->m_velocity;

    // mp->m_position = mp->m_position.array().min(
    //     (m_grid->m_dim.array()).cast<float>() * m_grid->m_spacing);
    // mp->m_position = mp->m_position.cwiseMax(0);
  }
}

std::string Simulator::exportVolumeData() {
  std::stringstream ss;
  ss << "../volumes/density-" << std::setfill('0') << std::setw(6)
     << m_stepCount << ".vol";

  std::ofstream os(ss.str());

  Vector3i dim = m_grid->dim();
  Vector3f bboxMin = m_grid->origin();
  Vector3f bboxMax = bboxMin + m_grid->extent();

  bboxMin = Vector3f(-0.5, -0.5, -0.5);
  bboxMax = Vector3f(0.5, 0.5, 0.5);

  os.write("VOL", 3);

  char version = 3;
  os.write((char *)&version, sizeof(char));

  int value = 1;
  os.write((char *)&value, sizeof(int));

  os.write((char *)&dim.x(), sizeof(int));
  os.write((char *)&dim.y(), sizeof(int));
  os.write((char *)&dim.z(), sizeof(int));

  value = 1;
  os.write((char *)&value, sizeof(int));

  os.write((char *)&bboxMin.x(), sizeof(float));
  os.write((char *)&bboxMin.y(), sizeof(float));
  os.write((char *)&bboxMin.z(), sizeof(float));

  os.write((char *)&bboxMax.x(), sizeof(float));
  os.write((char *)&bboxMax.y(), sizeof(float));
  os.write((char *)&bboxMax.z(), sizeof(float));

  auto nodes = m_grid->nodes();
  for (unsigned i = nodes.size(); i-- > 0;) {
    auto node = nodes[i];
    float value = (float)node->mass();
    os.write((char *)&value, sizeof(float));
  }

  return ss.str();
}


#include "GridFilter.hh"
#include <Eigen/Core>
#include <fstream>
#include <cmath>
#include <string>
using namespace std;
using namespace Eigen;

//#define DEBUG_GRIDFILTER

void GridFilter::ComputeCrossSectionOriginal(double value) {
    auto lambda = [&value, this](double x){ return std::abs(x - value) <= m_AllowableError ? 1.0 : 0.0; };
    m_CrossSectionModified = Eigen::MatrixXd::Zero(m_PMrows, m_PMcols);
    #ifdef DEBUG_GRIDFILTER
    std::cout << "I am computed!!!" << std::endl;
    #endif
    m_CrossSecOriginal = m_ParaboloidMatrix.unaryExpr(lambda);
}

void GridFilter::ComputeGradient(double xStep, double yStep) {
    /**
     *
     * Computes dxPM and dyPM (components of gradient)
     * Gradient matrixes are the folowing
     *
     *	- dx - size [NxM-1]
     *	- dy - size [N-1xM]
     *
     * as gradient is computed with the neighbour elements in matrix
     *
     */
    m_dxPM = (m_ParaboloidMatrix.rightCols(m_PMcols - 1) - m_ParaboloidMatrix.leftCols(m_PMcols - 1)) / xStep;
    m_dyPM = (m_ParaboloidMatrix.bottomRows(m_PMrows - 1) - m_ParaboloidMatrix.topRows(m_PMrows - 1)) / yStep;
}

int GridFilter::ComputeCurrentDeviation() {
    /**
     *
     * Algorithm:
     *	- Compute non-zero elements in original cross-section
     *	- For each element in matrixes compute respectivetly:
     *	  dx[i, j]*CrossSectionOriginal[i, j] and dy[i, j]*CrossSectionOriginal[i, j]
     *	  (this will leave only contour's gradient points)
     *	- Find the sqrt of sum of squares (to fing the length of gradient vector)
     *	- Sum all this values and divide by the number of non-zero values to find the avarage value of contour's gradient
     *	- Product with multiplier GradientInfluence, ceil and product with InitialDeviation
     *
     * \return Deviation from the original coutour - the number of points that will be included in extended contour.
     *
     */

    int numOfNonZero = (m_CrossSecOriginal.array() != 0).count();
    if (numOfNonZero == 0) {
        std::cerr << "No contour found on this level. If you are sure that it shold be here, try the following:" << std::endl
            << "- make grid step smaller " << std::endl
            << "- make tolerance higher" << std::endl;
        return 0;
    }
    #ifdef DEBUG_GRIDFILTER
    std::cout << "numOfNonZero = "  << numOfNonZero << std::endl;
    #endif
    ComputeAbsGradMatrix();
    double  tmp =  m_AbsGrad.sum() / numOfNonZero;
    #ifdef DEBUG_GRIDFILTER
    std::cout << "grad_len = "  << tmp << std::endl;
    #endif
    return std::ceil(tmp) * m_InitialDeviation;
}

void GridFilter::ComputeAbsGradMatrix() {
    int rowsnum = m_PMrows - 1, colsnum = m_PMcols - 1;
    m_AbsGrad = ((m_dxPM.block(0, 0, rowsnum, colsnum).array() *
                     m_CrossSecOriginal.block(0, 0, rowsnum, colsnum).array()).square() +
                    (m_dyPM.block(0, 0, rowsnum, colsnum).array() *
                     m_CrossSecOriginal.block(0, 0, rowsnum, colsnum).array()).square())
                    .sqrt() * m_GradientInfluence ;

}

void GridFilter::GetCrossSectionOriginal(Eigen::MatrixXd& CSOmatTarget, double value, bool isCScomputed ) {
    /**
     *
     * Returns cross-section z = value of ParaboloidMatrix: the plain contains contour
     * \param[in] CSOmatTarget The matrix where result will be written
     * \param[in] value The value for compute cross-section plane z = value
     * \param[in] isCScomputed It is false if CrossSecOriginal is not computed yet. It is necessary to avoid computing twice and ensure that it is not rubbish in this matrix
     *
     */
    if (! isCScomputed)  ComputeCrossSectionOriginal(value);
    CSOmatTarget = m_CrossSecOriginal;
}

void GridFilter::GetCrossSectionExtended(MatrixXd & CSEmatTarget,
                                         double value, int deviation, bool isCScomputed, bool isDevConst) {
    /**
     *
     * Returns cross-section plane z = value of ParaboloidMatrix with the extended contour
     * \param[in] CSEmatTarget The matrix where result will be written
     * \param[in] value The value for compute cross-section plane z = value
     * \param[in] deviation Deviation of the original contour
     * \param[in] isCScomputed It is false if CrossSecOriginal is not computed yet. It is necessary to avoid computing twice and ensure that it is not rubbish in this matrix
     *
     */

    Eigen::MatrixXd tmpMat;
    GetCrossSectionOriginal(tmpMat, value, isCScomputed);
    #ifdef DEBUG_GRIDFILTER
    std::cout << " deviation = " << deviation << std::endl;
    #endif
    if (deviation != 0) addPoints(deviation, isDevConst);
    else {
        CSEmatTarget = MatrixXd::Zero(m_PMrows, m_PMcols);
    }
    GetModifiedCrossSection(CSEmatTarget);
}

void GridFilter::GetCrossSectionExtendedAutoDev(Eigen::MatrixXd& CSEADmatTarget, double value) {
    /**
     *
     * Returns cross-section z = value of ParaboloidMatrix (the plain contains contour) using value only. The deviation is computed automaticly and depends on gradient at contour points.
     * \param[in] CSEADmatTarget The matrix where result will be written
     * \param[in] value The value for compute cross-section plane z = value
     *
     */
    ComputeCrossSectionOriginal(value);
    GetCrossSectionExtended(CSEADmatTarget, value, ComputeCurrentDeviation(), true);
}

void GridFilter::GetCrossSectionExtendedIrregular(Eigen::MatrixXd& CSEImatTarget, double value) {
     /**
     *
     * Returns cross-section z = value of ParaboloidMatrix (the plain contains contour) using value only. The deviation is computed automaticly,  depends on gradient at contour points and not constant for the level.
     * \param[in] CSEImatTarget The matrix where result will be written
     * \param[in] value The value for compute cross-section plane z = value
     *
     */
    ComputeCrossSectionOriginal(value);
    ComputeAbsGradMatrix();
    GetCrossSectionExtended(CSEImatTarget, value, 1, true, false);
}

void GridFilter::makeCorridor(int curr_x, int curr_y, int deviation) {
    /**
     * Adds neighbour points to the InterestingPoints matrix for point (curr_x, curr_y)
     * \param[in] curr_x x-coordinate of the considered point
     * \param[in] curr_y y-coordinate of the considered point
     * \param[in] deviation Deviation from the original contour
     */
    for (int i = curr_x - deviation; i < curr_x + deviation + 1; i++) {
        if (i < 0 || i >=  m_PMrows) continue;
        for (int j = curr_y - deviation; j < curr_y + deviation + 1; j++) {
            if (j <  0 || j >= m_PMcols) continue;
            if (m_CrossSecOriginal(i, j) == 1.0) continue;
            m_InterestingPoints.conservativeResize(2, m_InterestingPoints.cols() + 1);
            m_InterestingPoints(0, m_InterestingPoints.cols() - 1) = i;
            m_InterestingPoints(1, m_InterestingPoints.cols() - 1) = j;
            m_CrossSectionModified(i, j) = 1.0;
        }
    }

}

void GridFilter::addPoints(int dev, bool isDevConst) {
    /**
     * Fills GridFilter#InterestingPoints matrix by extended contour points.
     */
    int deviation = 0;
    if(isDevConst) deviation = dev;
    m_InterestingPoints.resize(2, 0);
    for(int i = 0; i < m_PMrows; i++) {
        for (int j = 0; j < m_PMcols; j++) {
            if (m_CrossSecOriginal(i, j) == 1.0) {
                if(!isDevConst)  deviation = std::ceil(m_AbsGrad(i, j)) * m_InitialDeviation;
                m_InterestingPoints.conservativeResize(2, m_InterestingPoints.cols() + 1);
                m_InterestingPoints(0, m_InterestingPoints.cols() - 1) = i;
                m_InterestingPoints(1, m_InterestingPoints.cols() - 1) = j;
                m_CrossSectionModified(i, j) = 1.0;
                makeCorridor(i, j, deviation);
            }
        }
    }
}


/**
 * @file
 * @author Denise Ratasich
 * @date 17.09.2013
 *
 * @brief Implementation of an Unscented Transform.
 */

#include "estimation/UnscentedTransform.h"

#include <stdexcept>
#include <vector>
#include <cmath>
#include <Eigen/Cholesky>

namespace estimation
{
  UnscentedTransform::UnscentedTransform (VectorXd x, MatrixXd Px, ITransformer* transformer)
  {
    // validation of params
    if (x.size() != Px.rows()  ||  x.size() != Px.cols())
      throw std::length_error("Initialization of Unscented Transform failed. "
			      "Invalid size of covariance, doesn't match that of vector x.");

    if (transformer == 0)
      throw std::runtime_error("Initialization of Unscented Transform failed. "
			       "No transformer passed.");

    // initializes parameters
    this->x = x;
    this->Px = Px;
    this->transformer = transformer;

    // set default scaling factors
    alpha = 1;			// spread
    beta = 2;			// distribution = Gaussian
    kappa = 3 - x.size();

    // initialize output; to get the size of y, we must propagate a
    // vector through the transformer
    VectorXd testY = transformer->transform(VectorXd::Zero(x.size()));
    y = VectorXd::Zero(testY.size());
    Py = MatrixXd::Zero(testY.size(),testY.size());
    Pxy = MatrixXd::Zero(x.size(),testY.size());

    locked = false;	// compute() not called yet
  }

  UnscentedTransform::~UnscentedTransform ()
  {
    // no space to free
  }

  void UnscentedTransform::compute (void)
  {
    std::vector<double> weights;
    std::vector<VectorXd> sigmaPoints_prior;
    std::vector<VectorXd> sigmaPoints_post;

    if (locked)
      throw new std::runtime_error("Unscented Transform. Called compute a second time - not allowed.");

    locked = true;

    try
    {
      // fill arrays: sigma points with associated weights
      generateSigmaPoints(sigmaPoints_prior, weights);

      // pop last element because its not for calculating the mean;
      // popped value will replace first element when calculating the
      // covariance
      double w0_cov = weights.back();
      weights.pop_back();

      // calculate mean y -----------------------------------
      for (int i = 0; i < sigmaPoints_prior.size(); i++)
      {
	// propagate the sigma points through the function and save
	// (needed for covariance) and covariance cannot be accumulated
	// here too because the mean is needed :(
	VectorXd post = transformer->transform(sigmaPoints_prior[i]);
	sigmaPoints_post.push_back(post);

	// accumulate to the approximated mean
	y += weights[i] * post;
      }

      // calculate covariances Py, Pxy ----------------------
      weights[0] = w0_cov;			// prepare weights
      VectorXd t(sigmaPoints_post[0].size());	// help vector (for transform)

      for (int i = 0; i < sigmaPoints_post.size(); i++)
      {
	t = sigmaPoints_post[i] - y;
	Py += weights[i] * t * t.transpose();
	Pxy += weights[i] * (sigmaPoints_prior[i] - x) * t.transpose();
      }
    }
    catch (std::exception& e)
    {
      std::string additionalInfo = "Computation of Unscented Transform failed. ";
      throw std::runtime_error(additionalInfo + e.what());
    }
  }

  // -----------------------------------------
  // getters and setters
  // -----------------------------------------

  VectorXd UnscentedTransform::mean (void) const
  {
    return y;
  }

  MatrixXd UnscentedTransform::covariance (void) const
  {
    return Py;
  }

  MatrixXd UnscentedTransform::crossCovarianceXY (void) const
  {
    return Pxy;
  }

  // -----------------------------------------
  // private functions
  // -----------------------------------------
  void UnscentedTransform::generateSigmaPoints (std::vector<VectorXd>& sigmaPoints, std::vector<double>& weights)
  {
    int L = x.size();

    // calculate values which are needed more often
    double alphaPow2 = alpha*alpha;
    double lambda = alphaPow2 * (L + kappa) - L;
    double lPlusLambda = L + lambda;

    // calculate matrix square root ----------------

    // matrix which should form the sigma points (matrix to take the
    // root of)
    MatrixXd msr(L,L);
    msr = lPlusLambda * Px;

    // calculate the root of the matrix; this is done here by standard
    // Cholesky decomposition (this works for symmetric,
    // positive-definite matrices, which Px is like)
    msr = msr.llt().matrixL();

    // generate sigma points -----------------------
    VectorXd col(x.size());

    // chi_0
    sigmaPoints.push_back(x);

    // chi_i = x + msr_i-1, i=1,..,L
    for (int i = 1; i <= L; i++)
      sigmaPoints.push_back(x + msr.col(i-1));

    // chi_i = x - msr_i-L-1, i=L+1,..,2L
    for (int i = 1; i <= L; i++)
      sigmaPoints.push_back(x - msr.col(i-1));

    // set weights --------------------------------

    // W_0 for mean
    weights.push_back(lambda / lPlusLambda);

    // W_1, W_2, ..., W_2L for mean AND covariance
    for (int i = 1; i <= 2*L; i++)
      weights.push_back(1 / (2 * lPlusLambda));

    // W_0 for covariance
    weights.push_back(lambda/lPlusLambda + 1 - alphaPow2 + beta);
  }
}


/*
 * Polygon.cpp
 *
 *  Created on: Nov 7, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#include <grid_map_core/Polygon.hpp>

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

#include <limits>
#include <algorithm>

namespace grid_map {

Polygon::Polygon()
    : timestamp_(0)
{
}

Polygon::Polygon(std::vector<Position> vertices)
    : Polygon()
{
  vertices_ = vertices;
}

Polygon::~Polygon() {}

bool Polygon::isInside(const Position& point) const
{
  int cross = 0;
  for (int i = 0, j = vertices_.size() - 1; i < vertices_.size(); j = i++) {
    if ( ((vertices_[i].y() > point.y()) != (vertices_[j].y() > point.y()))
           && (point.x() < (vertices_[j].x() - vertices_[i].x()) * (point.y() - vertices_[i].y()) /
            (vertices_[j].y() - vertices_[i].y()) + vertices_[i].x()) )
    {
      cross++;
    }
  }
  return bool(cross % 2);
}

void Polygon::addVertex(const Position& vertex)
{
  vertices_.push_back(vertex);
}

const Position& Polygon::getVertex(const size_t index) const
{
  return vertices_.at(index);
}

void Polygon::removeVertices()
{
  vertices_.clear();
}

const Position& Polygon::operator [](const size_t index) const
{
  return getVertex(index);
}

const std::vector<Position>& Polygon::getVertices() const
{
  return vertices_;
}

size_t Polygon::nVertices() const
{
  return vertices_.size();
}

const std::string& Polygon::getFrameId() const
{
  return frameId_;
}

void Polygon::setFrameId(const std::string& frameId)
{
  frameId_ = frameId;
}

uint64_t Polygon::getTimestamp() const
{
  return timestamp_;
}

void Polygon::setTimestamp(const uint64_t timestamp)
{
  timestamp_ = timestamp;
}

void Polygon::resetTimestamp()
{
  timestamp_ = 0.0;
}

double Polygon::getArea() const
{
  double area = 0.0;
  int j = vertices_.size() - 1;
  for (int i = 0; i < vertices_.size(); i++) {
    area += (vertices_.at(j).x() + vertices_.at(i).x())
        * (vertices_.at(j).y() - vertices_.at(i).y());
    j = i;
  }
  return std::abs(area / 2.0);
}

Position Polygon::getCentroid() const
{
  Position centroid = Position::Zero();
  std::vector<Position> vertices = getVertices();
  vertices.push_back(vertices.at(0));
  double area = 0.0;
  for (int i = 0; i < vertices.size() - 1; i++) {
    const double a = vertices[i].x() * vertices[i+1].y() - vertices[i+1].x() * vertices[i].y();
    area += a;
    centroid.x() += a * (vertices[i].x() + vertices[i+1].x());
    centroid.y() += a * (vertices[i].y() + vertices[i+1].y());
  }
  area *= 0.5;
  centroid /= (6.0 * area);
  return centroid;
}

void Polygon::getBoundingBox(Position& center, Length& length) const
{
  double minX = std::numeric_limits<double>::infinity();
  double maxX = -std::numeric_limits<double>::infinity();
  double minY = std::numeric_limits<double>::infinity();
  double maxY = -std::numeric_limits<double>::infinity();
  for (const auto& vertex : vertices_) {
    if (vertex.x() > maxX) maxX = vertex.x();
    if (vertex.y() > maxY) maxY = vertex.y();
    if (vertex.x() < minX) minX = vertex.x();
    if (vertex.y() < minY) minY = vertex.y();
  }
  center.x() = (minX + maxX) / 2.0;
  center.y() = (minY + maxY) / 2.0;
  length.x() = (maxX - minX);
  length.y() = (maxY - minY);
}

bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const
{
  Eigen::MatrixXd V(nVertices(), 2);
  for (unsigned int i = 0; i < nVertices(); ++i)
    V.row(i) = vertices_[i];

  // Create k, a list of indices from V forming the convex hull.
  // TODO: Assuming counter-clockwise ordered convex polygon.
  // MATLAB: k = convhulln(V);
  Eigen::MatrixXi k;
  k.resizeLike(V);
  for (unsigned int i = 0; i < V.rows(); ++i)
    k.row(i) << i, (i+1) % V.rows();
  Eigen::RowVectorXd c = V.colwise().mean();
  V.rowwise() -= c;
  A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN);

  unsigned int rc = 0;
  for (unsigned int ix = 0; ix < k.rows(); ++ix) {
    Eigen::MatrixXd F(2, V.cols());
    F.row(0) << V.row(k(ix, 0));
    F.row(1) << V.row(k(ix, 1));
    Eigen::FullPivLU<Eigen::MatrixXd> luDecomp(F);
    if (luDecomp.rank() == F.rows()) {
      A.row(rc) = F.colPivHouseholderQr().solve(Eigen::VectorXd::Ones(F.rows()));
      ++rc;
    }
  }

  A = A.topRows(rc);
  b = Eigen::VectorXd::Ones(A.rows());
  b = b + A * c.transpose();

  return true;
}

bool Polygon::thickenLine(const double thickness)
{
  if (vertices_.size() != 2) return false;
  const Vector connection(vertices_[1] - vertices_[0]);
  const Vector orthogonal = thickness * Vector(connection.y(), -connection.x()).normalized();
  std::vector<Position> newVertices;
  newVertices.reserve(4);
  newVertices.push_back(vertices_[0] + orthogonal);
  newVertices.push_back(vertices_[0] - orthogonal);
  newVertices.push_back(vertices_[1] - orthogonal);
  newVertices.push_back(vertices_[1] + orthogonal);
  vertices_ = newVertices;
  return true;
}

bool Polygon::offsetInward(const double margin)
{
  // Create a list of indices of the neighbours of each vertex.
  // TODO: Assuming counter-clockwise ordered convex polygon.
  std::vector<Eigen::Array2i> neighbourIndices;
  const unsigned int n = nVertices();
  neighbourIndices.resize(n);
  for (unsigned int i = 0; i < n; ++i) {
    neighbourIndices[i] << (i > 0 ? (i-1)%n : n-1), (i + 1) % n;
  }

  std::vector<Position> copy(vertices_);
  for (unsigned int i = 0; i < neighbourIndices.size(); ++i) {
    Eigen::Vector2d v1 = vertices_[neighbourIndices[i](0)] - vertices_[i];
    Eigen::Vector2d v2 = vertices_[neighbourIndices[i](1)] - vertices_[i];
    v1.normalize();
    v2.normalize();
    const double angle = acos(v1.dot(v2));
    copy[i] += margin / sin(angle) * (v1 + v2);
  }
  vertices_ = copy;
  return true;
}

std::vector<Polygon> Polygon::triangulate(const TriangulationMethods& method) const
{
  // TODO Add more triangulation methods.
  // https://en.wikipedia.org/wiki/Polygon_triangulation
  std::vector<Polygon> polygons;
  if (vertices_.size() < 3)
    return polygons;

  size_t nPolygons = vertices_.size() - 2;
  polygons.reserve(nPolygons);

  if (nPolygons < 1) {
    // Special case.
    polygons.push_back(*this);
  } else {
    // General case.
    for (size_t i = 0; i < nPolygons; ++i) {
      Polygon polygon({vertices_[0], vertices_[i + 1], vertices_[i + 2]});
      polygons.push_back((polygon));
    }
  }

  return polygons;
}

Polygon Polygon::fromCircle(const Position center, const double radius,
                                  const int nVertices)
{
  Eigen::Vector2d centerToVertex(radius, 0.0), centerToVertexTemp;

  Polygon polygon;
  for (int j = 0; j < nVertices; j++) {
    double theta = j * 2 * M_PI / (nVertices - 1);
    Eigen::Rotation2D<double> rot2d(theta);
    centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
    polygon.addVertex(center + centerToVertexTemp);
  }
  return polygon;
}

Polygon Polygon::convexHullOfTwoCircles(const Position center1,
                                   const Position center2, const double radius,
                                   const int nVertices)
{
  if (center1 == center2) return fromCircle(center1, radius, nVertices);
  Eigen::Vector2d centerToVertex, centerToVertexTemp;
  centerToVertex = center2 - center1;
  centerToVertex.normalize();
  centerToVertex *= radius;

  grid_map::Polygon polygon;
  for (int j = 0; j < ceil(nVertices / 2.0); j++) {
    double theta = M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
    Eigen::Rotation2D<double> rot2d(theta);
    centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
    polygon.addVertex(center1 + centerToVertexTemp);
  }
  for (int j = 0; j < ceil(nVertices / 2.0); j++) {
    double theta = 3 * M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
    Eigen::Rotation2D<double> rot2d(theta);
    centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
    polygon.addVertex(center2 + centerToVertexTemp);
  }
  return polygon;
}

Polygon Polygon::convexHull(Polygon& polygon1, Polygon& polygon2)
{
  std::vector<Position> vertices;
  vertices.reserve(polygon1.nVertices() + polygon2.nVertices());
  vertices.insert(vertices.end(), polygon1.getVertices().begin(), polygon1.getVertices().end());
  vertices.insert(vertices.end(), polygon2.getVertices().begin(), polygon2.getVertices().end());

  return monotoneChainConvexHullOfPoints(vertices);
}

Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector<Position>& points)
{
  // Adapted from https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain
  if (points.size() <= 3) {
    return Polygon(points);
  }
  std::vector<Position> pointsConvexHull(2 * points.size());

  // Sort points lexicographically.
  auto sortedPoints(points);
  std::sort(sortedPoints.begin(), sortedPoints.end(), sortVertices);

  int k = 0;
  // Build lower hull
  for (int i = 0; i < sortedPoints.size(); ++i) {
    while (k >= 2 && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
      k--;
    }
    pointsConvexHull.at(k++) = sortedPoints.at(i);
  }

  // Build upper hull.
  for (int i = sortedPoints.size() - 2, t = k + 1; i >= 0; i--) {
    while (k >= t && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
      k--;
    }
    pointsConvexHull.at(k++) = sortedPoints.at(i);
  }
  pointsConvexHull.resize(k - 1);

  Polygon polygon(pointsConvexHull);
  return polygon;
}

bool Polygon::sortVertices(const Eigen::Vector2d& vector1,
                           const Eigen::Vector2d& vector2)
{
  return (vector1.x() < vector2.x()
      || (vector1.x() == vector2.x() && vector1.y() < vector2.y()));
}

double Polygon::computeCrossProduct2D(const Eigen::Vector2d& vector1,
                                      const Eigen::Vector2d& vector2)
{
  return (vector1.x() * vector2.y() - vector1.y() * vector2.x());
}

double Polygon::vectorsMakeClockwiseTurn(const Eigen::Vector2d &pointOrigin,
                                         const Eigen::Vector2d &pointA,
                                         const Eigen::Vector2d &pointB)
{
  return computeCrossProduct2D(pointA - pointOrigin, pointB - pointOrigin) <= 0;
}

const std::vector<Position> Polygon::getInterpolatedConvexHull(const double stepSize) {
  std::vector<Position> result;
  Polygon hull = monotoneChainConvexHullOfPoints(this->getVertices());

  for (int curIdx = 0; curIdx < hull.nVertices(); curIdx++){
    int nextIdx = curIdx < hull.nVertices() -1 ? curIdx + 1 : 0;

    Position curPos = hull.getVertex(curIdx);
    Position nextPos = hull.getVertex(nextIdx);

    Position diff = nextPos - curPos;
    double diffDistance = diff.norm();
    int steps = std::max(1, (int) ceil(diffDistance / stepSize));
    Position direction(
      diff[0] / steps,
      diff[1] / steps
    );

    result.push_back(curPos);
    for (int i = 0; i < steps; i++) {
      double factor = ((double) i) / steps;
      Position tmp = curPos + factor * diff;
      result.push_back(tmp);
    }
  }

  return result;
}

} /* namespace grid_map */


// Main code for Bayesian Inference with DIAMONDS
// Created by Enrico Corsaro @ OACT - February 2017
// e-mail: emncorsaro@gmail.com
// Source code file "SuperGaussianModel.cpp"

// To compile in Mac OS:
// clang++ -o SuperGaussianFit SuperGaussianFit.cpp -L../../build/ -I ../../include/ -l diamonds -stdlib=libc++ -std=c++11 -Wno-deprecated-register
// To compile in Linux OS:
// g++ -o SuperGaussianFit SuperGaussianFit.cpp -L../../build/ -I../../include/ -ldiamonds -std=c++11

#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <Eigen/Dense>
#include "Functions.h"
#include "File.h"
#include "MultiEllipsoidSampler.h"
#include "KmeansClusterer.h"
#include "EuclideanMetric.h"
#include "Prior.h"
#include "UniformPrior.h"
#include "NormalPrior.h"
#include "SuperGaussianModel.h"
#include "NormalLikelihood.h"
#include "FerozReducer.h"
#include "PowerlawReducer.h"
#include "Results.h"
#include "Ellipsoid.h"
#include "PrincipalComponentProjector.h"

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

    // Check number of arguments for main function

    if (argc != 3)
    {
        cerr << "Usage: ./SuperGaussianFit <data filename> <prior filename>" << endl;
        exit(EXIT_FAILURE);
    }

    // ---------------------------
    // ----- Read input data -----
    // ---------------------------

    unsigned long Nrows;
    int Ndimensions;              // Number of parameters for which prior distributions are defined
    int Ncols;
    ArrayXXd data;

    string baseInputDirName = "";
    string inputFileName(argv[1]);
    string outputPathPrefix = "SuperGaussianFit_";

    ifstream inputFile;
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nrows, Ncols);
    data = File::arrayXXdFromFile(inputFile, Nrows, Ncols);
    inputFile.close();

    // Creating arrays for each data type

    ArrayXd covariates = data.col(0);
    ArrayXd observations = data.col(1);
    ArrayXd uncertainties = data.col(2);

    // -------------------------------------------------------
    // ----- First step. Set up all prior distributions -----
    // -------------------------------------------------------

    // Uniform Prior
    unsigned long Nparameters;

    // ---- Read prior hyper parameters for resolved modes -----
    string inputFileNamePrior(argv[2]);
    File::openInputFile(inputFile, inputFileNamePrior);
    File::sniffFile(inputFile, Nparameters, Ncols);
    ArrayXXd hyperParameters;
    Ndimensions = Nparameters;

    if (Ncols == 1)
    {
        Ncols = 2;
        hyperParameters.conservativeResize(Nparameters, Ncols);
    }

    hyperParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    ArrayXd hyperParametersMinima = hyperParameters.col(0);
    ArrayXd hyperParametersMaxima = hyperParameters.col(1);

    int NpriorTypes = 1;                                        // Total number of prior types included in the computation
    vector<Prior*> ptrPriors(NpriorTypes);
    ArrayXd parametersMinima(Ndimensions);
    ArrayXd parametersMaxima(Ndimensions);
    parametersMinima << hyperParametersMinima;      // Minima values for the free parameters (free parameter #1, free parameter #2, ..., etc.)
    parametersMaxima << hyperParametersMaxima;      // Maxima values for the free parameters (same order as minima)
    UniformPrior uniformPrior(parametersMinima, parametersMaxima);
    ptrPriors[0] = &uniformPrior;

    string fullPathHyperParameters = outputPathPrefix + "hyperParametersUniform.txt";       // Print prior hyper parameters as output
    uniformPrior.writeHyperParametersToFile(fullPathHyperParameters);

    // -------------------------------------------------------------------
    // ---- Second step. Set up the models for the inference problem -----
    // -------------------------------------------------------------------

    SuperGaussianModel model(covariates);      // Super Gaussian function

    // -----------------------------------------------------------------
    // ----- Third step. Set up the likelihood function to be used -----
    // -----------------------------------------------------------------

    NormalLikelihood likelihood(observations, uncertainties, model);

    // -------------------------------------------------------------------------------
    // ----- Fourth step. Set up the K-means clusterer using an Euclidean metric -----
    // -------------------------------------------------------------------------------

    inputFileName = "Xmeans_configuringParameters.txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);

    if (Nparameters != 2)
    {
        cerr << "Wrong number of input parameters for X-means algorithm." << endl;
        exit(EXIT_FAILURE);
    }

    ArrayXd configuringParameters;
    configuringParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    int minNclusters = configuringParameters(0);
    int maxNclusters = configuringParameters(1);

    if ((minNclusters <= 0) || (maxNclusters <= 0) || (maxNclusters < minNclusters))
    {
        cerr << "Minimum or maximum number of clusters cannot be <= 0, and " << endl;
        cerr << "minimum number of clusters cannot be larger than maximum number of clusters." << endl;
        exit(EXIT_FAILURE);
    }

    int Ntrials = 10;
    double relTolerance = 0.01;

    bool printNdimensions = false;
    PrincipalComponentProjector projector(printNdimensions);
    bool featureProjectionActivated = true;

    EuclideanMetric myMetric;
    KmeansClusterer kmeans(myMetric, projector, featureProjectionActivated,
                           minNclusters, maxNclusters, Ntrials, relTolerance);

    // ---------------------------------------------------------------------
    // ----- Sixth step. Configure and start nested sampling inference -----
    // ---------------------------------------------------------------------

    inputFileName = "NSMC_configuringParameters.txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);
    configuringParameters.setZero();
    configuringParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    if (Nparameters != 8)
    {
        cerr << "Wrong number of input parameters for NSMC algorithm." << endl;
        exit(EXIT_FAILURE);
    }

    bool printOnTheScreen = true;                       // Print results on the screen
    int initialNobjects = configuringParameters(0);     // Initial number of live points
    int minNobjects = configuringParameters(1);         // Minimum number of live points
    int maxNdrawAttempts = configuringParameters(2);    // Maximum number of attempts when trying to draw a new sampling point
    int NinitialIterationsWithoutClustering = configuringParameters(3); // The first N iterations, we assume that there is only 1 cluster
    int NiterationsWithSameClustering = configuringParameters(4);       // Clustering is only happening every N iterations.
    double initialEnlargementFraction = configuringParameters(5);   // Fraction by which each axis in an ellipsoid has to be enlarged.
                                                                    // It can be a number >= 0, where 0 means no enlargement.
    double shrinkingRate = configuringParameters(6);        // Exponent for remaining prior mass in ellipsoid enlargement fraction.
                                                            // It is a number between 0 and 1. The smaller the slower the shrinkage
                                                            // of the ellipsoids.
    double terminationFactor = configuringParameters(7);    // Termination factor for nested sampling process.

    MultiEllipsoidSampler nestedSampler(printOnTheScreen, ptrPriors, likelihood, myMetric, kmeans,
                                        initialNobjects, minNobjects, initialEnlargementFraction, shrinkingRate);

    double tolerance = 1.e2;
    double exponent = 0.4;
    PowerlawReducer livePointsReducer(nestedSampler, tolerance, exponent, terminationFactor);

    nestedSampler.run(livePointsReducer, NinitialIterationsWithoutClustering, NiterationsWithSameClustering,
                      maxNdrawAttempts, terminationFactor, 0, outputPathPrefix);

    nestedSampler.outputFile << "# List of configuring parameters used for the ellipsoidal sampler and X-means" << endl;
    nestedSampler.outputFile << "# Row #1: Minimum Nclusters" << endl;
    nestedSampler.outputFile << "# Row #2: Maximum Nclusters" << endl;
    nestedSampler.outputFile << "# Row #3: Initial Enlargement Fraction" << endl;
    nestedSampler.outputFile << "# Row #4: Shrinking Rate" << endl;
    nestedSampler.outputFile << minNclusters << endl;
    nestedSampler.outputFile << maxNclusters << endl;
    nestedSampler.outputFile << initialEnlargementFraction << endl;
    nestedSampler.outputFile << shrinkingRate << endl;
    nestedSampler.outputFile.close();

    // -------------------------------------------------------
    // ----- Last step. Save the results in output files -----
    // -------------------------------------------------------

    Results results(nestedSampler);
    results.writeParametersToFile("parameter");
    results.writeLogLikelihoodToFile("logLikelihood.txt");
    results.writeLogWeightsToFile("logWeights.txt");
    results.writeEvidenceInformationToFile("evidenceInformation.txt");
    results.writePosteriorProbabilityToFile("posteriorDistribution.txt");
    results.writeLogEvidenceToFile("logEvidence.txt");
    results.writeLogMeanLiveEvidenceToFile("logMeanLiveEvidence.txt");

    double credibleLevel = 68.3;
    bool writeMarginalDistributionToFile = true;
    results.writeParametersSummaryToFile("parameterSummary.txt", credibleLevel, writeMarginalDistributionToFile);

    cout << "Process completed." << endl;

    return EXIT_SUCCESS;
}


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

#ifndef BOOST_MATH_TOOLS_SIMPLE_CONTINUED_FRACTION_HPP
#define BOOST_MATH_TOOLS_SIMPLE_CONTINUED_FRACTION_HPP

#include <vector>
#include <ostream>
#include <iomanip>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <boost/core/demangle.hpp>

namespace boost::math::tools {

template<typename Real, typename Z = int64_t>
class simple_continued_fraction {
public:
    simple_continued_fraction(Real x) : x_{x} {
        using std::floor;
        using std::abs;
        using std::sqrt;
        using std::isfinite;
        if (!isfinite(x)) {
            throw std::domain_error("Cannot convert non-finites into continued fractions.");
        }
        b_.reserve(50);
        Real bj = floor(x);
        b_.push_back(static_cast<Z>(bj));
        if (bj == x) {
           b_.shrink_to_fit();
           return;
        }
        x = 1/(x-bj);
        Real f = bj;
        if (bj == 0) {
           f = 16*std::numeric_limits<Real>::min();
        }
        Real C = f;
        Real D = 0;
        int i = 0;
        // the "1 + i++" lets the error bound grow slowly with the number of convergents.
        // I have not worked out the error propagation of the Modified Lentz's method to see if it does indeed grow at this rate.
        // Numerical Recipes claims that no one has worked out the error analysis of the modified Lentz's method.
        while (abs(f - x_) >= (1 + i++)*std::numeric_limits<Real>::epsilon()*abs(x_))
        {
          bj = floor(x);
          b_.push_back(static_cast<Z>(bj));
          x = 1/(x-bj);
          D += bj;
          if (D == 0) {
             D = 16*std::numeric_limits<Real>::min();
          }
          C = bj + 1/C;
          if (C==0) {
             C = 16*std::numeric_limits<Real>::min();
          }
          D = 1/D;
          f *= (C*D);
       }
       // Deal with non-uniqueness of continued fractions: [a0; a1, ..., an, 1] = a0; a1, ..., an + 1].
       // The shorter representation is considered the canonical representation,
       // so if we compute a non-canonical representation, change it to canonical:
       if (b_.size() > 2 && b_.back() == 1) {
          b_[b_.size() - 2] += 1;
          b_.resize(b_.size() - 1);
       }
       b_.shrink_to_fit();

       for (size_t i = 1; i < b_.size(); ++i) {
         if (b_[i] <= 0) {
            std::ostringstream oss;
            oss << "Found a negative partial denominator: b[" << i << "] = " << b_[i] << "."
                << " This means the integer type '" << boost::core::demangle(typeid(Z).name())
                << "' has overflowed and you need to use a wider type,"
                << " or there is a bug.";
            throw std::overflow_error(oss.str());
         }
       }
    }

    Real khinchin_geometric_mean() const {
        if (b_.size() == 1) {
         return std::numeric_limits<Real>::quiet_NaN();
        }
         using std::log;
         using std::exp;
         // Precompute the most probable logarithms. See the Gauss-Kuzmin distribution for details.
         // Example: b_i = 1 has probability -log_2(3/4) ≈ .415:
         // A random partial denominator has ~80% chance of being in this table:
         const std::array<Real, 7> logs{std::numeric_limits<Real>::quiet_NaN(), Real(0), log(static_cast<Real>(2)), log(static_cast<Real>(3)), log(static_cast<Real>(4)), log(static_cast<Real>(5)), log(static_cast<Real>(6))};
         Real log_prod = 0;
         for (size_t i = 1; i < b_.size(); ++i) {
            if (b_[i] < static_cast<Z>(logs.size())) {
               log_prod += logs[b_[i]];
            }
            else
            {
               log_prod += log(static_cast<Real>(b_[i]));
            }
         }
         log_prod /= (b_.size()-1);
         return exp(log_prod);
    }

    Real khinchin_harmonic_mean() const {
        if (b_.size() == 1) {
          return std::numeric_limits<Real>::quiet_NaN();
        }
        Real n = b_.size() - 1;
        Real denom = 0;
        for (size_t i = 1; i < b_.size(); ++i) {
            denom += 1/static_cast<Real>(b_[i]);
        }
        return n/denom;
    }

    const std::vector<Z>& partial_denominators() const {
      return b_;
    }

    template<typename T, typename Z2>
    friend std::ostream& operator<<(std::ostream& out, simple_continued_fraction<T, Z2>& scf);

private:
    const Real x_;
    std::vector<Z> b_;
};

template<typename Real, typename Z2>
std::ostream& operator<<(std::ostream& out, simple_continued_fraction<Real, Z2>& scf) {
   constexpr const int p = std::numeric_limits<Real>::max_digits10;
   if constexpr (p == 2147483647) {
      out << std::setprecision(scf.x_.backend().precision());
   } else {
      out << std::setprecision(p);
   }

   out << "[" << scf.b_.front();
   if (scf.b_.size() > 1)
   {
      out << "; ";
      for (size_t i = 1; i < scf.b_.size() -1; ++i)
      {
         out << scf.b_[i] << ", ";
      }
      out << scf.b_.back();
   }
   out << "]";
   return out;
}

}
#endif