#include <Eigen/Core>
#include <Eigen/Sparse>
#include <iostream>
#include <vector>

//CRS
void compressedSparseRow()
{
/*
    0 1 2 3 4 5 6 7 8
   ┌                 ┐
 0 |0 0 0 0 0 0 0 3 0|
 1 |0 0 8 0 0 1 0 0 0|
 2 |0 0 0 0 0 0 0 0 0|
 3 |4 0 0 0 0 0 0 0 0|
 4 |0 0 0 0 0 0 0 0 0|
 5 |0 0 2 0 0 0 0 0 0|
 6 |0 0 0 6 0 0 0 0 0|
 7 |0 9 0 0 5 0 0 0 0|
   └                 ┘
*/

    int rows=8;
    int cols=9;
    Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> dense_mat;
    dense_mat.resize(rows,cols);

    dense_mat<<0, 0, 0, 0, 0, 0, 0, 3, 0,
                0, 0, 8, 0, 0, 1, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0,
                4, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 2, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 6, 0, 0, 0, 0, 0,
                0, 9, 0, 0, 5, 0, 0, 0, 0;

    //specify a tolerance:
    //sparse_mat = dense_mat.sparseView(epsilon,reference);
    Eigen::SparseMatrix<double,Eigen::RowMajor> sparse_mat = dense_mat.sparseView();

    std::cout<<"sparse matrix number of cols:"<<sparse_mat.cols()   <<std::endl;
    std::cout<<"sparse matrix numner of rows:"<<sparse_mat.rows()   <<std::endl;

    std::cout<<"sparse matrix values:"<<std::endl;
    auto valuePtr=sparse_mat.valuePtr();
    for(int i=0;i <sparse_mat.nonZeros();i++)
    {
        std::cout<<*(valuePtr++)  <<", ";
    }
    std::cout<< "\n";

    std::cout<<"COL_INDEX array size:"<<sparse_mat.innerSize()  <<std::endl;
    std::cout<<"COL_INDEX array :" <<std::endl;
    auto colIndexPtr=sparse_mat.innerIndexPtr();

    for(int i=0;i <sparse_mat.innerSize();i++)
    {
        std::cout<<*(colIndexPtr++)  <<", ";
    }
    std::cout<< "\n";

    std::cout<<"ROW_INDEX array size:"<<sparse_mat.outerSize()   <<std::endl;
    std::cout<<"ROW_INDEX array:" <<std::endl;
    auto rowIndexPtr=sparse_mat.outerIndexPtr();
    for(int i=0;i <sparse_mat.outerSize();i++)
    {
        std::cout<<*(rowIndexPtr++)  <<", ";
    }
    std::cout<< "\n ";

    std::cout<<"*********sparse matrix*********\n"<<sparse_mat  <<std::endl;

    std::cout<<"*********dense matrix*********\n"<<dense_mat  <<std::endl;

}

int main1()
{
    compressedSparseRow();
}

typedef Eigen::SparseMatrix<double> SpMat; // declares a column-major sparse matrix type of double
typedef Eigen::Triplet<double> T;//structure to hold a non zero as a triplet (i,j,value).

void buildProblem(std::vector<T>& coefficients, Eigen::VectorXd& b, int n){}
void saveAsBitmap(const Eigen::VectorXd& x, int n, const char* filename){}
//https://eigen.tuxfamily.org/dox/classEigen_1_1Triplet.html
int main(int argc, char** argv)
{
  if(argc!=2) {
    std::cerr << "Error: expected one and only one argument.\n";
    return -1;
  }

  int n = 300;  // size of the image
  int m = n*n;  // number of unknows (=number of pixels)

  // Assembly:
  std::vector<T> coefficients;            // list of non-zeros coefficients
  Eigen::VectorXd b(m);                   // the right hand side-vector resulting from the constraints
  buildProblem(coefficients, b, n);

  SpMat A(m,m);
  A.setFromTriplets(coefficients.begin(), coefficients.end());

  // Solving:
  Eigen::SimplicialCholesky<SpMat> chol(A);  // performs a Cholesky factorization of A
  Eigen::VectorXd x = chol.solve(b);         // use the factorization to solve for the given right hand side

  // Export the result to a file:
  saveAsBitmap(x, n, argv[1]);

  return 0;
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_

//[safe_prime

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/miller_rabin.hpp>
#include <iostream>
#include <iomanip>

int main()
{
   using namespace boost::random;
   using namespace boost::multiprecision;

   typedef cpp_int int_type;
   mt11213b base_gen(clock());
   independent_bits_engine<mt11213b, 256, int_type> gen(base_gen);
   //
   // We must use a different generator for the tests and number generation, otherwise
   // we get false positives.
   //
   mt19937 gen2(clock());

   for(unsigned i = 0; i < 100000; ++i)
   {
      int_type n = gen();
      if(miller_rabin_test(n, 25, gen2))
      {
         // Value n is probably prime, see if (n-1)/2 is also prime:
         std::cout << "We have a probable prime with value: " << std::hex << std::showbase << n << std::endl;
         if(miller_rabin_test((n-1)/2, 25, gen2))
         {
            std::cout << "We have a safe prime with value: " << std::hex << std::showbase << n << std::endl;
            return 0;
         }
      }
   }
   std::cout << "Ooops, no safe primes were found - probably a bad choice of seed values!" << std::endl;
   return 0;
}

//]


#ifndef MIXMAT_HPP
#define MIXMAT_HPP

#include <vector>
#include <cstddef>
#include <iostream>
#include <wignerSymbols.h>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <cassert>
#include <exception>

#include <Eigen/Core>
#include <Eigen/Dense>

enum mixMatType
{
    TT,
    EE,
};

template<typename _realScalarType>
class MixingMatrix
{
public:
    /**
     * \typedef _realScalarType realScalarType
     * \brief real floating point type
     */
    typedef _realScalarType realScalarType;

    /**
     * \typedef typename Eigen::Matrix<realScalarType,Eigen::Dynamic,Eigen::Dynamic> mixingMatrixType
     * \brief mixing matrix type
     */
    typedef Eigen::Matrix<realScalarType,Eigen::Dynamic,Eigen::Dynamic> mixingMatrixType;

    /**
     * \typedef typename mixingMatrixType::Index indexType
     * \brief integral type
     */
    typedef typename mixingMatrixType::Index indexType;

    /**
     * \typedef Eigen::LLT<mixingMatrixType> LLTType;
     * \brief Cholesky decompostion type
     */
    typedef Eigen::LLT<mixingMatrixType> LLTType;

    typedef Eigen::Matrix<realScalarType,Eigen::Dynamic,1> powSpecType;

    MixingMatrix(powSpecType const & W,mixMatType const mt)
    :mW(W)
    {
        const indexType lMax = W.rows()-1;
        const indexType matDim(lMax+1);
        mM = mixingMatrixType::Zero(matDim,matDim);
        mLMax = lMax;
        mMT = mt;
        std::cout<<"lMax = "<<lMax<<std::endl;
    }

    void computeMixingMatrixWithRecursion()
    {
        if(mMT == TT)
        {
            std::cout<<"Computing TT mixing matrix"<<std::endl;

            const indexType matDim(mLMax+1);
            mM = mixingMatrixType::Zero(matDim,matDim);

            for(indexType l1=0;l1<=mLMax;++l1)
            {
                for(indexType l2=0;l2<=mLMax;++l2)
                {
                    std::vector<double> wig3j =
                        WignerSymbols::wigner3j((double)l1,(double)l2,
                            (double)0,(double)0,(double)0);

                    indexType l3Min = std::max(std::abs(l1-l2),indexType(0));
                    indexType l3max = std::min( l3Min + indexType(wig3j.size()-1),mLMax ) ;

                    for(indexType l3=l3Min;l3<=l3max;++l3)
                    {
                        if( selectWig3j( l3,l1,l2,indexType(0),indexType(0),indexType(0) )  )
                        {
                            mM(l1,l2) += realScalarType(2*l3+1)*mW(l3)*wig3j[l3 - l3Min]*wig3j[l3 - l3Min];
                        }
                    }

                    mM(l1,l2) *= realScalarType(2*l2+1)/(4*M_PI);
                }
            }
        }
        else if(mMT == EE)
        {
            std::cout<<"Computing EE mixing matrix"<<std::endl;

            const indexType matDim(mLMax+1);
            mM = mixingMatrixType::Zero(matDim,matDim);

            for(indexType l1=0;l1<=mLMax;++l1)
            {
                for(indexType l2=0;l2<=mLMax;++l2)
                {
                    std::vector<double> wig3j =
                        WignerSymbols::wigner3j((double)l1,(double)l2,
                            (double)0,(double)2,(double)-2);

                    indexType l3Min = std::max(std::abs(l1-l2),indexType(0));
                    indexType l3max = std::min( l3Min + indexType(wig3j.size()-1),mLMax ) ;

                    for(indexType l3=l3Min;l3<=l3max;++l3)
                    {
                        if( selectWig3j( l3,l1,l2,indexType(0),indexType(2),indexType(-2) ) && (l1+l2+l3)%2 == 0 )
                        {
                            mM(l1,l2) += realScalarType(2*l3+1)*realScalarType(2)*mW(l3)*wig3j[l3 - l3Min]*wig3j[l3 - l3Min];
                        }
                    }

                    mM(l1,l2) *= realScalarType(2*l2+1)/(8*M_PI);
                }
            }

        }

    }

    bool selectWig3j(indexType const l1,indexType const l2,indexType const l3,
        indexType const m1,indexType const m2,indexType const m3)
    {
        bool select(true);

        select = (
            std::abs(m1+m2+m3) == indexType(0)
            && l3 >= std::abs(l1-l2)
            && l3 <= l1+l2
            && std::abs(m1) <= l1
            && std::abs(m2) <= l2
            && std::abs(m3) <= l3
        );

        return ( select );
    }

    void save(std::string const fileName)
    {
        writeMatrixToFile(fileName,mM,10,",");
    }

    void applyMixingMatrix(powSpecType & C)
    {
        C = mM*C;
    }

    void applyMixingMatrixInv(powSpecType & C)
    {
        LLTType lltOfM;
        lltOfM.compute(mM);
        assert(lltOfM.info() == Eigen::Success);
        C = lltOfM.solve(C);
    }

private:

    void writeMatrixToFile(std::string const & fileName,
        mixingMatrixType const& mat,unsigned int precision,
        std::string const& separation)
    {
        std::ofstream outFile;
        outFile.open(fileName.c_str(),std::ios::trunc);

        if(outFile.is_open())
        {
            outFile<<std::scientific;
            for(indexType i=0;i<mat.rows();++i)
            {
                for(indexType j=0;j<mat.cols()-1;++j)
                {
                    outFile<<std::setprecision(precision)<<mat(i,j)<<separation;
                }
                outFile<<mat(i,(mat.cols()-1) )<<std::endl;
            }
            outFile.close();
        }
        else
        {
            std::string msg = std::string("Error in opening the file ") +
                fileName;
            throw std::runtime_error(msg);
        }
    }

    powSpecType mW;
    mixingMatrixType mM;
    indexType mLMax;
    mixMatType mMT;

};

#endif //MIXMAT_HPP


/**
 * @file k_means_clustering.hpp
 * @brief
 * @author Piotr Smulewicz, Piotr Wygocki
 * @version 1.0
 * @date 2014-06-25
 */
#ifndef PAAL_K_MEANS_CLUSTERING_HPP
#define PAAL_K_MEANS_CLUSTERING_HPP

#include "paal/clustering/k_means_clustering_engine.hpp"
#include "paal/utils/irange.hpp"

#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/empty.hpp>

#include <vector>

namespace paal {

/**
 * @brief  return centroid that minimize within-cluster sum of squares
 */
template <typename Cluster, typename OutputIterator>
void centroid_minimalize_w_c_s_s(Cluster && cluster, OutputIterator out) {
    assert(!boost::empty(cluster));
    using point_t = range_to_elem_t<Cluster>;
    using coordinate_t = range_to_elem_t<point_t>;

    auto dim = boost::size(*std::begin(cluster));
    for(auto idx : irange(dim)) {
        coordinate_t res{};
        for (auto && point : cluster) {
            res += std::begin(point)[idx];
        }
        *out = res / boost::size(cluster);
        ++out;
   }
}

/**
 * @brief centroid minimize within cluster sum of squares
 * @param clusters
 * @param out
 * @tparam Clusters
 * @tparam OutputIterator
 */
template <typename Clusters, typename OutputIterator>
void centroids_minimalize_w_c_s_s(Clusters && clusters, OutputIterator out) {
    assert(!boost::empty(clusters));
    assert(!boost::empty(*std::begin(clusters)));

    using cluster_t = range_to_elem_t<Clusters>;
    using point_t = range_to_elem_t<cluster_t>;
    using coordinate_t = range_to_elem_t<point_t>;

    auto size = boost::size(*std::begin(*begin(clusters)));
    for (auto && cluster : clusters) {
        std::vector<coordinate_t> point(size);
        centroid_minimalize_w_c_s_s(cluster, point.begin());
        *out = point;
        ++out;
    }
}

/**
 * @brief this is solve k_means_clustering problem
 * and return vector of cluster
 * example:
 *  \snippet k_means_clustering_example.cpp K Means Clustering Example
 *
 * complete example is k_means_clustering_example.cpp
 * @param points
 * @param centers
 * @param result pairs of point and id of cluster
 * (number form 0,1,2 ...,number_of_cluster-1)
 * @param visitor
 * @tparam Points
 * @tparam OutputIterator
 * @tparam CoordinateType
 * @tparam Visitor
 */
template <typename Points, class Centers, class OutputIterator,
          class Visitor = k_means_visitor>
auto k_means(Points &&points, Centers &&centers, OutputIterator result,
             Visitor visitor = Visitor{}) {
    using point_t = range_to_elem_t<Points>;
    using center_t = range_to_elem_t<Centers>;

    center_t center{ *std::begin(centers) };

    return k_means(
        points, centers,
        [&](std::vector<point_t> const & points)->center_t const & {
            centroid_minimalize_w_c_s_s(points, std::begin(center));
            return center;
        },
        [&](point_t const &point) { return closest_to(point, centers); },
        result, utils::equal_to{}, visitor);
}

} //!paal

#endif /* PAAL_K_MEANS_CLUSTERING_HPP */


#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/bandwidth.hpp>
#include <boost/numeric/bindings/lapack/computational/gbtrf.hpp>
#include <boost/numeric/bindings/lapack/computational/gbtrs.hpp>
#include <vector>
#include <stdexcept>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;

// solves the equation Ax = B, and puts the solution in B
// A is mutated by this routine
template <typename MatrA, typename MatrB>
void InPlaceSolve(MatrA& a, MatrB& b)
{
  // if the matrix has kl lower and ku upper diagonals, then we should have
  // allocated kl lower and kl+ku upper diagonals
  fortran_int_t const kl = bindings::bandwidth_lower(a);
  fortran_int_t const ku = bindings::bandwidth_upper(a) - kl;
  std::vector<fortran_int_t> piv(a.size1());
  int ret = lapack::gbtrf(/*kl, ku, */a, piv);
  if(ret < 0)
  {
    //CStdString err;
    //err.Format("banded::Solve: argument %d in DGBTRF had an illegal value", -ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: argument %d in DGBTRF had an illegal value");
  }
  if(ret > 0)
  {
    //CStdString err;
    //err.Format("banded::Solve: the (%d,%d) diagonal element is 0 after DGBTRF", ret, ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: the (%d,%d) diagonal element is 0 after DGBTRF");
  }

  ret = lapack::gbtrs(/*kl, ku, */a, piv, b);
  if(ret < 0)
  {
    //CStdString err;
    //err.Format("banded::Solve: argument %d in DGBTRS had an illegal value", -ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: argument %d in DGBTRS had an illegal value");
  }
}

template<typename T>
void do_typename()
{
  using namespace boost::numeric::ublas;
  // if the matrix has kl lower and ku upper diagonals, then we should
  // allocate kl lower and kl+ku upper diagonals
  size_t sz = 1000, kl = 1, ku = 1;
  ublas::banded_matrix<T> a(sz, sz, kl, kl+ku);
  ublas::vector<T> b(sz);
  // fill values in a and b
  for(size_t i = 0; i < sz; ++i)
  {
    a(i,i) = i;
    b(i) = i;
  }
  for(size_t i = 1; i < sz; ++i)
  {
    a(i,i-1) = i;
    a(i-1,i) = 1;
  }
  InPlaceSolve(a, b);
}

int main()
{
  do_typename<float>();
  do_typename<double>();
  do_typename<std::complex<float> >();
  do_typename<std::complex<double> >();
  return 0;
}


#include <iostream>
#include <complex>
#include <cmath>
#include <Eigen/Dense>
#include "mex.h"

const std::complex<double> i1(0.0, 1.0);

Eigen::Matrix3cd T;

void update_T(double a, double p)
{
    T(0,0) = pow(cos(a/2),2);
    T(0,1) = pow(sin(a/2),2)*(cos(2*p)+1.0*i1*sin(2*p));
    T(0,2) = sin(a)*(sin(p)-1.0*i1*cos(p));
    T(1,0) = std::conj(T(0,1));
    T(1,1) = T(0,0);
    T(1,2) = std::conj(T(0,2));
    T(2,0) = -0.5*sin(a)*(sin(p)+1.0*i1*cos(p));
    T(2,1) = std::conj(T(2,0));
    T(2,2) = cos(a);
}

void EPG_GRE(double *signal_real, double *signal_imag, double *path_real, double *path_imag, double *N, double *FA, double *RF, double *TR, double *T1, double *T2, double *M0)
{
    int n = (int) N[0];

    //max order of EPG simulation
    int KMAX = n;
    if(n>25){KMAX = 25;}

    ///// MEMORY ALLOCATION
    // relaxation operators
    double E1 = exp(-TR[0]/T1[0]);
    double E2 = exp(-TR[0]/T2[0]);

    // signal vectors
    Eigen::VectorXcd F_plus(KMAX);
    F_plus.setZero();

    Eigen::VectorXcd F_minus(KMAX);
    F_minus.setZero();

    Eigen::VectorXcd Z(KMAX);
    Z.setZero();

    Eigen::Vector3cd aux_states(0, 0, 0);

    /////SIGNAL SIMULATION
    Z(0) = M0[0];

    // transition matrix
    update_T(FA[0], RF[0]);

    //apply first pulse
    aux_states(0) = F_plus(0);
    aux_states(1) = F_minus(0);
    aux_states(2) = Z(0);
    aux_states = T * aux_states;
    F_plus(0) = aux_states(0);
    F_minus(0) = aux_states(1);
    Z(0) = aux_states(2);

    //save F_minus_zero >> this is the measured signal
    signal_real[0] = F_minus(0).real();
    signal_imag[0] = F_minus(0).imag();

    //simulate rest of the sequence
    for (int j=1; j<n; j++){

	    //apply E
    	for (int k=0; k<(std::min(j,KMAX)); k++)
    	{
    		F_plus(k) = F_plus(k) * E2;
    		F_minus(k) = F_minus(k) * E2;
    		Z(k) = Z(k) * E1;
    	}
    	Z(0) += (1.0 - E1)*M0[0];

    	//apply S
    	for (int k=(std::min(KMAX-1,j)); k>0; k--){ F_plus(k) = F_plus(k-1); }
    	F_plus(0) = std::conj(F_minus(1));
    	for (int k=0; k<(std::min(KMAX-1,j)); k++){ F_minus(k) = F_minus(k+1); }
    	F_minus(std::min(KMAX-1,j)) = 0.0 + 0.0*i1;

    	//apply T
        update_T(FA[j], RF[j]);
    	for (int k=0; k<(std::min(KMAX,j+1)); k++)
    	{
    		aux_states(0) = F_plus(k);
    		aux_states(1) = F_minus(k);
    		aux_states(2) = Z(k);
    		aux_states = T * aux_states;

    		F_plus(k) = aux_states(0);
    		F_minus(k) = aux_states(1);
    		Z(k) = aux_states(2);
    	}

    	//save F_minus_zero
    	signal_real[j] = F_minus(0).real();
        signal_imag[j] = F_minus(0).imag();

        //save last states
        if(j==(n-1))
        {
            for (int k=0; k<KMAX; k++)
            {
                path_real[3*k] = F_plus(k).real();
                path_real[3*k+1] = F_minus(k).real();
                path_real[3*k+2] = Z(k).real();
                path_imag[3*k] = F_plus(k).imag();
                path_imag[3*k+1] = F_minus(k).imag();
                path_imag[3*k+2] = Z(k).imag();
            }
        }
    }
}

void mexFunction(int nlhs, mxArray *plhs[], int rhs, const mxArray *prhs[])
{
    /* Declare arrays necessary to compute signals */
    double *N;
    double *FA;
    double *RF;
    double *TR;
    double *T1;
    double *T2;
    double *M0;

    double *signal_real;
    double *signal_imag;
    double *path_real;
    double *path_imag;

    /* Check for proper number of arguments */

    /* Create pointers to the inputs */
    N = mxGetPr(prhs[0]);
    FA = mxGetPr(prhs[1]);
    RF = mxGetPr(prhs[2]);
    TR = mxGetPr(prhs[3]);
    T1 = mxGetPr(prhs[4]);
    T2 = mxGetPr(prhs[5]);
    M0 = mxGetPr(prhs[6]);

    /* Create pointers to the outputs */
    plhs[0] = mxCreateDoubleMatrix((mwSize)*N, 1, mxCOMPLEX);
    plhs[1] = mxCreateDoubleMatrix(3, 25, mxCOMPLEX);

    // Check this link to see how to return complex arrays:
    // http://matlab.izmiran.ru/help/techdoc/matlab_external/ch04cre9.html
    signal_real = mxGetPr(plhs[0]);
    signal_imag = mxGetPi(plhs[0]);
    path_real = mxGetPr(plhs[1]);
    path_imag = mxGetPi(plhs[1]);

    /* Call the computational routine */
    EPG_GRE(signal_real, signal_imag, path_real, path_imag, N, FA, RF, TR, T1, T2, M0);

}


// main.cpp -- example use of CppGPs for Gaussian process regression

//
// Eigen macros for using externel routines (may need to be moved to header file)
// ( see https://eigen.tuxfamily.org/dox/TopicUsingBlasLapack.html )
//#define EIGEN_USE_BLAS            // Must also link to BLAS library
//#define EIGEN_USE_LAPACKE_STRICT  // Must also link to LAPACKE library
//#define EIGEN_USE_MKL_ALL         // Must also link to Intel MKL library
#include <iostream>
#include <iomanip>
#include <cmath>
#include <vector>
#include <string>
#include <memory>
#include <random>
#include <chrono>
#include <fstream>
#include <boost/range/irange.hpp>
#include "GPs.h"

#include <limits>

// Specify the target function for Gaussian process regression
double targetFunc(Eigen::MatrixXd X)
{

  if ( X.size() == 1 )
    {
      // Define the target function to be an oscillatory, non-periodic function
      double oscillation = 30.0;
      double xshifted = 0.5*(X(0) + 1.0);
      return std::sin(oscillation*(xshifted-0.1))*(0.5-(xshifted-0.1))*15.0;
    }
  else
    {
      // Define the target function to be the Marr wavelet (a.k.a "Mexican Hat" wavelet)
      // ( see https://en.wikipedia.org/wiki/Mexican_hat_wavelet )
      double sigma = 0.25;
      double pi = std::atan(1)*4;
      double radialTerm = std::pow(X.squaredNorm()/sigma,2);
      return 2.0 / (std::sqrt(pi*sigma) * std::pow(pi,1.0/4.0)) * (1.0 - radialTerm) * std::exp(-radialTerm);
    }
}

// Example use of CppGPs code for Gaussian process regression
int main(int argc, char const *argv[])
{

  // Retrieve aliases from GP namescope
  using Matrix = Eigen::MatrixXd;
  using Vector = Eigen::VectorXd;

  // Convenience using-declarations
  using std::cout;
  using std::endl;
  using GP::GaussianProcess;
  using GP::linspace;
  using GP::sampleNormal;
  using GP::sampleUnif;
  using GP::RBF;

  // Used for timing code with chrono
  using GP::high_resolution_clock;
  using GP::time;
  using GP::getTime;

  // Set random seed based on system clock
  std::srand(static_cast<unsigned int>(high_resolution_clock::now().time_since_epoch().count()));

  // Fix random seed for debugging and testing
  //std::srand(static_cast<unsigned int>(0));

  //
  //   [ Define Training Data ]
  //

  // Specify the input dimensions
  //int inputDim = 1;
  int inputDim = 2;

  // Specify observation data count
  int obsCount;
    if ( inputDim == 1 )
      obsCount = 250;
    else
      obsCount = 1000;

  // Specify observation noise level
  auto noiseLevel = 1.0;

  // Define random noise to add to target observations
  auto noise = sampleNormal(obsCount) * noiseLevel;

  // Define observations by sampling random uniform distribution
  Matrix X = sampleUnif(-1.0, 1.0, obsCount, inputDim);
  Matrix y;  y.resize(obsCount, 1);

  // Define target observations 'y' by applying 'targetFunc'
  // to the input observations 'X' and adding a noise vector
  for ( auto i : boost::irange(0,obsCount) )
    y(i) = targetFunc(X.row(i)) + noise(i);

  //
  //   [ Construct Gaussian Process Model ]
  //

  // Initialize Gaussian process model
  GaussianProcess model;

  // Specify training observations for GP model
  model.setObs(X,y);

  // Fix noise level [ noise level is fit to the training data by default ]
  //model.setNoise(0.33);

  // Initialize a RBF covariance kernel and assign it to the model
  RBF kernel;
  model.setKernel(kernel);

  // Specify hyperparameter bounds  [ including noise bounds (index 0) ]
  // Vector lbs(2); lbs << 0.0001 , 0.01;
  // Vector ubs(2); ubs << 5.0    , 100.0;
  // model.setBounds(lbs, ubs);

  // Specify hyperparameter bounds  [ excluding noise bounds ]
  Vector lbs(1);  lbs <<  0.01;
  Vector ubs(1);  ubs <<  100.0;
  model.setBounds(lbs, ubs);

  // Fit covariance kernel hyperparameters to the training data
  time start = high_resolution_clock::now();
  model.fitModel();
  time end = high_resolution_clock::now();
  auto computationTime = getTime(start, end);

  // Display computation time required for fitting the GP model
  cout << "\nComputation Time: ";
  cout << computationTime << " s" << endl;

  // Retrieve the tuned/optimized kernel hyperparameters
  auto optParams = model.getParams();
  cout << "\nOptimized Hyperparameters:" << endl << optParams.transpose() << "  ";
  auto noiseL = model.getNoise();
  cout << "(Noise = " << noiseL << ")\n" << endl;

  // Display the negative log marginal likelihood (NLML) of the optimized model
  cout << "NLML:  " << std::fixed << std::setprecision(4) << model.computeNLML() << endl << endl;

  //double eps = std::numeric_limits<double>::min();
  //cout << "Epsilon: " << eps << endl;

  //
  //   [ Posterior Predictions and Samples ]
  //

  // Define test mesh for GP model predictions
  int predCount;
  if ( inputDim == 1 )
    predCount = 100;
  else
    predCount = 1000;
  //int predRes = static_cast<int>(std::sqrt(predCount));
  int predRes = static_cast<int>(std::pow(predCount,1.0/inputDim));
  //auto testMesh = linspace(0.0, 1.0, predCount);
  auto testMesh = linspace(-1.0, 1.0, predRes, inputDim);
  model.setPred(testMesh);

  // Reset predCount to account for rounding
  predCount = std::pow(predRes,inputDim);

  // Compute predicted means and variances for the test points
  model.predict();
  Matrix pmean = model.getPredMean();
  Matrix pvar = model.getPredVar();
  Matrix pstd = pvar.array().sqrt().matrix();

  // Get sample paths from the posterior distribution of the model
  int sampleCount;
  Matrix samples;
  if ( inputDim == 1 )
    {
      sampleCount = 25;
      samples = model.getSamples(sampleCount);
    }

  //
  //   [ Save Results for Plotting ]
  //

  // Save true and predicted means/variances to file
  std::string outputFile = "predictions.csv";
  //Matrix trueSoln = testMesh.unaryExpr(std::ptr_fun(targetFunc));

  Matrix trueSoln(predCount,1);
  for ( auto i : boost::irange(0,predCount) )
    trueSoln(i,0) = targetFunc(testMesh.row(i));

  std::ofstream fout;
  fout.open(outputFile);
  for ( auto i : boost::irange(0,predCount) )
    {
      for ( auto j : boost::irange(0,inputDim) )
        fout << testMesh(i,j) << ",";

      fout << trueSoln(i) << "," << pmean(i) << "," << pstd(i) << "\n";
    }
  fout.close();

  // Save observations to file
  std::string outputObsFile = "observations.csv";
  fout.open(outputObsFile);
  for ( auto i : boost::irange(0,obsCount) )
    {
      for ( auto j : boost::irange(0,inputDim) )
        fout << X(i,j) << ",";

      fout << y(i) << "\n";
    }
  fout.close();

  if ( inputDim == 1 )
    {
      // Save samples to file
      std::string outputSampleFile = "samples.csv";
      fout.open(outputSampleFile);
      for ( auto j : boost::irange(0,sampleCount) )
        {
          for ( auto i : boost::irange(0,predCount) )
            fout << samples(i,j) << ((i<predCount-1) ? "," : "\n");
        }
      fout.close();
    }

  return 0;

}


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;
void setZeroMean(mat& A)

{
    int n_rows;
    int D;
    n_rows=A.n_rows;
    cout<<"number row:"<<n_rows<<endl;
    D=A.n_cols;
    cout<<"number col:"<<D<<endl;
    mat Mean=mean(A); // should be 1 row D coln;
    cout<<"Mean ="<<Mean<<endl;
// subtract the mean - use score_out as temporary matrix
    A =A-repmat(mean(A), n_rows, 1);
    return ;
}
int main(int argc, char** argv)
{
    mat A = randu<mat>(4,5); //random matrix 4 rows 5 cols
    setZeroMean(A);
    mat Mean=mean(A);
    cout<<" centered data new Mean ="<<Mean<<endl;

    mat B = randu<mat>(4,5);

//    cout << A*B.t() << endl;

    return 0;
}


//
// diff.cpp
//
// Copyright (c) 2021 Izadori
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

#include <Eigen/Eigen>

template <typename Derived>
const Eigen::MatrixXd Diff(const Eigen::MatrixBase<Derived> & m, const unsigned int n = 1)
{
    Eigen::MatrixXd mr = m;

    for(unsigned int i = 0; i < n; i++)
    {
        mr = (mr.topRows(mr.rows() - 1) - mr.bottomRows(mr.rows() - 1)).eval();
    }

    return mr;
}

template <typename Derived>
const Eigen::SparseMatrix<double> Diff(const Eigen::SparseMatrixBase<Derived> & m, const unsigned int n = 1)
{
    Eigen::SparseMatrix<double> mr = m;

    for(unsigned int i = 0; i < n; i++)
    {
        mr = (mr.topRows(mr.rows() - 1) - mr.bottomRows(mr.rows() - 1)).eval();
    }

    return mr;
}


/**************************************
 * Class Name: PRNG, PrimeGen
 * Function: 伪随机数与伪随机素数比特生成器
 * Introduction: PrimeGen 继承于 PRNG 类
 * ***********************************/

#include "Random.h"
#include "TDES.h"
#include <iostream>
#include <ctime>
#include <cstring>
#include <NTL/ZZ.h>

using namespace std;
using namespace NTL;

typedef unsigned long long size_ul;

/*---------------------------------
 * Class: PRNG
 *-------------------------------*/

// 构造函数，进行初始化操作
PRNG::PRNG(int n) {
    m = n;
    x = new bitset<64>[m];
    s = "";
}

// 析构函数，释放内存空间
PRNG::~PRNG() {
    delete [] x;
}

// 通过外界修改 m 的数值
void PRNG::SetM(int n) {
    m = n;
    delete [] x;
    x = new bitset<64>[m];
}

// 伪随机数比特生成函数
ZZ PRNG::GenerateRandom() {
    // 获取本地时间日期
    time_t now = time(0);
    bitset<64> Date(now);

    // 产生 64 比特随机种子 s
    size_ul ss = size_ul(random()) << 32 + random();
    bitset<64> s(ss);

    TDES td;
    td.plain = Date;
    bitset<64> Im = td.TDESEncrypt();
    for (int i = 0; i < m; i++) {
        td.plain = Im ^ s;
        x[i] = td.TDESEncrypt();
        td.plain = x[i] ^ Im;
        s = td.TDESEncrypt();
    }

    // 生成字符串类型保存
    bitsToString();
    ZZ res = bitsToNumber();
    return res;
}

// 将比特数转化为 ZZ 型数字
ZZ PRNG::bitsToNumber() {
    ZZ res(0);
    for (int i = 0; i < m; i++) {
        string str = x[i].to_string();
        for (int j = 0; j < 64; j++) {
            res = res * 2;
            if (str[j] == '1') {
                res = res + 1;
            }
        }
    }
    return res;
}

// 比特位转为字符串类型（01字符串）
void PRNG::bitsToString() {
    s = "";
    for (int i = 0; i < m; i++) {
        s += x[i].to_string();
    }
}

// 以十六进制打印比特数值
void PRNG::PrintInDER() {
    cout << "modulus:";
    string pairstr = "";
    for (size_t i = 0; i < s.length(); i += 4) {
        int index = 0;

        for (size_t j = i; j < i + 4; j++) {
            index = index << 1;
            if (s[j] == '1') {
                index += 1;
            }
        }
        pairstr += DERTable[index];
        if (pairstr.length() == 2) {
            cout << pairstr;
            if (i + 4 < s.length()) {
                cout << ":";
            }
            pairstr = "";
        }
        if (i % 120 == 0) {
            cout << endl << '\t';
        }
    }
    cout << endl;
}

/*---------------------------------
 * Class: PrimeGen
 *-------------------------------*/

PrimeGen::PrimeGen(int n) : PRNG(n) {
    cout << "Create a pseudorandom number generator.\n";
}

// 生成随机素数，使用 NTL 中库函数检验
// 素性检测算法为 Miller-Rabin 检测
ZZ PrimeGen::GeneratePrime() {
    ZZ res(0);
    // 此处进行 srandom，为产生 64 比特随机种子
    srandom((unsigned)time(NULL));
    do {
        res = GenerateRandom();
    } while(!ProbPrime(res, m * 32));

    return res;
}


/****************************************
 * Creator : Léo Gaspard                *
 * Mail    : leo.gaspard@outlook.fr     *
 * Git     : https://github.com/NehZio  *
 *                                      *
 * Date    : 26/08/2019                 *
 *                                      *
 * File    : main.cpp                   *
 ****************************************/

#include <iostream>
#include <iomanip>
#include <complex>
#include <Eigen/Dense>
#include <eigen3/unsupported/Eigen/MatrixFunctions>

int main()
{
	int norb(3);
	int nvec(3);

	Eigen::MatrixXcd c(nvec,nvec);
	Eigen::MatrixXcd d(norb,nvec);
	Eigen::MatrixXcd p(nvec,nvec);
	Eigen::MatrixXcd s(nvec,nvec);
	Eigen::MatrixXcd invSqrtS(nvec,nvec);
	Eigen::MatrixXcd pON(nvec,nvec);
	Eigen::MatrixXcd heff(nvec,nvec);
	Eigen::MatrixXd e(nvec,nvec);

	Eigen::FullPivLU<Eigen::MatrixXcd> lu;
	Eigen::ComplexEigenSolver<Eigen::MatrixXcd> eig;

	std::cout << std::fixed << std::setprecision(6);
	std::cout << std::endl << "This program will perform Heff calculations using the method described in : \n Chem. Rev. 2014, 114, 1, 429-492" << std::endl;

//*************************************************
/*
 * Manual initialization of c
 */

	c(0,1) = {-0.4867551E+00,0.0000E-10};
	c(0,0) = {-0.2653160E+00,0.0000E-10};
	c(0,2) = { 0.7520712E+00,0.0000E-10};

	c(1,1) = { 0.6126111E+00,0.0000E-10};
	c(1,0) = {-0.7456646E+00,0.0000E-10};
	c(1,2) = { 0.1330535E+00,0.0000E-10};

	c(2,1) = {-0.5748099E+00,0.0000E-10};
	c(2,0) = {-0.5748095E+00,0.0000E-10};
	c(2,2) = {-0.5748079E+00,0.0000E-10};

/*
 * Manual initializaion of e
 */
	e(0,0) =-715.10E+00;
	e(1,1) =-231.20E+00;
	e(2,2) =   0.00E+00;
//*************************************************

	std::cout << std::endl << "c Matrix : " << std::endl << c << std::endl;
	std::cout << std::endl << "e Matrix :  " <<  std::endl << e << std::endl << std::endl;

	/***********************
	 * Calculation of the  *
	 * projection operator *
	 ***********************/

	std::cout << std::endl << "Projection matrix P = |I>*<I| :  " << std::endl;
        d = Eigen::MatrixXcd::Identity(norb,nvec);
	std::cout << std::endl << d << std::endl;

	/**********************
	 * Calculation of the *
	 * projected vectors  *
	 **********************/

	std::cout << std::endl << "Projected vectors into the model space :" << std::endl;
	p = c*d;
	std::cout << std::endl << p << std::endl;

	/***********************
	 * Calculation of the  *
	 * overlap matrix      *
	 ***********************/

	std::cout << std::endl << "Overlap Matrix S = c*c^T: " << std::endl;
	s = p*p.transpose();
	std::cout << std::endl << s << std::endl;

	/***********************
	 * Calculation of the  *
	 * S^(-1/2) matrix     *
	 ***********************/

	lu.compute(s.sqrt());

	if(lu.isInvertible())
	{
		invSqrtS =  lu.inverse();
	}
	else
	{
		std::cout << std::endl << "Error : The square root of the overlap matrix is not invertible" << std::endl;
		return 0;
	}

	std::cout << std::endl << "S^(-1/2) Matrix : " << std::endl;

	std::cout << std::endl << invSqrtS << std::endl;

	/***********************
	 * Calculation of the  *
	 * otrhonormalized     *
	 * projected vectors   *
	 ***********************/

	pON =  invSqrtS*p;
	std::cout << std::endl << "Orthonormalized projected vectors : " << std::endl;
	std::cout << std::endl << pON << std::endl;

	/***********************
	 * Calculation of the  *
	 * effective           *
	 * Hamiltonian         *
	 ***********************/

	heff = pON.transpose()*e*pON.conjugate();

	std::cout << std::endl << "Effective Hamiltonian |S,Ms> basis : " << std::endl;
	std::cout << std::endl << heff << std::endl;

	/***********************
	 * Diagonalization of  *
	 * the effective       *
	 * Hamiltonian         *
	 ***********************/

	eig.compute(heff);

	std::cout << std::endl << "Eigenvalues of the effective Hamiltonian :" << std::endl;
	std::cout << std::endl << eig.eigenvalues() << std::endl;

	std::cout << std::endl << "Eigenvectors of the effective Hamiltonian :" << std::endl;
	std::cout << std::endl << eig.eigenvectors() << std::endl;
	return 0;
}


#include <chrono>
#include <iostream>
#include <stdexcept>
#include <Eigen/Dense>
#include <cmath>

using Eigen::Vector3d;
using std::runtime_error;

namespace lambert {
    struct LambertResults {
        Vector3d v0;
        Vector3d v;
    };

    double c2(double psi) {
        double res;
        auto eps = 1.0;
        if (psi > eps) {
            res = (1 - cos(sqrt(psi))) / psi;
        } else if (psi < -eps) {
            res = (cosh(sqrt(-psi)) - 1) / (-psi);
        } else {
            res = 1.0 / 2.0;
            auto delta = (-psi) / tgamma(2 + 2 + 1);
            auto k = 1;
            while (res + delta != res) {
                res += delta;
                k += 1;
                delta = pow(-psi, k) / tgamma(2*k + 2 + 1);
            }
        }
        return res;
    }

    double c3(double psi) {
        double res;
        auto eps = 1.0;
        if (psi > eps) {
            res = (sqrt(psi) - sin(sqrt(psi))) / (psi * sqrt(psi));
        } else if (psi < -eps) {
            res = (sinh(sqrt(-psi)) - sqrt(-psi)) / (-psi * sqrt(-psi));
        } else {
            res = 1.0 / 6.0;
            auto delta = (-psi) / tgamma(2 + 3 + 1);
            int k = 1;
            while (res + delta != res) {
                res += delta;
                k += 1;
                delta = pow(-psi, k) / tgamma(2*k + 3 + 1);
            }
        }
        return res;
    }

    LambertResults lambert(double k, Vector3d r0, Vector3d r, double tof,
            bool shortway=true, int numiter=35, double rtol=1e-8) {
        int t_m;
        if (shortway) {
            t_m = 1;
        } else {
            t_m = -1;
        }
        auto norm_r0 = r0.norm();
        auto norm_r = r.norm();
        auto cos_dnu = r0.dot(r) / (norm_r * norm_r0);

        auto A = t_m * sqrt(norm_r * norm_r0 * (1 + cos_dnu));

        if (A == 0.0) {
            throw runtime_error("Cannot compute orbit, phase angle is 180 degrees");
        }

        auto psi = 0.0;
        auto psi_low = -4*M_PI;
        auto psi_up = 4*M_PI;

        auto count = 0;
        double y;
        while (count < numiter) {
            y = norm_r0 + norm_r + A * (psi * c3(psi) - 1) / sqrt(c2(psi));
            if (A > 0.0 & y < 0.0) {
                while (y < 0.0) {
                    psi_low = psi;
                    psi = (0.8 * (1.0 / c3(psi)) *
                            (1.0 - (norm_r0 + norm_r) * sqrt(c2(psi)) / A));
                    y = norm_r0 + norm_r + A * (psi * c3(psi) - 1) / sqrt(c2(psi));
                }
            }
            auto xi = sqrt(y / c2(psi));
            auto tof_new = (pow(xi, 3) * c3(psi) + A * sqrt(y)) / sqrt(k);

            if (fabs((tof_new - tof) / tof) < rtol) {
                break;
            } else {
                count += 1;
                if (tof_new <= tof) {
                    psi_low = psi;
                } else {
                    psi_up = psi;
                }
                psi = (psi_up + psi_low) / 2;
            }
        }
        if (count > numiter) {
            throw runtime_error("Maximum number of iterations reached");
        }
        auto f = 1 - y / norm_r0;
        auto g = A * sqrt(y / k);
        auto gdot = 1 - y / norm_r;
        auto v0 = (r - f * r0) / g;
        auto v = (gdot * r - r0) / g;
        LambertResults results = {v, v0};
        return results;
    }

    void benchmark(int times) {
        auto mu = 3.986004418e5;
        Vector3d r0(5000.0, 10000.0, 2100.0);
        Vector3d r(-14600.0, 2500.0, 7000.0);
        auto tof = 3600.0;

        auto best = std::numeric_limits<double>::infinity();
        auto worst = -std::numeric_limits<double>::infinity();
        double all = 0;
        for (auto i=0; i < times; i++) {
            auto begin = std::chrono::high_resolution_clock::now();

            lambert(mu, r0, r, tof);

            auto end = std::chrono::high_resolution_clock::now();
            auto current = std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count()/1e9;
            all += current;
            if (current < best) {
                best = current;
            }
            if (current > worst) {
                worst = current;
            }
        }
        std::cout << "[" << all/times << "," << best << "," << worst << "]" << std::endl;
    }
}


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

using namespace Eigen;
using namespace std;
MatrixXf computeLinkPos(MatrixXf L, MatrixXf P, Matrix3f R, MatrixXf s, MatrixXf pb);
MatrixXf computeActuation(MatrixXf L, float lc);

MatrixXf computeSliderControl(float re,float rb,float lc,float D,float Pz,float xd, float yd, float zd,float phi,float theta,float psi) {
// float* computeSliderControl(float re,float rb,float lc,float D,float Pz,float xd, float yd, float zd,float phi,float theta,float psi) {
  // void computeSliderControl(float re,float rb,float lc,float D,float Pz,float xd, float yd, float zd,float phi,float theta,float psi) {
  // - スライダー間距離D[mm]        : 56+(12/2)*2=68mm
  // - ロッド長さlc[mm]                  : 262+(8/2)*2=268mm
  // - スライダー設置半径rb[mm]    : sqrt(269.7^2-200.5^2)=180.38248
  // - テーブル半径re[mm]               :90mm
  // - テーブル初期高さPz[mm]       :438.17mm
  // float re = 90;
  // float Pz = 438.17;
  // float D = 68;
  // float lc = 268;
  // float rb = 180.38;

  float th = asin(D/(2*rb)); //% theta1: angle (linear actuator)
  float th2 = asin(D/(2*re)); //% theta2: angle (end effector)

  MatrixXf pb(6,3);
  pb <<
    rb*cos(th),rb*sin(th),0,
    rb*cos(-th),rb*sin(-th),0,
    rb*cos(2*M_PI/3+th),rb*sin(2*M_PI/3+th),0,
    rb*cos(2*M_PI/3-th),rb*sin(2*M_PI/3-th),0,
    rb*cos(4*M_PI/3+th),rb*sin(4*M_PI/3+th),0,
    rb*cos(4*M_PI/3-th),rb*sin(4*M_PI/3-th),0;

  MatrixXf s_local(6,3);
  s_local <<
    re*cos(th2),re*sin(th2),0,
    re*cos(-th2),re*sin(-th2),0,
    re*cos(2*M_PI/3+th2),re*sin(2*M_PI/3+th2),0,
    re*cos(2*M_PI/3-th2),re*sin(2*M_PI/3-th2),0,
    re*cos(4*M_PI/3+th2),re*sin(4*M_PI/3+th2),0,
    re*cos(4*M_PI/3-th2),re*sin(4*M_PI/3-th2),0;

  MatrixXf sliders(6,3);
  sliders <<
    rb*cos(th),rb*sin(th),Pz,
    rb*cos(-th),rb*sin(-th),Pz,
    rb*cos(2*M_PI/3+th),rb*sin(2*M_PI/3+th),Pz,
    rb*cos(2*M_PI/3-th),rb*sin(2*M_PI/3-th),Pz,
    rb*cos(4*M_PI/3+th),rb*sin(4*M_PI/3+th),Pz,
    rb*cos(4*M_PI/3-th),rb*sin(4*M_PI/3-th),Pz;

  // float phi = M_PI/24; //% rotation around X axis
  // float theta = M_PI/12; //% rotation around Y axis
  // float psi = M_PI/16; //% rotation around Z axis

  Matrix3f R;
  R <<
    cos(phi)*cos(theta),cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi),cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi),
    sin(phi)*cos(theta),sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi),sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi),
    -sin(theta),cos(theta)*sin(psi),cos(theta)*cos(psi);

  Vector3f a;
  a << 0,0,1;

  MatrixXf s(6,3);
  s = s_local*R;//6x3 x 3x3 => 6x3

  MatrixXf P(1,3);
  P << xd,yd,zd; //% Position Vector of the end effector
  // P << 50,50,height; //% Position Vector of the end effector

  // cout << "*** re: " << endl << s << endl;
  // cout << "*** rb: " << endl << rb << endl;
  // cout << "*** pb:  " << endl << pb << endl;
  // cout << "*** R:  " << endl<< R << endl;

  MatrixXf L(6,3);
  L << computeLinkPos(L, P, R, s, pb);
  // cout << "*** L:  " << endl << L << endl;

  MatrixXf C(1,6);
  C << computeActuation(L,lc);
  cout << "*** C:  " << endl << C << endl;

  return C;
  // float* arrayf = C.data();
}

// %% compute position vector of rod end of end effector
MatrixXf computeLinkPos(MatrixXf L, MatrixXf P, Matrix3f R, MatrixXf s, MatrixXf pb){
  // L = P + R*s - pb;
  for(int i=0;i<6;i++){
    L.block(i,0,1,3) << P + (s.block(i,0,1,3))*R - pb.block(i,0,1,3);
  }
  return L;
}

MatrixXf computeActuation(MatrixXf L, float lc){
  MatrixXf C(1,6);
  for (int i=0;i<6;i++){
    float lxi = L(i,0);
    float lyi = L(i,1);
    float lzi = L(i,2);
    C.block(0,i,1,1) << lzi - sqrt(lc*lc-lxi*lxi-lyi*lyi);
  }
  return C;
}

extern "C" {
  void solveInverseMechanism(float* arrayf, float re,float rb,float lc,float D,float Pz,float xd, float yd, float zd,float phi,float theta,float psi){
    MatrixXf C(1,6);
    C << computeSliderControl(re,rb,lc,D,Pz,xd,yd,zd,phi,theta,psi);
    memcpy(arrayf,C.data(),C.size()*sizeof(float));
  }
}


#ifndef CANNON_MATH_MULTIVARIATE_NORMAL_H
#define CANNON_MATH_MULTIVARIATE_NORMAL_H

/*!
 * \file cannon/math/multivariate_normal.hpp
 * \brief File containing MultivariateNormal class definition.
 */

#include <random>

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace math {

    /*!
     * \brief Class representing a multivariate normal distribution of
     * arbitrary dimensionality.
     */
    class MultivariateNormal {
      public:
        MultivariateNormal() = delete;

        /*!
         * \brief Constructor taking a covariance matrix. The distribution is
         * assumed to be centered at zero.
         */
        MultivariateNormal(const Ref<const MatrixXd>& covar) :
          MultivariateNormal(VectorXd::Zero(covar.rows()), covar) {}

        /*!
         * \brief Constructor taking a mean and covariance matrix.
         */
        MultivariateNormal(const Ref<const VectorXd> &mean, const Ref<const MatrixXd> &covar)
            : mean_(mean),
              transform_(MatrixXd::Zero(mean.size(), mean.size())) {
          SelfAdjointEigenSolver<MatrixXd> eigen_solver(covar);
          transform_ = eigen_solver.eigenvectors() *
            eigen_solver.eigenvalues().cwiseSqrt().asDiagonal();
        }

        /*!
         * \brief Sample a point from this distribution.
         *
         * \returns A sample from the multivariate normal distribution
         * represented by this object.
         */
        VectorXd sample() const {
          return mean_ + (transform_ * VectorXd::NullaryExpr(mean_.size(),
                [&](){return dist(gen);}));
        }

        /*!
         * \brief Set the seed for all normal distributions.
         *
         * \param seed The new seed to use.
         */
        static void set_seed(int seed);

        static std::random_device rd; //!< Physical random number generator.
        static std::mt19937 gen; //!< Pseudo-random number generator.
        static std::normal_distribution<double> dist; //!< 1D normal distribution for sampling

      private:
        VectorXd mean_; //!< Mean of this normal distribution
        MatrixXd transform_; //!< Transform used to compute multivariate samples

    };

  }
}

#endif /* ifndef CANNON_MATH_MULTIVARIATE_NORMAL_H */


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

#ifndef SMOOTH__INTERNAL__SO3_HPP_
#define SMOOTH__INTERNAL__SO3_HPP_

#include <Eigen/Core>

#include "common.hpp"

namespace smooth {

/**
 * @brief SO(3) Lie group represented as S^3
 *
 * Memory layout
 * -------------
 * Group:    qx qy qz qw  (same as Eigen quaternion)
 * Tangent:  Ωx Ωy Ωz
 *
 * Lie group Matrix form
 * ---------------------
 * 3x3 rotation matrix
 *
 * Lie algebra Matrix form
 * -----------------------
 * [  0 -Ωz  Ωy ]
 * [  Ωz  0 -Ωx ]
 * [ -Ωy Ωx   0 ]
 *
 * Constraints
 * -----------
 * Group:   qx * qx + qy * qy + qz * qz + qw * qw = 1
 * Tangent: -pi < Ωx, Ωy, Ωz <= pi
 */
template<typename _Scalar>
class SO3Impl
{
public:
  using Scalar = _Scalar;

  static constexpr Eigen::Index RepSize = 4;
  static constexpr Eigen::Index Dim     = 3;
  static constexpr Eigen::Index Dof     = 3;
  static constexpr bool IsCommutative   = false;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out) { g_out << Scalar(0), Scalar(0), Scalar(0), Scalar(1); }

  static void setRandom(GRefOut g_out)
  {
    g_out = Eigen::Quaternion<Scalar>::UnitRandom().coeffs();
    if (g_out[3] < Scalar(0)) { g_out *= Scalar(-1); }
  }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    Eigen::Map<const Eigen::Quaternion<Scalar>> q(g_in.data());
    m_out = q.toRotationMatrix();
  }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    Eigen::Map<const Eigen::Quaternion<Scalar>> q1(g_in1.data());
    Eigen::Map<const Eigen::Quaternion<Scalar>> q2(g_in2.data());
    g_out = (q1 * q2).coeffs();
    if (g_out[3] < Scalar(0)) { g_out *= Scalar(-1); }
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    Eigen::Map<const Eigen::Quaternion<Scalar>> q(g_in.data());
    g_out = q.inverse().coeffs();
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    using std::atan2, std::sqrt;
    const Scalar xyz2 = g_in[0] * g_in[0] + g_in[1] * g_in[1] + g_in[2] * g_in[2];

    const auto phi = [&]() -> Scalar {
      if (xyz2 < Scalar(eps2)) {
        // https://www.wolframalpha.com/input/?i=series+atan%28y%2Fx%29+%2F+y+at+y%3D0
        return Scalar(2) / g_in[3] - Scalar(2) * xyz2 / (Scalar(3) * g_in[3] * g_in[3] * g_in[3]);
      } else {
        const Scalar xyz = sqrt(xyz2);
        return Scalar(2) * atan2(xyz, g_in[3]) / xyz;
      }
    }();

    a_out << g_in[0], g_in[1], g_in[2];
    a_out *= phi;
  }

  static void Ad(GRefIn g_in, TMapRefOut A_out)
  {
    Eigen::Map<const Eigen::Quaternion<Scalar>> q(g_in.data());
    A_out = q.toRotationMatrix();
  }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    using std::sqrt, std::cos, std::sin;

    const Scalar th2 = a_in.squaredNorm();

    const auto [A, B] = [&]() -> std::array<Scalar, 2> {
      if (th2 < Scalar(eps2)) {
        return {
          // https://www.wolframalpha.com/input/?i=series+sin%28x%2F2%29%2Fx+at+x%3D0
          Scalar(1) / Scalar(2) - th2 / Scalar(48),
          // https://www.wolframalpha.com/input/?i=series+cos%28x%2F2%29+at+x%3D0
          Scalar(1) - th2 / Scalar(8),
        };
      } else {
        const Scalar th = sqrt(th2);
        return {
          sin(th / Scalar(2)) / th,
          cos(th / Scalar(2)),
        };
      }
    }();

    g_out << A * a_in.x(), A * a_in.y(), A * a_in.z(), B;
    if (g_out[3] < Scalar(0)) { g_out *= Scalar(-1); }
  }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out << Scalar(0), -a_in(2), a_in(1), a_in(2), Scalar(0), -a_in(0), -a_in(1), a_in(0),
      Scalar(0);
  }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    a_out << (A_in(2, 1) - A_in(1, 2)) / Scalar(2), (A_in(0, 2) - A_in(2, 0)) / Scalar(2),
      (A_in(1, 0) - A_in(0, 1)) / Scalar(2);
  }

  static void ad(TRefIn a_in, TMapRefOut A_out) { hat(a_in, A_out); }

  static void dr_exp(TRefIn a_in, TMapRefOut A_out)
  {
    using std::sqrt, std::sin, std::cos;
    const Scalar th2 = a_in.squaredNorm();

    const auto [A, B] = [&]() -> std::array<Scalar, 2> {
      if (th2 < Scalar(eps2)) {
        return {
          // https://www.wolframalpha.com/input/?i=series+%281-cos+x%29+%2F+x%5E2+at+x%3D0
          Scalar(1) / Scalar(2) - th2 / Scalar(24),
          // https://www.wolframalpha.com/input/?i=series+%28x+-+sin%28x%29%29+%2F+x%5E3+at+x%3D0
          Scalar(1) / Scalar(6) - th2 / Scalar(120),
        };
      } else {
        const Scalar th = sqrt(th2);
        return {
          (Scalar(1) - cos(th)) / th2,
          (th - sin(th)) / (th2 * th),
        };
      }
    }();

    using TangentMap = Eigen::Matrix<Scalar, Dof, Dof>;
    TangentMap ad_a;
    ad(a_in, ad_a);
    A_out.noalias() = TangentMap::Identity() - A * ad_a + B * ad_a * ad_a;
  }

  static void dr_expinv(TRefIn a_in, TMapRefOut A_out)
  {
    using std::sqrt, std::sin, std::cos;
    const Scalar th2 = a_in.squaredNorm();

    const auto A = [&]() -> Scalar {
      if (th2 < Scalar(eps2)) {
        // https://www.wolframalpha.com/input/?i=series+1%2Fx%5E2-%281%2Bcos+x%29%2F%282*x*sin+x%29+at+x%3D0
        return Scalar(1) / Scalar(12) + th2 / Scalar(720);
      } else {
        const Scalar th = sqrt(th2);
        return Scalar(1) / th2 - (Scalar(1) + cos(th)) / (Scalar(2) * th * sin(th));
      }
    }();

    using TangentMap = Eigen::Matrix<Scalar, Dof, Dof>;
    TangentMap ad_a;
    ad(a_in, ad_a);
    A_out.noalias() = TangentMap::Identity() + ad_a / Scalar(2) + A * ad_a * ad_a;
  }

  static void d2r_exp(TRefIn a_in, THessRefOut H_out)
  {
    const auto [A, B, dA_over_th, dB_over_th] = [&]() -> std::array<Scalar, 4> {
      using std::sqrt;

      const Scalar th2 = a_in.squaredNorm();
      const Scalar th  = sqrt(th2);

      if (th2 < Scalar(eps2)) {
        return {
          Scalar(0.5) - th2 / 24,
          Scalar(1. / 6) - th2 / 120,
          -Scalar(1) / 48,
          -Scalar(1) / 60,
        };
      } else {
        const Scalar sTh = sin(th);
        const Scalar cTh = cos(th);
        const Scalar th3 = th2 * th;
        const Scalar th4 = th2 * th2;
        const Scalar th5 = th3 * th2;
        return {
          (Scalar(1) - cTh) / th2,
          (th - sTh) / th3,
          sTh / th3 + 2 * cTh / th4 - 2 / th4,
          -cTh / th4 - 2 / th4 + 3 * sTh / th5,
        };
      }
    }();

    // -A * d(ad) + B * d(ad^2)
    // clang-format off
    H_out <<
      0., -2 * B * a_in.y(), -2 * B * a_in.z(), B * a_in.y(), B * a_in.x(), -A, B * a_in.z(), A, B * a_in.x(),
      B * a_in.y(), B * a_in.x(), A, -2 * B * a_in.x(), 0., -2 * B * a_in.z(), -A, B * a_in.z(), B * a_in.y(),
      B * a_in.z(), -A, B * a_in.x(), A, B * a_in.z(), B * a_in.y(), -2 * B * a_in.x(), -2 * B * a_in.y(), 0.;
    // clang-format on

    // add -dA * ad + dB * ad^2
    Eigen::Matrix3<Scalar> ad_a;
    ad(a_in, ad_a);
    const Eigen::Matrix3<Scalar> ad_a2 = ad_a * ad_a;

    for (auto i = 0u; i < 3; ++i) {
      const Scalar dA_dxi = dA_over_th * a_in(i);
      const Scalar dB_dxi = dB_over_th * a_in(i);
      for (auto j = 0u; j < 3; ++j) {
        H_out.col(i + 3 * j) -= dA_dxi * ad_a.row(j).transpose();
        H_out.col(i + 3 * j) += dB_dxi * ad_a2.row(j).transpose();
      }
    }
  }

  static void d2r_expinv(TRefIn a_in, THessRefOut H_out)
  {
    const auto [A, dA_over_th] = [&]() -> std::array<Scalar, 2> {
      using std::sqrt;

      const Scalar th2 = a_in.squaredNorm();
      const Scalar th  = sqrt(th2);
      if (th2 < Scalar(eps2)) {
        return {
          Scalar(1) / Scalar(12) + th2 / Scalar(720),
          Scalar(1) / Scalar(360),
        };
      } else {
        const Scalar th3 = th2 * th;
        const Scalar th4 = th2 * th2;
        const Scalar sTh = sin(th);
        const Scalar cTh = cos(th);
        return {
          Scalar(1) / th2 - (Scalar(1) + cTh) / (Scalar(2) * th * sTh),
          1 / (2 * th2) + cTh * cTh / (2 * th2 * sTh * sTh) + cTh / (2 * th2 * sTh * sTh)
            + cTh / (2 * th3 * sTh) + 1 / (2 * th3 * sTh) - 2 / th4,
        };
      }
    }();

    // A * d(ad^2)
    // clang-format off
    H_out <<
      0, -2 * A * a_in.y(), -2 * A * a_in.z(), A * a_in.y(), A * a_in.x(), 0.5, A * a_in.z(), -0.5, A * a_in.x(),
      A * a_in.y(), A * a_in.x(), -0.5, -2 * A * a_in.x(), 0, -2 * A * a_in.z(), 0.5, A * a_in.z(), A * a_in.y(),
      A * a_in.z(), 0.5, A * a_in.x(), -0.5, A * a_in.z(), A * a_in.y(), -2 * A * a_in.x(), -2 * A * a_in.y(), 0;
    // clang-format on

    // add dA * ad^2
    Eigen::Matrix3<Scalar> ad_a;
    ad(a_in, ad_a);
    const Eigen::Matrix3<Scalar> ad_a2 = ad_a * ad_a;
    for (auto i = 0u; i < 3; ++i) {
      const Scalar dA_dxi = dA_over_th * a_in(i);
      for (auto j = 0u; j < 3; ++j) { H_out.col(i + 3 * j) += dA_dxi * ad_a2.row(j).transpose(); }
    }
  }
};

}  // namespace smooth

#endif  // SMOOTH__INTERNAL__SO3_HPP_


// inverse_chi_squared_distribution_example.cpp

// Copyright Paul A. Bristow 2010.
// Copyright Thomas Mang 2010.

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

// Example 1 of using inverse chi squared distribution
#include <boost/math/distributions/inverse_chi_squared.hpp>
using boost::math::inverse_chi_squared_distribution;  // inverse_chi_squared_distribution.
using boost::math::inverse_chi_squared; //typedef for nverse_chi_squared_distribution double.

#include <iostream>
using std::cout;    using std::endl;
#include <iomanip>
using std::setprecision;
using std::setw;
#include <cmath>
using std::sqrt;

template <class RealType>
RealType naive_pdf1(RealType df, RealType x)
{ // Formula from Wikipedia http://en.wikipedia.org/wiki/Inverse-chi-square_distribution
  // definition 1 using tgamma for simplicity as a check.
   using namespace std; // For ADL of std functions.
   using boost::math::tgamma;
   RealType df2 = df / 2;
   RealType result = (pow(2., -df2) * pow(x, (-df2 -1)) * exp(-1/(2 * x) ) )
      / tgamma(df2);  //
   return result;
}

template <class RealType>
RealType naive_pdf2(RealType df, RealType x)
{ // Formula from Wikipedia http://en.wikipedia.org/wiki/Inverse-chi-square_distribution
  // Definition 2, using tgamma for simplicity as a check.
  // Not scaled, so assumes scale = 1 and only uses nu aka df.
   using namespace std; // For ADL of std functions.
   using boost::math::tgamma;
   RealType df2 = df / 2;
   RealType result = (pow(df2, df2) * pow(x, (-df2 -1)) * exp(-df/(2*x) ) )
     / tgamma(df2);
   return result;
}

template <class RealType>
RealType naive_pdf3(RealType df, RealType scale, RealType x)
{ // Formula from Wikipedia http://en.wikipedia.org/wiki/Scaled-inverse-chi-square_distribution
  // *Scaled* version, definition 3, df aka nu, scale aka sigma^2
  // using tgamma for simplicity as a check.
   using namespace std; // For ADL of std functions.
   using boost::math::tgamma;
   RealType df2 = df / 2;
   RealType result = (pow(scale * df2, df2) * exp(-df2 * scale/x) )
     / (tgamma(df2) * pow(x, 1+df2));
   return result;
}

template <class RealType>
RealType naive_pdf4(RealType df, RealType scale, RealType x)
{ // Formula from http://mathworld.wolfram.com/InverseChi-SquaredDistribution.html
  // Weisstein, Eric W. "Inverse Chi-Squared Distribution." From MathWorld--A Wolfram Web Resource.
  // *Scaled* version, definition 3, df aka nu, scale aka sigma^2
  // using tgamma for simplicity as a check.
   using namespace std; // For ADL of std functions.
   using boost::math::tgamma;
   RealType nu = df; // Wolfram uses greek symbols nu,
   RealType xi = scale; // and xi.
   RealType result =
     pow(2, -nu/2) *  exp(- (nu * xi)/(2 * x)) * pow(x, -1-nu/2) * pow((nu * xi), nu/2)
     / tgamma(nu/2);
   return result;
}

int main()
{

  cout << "Example (basic) using Inverse chi squared distribution. " << endl;

  // TODO find a more practical/useful example.  Suggestions welcome?

#ifdef BOOST_NO_CXX11_NUMERIC_LIMITS
  int max_digits10 = 2 + (boost::math::policies::digits<double, boost::math::policies::policy<> >() * 30103UL) / 100000UL;
  cout << "BOOST_NO_CXX11_NUMERIC_LIMITS is defined" << endl;
#else
  int max_digits10 = std::numeric_limits<double>::max_digits10;
#endif
  cout << "Show all potentially significant decimal digits std::numeric_limits<double>::max_digits10 = "
    << max_digits10 << endl;
  cout.precision(max_digits10); //

  inverse_chi_squared ichsqdef; // All defaults  - not very useful!
  cout << "default df = " << ichsqdef.degrees_of_freedom()
    << ", default scale = " <<  ichsqdef.scale() << endl; //  default df = 1, default scale = 0.5

   inverse_chi_squared ichsqdef4(4); // Unscaled version, default scale = 1 / degrees_of_freedom
   cout << "default df = " << ichsqdef4.degrees_of_freedom()
    << ", default scale = " <<  ichsqdef4.scale() << endl; //  default df = 4, default scale = 2

   inverse_chi_squared ichsqdef32(3, 2); // Scaled version, both degrees_of_freedom and scale specified.
   cout << "default df = " << ichsqdef32.degrees_of_freedom()
    << ", default scale = " <<  ichsqdef32.scale() << endl; // default df = 3, default scale = 2

  {
    cout.precision(3);
    double nu = 5.;
    //double scale1 = 1./ nu; // 1st definition sigma^2 = 1/df;
    //double scale2 = 1.; // 2nd definition sigma^2 = 1
    inverse_chi_squared ichsq(nu, 1/nu); // Not scaled
    inverse_chi_squared sichsq(nu, 1/nu); // scaled

    cout << "nu = " << ichsq.degrees_of_freedom() << ", scale = " << ichsq.scale() << endl;

    int width = 8;

    cout << "     x        pdf      pdf1   pdf2  pdf(scaled)    pdf       pdf      cdf     cdf" << endl;
    for (double x = 0.0; x < 1.; x += 0.1)
    {
      cout
        << setw(width) << x
        << ' ' << setw(width) << pdf(ichsq, x) // unscaled
        << ' ' << setw(width) << naive_pdf1(nu,  x) // Wiki def 1 unscaled matches graph
        << ' ' << setw(width) << naive_pdf2(nu,  x) // scale = 1 - 2nd definition.
        << ' ' << setw(width) << naive_pdf3(nu, 1/nu, x) // scaled
        << ' ' << setw(width) << naive_pdf4(nu, 1/nu, x) // scaled
        << ' ' << setw(width) << pdf(sichsq, x)  // scaled
        << ' ' << setw(width) << cdf(sichsq, x)  // scaled
        << ' ' << setw(width) << cdf(ichsq, x)  // unscaled
       << endl;
    }
  }

  cout.precision(max_digits10);

  inverse_chi_squared ichisq(2., 0.5);
  cout << "pdf(ichisq, 1.) = " << pdf(ichisq, 1.) << endl;
  cout << "cdf(ichisq, 1.) = " << cdf(ichisq, 1.) << endl;

  return 0;
}  // int main()

/*

Output is:
 Example (basic) using Inverse chi squared distribution.
  Show all potentially significant decimal digits std::numeric_limits<double>::max_digits10 = 17
  default df = 1, default scale = 1
  default df = 4, default scale = 0.25
  default df = 3, default scale = 2
  nu = 5, scale = 0.2
       x        pdf      pdf1   pdf2  pdf(scaled)    pdf       pdf      cdf     cdf
         0        0    -1.#J    -1.#J    -1.#J    -1.#J        0        0        0
       0.1     2.83     2.83 3.26e-007     2.83     2.83     2.83   0.0752   0.0752
       0.2     3.05     3.05  0.00774     3.05     3.05     3.05    0.416    0.416
       0.3      1.7      1.7    0.121      1.7      1.7      1.7    0.649    0.649
       0.4    0.941    0.941    0.355    0.941    0.941    0.941    0.776    0.776
       0.5    0.553    0.553    0.567    0.553    0.553    0.553    0.849    0.849
       0.6    0.345    0.345    0.689    0.345    0.345    0.345    0.893    0.893
       0.7    0.227    0.227    0.728    0.227    0.227    0.227    0.921    0.921
       0.8    0.155    0.155    0.713    0.155    0.155    0.155     0.94     0.94
       0.9     0.11     0.11    0.668     0.11     0.11     0.11    0.953    0.953
         1   0.0807   0.0807     0.61   0.0807   0.0807   0.0807    0.963    0.963
  pdf(ichisq, 1.) = 0.30326532985631671
  cdf(ichisq, 1.) = 0.60653065971263365

*/


#include "core/BezierCurve.h"

#include "core/Util.h"
#include <boost/math/quadrature/trapezoidal.hpp>

BezierCurve::BezierCurve(const vector_t& control0, const vector_t& control1, const vector_t& control2, const vector_t& control3)
    : _control0(control0), _control1(control1), _control2(control2), _control3(control3) {

}

vector_t BezierCurve::Position(float t) const {
  if (t <= 0) {
    return _control0;
  } else if (t >= 1) {
    return _control3;
  } else {
    return (1 - t) * (1 - t) * (1 - t) * _control0 +
      3 * (1 - t) * (1 - t) * t * _control1 +
      3 * (1 - t) * t * t * _control2 +
      t * t * t * _control3;
  }
}

vector_t BezierCurve::Heading(float t) const {
  if (t <= 0) {
    return (_control1 - _control0).normalized();
  } else if (t >= 1) {
    return (_control3 - _control2).normalized();
  } else {
    return (3 * (1 - t) * (1 - t) * (_control1 - _control0) +
      6 * (1 - t) * t * (_control2 - _control1) +
      3 * t * t * (_control3 - _control2)).normalized();
  }
}

float BezierCurve::Length(float start, float end) const {
  if (start >= end) {
    return 0;
  } else {
    return boost::math::quadrature::trapezoidal(
        [this](float t) {
          vector_t grad = -3 * (1 - t) * (1 - t) * _control0 +
              3 * (1 - t) * (1 - 3 * t) * _control1 +
              3 * t * (2 - 3 * t) * _control2 +
              3 * t * t * _control3;
          return grad.norm();
        },
        std::max(start, 0.0f), std::min(end, 1.0f), 1e-12f);
  }
}


/* MonteCarlo.hpp
-Description:
	*Declare and define MonteCarlo template class that calculates the price of a financial option using the Euler-Maruyama scheme.
-State Objects:
	*variate_generator rng: Boost implemented normal random number generator for Monte Carlo path generation.
-Class Methods:
	// Constructors/Destructors:
	*MonteCarlo(double t, double s, double k, double b, double r, double sigma, unsigned long NSIM, unsigned long NT, bool isCall): Overloaded Constructor.
	*MonteCarlo(const MonteCarlo&): Copy constructor.
	*~MonteCarlo(): Destructor.
	// Accessors:
	*unsigned long NSIM() const: Return the number of trials to be run in Monte Carlo.
	*unsigned long NT() const: Return number of sub-intervals in time to be used in generating each path.
	// Mutators:
	*void NSIM(unsigned long): Set number of trials.
	*void NT(unsigned long): Set number of time sub-intervals.
	// Misc. Methods:
	*double Price() const: Calculate price of associated option using Monte Carlo method with Euler-Maruyama scheme.
*/

#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <ctime>
#include <cmath>
#include <string>
#include <vector>
#include "MonteCarlo.hpp"
#include "MonteCarloExcept.hpp"
#include "Option.hpp"
#include "OptionExcept.hpp"

namespace Options
{
	////////////////////////////
	// Constructors/Destructor:
	////////////////////////////
	MonteCarlo::MonteCarlo(double t, double s, double k, double b, double r, double sigma, bool isCall, unsigned long NSIM_in, unsigned long NT_in) : /* Overloaded Constructor. Set all parameters. */
		Option(t, s, k, b, r, sigma, isCall), nT(NT_in), nSIM(NSIM_in)
	{
		mainRNG = new boost::variate_generator<boost::mt19937, boost::normal_distribution<double> >(boost::mt19937(time(0)), boost::normal_distribution<>(0, 1));
	}
	MonteCarlo::MonteCarlo(const MonteCarlo &in) : Option(in), nT(in.nT), nSIM(in.nSIM) /* Copy Constructor. */
	{
			mainRNG = new boost::variate_generator<boost::mt19937, boost::normal_distribution<double> >(boost::mt19937(time(0)), boost::normal_distribution<>(0, 1));
	}
	MonteCarlo::MonteCarlo(const Option &in, unsigned long NSIM, unsigned long NT): 							/* Copy Constructor 2, with NT and NSIM setting*/
		Option(in), nSIM(NSIM), nT(NT)
	{
		mainRNG = new boost::variate_generator<boost::mt19937, boost::normal_distribution<double> >(boost::mt19937(time(0)), boost::normal_distribution<>(0, 1));
	}
	MonteCarlo::~MonteCarlo()													/* Destructor. */
	{
		delete mainRNG;
	}
	////////////////////////////
	// Accessors:
	////////////////////////////
	unsigned long MonteCarlo::NSIM() const							/* Return number of simulations run in the Monte Carlo. */
	{
		return nSIM;
	}
	unsigned long MonteCarlo::NT() const								/* Return number of time intervals run in the Monte Carlo path generation. */
	{
		return nT;
	}
	////////////////////////////
	// Mutators:
	////////////////////////////
	void MonteCarlo::NSIM(unsigned long NSIM_in)						/* Set the number of simulations to run in the Monte Carlo. */
	{
		nSIM = NSIM_in;
	}
	void MonteCarlo::NT(unsigned long NT_in)							/* Set the number of time intervals to run in the Monte Carlo path generation. */
	{
		nT = NT_in;
	}
	////////////////////////////
	// Misc. Methods:
	////////////////////////////
	double MonteCarlo::Price() const		/* Get price of option using Monte Carlo method with Euler-Maruyama pdf. */
	{
		// NSIM is number of paths to generate, 1/NT is the time divisor (discretizes "t" into number of subintervals).
		// As NSIM AND NT approach infinity, GeneratePrice approaches the exact price given by the Black-Scholes equation or the American Perpetual Option pricing method.
		double S, output = 0, payoff = 0;
		for (unsigned long currTrial = 0; currTrial < nSIM; currTrial++)
		{
			// Reset the path:
			S = Param("s");
			// Generate path using Euler-Maruyama partial differentiation scheme:
			for (double currTimeInterval = 0; currTimeInterval < Param("t"); currTimeInterval += 1.0 / (double) nT)
			{
				S += (Param("r") * S * (((double) 1.0 / (double) nT))) + (Param("sigma") * S * std::sqrt((double) 1.0 / (double) nT) * (*mainRNG)());
			}
			// Calculate payoff of option at expiry depending on type:
			if (Option::Type())
			{
				payoff = std::max(S - Param("k"), 0.0);
			}
			else
			{
				payoff = std::max(Param("k") - S, 0.0);
			}
			// Discount the payoff back to present and accumulate:
			output += payoff / (double) nSIM;
		}
		// Return the average discounted payoff:
		return output * exp(-Param("r") * Param("t"));
	}
	////////////////////////////
	// Overloaded Operators:
	////////////////////////////
	MonteCarlo& MonteCarlo::operator=(const MonteCarlo &in)									/* Assignment Operator. */
	{
		if (this != &in)
		{
			Option::operator=(in);
		}
		return *this;
	}
}


// EigenConsoleApplication1.cpp : Defines the entry point for the console application.
//

//#include "stdafx.h"
#include <ctime>
#include <Eigen/Dense>
#include <iostream>
#include <complex>      // std::complex

//using Eigen::MatrixXd;
//using Eigen::Matrix3cd;

void rf_pulse_matrix(Eigen::Matrix3cd *rfpulse, double alpha, double phi)
{
	const double PI = 3.1415926535897932384626433832795;

	double alpha_half = alpha / 2.0;
	double c_a_h = cos(alpha_half);
	double c_a_h2 = c_a_h * c_a_h;
	double s_a_h = sin(alpha_half);
	double s_a_h2 = s_a_h * s_a_h;
	double c_a = cos(alpha);
	double s_a = sin(alpha);

	std::complex<double> i_phi = std::complex<double>(0.0, phi);
	std::complex<double> i_half = std::complex<double>(0.0, 0.5);

	(*rfpulse)(0, 0) = c_a_h2;
	(*rfpulse)(0, 1) = exp(i_phi*2.0)*s_a_h2;
	(*rfpulse)(0, 2) = -1.0*std::complex<double>(0.0, 1.0)*exp(i_phi)*s_a;

	(*rfpulse)(1, 0) = exp(-2.0 * i_phi)*s_a_h2;
	(*rfpulse)(1, 1) = c_a_h2;
	(*rfpulse)(1, 2) = std::complex<double>(0.0, 1.0) * exp(-1.0*i_phi)*s_a;

	(*rfpulse)(2, 0) = -1.0*i_half*exp(-1.0*i_phi)*s_a;
	(*rfpulse)(2, 1) = 1.0*i_half*exp(1.0*i_phi)*s_a;
	(*rfpulse)(2, 2) = c_a;

	//std::cout << *rfpulse << std::endl;
}

void epg_grad(Eigen::MatrixXcd *FpFmZ, int num_cols)
{

	for (int i = 0; i < num_cols - 1; i++)
	{
		//std::cout << i << "\t" << num_cols - 1 - i << "\t" << num_cols - 1 - i - 1 << "\t" << (*FpFmZ)(0,i) << std::endl;
		(*FpFmZ)(0, num_cols - 1 - i) = (*FpFmZ)(0, num_cols - 1 - i - 1);
	}

	(*FpFmZ)(0, 0) = std::complex<double>(0.0, 0.0);

	for (int i = 0; i < num_cols - 1; i++)
		(*FpFmZ)(1, i) = (*FpFmZ)(1, i + 1);

	(*FpFmZ)(1, num_cols - 1) = std::complex<double>(0.0, 0.0);

	(*FpFmZ)(0, 0) = conj((*FpFmZ)(1, 0));
}

void  cpmg_epg(double *signal, int Nechos, double rf_180, double T1, double T2, double Techo)
{
	const double PI = 3.1415926535897932384626433832795;
	int    num_cols = 2 * Nechos;

	Eigen::MatrixXcd FpFmZ(3, 34);
	Eigen::Matrix3cd rfpulse90y = Eigen::Matrix3cd::Zero();
	Eigen::Matrix3cd rfpulse180x = Eigen::Matrix3cd::Zero();
	Eigen::Matrix3d ee = Eigen::Matrix3d::Zero();
	//Eigen::MatrixXd signal(1, 17);

	FpFmZ = FpFmZ.Zero(3, 34);

	// ****************************************
	// Initialize diagonal of relaxation matrix
	// ****************************************

	ee(0, 0) = exp(-Techo / 2.0 / T2);
	ee(1, 1) = exp(-Techo / 2.0 / T2);
	ee(2, 2) = exp(-Techo / 2.0 / T1);

	// ********************************
	// Set Z magnetization to 1.0 +0.0j
	// ********************************

	FpFmZ(0, 0) = 0.0;
	FpFmZ(1, 0) = 0.0;
	FpFmZ(2, 0) = 1.0;

	// *********************************
	// Create 90(y) rotation matrix
	// *********************************

	rf_pulse_matrix(&rfpulse90y, PI / 2.0, PI / 2.0);

	// *********************************
	// Create 180(x) rotation matrix
	// *********************************

	rf_pulse_matrix(&rfpulse180x, rf_180*PI / 180.0, 0.0);

	// *********************************
	// Apply 90 degree pulse
	// *********************************

	FpFmZ = rfpulse90y * FpFmZ;

	// ****************************************
	// Apply 180 pulses of CPMG sequence
	// ****************************************

	for (int i = 0; i < Nechos; i++)
	{

		FpFmZ = ee * FpFmZ;
		FpFmZ(2, 0) = FpFmZ(2, 0) + 1.0 - ee(2, 2);
		epg_grad(&FpFmZ, 34);

		FpFmZ = rfpulse180x * FpFmZ;

		FpFmZ = ee * FpFmZ;
		FpFmZ(2, 0) = FpFmZ(2, 0) + 1.0 - ee(2, 2);
		epg_grad(&FpFmZ, 34);

		signal[i] = real(FpFmZ(0, 0));
	}

}

//int main()
//{
//	const double PI = 3.1415926535897932384626433832795;
//	std::clock_t    start;
//
//	double T1 = 3000.0;
//	double T2 = 50.0;
//	double Techo = 10.0;
//	int    Nechos = 17;
//	int    num_cols = 2 * Nechos;
//
//	Eigen::MatrixXcd FpFmZ(3, num_cols);
//	Eigen::Matrix3cd rfpulse90y = Eigen::Matrix3cd::Zero();
//	Eigen::Matrix3cd rfpulse180x = Eigen::Matrix3cd::Zero();
//	Eigen::Matrix3d ee = Eigen::Matrix3d::Zero();
//
//	double *signal = new  double [Nechos] { 0.0 };
//
//	start = std::clock();
//
//	for (int j = 0; j < 10000; j++)
//		cpmg_epg(signal, Nechos, PI, T1, T2, Techo);
//
//	std::cout << "Time: " << (std::clock() - start) / (double)(CLOCKS_PER_SEC / 1000) << " ms" << std::endl;
//
//	for (int i = 0; i<Nechos; i++)
//		std::cout << signal[i] << std::endl;
//
//
//
//}


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

using namespace Eigen;
using namespace std;

int main()
{
  Matrix3f m = Matrix3f::Random();
  m = (m + Matrix3f::Constant(1.2)) * 50;
  cout << "m =" << endl << m << endl;
  Vector3f v(1,2,3);

  cout << "m * v =" << endl << m * v << endl;
}


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

#include "Math.hpp"
#include "../../Array.hpp"
#include "../../MatVec.hpp"
#include "../../Ray3.hpp"
#include "../../Triangle3.hpp"
#include <Eigen/SVD>

namespace Browse3D {

Matrix3
orthonormalBasisMatrix(Vector3 const & u, Vector3 const & v, Vector3 const & w)
{
  Vector3 w2 = w.normalized();
  Vector3 u2 = v.cross(w2).normalized();
  Vector3 v2 = w2.cross(u2);

  Matrix3 ret; ret << u2, v2, w2;
  return ret;
}

Matrix3
orthonormalBasisMatrix(Matrix3 const & m)
{
  return orthonormalBasisMatrix(m.col(0), m.col(1), m.col(2));
}

Real
estimateScalingFactor(Matrix3 const & m)
{
  return (m.col(0).norm() + m.col(1).norm() + m.col(2).norm()) / 3.0f;
}

Vector3
reflectPoint(Vector3 const & p, Plane3 const & mirror_plane)
{
  // Assume Plane3::normal() is unit
  return p - 2 * (p - mirror_plane.getPoint()).dot(mirror_plane.getNormal()) * mirror_plane.getNormal();
}

Vector3
reflectVector(Vector3 const & v, Plane3 const & mirror_plane)
{
  // Assume Plane3::normal() is unit
  return v - 2 * v.dot(mirror_plane.getNormal()) * mirror_plane.getNormal();
}

bool
lineIntersectsTriangle(Line3 const & line, Vector3 const & v0, Vector3 const & edge01, Vector3 const & edge02, Real * time)
{
  // The code is taken from the ray-triangle intersection test in Dave Eberly's Wild Magic library, v5.3, released under the
  // Boost license: http://www.boost.org/LICENSE_1_0.txt .

  static Real const EPS = 1e-30f;

  Vector3 diff = line.getPoint() - v0;
  Vector3 normal = edge01.cross(edge02);

  // Solve Q + t*D = b1*E1 + b2*E2 (Q = diff, D = line direction, E1 = edge01, E2 = edge02, N = Cross(E1,E2)) by
  //   |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2))
  //   |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q))
  //   |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N)

  Real DdN = line.getDirection().dot(normal);
  int sign;
  if (DdN > EPS)
    sign = 1;
  else if (DdN < -EPS)
  {
    sign = -1;
    DdN = -DdN;
  }
  else
  {
    // Line and triangle are parallel, call it a "no intersection" even if the line does intersect
    return false;
  }

  Real DdQxE2 = sign * line.getDirection().dot(diff.cross(edge02));
  if (DdQxE2 >= 0)
  {
    Real DdE1xQ = sign * line.getDirection().dot(edge01.cross(diff));
    if (DdE1xQ >= 0)
    {
      if (DdQxE2 + DdE1xQ <= DdN)
      {
        // Line intersects triangle
        Real QdN = -sign * diff.dot(normal);
        if (time) *time = QdN / DdN;
        return true;
      }
      // else: b1 + b2 > 1, no intersection
    }
    // else: b2 < 0, no intersection
  }
  // else: b1 < 0, no intersection

  return false;
}

#define CLOSEST_PT_SEGMENT_SEGMENT(VecType) \
Real \
closestPtSegmentSegment(VecType const & p1, VecType const & q1, VecType const & p2, VecType const & q2, Real & s, Real & t, \
                        VecType & c1, VecType & c2) \
{ \
  static Real const EPSILON = 1e-20f; \
 \
  VecType d1 = q1 - p1; /* Direction vector of segment S1 */ \
  VecType d2 = q2 - p2; /* Direction vector of segment S2 */ \
  VecType r = p1 - p2; \
  Real a = d1.squaredNorm(); /* Squared length of segment S1, always nonnegative */ \
  Real e = d2.squaredNorm(); /* Squared length of segment S2, always nonnegative */ \
  Real f = d2.dot(r); \
 \
  /* Check if either or both segments degenerate into points */ \
  if (a <= EPSILON && e <= EPSILON) \
  { \
    /* Both segments degenerate into points */ \
    s = t = 0; \
    c1 = p1; \
    c2 = p2; \
    return (c1 - c2).dot(c1 - c2); \
  } \
  if (a <= EPSILON) \
  { \
    /* First segment degenerates into a point */ \
    s = 0; \
    t = f / e; /* s = 0 => t = (b*s + f) / e = f / e */ \
    t = Math::clamp(t, (Real)0, (Real)1); \
  } \
  else \
  { \
    Real c = d1.dot(r); \
    if (e <= EPSILON) \
    { \
        /* Second segment degenerates into a point */ \
        t = 0; \
        s = Math::clamp(-c / a, (Real)0, (Real)1); /* t = 0 => s = (b*t - c) / a = -c / a */ \
    } \
    else \
    { \
      /* The general nondegenerate case starts here */ \
      Real b = d1.dot(d2); \
      Real denom = a * e - b * b; /* Always nonnegative */ \
 \
      /* If segments not parallel, compute closest point on L1 to L2, and */ \
      /* clamp to segment S1. Else pick arbitrary s (here 0) */ \
      if (denom != 0) \
        s = Math::clamp((b * f - c * e) / denom, (Real)0, (Real)1); \
      else \
        s = 0; \
 \
      /* Compute point on L2 closest to S1(s) using */ \
      /* t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e */ \
      t = (b * s + f) / e; \
 \
      /* If t in [0,1] done. Else clamp t, recompute s for the new value */ \
      /* of t using s = Dot((P2+D2*t)-P1,D1) / Dot(D1,D1)= (t*b - c) / a */ \
      /* and clamp s to [0, 1] */ \
      if (t < 0) \
      { \
        t = 0; \
        s = Math::clamp(-c / a, (Real)0, (Real)1); \
      } \
      else if (t > 1) \
      { \
        t = 1; \
        s = Math::clamp((b - c) / a, (Real)0, (Real)1); \
      } \
    } \
  } \
 \
  c1 = p1 + s * d1; \
  c2 = p2 + t * d2; \
  return (c1 - c2).squaredNorm(); \
}

CLOSEST_PT_SEGMENT_SEGMENT(Vector2)
CLOSEST_PT_SEGMENT_SEGMENT(Vector3)

#define CLOSEST_PT_SEGMENT_LINE(VecType) \
Real \
closestPtSegmentLine(VecType const & p1, VecType const & q1, VecType const & p2, VecType const & q2, Real & s, Real & t, \
                     VecType & c1, VecType & c2) \
{ \
  static Real const EPSILON = 1e-20f; \
 \
  VecType d1 = q1 - p1; /* Direction vector of segment S1 */ \
  VecType d2 = q2 - p2; /* Direction vector of line L2 */ \
  VecType r = p1 - p2; \
  Real a = d1.squaredNorm(); /* Squared length of segment S1, always nonnegative */ \
  Real e = d2.squaredNorm(); /* Squared length of segment supporting L2, always nonnegative and assumed to be non-zero */ \
  Real f = d2.dot(r); \
 \
  if (a <= EPSILON) \
  { \
    /* Segment degenerates into a point */ \
    s = 0; \
    t = f / e; /* s = 0 => t = (b*s + f) / e = f / e */ \
  } \
  else \
  { \
    /* The general nondegenerate case starts here */ \
    Real c = d1.dot(r); \
    Real b = d1.dot(d2); \
    Real denom = a * e - b * b; /* Always nonnegative */ \
\
    /* If segments not parallel, compute closest point on L1 to L2, and */ \
    /* clamp to segment S1. Else pick arbitrary s (here 0) */ \
    if (denom != 0) \
      s = Math::clamp((b * f - c * e) / denom, (Real)0, (Real)1); \
    else \
      s = 0; \
\
    /* Compute point on L2 closest to S1(s) using */ \
    /* t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e */ \
    t = (b * s + f) / e; \
  } \
 \
  c1 = p1 + s * d1; \
  c2 = p2 + t * d2; \
  return (c1 - c2).squaredNorm(); \
}

CLOSEST_PT_SEGMENT_LINE(Vector2)
CLOSEST_PT_SEGMENT_LINE(Vector3)

Real
closestPtLineTriangle(Line3 const & line, Vector3 const & v0, Vector3 const & edge01, Vector3 const & edge02, Real & s,
                      Vector3 & c1, Vector3 & c2)
{
  if (lineIntersectsTriangle(line, v0, edge01, edge02, &s))
  {
    c1 = c2 = line.getPoint() + s * line.getDirection();
    return 0;
  }

  // Either (1) the line is not parallel to the triangle and the point of
  // intersection of the line and the plane of the triangle is outside the
  // triangle or (2) the line and triangle are parallel.  Regardless, the
  // closest point on the triangle is on an edge of the triangle.  Compare
  // the line to all three edges of the triangle.
  Real min_sqdist = -1;
  Vector3 v[3] = { v0, v0 + edge01, v0 + edge02 };
  Vector3 tmp_c1, tmp_c2;
  float tmp_s, t;
  for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
  {
    Real sqdist = closestPtSegmentLine(v[i0], v[i1], line.getPoint(), line.getPoint() + line.getDirection(),
                                       t, tmp_s, tmp_c2, tmp_c1);
    if (min_sqdist < 0 || sqdist < min_sqdist)
    {
      min_sqdist = sqdist;
      s = tmp_s;
      c1 = tmp_c1;
      c2 = tmp_c2;
    }
  }

  return min_sqdist;
}

Real
closestPtRayTriangle(Ray3 const & ray, Vector3 const & v0, Vector3 const & edge01, Vector3 const & edge02, Real & s,
                     Vector3 & c1, Vector3 & c2)
{
  Real sqdist = closestPtLineTriangle(Line3::fromPointAndDirection(ray.getOrigin(), ray.getDirection()), v0, edge01, edge02,
                                      s, c1, c2);
  if (s >= 0)
    return sqdist;

  // Not the most efficient way but the most convenient
  LocalTriangle3 tri(v0, v0 + edge01, v0 + edge02);
  s = 0;
  c1 = ray.getOrigin();
  c2 = tri.closestPoint(c1);

  return (c1 - c2).squaredNorm();
}

RigidTransform3
closestRigidTransform(std::vector<CoordinateFrame3> const & src, std::vector<CoordinateFrame3> const & dst)
{
  alwaysAssertM(src.size() == dst.size(), "Source and destination sets must have same number of frames");

  if (src.empty())
    return RigidTransform3::identity();
  else if (src.size() == 1)
    return RigidTransform3(dst[0]) * RigidTransform3(src[0].inverse());
  else if (src.size() == 2)
  {
    // First map line segment to line segment...
    Vector3 src_mean = 0.5f * (src[0].getTranslation() + src[1].getTranslation());
    Vector3 dst_mean = 0.5f * (dst[0].getTranslation() + dst[1].getTranslation());

    static Real const MIN_SQLEN = 1.0e-8f;

    Vector3 src_axis = src[1].getTranslation() - src[0].getTranslation();
    Vector3 dst_axis = dst[1].getTranslation() - dst[0].getTranslation();

    if (src_axis.squaredNorm() > MIN_SQLEN && dst_axis.squaredNorm() > MIN_SQLEN)
    {
      Matrix3 rot = Math::rotationArc(src_axis, dst_axis);

      // Now rotate around the mapped segment to align the z axes of the frames...
      Vector3 src_dir = rot * (src[0].getRotation().col(2) + src[1].getRotation().col(2));
      Vector3 dst_dir = dst[0].getRotation().col(2) + dst[1].getRotation().col(2);

      // Transform src_dir and dst_dir to be perpendicular to dst_axis
      dst_axis = dst_axis.normalized();
      src_dir = src_dir - src_dir.dot(dst_axis) * dst_axis;
      dst_dir = dst_dir - dst_dir.dot(dst_axis) * dst_axis;

      if (src_dir.squaredNorm() > MIN_SQLEN && dst_dir.squaredNorm() > MIN_SQLEN)
      {
        src_dir = src_dir.normalized();
        dst_dir = dst_dir.normalized();

        Vector3 src_perp = dst_axis.cross(src_dir);
        Vector3 dst_perp = dst_axis.cross(dst_dir);

        Matrix3 src_basis; src_basis << src_dir, src_perp, dst_axis;
        Matrix3 dst_basis; dst_basis << dst_dir, dst_perp, dst_axis;
        Matrix3 extra_rot = dst_basis * src_basis.transpose();  // inverse == tranpose for rotation matrices

        rot = extra_rot * rot;
      }

      CoordinateFrame3 cf(RigidTransform3::_fromAffine(AffineTransform3(rot, dst_mean - rot * src_mean)));
      // THEA_CONSOLE.nospace() << "R = " << toString(cf.getRotation()) << ", T = " << toString(cf.getTranslation());

      return RigidTransform3(cf);
    }
    else
      return RigidTransform3::translation(dst_mean - src_mean);
  }

  // ICP algorithm of Arun et al. 1987.

  size_t num_frames = src.size();
  Vector3 src_mean = Vector3::Zero(), dst_mean = Vector3::Zero();
  for (size_t i = 0; i < num_frames; ++i)
  {
    CoordinateFrame3 const & src_frame = src[i];
    CoordinateFrame3 const & dst_frame = dst[i];

    // THEA_CONSOLE.nospace() << "src[" << i << "] = R: " << toString(src_frame.getRotation()) << ", T: "
    //                    << toString(src_frame.getTranslation());
    // THEA_CONSOLE.nospace() << "dst[" << i << "] = R: " << toString(dst_frame.getRotation()) << ", T: "
    //                    << toString(dst_frame.getTranslation());

    src_mean += src_frame.getTranslation();
    dst_mean += dst_frame.getTranslation();
  }

  src_mean /= num_frames;
  dst_mean /= num_frames;

  Matrix3 corr = Matrix3::Zero();
  Vector3 src_pt, dst_pt;
  for (size_t i = 0; i < num_frames; ++i)
  {
    CoordinateFrame3 const & src_frame = src[i];
    CoordinateFrame3 const & dst_frame = dst[i];

    src_pt = src_frame.getTranslation() - src_mean;
    dst_pt = dst_frame.getTranslation() - dst_mean;

    for (int r = 0; r < 3; ++r)
      for (int c = 0; c < 3; ++c)
        corr(r, c) += src_pt[r] * dst_pt[c];
  }

  Eigen::JacobiSVD<Matrix3> svd(corr, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Matrix3 U_T = svd.matrixU().transpose();
  Matrix3 V = svd.matrixV();
  Matrix3 rotation = V * U_T;

  if (rotation.determinant() < 0)
  {
    // FIXME: One of the columns (which?) of V should be negated. See Eggert et al. '97, "Estimating 3D rigid transformations: a
    // comparison of four major algorithms".
    //
    // For now, we'll do the dumb but safe thing and test each option for the minimum error.

    THEA_WARNING << "Estimated rigid transform involves reflection, fixing by negating a column of V";

    Real min_sqerr = -1;
    for (int i = 0; i < 3; ++i)
    {
      // Swap i'th column of V
      Matrix3 V_i = V;
      V_i(0, i) = -V_i(0, i);
      V_i(1, i) = -V_i(1, i);
      V_i(2, i) = -V_i(2, i);

      Matrix3 candidate_rot = V_i * U_T;
      Real sqerr = 0;
      for (size_t j = 0; j < num_frames; ++j)
      {
        CoordinateFrame3 const & src_frame = src[j];
        CoordinateFrame3 const & dst_frame = dst[j];

        src_pt = src_frame.getTranslation() - src_mean;
        dst_pt = dst_frame.getTranslation() - dst_mean;
        sqerr += (candidate_rot * src_pt - dst_pt).squaredNorm();
      }

      if (min_sqerr < 0 || sqerr < min_sqerr)
      {
        rotation = candidate_rot;
        min_sqerr = sqerr;
      }
    }
  }

  Vector3 translation = dst_mean - rotation * src_mean;

  CoordinateFrame3 cf(RigidTransform3::_fromAffine(AffineTransform3(rotation, translation)));
  // THEA_CONSOLE.nospace() << "R = " << toString(rotation) << ", T = " << toString(translation);

  return RigidTransform3(cf);
}

int
bvhDepth(intx num_elems, int max_elems_in_leaf)
{
  alwaysAssertM(num_elems >= 0, "Can't compute BVH depth for negative number of elements");
  alwaysAssertM(max_elems_in_leaf > 0, "Can't compute BVH depth for non-positive number of elements at leaf");

  int max_depth = (num_elems > 0 ? (int)std::ceil(Math::fastLog2(num_elems / (double)max_elems_in_leaf)) : 0);
  return max_depth;
}

void
getBarycentricCoordinates2(Real qx, Real qy, Real x0, Real y0, Real x1, Real y1, Real x2, Real y2,
                           Real & bc0, Real & bc1, Real & bc2)
{
  Real a = y1 - y2;
  Real b = x0 - x2;
  Real c = y2 - y0;
  Real d = x2 - x1;

  Real det = a * b - c * d;
  if (std::abs(det) < 1.0e-30f)
    bc0 = bc1 = bc2 = 1.0f / 3;
  else
  {
    Real qx_rel_3 = qx - x2;
    Real qy_rel_3 = qy - y2;

    bc0 = (a * qx_rel_3 + d * qy_rel_3) / det;
    bc1 = (c * qx_rel_3 + b * qy_rel_3) / det;
    bc2 = 1 - bc0 - bc1;
  }
}

void
getBarycentricCoordinates2(Vector2 const & q, Vector2 const & v0, Vector2 const & v1, Vector2 const & v2,
                           Real & bc0, Real & bc1, Real & bc2)
{
  getBarycentricCoordinates2(q.x(), q.y(), v0.x(), v0.y(), v1.x(), v1.y(), v2.x(), v2.y(), bc0, bc1, bc2);
}

void
getBarycentricCoordinates3(Vector3 const & q, Vector3 const & v0, Vector3 const & v1, Vector3 const & v2,
                           Real & bc0, Real & bc1, Real & bc2)
{
  switch (Math::maxAbsAxis((v1 - v0).cross(v2 - v0)))
  {
    case 0:           getBarycentricCoordinates2(q.tail<2>(), v0.tail<2>(), v1.tail<2>(), v2.tail<2>(), bc0, bc1, bc2); break;
    case 1:           getBarycentricCoordinates2(Vector2( q[0],  q[2]),
                                                 Vector2(v0[0], v0[2]),
                                                 Vector2(v1[0], v1[2]),
                                                 Vector2(v2[0], v2[2]), bc0, bc1, bc2); break;
    default /* 2 */:  getBarycentricCoordinates2(q.head<2>(), v0.head<2>(), v1.head<2>(), v2.head<2>(), bc0, bc1, bc2);
  }
}

Vector3
triRandomPoint(Vector3 const & v0, Vector3 const & v1, Vector3 const & v2)
{
  return triRandomPointFromEdges(v0, v1 - v0, v2 - v0);
}

Vector3
triRandomPointFromEdges(Vector3 const & v0, Vector3 const & edge01, Vector3 const & edge02)
{
  // From G3D::Triangle

  // Choose a random point in the parallelogram

  float s = Random::common().uniform01();
  float t = Random::common().uniform01();

  if (t > 1.0f - s)
  {
    // Outside the triangle; reflect about the diagonal of the parallelogram
    t = 1.0f - t;
    s = 1.0f - s;
  }

  return s * edge01 + t * edge02 + v0;
}

Vector3
quadRandomPoint(Vector3 const & v0, Vector3 const & v1, Vector3 const & v2, Vector3 const & v3)
{
  Vector3 edge01 = v1 - v0;
  Vector3 edge03 = v3 - v0;
  Vector3 edge21 = v1 - v2;
  Vector3 edge23 = v3 - v2;

  Real tri0_double_area = edge01.cross(edge03).norm();
  Real tri1_double_area = edge21.cross(edge23).norm();

  float tri = Random::common().uniform(0, tri0_double_area + tri1_double_area);
  if (tri <= tri0_double_area)
    return triRandomPointFromEdges(v0, edge01, edge03);
  else
    return triRandomPointFromEdges(v2, edge21, edge23);
}

Real
raySphereIntersectionTime(Ray3 const & ray, Vector3 const & center, Real radius)
{
  Vector3 pmc = ray.getOrigin() - center;

  double s[3];
  s[2] = ray.getDirection().squaredNorm();
  s[1] = 2 * pmc.dot(ray.getDirection());
  s[0] = pmc.squaredNorm() - radius * radius;

  double roots[2];
  int num_roots = Math::solveQuadratic(s[0], s[1], s[2], roots);

  double min_root = -1;
  for (int i = 0; i < num_roots; ++i)
    if (roots[i] >= 0 && (min_root < 0 || roots[i] < min_root))
      min_root = roots[i];

#if 0
  if (min_root >= 0)
    THEA_CONSOLE << "min_root =" << min_root;
#endif

  return (Real)min_root;
}

Real
rayTorusIntersectionTime(Ray3 const & ray, Real torus_radius, Real torus_width)
{
  double r2pw2 = torus_radius * torus_radius + torus_width * torus_width;
  double r2mw2 = r2pw2 - 2 * torus_width * torus_width;

  Vector3 p2 = ray.getOrigin().cwiseProduct(ray.getOrigin());
  Vector3 pu = ray.getOrigin().cwiseProduct(ray.getDirection());
  Vector3 u2 = ray.getDirection().cwiseProduct(ray.getDirection());

  double s[5];
  s[4] = u2[0] * (u2[0] + 2 * u2[1]) + u2[1] * (u2[1] + 2 * u2[2]) + u2[2] * (u2[2] + 2 * u2[0]);
  s[3] = 4 * (pu[0] + pu[1] + pu[2]) * (u2[0] + u2[1] + u2[2]);
  s[2] = 2 * (r2mw2 * u2[2] - r2pw2 * (u2[0] + u2[1]))
       + 8 * (pu[0] * pu[1] + pu[1] * pu[2] + pu[2] * pu[0])
       + 6 * (pu[0] * pu[0] + pu[1] * pu[1] + pu[2] * pu[2])
       + 2 * (p2[0] * (u2[1] + u2[2]) + p2[1] * (u2[2] + u2[0]) + p2[2] * (u2[0] + u2[1]));
  s[1] = 4 * (r2mw2 * pu[2] - r2pw2 * (pu[0] + pu[1]) + (p2[0] + p2[1] + p2[2]) * (pu[0] + pu[1] + pu[2]));
  s[0] = 2 * (r2mw2 * p2[2] - r2pw2 * (p2[0] + p2[1]) + p2[0] * p2[1] + p2[1] * p2[2] + p2[2] * p2[0])
       + p2[0] * p2[0] + p2[1] * p2[1] + p2[2] * p2[2] + r2mw2 * r2mw2;

  double roots[4];
  int num_roots = Math::solveQuartic(s[0], s[1], s[2], s[3], s[4], roots);

  double min_root = -1;
  for (int i = 0; i < num_roots; ++i)
    if (roots[i] >= 0 && (min_root < 0 || roots[i] < min_root))
      min_root = roots[i];

#if 0
  if (min_root >= 0)
    THEA_CONSOLE << "min_root =" << min_root;
#endif

  return (Real)min_root;
}

} // namespace Browse3D


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

#ifndef _DLQR_H_
#define _DLQR_H_

#include <utility>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/LU>
#include <eigen3/Eigen/Eigenvalues>
//#include <armadillo>

namespace control
{
  /// @class dLQR
  /// @brief A discrete time LQR controller class. The LQR controller minimizes the Hinf control cost,
  /// as defined by: J = \sum_t[(Xd(t)-X(t))'Q(Xd(t)-X(t)) + u(t)'Ru(t) + 2*(Xd(t)-X(t))'Nu(t)].
  class dLQR
  {
  public:
    /// @brief Constructor
    /// @param[in] K is the LQR gain matrix for u=K(Xd-X), which drives X to Xd.
    dLQR(const Eigen::MatrixXd& K);

    /// @brief Constructor using system x[n+1]=Ax[n]+Bu[n]
    /// @param[in] A specifies the discrete-time state space equation.
    /// @param[in] B specifies the discrete-time state space equation.
    /// @param[in] Q specifies the state space error penalty.
    /// @param[in] R specifies the control effort penalty.
    /// @param[in] N specifies the error-effort cross penalty.
    /// @param[in] dt specifies the control interval.
    dLQR(const Eigen::MatrixXd& A,
         const Eigen::MatrixXd& B,
         const Eigen::MatrixXd& Q,
         const Eigen::MatrixXd& R,
         const Eigen::MatrixXd& N);

    dLQR(){};

    ~dLQR(){ initialized = false; };

    /// @brief Calculate the ARE solution given A, B, Q, R, N matrices. This algorithm is based
    /// on the work of https://github.com/TakaHoribe/Riccati_Solver/ and W. F. Arnold and A. J. Laub,
    /// "Generalized eigenproblem algorithms and software for algebraic Riccati equations," in
    /// Proceedings of the IEEE, vol. 72, no. 12, pp. 1746-1754, Dec. 1984, doi: 10.1109/PROC.1984.13083.
    /// The DARE solver is based on: https://github.com/arunabh1904/LQR-ROS/blob/master/lqr_obsavoid/src/are_solver.cpp
    /// The WIP DARE solver with N term used is based on: https://github.com/scipy/scipy/blob/v1.7.1/scipy/linalg/_solvers.py#L529-L734
    Eigen::MatrixXd care(const Eigen::MatrixXd& A,
                          const Eigen::MatrixXd& B,
                          const Eigen::MatrixXd& Q,
                          const Eigen::MatrixXd& R,
                          const Eigen::MatrixXd& N);

    Eigen::MatrixXd dare(const Eigen::MatrixXd& A,
                          const Eigen::MatrixXd& B,
                          const Eigen::MatrixXd& Q,
                          const Eigen::MatrixXd& R,
                          const Eigen::MatrixXd& N);

    /// @brief Input the current states and desired states, output the control values
    /// @param X is the most recently state space
    /// @param Xd is the most recently desired state space
    /// @return the control value from the LQR algorithm
    const Eigen::VectorXd& calculateControl(const Eigen::VectorXd& X, const Eigen::VectorXd& Xd);

    void balance_matrix(const Eigen::MatrixXd &A, Eigen::MatrixXd &Aprime, Eigen::VectorXd &D);

    /// @brief Get the current LQR gain.
    /// @return The current K matrix.
    inline const Eigen::MatrixXd& getK() const { return K_; };

    /// @brief Get the current value of the control error
    inline const Eigen::VectorXd& CurrentError() const { return e_; };

    /// @brief Get the current control value (calculated based on most recent errors)
    /// @return the current control value
    inline const Eigen::VectorXd& CurrentControl() const { return u_; };
    inline bool isInitialized() const { return initialized; };

  private:
    /// @brief is initialized
    bool initialized = false;

    /// @brief LQR gain.
    Eigen::MatrixXd K_;

    /// @brief Current state space error used in LQR control algorithm
    Eigen::VectorXd e_;

    /// @brief Current control value
    Eigen::VectorXd u_;

  }; // End class dLQR

} // End namespace control

#endif