#include <ctime>
#include <string>
#include <algorithm>
#include <iostream>
#include <random>
#include <math.h>
#include <vtkNew.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkCellLocator.h>
#include <vtkDelaunay3D.h>
#include <vtkDataSetSurfaceFilter.h>
#include <vtkPolyData.h>
#include <vtkDoubleArray.h>
#include <vtkPoints.h>
#include <vtkIdFilter.h>
#include <vtkPointData.h>
#include <vtkCellArray.h>
#include <vtkIdList.h>
#include <vtkLinearSubdivisionFilter.h>
#include <vtkGenericCell.h>
#include <vtkVertexGlyphFilter.h>
#include <vtkDataObject.h>
#include <Eigen/Dense>

extern "C" void shexpandlsq_wrapper_(double* cilm, double* d, double* lat,
	double* lon, int* N, int* lmax, double* chi2);

extern "C" void makegridpoints_wrapper_(double* cilm, int* lmax, int* n,
	double* lat, double* lon, double* points, int* dealloc);

extern "C" void glqgridcoord_wrapper_(double* latglq, double* longlq,
	int* lmax, int* nlat, int* nlong);

extern "C" void shglq_wrapper_(int* lmax, double* zero, double* w,
	double* plx);

extern "C" void shexpandglq_wrapper_(double* cilm, int* lmax, double* gridglq,
	double* w, double* plx);

extern "C" void makegridglq_wrapper_(double* gridglq, double* cilm, int* lmax,
	double* plx);

extern "C" void shexpanddh_wrapper_(double* grid, int* n, double* cilm,
	int* lmax);

extern "C" void makegriddh_wrapper_(double* grid, int* n, double* cilm,
	int* lmax);

extern "C" void shpowerspectrum_wrapper_(double* cilm, int* lmax,
	double* pspectrum);

int main(){

    // Read the polydata
    vtkNew<vtkPolyDataReader> reader;
    reader->SetFileName("T7.vtk");
    reader->Update();
    auto pointCloud = reader->GetOutput();
    int N = pointCloud->GetNumberOfPoints();

    // Copy the point coordinates into a matrix
    Eigen::Map<Eigen::Matrix3Xd> ptsMat((double_t*)pointCloud->GetPoints()
	    ->GetData()->GetVoidPointer(0),3,N);

    // Calculate average radius
    double_t R = ptsMat.colwise().norm().mean();

    // Calculate points on a sphere of average radius and displacements
    vtkNew<vtkPoints> spherePts;
    vtkNew<vtkDoubleArray> displacements;
    displacements->SetName("Displacements");
    displacements->SetNumberOfComponents(1);
    for(auto i = 0; i < N; ++i){
	Eigen::Vector3d p, q;
	pointCloud->GetPoint(i, &p(0));
	q =  R*p.normalized();
	spherePts->InsertNextPoint( &q(0) );
	displacements->InsertNextTuple1(p.norm() - R);
    }

    // Create a mesh on the sphere
    vtkNew<vtkPolyData> sphere;
    sphere->SetPoints( spherePts );
    sphere->GetPointData()->AddArray( displacements );
    vtkNew<vtkIdFilter> idf;
    idf->SetIdsArrayName( "OrigIds" );
    idf->PointIdsOn();
    idf->SetInputData( sphere );
    vtkNew<vtkDelaunay3D> d3D;
    d3D->SetInputConnection( idf->GetOutputPort() );
    vtkNew<vtkDataSetSurfaceFilter> dssf;
    dssf->SetInputConnection(d3D->GetOutputPort());
    dssf->Update();
    auto final = dssf->GetOutput()->GetPolys();
    auto idArray = dssf->GetOutput()->GetPointData()->GetArray( "OrigIds" );
    final->InitTraversal();
    vtkNew<vtkCellArray> triangles;
    vtkNew<vtkIdList> ids;
    while( final->GetNextCell( ids ) ){
	triangles->InsertNextCell(3);
	for( auto i = 0; i < 3; ++i )
	    triangles->InsertCellPoint( (vtkIdType)idArray->GetTuple1(
			ids->GetId(i) ) );
    }
    sphere->SetPolys( triangles );
    vtkNew<vtkPolyDataWriter> writer;
    writer->SetFileName("AverageSphere.vtk");
    writer->SetInputData(sphere);
    writer->Write();

    // Create a locator to identify the cells that contain the Gauss quadrature
    // points
    vtkNew<vtkCellLocator> cellLoc;
    cellLoc->SetDataSet( sphere );
    cellLoc->BuildLocator();

    //**********************************************************************//
    // Create the Gauss-Lengendre grid
    int nlat, nlong, lmax = 7;
    Eigen::VectorXd latglq(lmax + 1);
    Eigen::VectorXd longlq(2*lmax + 1);
    glqgridcoord_wrapper_(latglq.data(), longlq.data(), &lmax, &nlat, &nlong);

    // Find the cell to which each point of the grid belongs and set its value
    // by linear interpolation
    auto coords = [&R]( const double_t T, const double_t P ){
	Eigen::Vector3d X;
	auto SinT = std::sin(T*M_PI/180.0);
	X << R*SinT*std::cos(P*M_PI/180.0), R*SinT*std::sin(P*M_PI/180.0),
	  R*std::cos( T*M_PI/180.0);
	return X;
    };
    /*
       vtkNew<vtkPoints> glqPoints;
       vtkNew<vtkCellArray> glqVerts;
       vtkNew<vtkDoubleArray> glqDisp;
       glqDisp->SetName("GLQDisp");
       glqDisp->SetNumberOfComponents(3);
       */
    Eigen::VectorXd gridglq((lmax + 1)*(2*lmax + 1));
    Eigen::VectorXd plx( (lmax + 1)*(lmax + 1)*(lmax + 2)/2 );
    Eigen::VectorXd w( lmax + 1 ), zero( lmax + 1 );
    Eigen::VectorXd cilm1( 2*(lmax + 1)*(lmax + 1) );
    Eigen::VectorXd pspectrum1( lmax + 1 );
    shglq_wrapper_(&lmax, zero.data(), w.data(), plx.data() );

    clock_t t = clock();
    for(auto z = 0; z < 1000; ++z){
	for( auto j = 0; j < 2*lmax + 1; ++j ){
	    for( auto i = 0; i < lmax + 1; ++i ){
		// Get the spherical coordinate
		auto X = coords( 90.0 - latglq(i), longlq(j) );
		// Locate cell
		Eigen::Vector3d closestPoint, pcoords, weights;
		vtkIdType cellId;
		int subId;
		double_t dist2;
		vtkNew<vtkGenericCell> genCell;
		cellLoc->FindClosestPoint( &X(0), &closestPoint(0), genCell,
			cellId, subId, dist2);
		// Get the parametric coordinates and weights
		genCell->EvaluatePosition( &X(0), &closestPoint(0), subId,
			&pcoords(0), dist2, &weights(0) );
		vtkNew<vtkIdList> pointIds;
		sphere->GetCellPoints( cellId, pointIds );
		// Interpolate to find the displacement at quadrature point
		gridglq(i + (lmax + 1)*j) =
		    weights[0]*displacements->GetTuple1(pointIds->GetId(0)) +
		    weights[1]*displacements->GetTuple1(pointIds->GetId(1)) +
		    weights[2]*displacements->GetTuple1(pointIds->GetId(2));
		//glqPoints->InsertNextPoint( &X(0) );
		//Eigen::Vector3d disp;
		//disp = gridglq(i + (lmax + 1)*j)*X.normalized();
		//glqDisp->InsertNextTuple( &disp(0) );
	    }
	}
	/*
	   vtkNew<vtkPolyData> glqBody;
	   glqBody->SetPoints( glqPoints );
	   glqBody->GetPointData()->SetVectors( glqDisp );
	   writer->SetInputData( glqBody );
	   writer->SetFileName( "GLQBody.vtk" );
	   writer->Write();
	   */

	// Expand using Gauss-Legendre Quadrature and get the power spectrum
	shexpandglq_wrapper_( cilm1.data(), &lmax, gridglq.data(), w.data(),
		plx.data());
    }
    t = clock() - t;
    std::cout<< "Time for 1000 steps GLQ = " << ((float)t)/CLOCKS_PER_SEC
       	<< std::endl;

    shpowerspectrum_wrapper_( cilm1.data(), &lmax, pspectrum1.data() );
    // Cross-check
    Eigen::VectorXd gridglq_out( (lmax + 1)*(2*lmax + 1) );
    makegridglq_wrapper_(gridglq_out.data(), cilm1.data(), &lmax, plx.data());
    std::cout<< "Error norm of GLQ = " << (gridglq - gridglq_out).norm()
	<< std::endl;
    //**********************************************************************//
    // Expand using Driscol-Healy and get the power spectrum
    int ndh = 2*(lmax + 1);
    /*
       vtkNew<vtkPoints> DHPoints;
       vtkNew<vtkCellArray> DHVerts;
       vtkNew<vtkDoubleArray> DHDisp;
       DHDisp->SetName("DHGridDisp");
       DHDisp->SetNumberOfComponents(3);
       */
    Eigen::VectorXd gridDH( ndh*ndh );
    Eigen::VectorXd cilm2( ndh*ndh/2 ), pspectrum2(lmax + 1);

    t = clock();
    for(auto z = 0; z < 1000; ++z){
	for( auto i = 0; i < ndh; ++i ){
	    auto latDH = i*(180.0/ndh);
	    for( auto j = 0; j < ndh; ++j ){
		// Get the spherical coordinate
		auto lonDH = j*360.0/ndh;
		auto X = coords( latDH, lonDH );
		// Locate cell
		Eigen::Vector3d closestPoint, pcoords, weights, dummy;
		vtkIdType cellId;
		int subId;
		double_t dist2;
		vtkNew<vtkGenericCell> genCell;
		cellLoc->FindClosestPoint( &X(0), &closestPoint(0), genCell,
			cellId, subId, dist2);
		// Get the parametric coordinates and weights
		auto out = genCell->EvaluatePosition( &closestPoint(0), &dummy(0),
			subId, &pcoords(0), dist2, &weights(0) );
		vtkNew<vtkIdList> pointIds;
		sphere->GetCellPoints( cellId, pointIds );
		// Interpolate to find the displacement at quadrature point
		gridDH(i + ndh*j) =
		    weights[0]*displacements->GetTuple1(pointIds->GetId(0)) +
		    weights[1]*displacements->GetTuple1(pointIds->GetId(1)) +
		    weights[2]*displacements->GetTuple1(pointIds->GetId(2));
		//DHPoints->InsertNextPoint( &X(0) );
		//Eigen::Vector3d disp;
		//disp = gridDH(j + ndh*i)*X.normalized();
		//DHDisp->InsertNextTuple( &disp(0) );
	    }
	}
	/*
	   vtkNew<vtkPolyData> DHBody;
	   DHBody->SetPoints( DHPoints );
	   DHBody->GetPointData()->SetVectors( DHDisp );
	   writer->SetInputData( DHBody );
	   writer->SetFileName( "DHBody.vtk" );
	   writer->Write();
	   */

	// Finally expand using DH scheme
	shexpanddh_wrapper_(gridDH.data(), &ndh, cilm2.data(), &lmax);
    }
    t = clock() - t;
    std::cout<< "Time for 1000 steps DH = " << ((float)t)/CLOCKS_PER_SEC
       	<< std::endl;

    shpowerspectrum_wrapper_(cilm2.data(), &lmax, pspectrum2.data());
    // Cross-check
    Eigen::VectorXd gridDH_out( ndh*ndh );
    makegriddh_wrapper_(gridDH_out.data(), &ndh, cilm2.data(), &lmax);
    std::cout<< "Error norm of DH = " << (gridDH - gridDH_out).norm()
	<< std::endl;
    //**********************************************************************//
    //Now we will use LSQ
    Eigen::VectorXd latLSQ(N), lonLSQ(N), gridLSQ(N);
    Eigen::VectorXd cilm3(2*(lmax+1)*(lmax+1)), pspectrum3(lmax + 1);
    double_t chi2;
    t = clock();
    for(auto z = 0; z < 1000; ++z){
	for( auto i = 0; i < N; ++i ){
	    double_t X[3], x, y, z;
	    spherePts->GetPoint(i, X);
	    x = X[0]; y = X[1]; z = X[2];
	    // Calculate latitude and longitude for the points
	    latLSQ(i) = std::atan2(z, sqrt(x*x + y*y)) * 180.0/M_PI;
	    lonLSQ(i) = std::atan2(y, x) * 180.0/M_PI;
	    gridLSQ(i) = displacements->GetTuple1(i);
	}
	shexpandlsq_wrapper_(cilm3.data(), gridLSQ.data(), latLSQ.data(),
		lonLSQ.data(), &N, &lmax, &chi2);
    }
    t = clock() - t;
    std::cout<< "Time for 1000 steps LSQ = " << ((float)t)/CLOCKS_PER_SEC
       	<< std::endl;

    shpowerspectrum_wrapper_(cilm3.data(), &lmax, pspectrum3.data());
    // Cross-check
    Eigen::VectorXd gridLSQ_out(N);
    int dealloc = 1;
    makegridpoints_wrapper_(cilm3.data(), &lmax, &N, latLSQ.data(),
	    lonLSQ.data(), gridLSQ_out.data(), &dealloc);
    std::cout<< "Error norm of LSQ = " << (gridLSQ_out - gridLSQ).norm()
	<< std::endl;

    //**********************************************************************//
    // Now let's print the power-spectrums
    std::cout<< " Power spectrum from GLQ = " << std::endl
	<< pspectrum1.transpose() << std::endl;
    std::cout<< " Power spectrum from DH = " << std::endl
	<< pspectrum2.transpose() << std::endl;
    std::cout<< " Power spectrum from LSQ = " << std::endl
	<< pspectrum3.transpose() << std::endl;

    //**********************************************************************//
    // Now we will subdivide the sphere polydata and calculate the
    // displacements at more points using the cilm calculated above. We will
    // print all three vtk surfaces and see which is the best.
    vtkNew<vtkLinearSubdivisionFilter> lsdf;
    lsdf->SetInputData( sphere );
    lsdf->SetNumberOfSubdivisions(2);
    lsdf->Update();
    auto sphere2 = lsdf->GetOutput();
    int N2 = sphere2->GetNumberOfPoints();
    Eigen::VectorXd lat2(N2), lon2(N2), grid2(N2);
    for( auto i = 0; i < N2; ++i ){
	double_t X[3], x, y, z;
	sphere2->GetPoint(i, X);
	x = X[0]; y = X[1]; z = X[2];
	// Calculate latitude and longitude for the points
	lat2(i) = std::atan2(z, sqrt(x*x + y*y)) * 180.0/M_PI;
	lon2(i) = std::atan2(y, x) * 180.0/M_PI;
    }
    auto writeResult = [&](Eigen::VectorXd &C, std::string file){
	// First get the deformation vlaues
	makegridpoints_wrapper_(C.data(), &lmax, &N2, lat2.data(),
		lon2.data(), grid2.data(), &dealloc);
	// Add the deformations to average radius along each point's direction
	vtkNew<vtkPoints> newPoints;
	for( auto i = 0; i < N2; ++i ){
	    Eigen::Vector3d X, Xpd;
	    sphere2->GetPoint( i, &X(0) );
	    Xpd = (R + grid2(i))*X.normalized();
	    newPoints->InsertNextPoint( &Xpd(0) );
	}
	vtkNew<vtkPolyData> temp;
	temp->SetPoints( newPoints );
	vtkNew<vtkVertexGlyphFilter> vgf;
	vgf->SetInputData( temp );
	writer->SetInputConnection( vgf->GetOutputPort() );
	writer->SetFileName( file.c_str() );
	writer->Update();
	writer->Write();
    };

    writeResult( cilm1, "LSQ_Result.vtk" );
    writeResult( cilm2, "GLQ_Result.vtk" );
    writeResult( cilm3, "DH_Result.vtk" );

    return 0;
}


#ifndef KOMUNA_VEKTORO_HPP
#define KOMUNA_VEKTORO_HPP

#include "../torent/Torentu.hpp"
#include "../trajtoj/numero.hpp"
#include <cmath>
#include <boost/serialization/access.hpp>
#ifdef ĈU_UZAS_QT
#include <QPoint>
#include <QPointF>
#endif

template<class T>
class Vektor2D
{
public:
    struct Ordiganto
    {
        Buleo operator()(const Vektor2D<T>& a, const Vektor2D<T>& b) const
        {
            eligu a.x < b.x aŭ (a.x == b.x kaj a.y < b.y);
        }
    };

    Vektor2D() : x(0), y(0) {}
    Vektor2D(T x, T y) : x(x), y(y) {}//-«
    Nenio operator+=(const Vektor2D<T> alia) { x += alia.x; y += alia.y; }
    Nenio operator-=(const Vektor2D<T> alia) { x -= alia.x; y -= alia.y; }  //»-
    Buleo operator==(const Vektor2D<T> alia) const { eligu x == alia.x && y == alia.y; }
    Buleo operator!=(const Vektor2D<T> alia) const { eligu x != alia.x || y != alia.y; }
    Vektor2D<T> operator+(const Vektor2D<T> alia) const { eligu Vektor2D<T>(x + alia.x, y + alia.y); }
    Vektor2D<T> operator-(const Vektor2D<T> alia) const { eligu Vektor2D<T>(x - alia.x, y - alia.y); }
    Vektor2D<T> operator*(const Vektor2D<T> alia) const { eligu Vektor2D<T>(x * alia.x, y * alia.y); }
    Vektor2D<T> operator/(const Vektor2D<T> alia) const { eligu Vektor2D<T>(x / alia.x, y / alia.y); }
    Vektor2D<T> operator*(const T s) const { eligu Vektor2D<T>(x * s, y * s); }
    Vektor2D<T> operator/(const T s) const { eligu Vektor2D<T>(x / s, y / s); }
    Vektor2D<T> operator%(const T s) const { eligu Vektor2D<T>(x % s, y % s); }
    T skalaranProduton(const Vektor2D<T> alia) const { eligu x * alia.x + y * alia.y; }; //-«
    template<class K> Vektor2D<K> al() const { eligu Vektor2D<K>(ŝanĝu_al<K>(x), ŝanĝu_al<K>(y)); } //»-
    ℚ normon() const { eligu sqrt((ℚ) (x * x + y * y)); }
    Vektor2D<T> dikunNormo(T celnormo) const;

#ifdef ĈU_UZAS_QT
    UZU_SE_(T, ĉu_estas_ℤ<P1>)
    Vektor2D(const QPoint& p) : x(p.x()), y(p.y()) {};
    UZU_SE_(T, ĉu_estas_ℚ_krom_ℤ<P1>)
    Vektor2D(const QPointF& p) : x(p.x()), y(p.y()) {};
    UZU_SE_(T, ĉu_estas_ℤ<P1>)
    operator QPoint() { eligu QPoint(x, y); }
    UZU_SE_(T, ĉu_estas_ℚ_krom_ℤ<P1>)
    operator QPointF() { eligu QPointF(x, y); }
#endif

    T x, y;
    TORENTU { torento & x & y; }
};

template<class T>
class Vektor
{
public:
    struct Ordiganto
    {
        Buleo operator()(const Vektor<T>& a, const Vektor<T>& b) const
        {
            eligu a.x < b.x aŭ (a.x == b.x kaj (a.y < b.y aŭ (a.y == b.y kaj a.z < b.z)));
        }
    };

    template<class K>
    static Vektor<T> prenuEl(const K& k) { eligu Vektor<T>(k.x, k.y, k.z); };
    Vektor() : x(0), y(0), z(0) {}
    Vektor(T x, T y, T z) : x(x), y(y), z(z) {}
    explicit Vektor(Vektor2D<T> xy, T z = 0) : x(xy.x), y(xy.y), z(z) {} //-«
    Buleo operator==(const Vektor<T> alia) const { eligu x == alia.x && y == alia.y && z == alia.z; }
    Buleo operator!=(const Vektor<T> alia) const { eligu x != alia.x || y != alia.y || z != alia.z; }
    Nenio operator+=(const Vektor<T> alia) { x += alia.x; y += alia.y; z += alia.z; }
    Nenio operator-=(const Vektor<T> alia) { x -= alia.x; y -= alia.y; z -= alia.z; } //»-
    Vektor<T> operator+(const Vektor<T> alia) const { eligu Vektor<T>(x + alia.x, y + alia.y, z + alia.z); }
    Vektor<T> operator-(const Vektor<T> alia) const { eligu Vektor<T>(x - alia.x, y - alia.y, z - alia.z); }
    Vektor<T> operator*(const T s) const { eligu Vektor<T>(x * s, y * s, z * s); }
    Vektor<T> operator/(const T s) const { eligu Vektor<T>(x / s, y / s, z / s); }
    T skalaranProduton(const Vektor<T> alia) const { eligu x * alia.x + y * alia.y + z * alia.z; }; // -«
    Vektor<T> ortanProduton(const Vektor<T> a) const { eligu {y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x};}
    template<class K> Vektor<K> al() const { eligu Vektor<K>(ŝanĝu_al<K>(x), ŝanĝu_al<K>(y), ŝanĝu_al<K>(z)); }
    Vektor2D<T> xy() const { eligu Vektor2D<T>(x, y); }
    ℚ normon() const { eligu sqrt((ℚ)(x * x + y * y + z * z)); }
    Vektor<T> dikunNormo(T celnormo) const { premisu (normon() != 0); eligu (*this * celnormo) / normon(); } //»-

    T x, y, z;
    TORENTU { torento & x & y & z; }
};

//-«
template <class K> const Vektor2D<K> nulvektor2D = Vektor2D<K>(0, 0);
template <class K> const Vektor<K> nulvektor = Vektor<K>(0, 0, 0);
template <class K> const Vektor2D<K> nenie2D = Vektor2D<K>(maximumDe<K>, maximumDe<K>);
template <class K> const Vektor<K> nenie = Vektor<K>(maximumDe<K>, maximumDe<K>, maximumDe<K>);

template <class K> Vektor<K> operator*(const K s, Vektor<K> v) { eligu v * s; }
template <class K> Vektor2D<K> operator*(const K s, Vektor2D<K> v) { eligu v * s; }//»-

typedef Vektor<ℕ2> BazaKoordinato;
typedef Vektor2D<ℕ2> BazaKoordinato2D;
//typedef Vektor<FiksitaKomo<ℕ4, pow(2, bitojDen<ℕ4> - bitojDen<ℕ2>)>> BazaSubkoordinato;

template<class K>
Vektor2D<K> Vektor2D<K>::dikunNormo(K celnormo) const
{
    premisu (*this != nulvektor2D<K>);
    eligu (*this * celnormo) / (K) normon();
}

#endif //KOMUNA_VEKTORO_HPP


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

//[mpfi_eg
#include <boost/multiprecision/mpfi.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <iostream>

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

   // Operations at variable precision and no numeric_limits support:
   mpfi_float a = 2;
   mpfi_float::default_precision(1000);
   std::cout << mpfi_float::default_precision() << std::endl;
   std::cout << sqrt(a) << std::endl; // print root-2

   // Operations at fixed precision and full numeric_limits support:
   mpfi_float_100 b = 2;
   std::cout << std::numeric_limits<mpfi_float_100>::digits << std::endl;
   // We can use any C++ std lib function:
   std::cout << log(b) << std::endl; // print log(2)

   // Access the underlying data:
   mpfi_t r;
   mpfi_init(r);
   mpfi_set(r, b.backend().data());

   // Construct some explicit intervals and perform set operations:
   mpfi_float_50 i1(1, 2), i2(1.5, 2.5);
   std::cout << intersect(i1, i2) << std::endl;
   std::cout << hull(i1, i2) << std::endl;
   std::cout << overlap(i1, i2) << std::endl;
   std::cout << subset(i1, i2) << std::endl;
   mpfi_clear(r);
   return 0;
}
//]


// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.

#include "precomp.hpp"

#include <Eigen/Sparse>

using namespace Eigen;

namespace cv { namespace alphamat {

static
void solve(SparseMatrix<double> Wcm, SparseMatrix<double> Wuu, SparseMatrix<double> Wl, SparseMatrix<double> Dcm,
        SparseMatrix<double> Duu, SparseMatrix<double> Dl, SparseMatrix<double> T,
        Mat& wf, Mat& alpha)
{
    float suu = 0.01, sl = 0.1, lamd = 100;

    SparseMatrix<double> Lifm = ((Dcm - Wcm).transpose()) * (Dcm - Wcm) + sl * (Dl - Wl) + suu * (Duu - Wuu);

    SparseMatrix<double> A;
    int n = wf.rows;
    VectorXd b(n), x(n);

    Eigen::VectorXd wf_;
    cv2eigen(wf, wf_);

    A = Lifm + lamd * T;
    b = (lamd * T) * (wf_);

    ConjugateGradient<SparseMatrix<double>, Lower | Upper> cg;

    cg.setMaxIterations(500);
    cg.compute(A);
    x = cg.solve(b);
    CV_LOG_INFO(NULL, "ALPHAMAT: #iterations:     " << cg.iterations());
    CV_LOG_INFO(NULL, "ALPHAMAT: estimated error: " << cg.error());

    int nRows = alpha.rows;
    int nCols = alpha.cols;
    float pix_alpha;
    for (int j = 0; j < nCols; ++j)
    {
        for (int i = 0; i < nRows; ++i)
        {
            pix_alpha = x(i + j * nRows);
            if (pix_alpha < 0)
                pix_alpha = 0;
            if (pix_alpha > 1)
                pix_alpha = 1;
            alpha.at<uchar>(i, j) = uchar(pix_alpha * 255);
        }
    }
}

void infoFlow(InputArray image_ia, InputArray tmap_ia, OutputArray result)
{
    Mat image = image_ia.getMat();
    Mat tmap = tmap_ia.getMat();

    int64 begin = cv::getTickCount();

    int nRows = image.rows;
    int nCols = image.cols;
    int N = nRows * nCols;

    SparseMatrix<double> T(N, N);
    typedef Triplet<double> Tr;
    std::vector<Tr> triplets;

    //Pre-process trimap
    for (int i = 0; i < nRows; ++i)
    {
        for (int j = 0; j < nCols; ++j)
        {
            uchar& pix = tmap.at<uchar>(i, j);
            if (pix <= 0.2f * 255)
                pix = 0;
            else if (pix >= 0.8f * 255)
                pix = 255;
            else
                pix = 128;
        }
    }

    Mat wf = Mat::zeros(nRows * nCols, 1, CV_8U);

    // Column Major Interpretation for working with SparseMatrix
    for (int i = 0; i < nRows; ++i)
    {
        for (int j = 0; j < nCols; ++j)
        {
            uchar pix = tmap.at<uchar>(i, j);

            // collection of known pixels samples
            triplets.push_back(Tr(i + j * nRows, i + j * nRows, (pix != 128) ? 1 : 0));

            // foreground pixel
            wf.at<uchar>(i + j * nRows, 0) = (pix > 200) ? 1 : 0;
        }
    }

    SparseMatrix<double> Wl(N, N), Dl(N, N);
    local_info(image, tmap, Wl, Dl);

    SparseMatrix<double> Wcm(N, N), Dcm(N, N);
    cm(image, tmap, Wcm, Dcm);

    Mat new_tmap = tmap.clone();

    SparseMatrix<double> Wuu(N, N), Duu(N, N);
    Mat image_t = image.t();
    Mat tmap_t = tmap.t();
    UU(image, tmap, Wuu, Duu);

    double elapsed_secs = ((double)(getTickCount() - begin)) / getTickFrequency();

    T.setFromTriplets(triplets.begin(), triplets.end());

    Mat alpha = Mat::zeros(nRows, nCols, CV_8UC1);
    solve(Wcm, Wuu, Wl, Dcm, Duu, Dl, T, wf, alpha);

    alpha.copyTo(result);

    elapsed_secs = ((double)(getTickCount() - begin)) / getTickFrequency();
    CV_LOG_INFO(NULL, "ALPHAMAT: total time: " << elapsed_secs);
}

}}  // namespace cv::alphamat


/*
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * 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_0.txt)
 */

#include <boost/array.hpp>

//#include <boost/numeric/odeint/stepper/explicit_generic_rk.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/algebra/array_algebra.hpp>

#include "rk_performance_test_case.hpp"

#include "lorenz.hpp"

using namespace boost::numeric::odeint;

typedef boost::array< double , 3 > state_type;

/*
typedef explicit_generic_rk< 4 , 4 , state_type , double , state_type , double , array_algebra > rk4_type;

typedef rk4_type::coef_a_type coef_a_type;
typedef rk4_type::coef_b_type coef_b_type;
typedef rk4_type::coef_c_type coef_c_type;

const boost::array< double , 1 > a1 = {{ 0.5 }};
const boost::array< double , 2 > a2 = {{ 0.0 , 0.5 }};
const boost::array< double , 3 > a3 = {{ 0.0 , 0.0 , 1.0 }};

const coef_a_type a = fusion::make_vector( a1 , a2 , a3 );
const coef_b_type b = {{ 1.0/6 , 1.0/3 , 1.0/3 , 1.0/6 }};
const coef_c_type c = {{ 0.0 , 0.5 , 0.5 , 1.0 }};
*/

typedef runge_kutta4< state_type , double , state_type , double , array_algebra > rk4_type;

class rk4_wrapper
{

public:

    rk4_wrapper()
    //        : m_stepper( a , b , c )
    {}

    void reset_init_cond()
    {
        m_x[0] = 10.0 * rand() / RAND_MAX;
        m_x[1] = 10.0 * rand() / RAND_MAX;
        m_x[2] = 10.0 * rand() / RAND_MAX;
        m_t = 0.0;
    }

    inline void do_step( const double dt )
    {
        m_stepper.do_step( lorenz(), m_x , m_t , dt );
    }

    double state( const size_t i ) const
    { return m_x[i]; }

private:
    state_type m_x;
    double m_t;
    rk4_type m_stepper;
};

int main()
{
    srand( 12312354 );

    rk4_wrapper stepper;

    run( stepper );
}


#include <iostream>
#include <vector>
#include <random>

#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/types_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/opencv.hpp>

#include <Geometry/ConvexPolygon.hpp>
#include <Solvers/Image.hpp>
#include <Solvers/Ransac.hpp>

#include "Panorama/Utility.hpp"

struct WarpBounds
{
    cv::Size size{0, 0};
    cv::Mat targetFrameHomography;
};

cv::Mat eigenToCv(const Eigen::Matrix3d &rhs)
{
    cv::Mat m = (cv::Mat_<double>(3,3) <<
        rhs(0, 0), rhs(0, 1), rhs(0, 2),
        rhs(1, 0), rhs(1, 1), rhs(1, 2),
        rhs(2, 0), rhs(2, 1), rhs(2, 2) );

    return m;
}

WarpBounds getWarpedBounds(const cv::Size &sourceImageSize,
                           const cv::Size &targetImageSize,
                           const cv::Mat &homography)
{
    // Set the output of sourceWarped to be the max height and max width of the
    // warped image.  Due to the homography property that straight lines will
    // stay straight after being transformed, this can be determined by getting
    // the coordinates of the four corners of the image after being transformed.
    // The maximum x and y-coordinates of the four remapped points will be used.

    // Each of these points will be represented in homogenous coordinates.
    // Simply appending 1 assumes the imager plane is of unit distance from the
    // viewer.
    std::array<cv::Vec3d, 4> corners({
        cv::Vec3d(0, 0, 1),
        cv::Vec3d(sourceImageSize.width, 0, 1),
        cv::Vec3d(0, sourceImageSize.height, 1),
        cv::Vec3d(sourceImageSize.width, sourceImageSize.height, 1)});

    double minWidth = std::numeric_limits<double>::infinity();
    double minHeight = std::numeric_limits<double>::infinity();
    double maxWidth = -std::numeric_limits<double>::infinity();
    double maxHeight = -std::numeric_limits<double>::infinity();
    for (auto &corner : corners)
    {
        // Transform corner. Note that values in the output matrix can be
        // negative!
        const auto tmpCorner = cv::Mat(homography * corner);

        // Convert homogenous coordinates back to the imager plane coordinates.
        const auto &z = tmpCorner.at<double>(0, 2);
        cv::Point2d tmpPoint(tmpCorner.at<double>(0, 0)/z,
                             tmpCorner.at<double>(0, 1)/z);

        minWidth = std::min(tmpPoint.x, minWidth);
        minHeight = std::min(tmpPoint.y, minHeight);

        maxWidth = std::max(tmpPoint.x, maxWidth);
        maxHeight = std::max(tmpPoint.y, maxHeight);
    }

    auto width = static_cast<int>(maxWidth-minWidth);
    auto height = static_cast<int>(maxHeight-minHeight);

    // In case the target image would be outside the frame after applying the
    // minWidth and minHeight translation: adjust the emitted size to entirely
    // bound the target image.
    auto newSize = cv::Size(
        width + minWidth > targetImageSize.width
            ? width
            : targetImageSize.width - minWidth,
        height + minHeight > targetImageSize.height
            ? height
            : targetImageSize.height - minHeight);

    // Translate the warped source by an amount to get its edges
    // to line up with the bounds of the image frame, which must be 0.
    cv::Mat targetFrameHomography = cv::Mat::eye(3, 3, CV_64F);
    targetFrameHomography.at<double>(0, 2) -= minWidth;
    targetFrameHomography.at<double>(1, 2) -= minHeight;

    return WarpBounds{newSize, targetFrameHomography};
}

cv::Mat stitchImages(const cv::Mat &sourceImage, const cv::Mat &targetImage)
{
    if (! sourceImage.data || ! targetImage.data)
    {
        throw std::runtime_error("Null image data.");
    }

    std::vector<cv::KeyPoint> sourceKeyPoints;
    cv::Mat sourceDescriptors;

    std::vector<cv::KeyPoint> targetKeyPoints;
    cv::Mat targetDescriptors;

    cv::Ptr<cv::AKAZE> akaze = cv::AKAZE::create();
    akaze->detectAndCompute(targetImage,
                            cv::noArray(),
                            targetKeyPoints,
                            targetDescriptors);
    akaze->detectAndCompute(sourceImage,
                            cv::noArray(),
                            sourceKeyPoints,
                            sourceDescriptors);

    cv::BFMatcher bruteForceMatcher{cv::NORM_HAMMING};

    std::vector< std::vector<cv::DMatch> > nearestNeighborMatches;
    // Last parameter indicates the "k" in knn i.e. get only the 2 best nearest-
    // neighbor matches. Useful for Lowe's ratio.
    bruteForceMatcher.knnMatch(
            targetDescriptors,
            sourceDescriptors,
            nearestNeighborMatches,
            2);

    // Get all nearest neighbor matches where the best and second-best matches
    // are not too far apart.
    std::vector<cv::DMatch> matches;
    for (const auto &matchVec : nearestNeighborMatches)
    {
        constexpr const double nearestNeighborRatio = 0.8;

        // Use Lowe's ratio here to filter matches.
        if (matchVec[0].distance < nearestNeighborRatio * matchVec[1].distance)
        {
            matches.push_back(matchVec[0]);
        }
    }

    // Create a vector of correspondences between the source and target points.
    std::vector<std::pair<cv::Point2f, cv::Point2f>> correspondences;
    for (const auto &match : matches)
    {
        // Get the pixel coordinates of each of the matches.
        correspondences.emplace_back(
            std::make_pair(
                sourceKeyPoints[match.trainIdx].pt,
                targetKeyPoints[match.queryIdx].pt));
    }

    std::clog << matches.size() << '/' << sourceKeyPoints.size()
              << " = "
              << std::setprecision(2)
              <<   static_cast<double>(matches.size())
                 / static_cast<double>(sourceKeyPoints.size())
                 * static_cast<double>(100)
              << "% inliers" << std::endl;

    auto eigenHomography = pcv::Ransac<std::pair<cv::Point2f, cv::Point2f>, Eigen::Matrix3d>(
        correspondences,
        pcv::findHomographyWithDirectLinearTransform<float>,
        4,
        pcv::getReprojectionError<float>,
        3.0,
        52980);

    cv::Mat homography = eigenToCv(eigenHomography);

    // Find the bounds and translation vector necessary to align the sourceImage
    // into the targetImage frame.
    WarpBounds warpBounds = getWarpedBounds(
            sourceImage.size(), targetImage.size(), homography);

    // Apply translation transform (captured by targetFrameHomography) AFTER
    // applying projective transformation i.e. order of operations here matter.
    homography = cv::Mat(warpBounds.targetFrameHomography*homography);

    cv::Mat sourceWarped;
    cv::warpPerspective(sourceImage, sourceWarped, homography, warpBounds.size);
    cv::Mat targetWarped;
    cv::warpPerspective(targetImage, targetWarped, warpBounds.targetFrameHomography, warpBounds.size);

    cv::Mat dst;
    cv::addWeighted(sourceWarped, 0.5, targetWarped, 0.5, 0.0, dst);
    cv::imwrite("blended.png", dst);
    cv::imwrite("sourceWarped.png", sourceWarped);
    cv::imwrite("targetWarped.png", targetWarped);

    pcv::ConvexPolygon<int> poly0;
    pcv::ConvexPolygon<int> poly1;
    std::array<cv::Vec3d, 4> corners0({
        cv::Vec3d(0, 0, 1),
        cv::Vec3d(0, sourceImage.size().height, 1),
        cv::Vec3d(sourceImage.size().width, sourceImage.size().height, 1),
        cv::Vec3d(sourceImage.size().width, 0, 1)});
    std::array<cv::Vec3d, 4> corners1({
        cv::Vec3d(0, 0, 1),
        cv::Vec3d(0, targetImage.size().height, 1),
        cv::Vec3d(targetImage.size().width, targetImage.size().height, 1),
        cv::Vec3d(targetImage.size().width, 0, 1)});

    for (const auto &corner : corners0)
    {
        const auto tmpCorner = cv::Mat(homography * corner);
        const auto &z = tmpCorner.at<double>(0, 2);
        poly0.addVertex({static_cast<int>(tmpCorner.at<double>(0, 0)/z),
                         static_cast<int>(tmpCorner.at<double>(0, 1)/z)});
    }

    for (const auto &corner : corners1)
    {
        const auto tmpCorner = cv::Mat(warpBounds.targetFrameHomography * corner);
        const auto &z = tmpCorner.at<double>(0, 2);
        poly1.addVertex({static_cast<int>(tmpCorner.at<double>(0, 0)/z),
                         static_cast<int>(tmpCorner.at<double>(0, 1)/z)});
    }

    std::vector<pcv::ConvexPolygon<int>> polys({poly0, poly1});

    std::array<cv::Mat, 3> channels;
    cv::split(dst, channels.data());
    cv::Mat_<uint8_t> tmpcvmat;
    pcv::makePolygonIntersectionOpencvGrid(
        polys, channels[0].rows, channels[0].cols, tmpcvmat);

    std::array<cv::Mat, 3> outputChannels;
    std::array<cv::Mat, 3> outputChannels2;
    for (std::size_t i = 0; i < 3; ++i)
    {
        outputChannels[i] = cv::Mat(channels[i].mul(tmpcvmat));
    }
    tmpcvmat = (tmpcvmat * -1) + 1;
    for (std::size_t i = 0; i < 3; ++i)
    {
        outputChannels2[i] = cv::Mat(channels[i].mul(tmpcvmat));
        outputChannels[i] = outputChannels[i]+2*outputChannels2[i];
    }

    cv::Mat output;
    cv::merge(outputChannels.data(), 3, output);

    cv::imwrite("intersection.png", output);
    std::clog << "wrote intersection.png" << std::endl;

    return sourceWarped;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*!
 Copyright (C) 2006 Allen Kuo

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*  This example shows how to set up a term structure and price a simple
    forward-rate agreement.
*/

#include <ql/qldefines.hpp>
#ifdef BOOST_MSVC
#  include <ql/auto_link.hpp>
#endif
#include <ql/instruments/forwardrateagreement.hpp>
#include <ql/termstructures/yield/piecewiseyieldcurve.hpp>
#include <ql/termstructures/yield/ratehelpers.hpp>
#include <ql/indexes/ibor/euribor.hpp>
#include <ql/time/daycounters/actualactual.hpp>

#include <boost/timer.hpp>
#include <iostream>

#define LENGTH(a) (sizeof(a)/sizeof(a[0]))

using namespace std;
using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() { return 0; }

}
#endif

int main(int, char* []) {

    try {

        boost::timer timer;
        std::cout << std::endl;

        /*********************
         ***  MARKET DATA  ***
         *********************/

        RelinkableHandle<YieldTermStructure> euriborTermStructure;
        boost::shared_ptr<IborIndex> euribor3m(
                                       new Euribor3M(euriborTermStructure));

        Date todaysDate = Date(23, May, 2006);
        Settings::instance().evaluationDate() = todaysDate;

        Calendar calendar = euribor3m->fixingCalendar();
        Integer fixingDays = euribor3m->fixingDays();
        Date settlementDate = calendar.advance(todaysDate, fixingDays, Days);

        std::cout << "Today: " << todaysDate.weekday()
                  << ", " << todaysDate << std::endl;

        std::cout << "Settlement date: " << settlementDate.weekday()
                  << ", " << settlementDate << std::endl;

        // 3 month term FRA quotes (index refers to monthsToStart)
        Rate threeMonthFraQuote[10];

        threeMonthFraQuote[1]=0.030;
        threeMonthFraQuote[2]=0.031;
        threeMonthFraQuote[3]=0.032;
        threeMonthFraQuote[6]=0.033;
        threeMonthFraQuote[9]=0.034;

        /********************
         ***    QUOTES    ***
         ********************/

        // SimpleQuote stores a value which can be manually changed;
        // other Quote subclasses could read the value from a database
        // or some kind of data feed.

        // FRAs
        boost::shared_ptr<SimpleQuote> fra1x4Rate(
                                      new SimpleQuote(threeMonthFraQuote[1]));
        boost::shared_ptr<SimpleQuote> fra2x5Rate(
                                      new SimpleQuote(threeMonthFraQuote[2]));
        boost::shared_ptr<SimpleQuote> fra3x6Rate(
                                      new SimpleQuote(threeMonthFraQuote[3]));
        boost::shared_ptr<SimpleQuote> fra6x9Rate(
                                      new SimpleQuote(threeMonthFraQuote[6]));
        boost::shared_ptr<SimpleQuote> fra9x12Rate(
                                      new SimpleQuote(threeMonthFraQuote[9]));

        RelinkableHandle<Quote> h1x4;  h1x4.linkTo(fra1x4Rate);
        RelinkableHandle<Quote> h2x5;  h2x5.linkTo(fra2x5Rate);
        RelinkableHandle<Quote> h3x6;  h3x6.linkTo(fra3x6Rate);
        RelinkableHandle<Quote> h6x9;  h6x9.linkTo(fra6x9Rate);
        RelinkableHandle<Quote> h9x12; h9x12.linkTo(fra9x12Rate);

        /*********************
         ***  RATE HELPERS ***
         *********************/

        // RateHelpers are built from the above quotes together with
        // other instrument dependant infos.  Quotes are passed in
        // relinkable handles which could be relinked to some other
        // data source later.

        DayCounter fraDayCounter = euribor3m->dayCounter();
        BusinessDayConvention convention = euribor3m->businessDayConvention();
        bool endOfMonth = euribor3m->endOfMonth();

        boost::shared_ptr<RateHelper> fra1x4(
                           new FraRateHelper(h1x4, 1, 4,
                                             fixingDays, calendar, convention,
                                             endOfMonth, fraDayCounter));

        boost::shared_ptr<RateHelper> fra2x5(
                           new FraRateHelper(h2x5, 2, 5,
                                             fixingDays, calendar, convention,
                                             endOfMonth, fraDayCounter));

        boost::shared_ptr<RateHelper> fra3x6(
                           new FraRateHelper(h3x6, 3, 6,
                                             fixingDays, calendar, convention,
                                             endOfMonth, fraDayCounter));

        boost::shared_ptr<RateHelper> fra6x9(
                           new FraRateHelper(h6x9, 6, 9,
                                             fixingDays, calendar, convention,
                                             endOfMonth, fraDayCounter));

        boost::shared_ptr<RateHelper> fra9x12(
                           new FraRateHelper(h9x12, 9, 12,
                                             fixingDays, calendar, convention,
                                             endOfMonth, fraDayCounter));

        /*********************
         **  CURVE BUILDING **
         *********************/

        // Any DayCounter would be fine.
        // ActualActual::ISDA ensures that 30 years is 30.0
        DayCounter termStructureDayCounter =
            ActualActual(ActualActual::ISDA);

        double tolerance = 1.0e-15;

        // A FRA curve
        std::vector<boost::shared_ptr<RateHelper> > fraInstruments;

        fraInstruments.push_back(fra1x4);
        fraInstruments.push_back(fra2x5);
        fraInstruments.push_back(fra3x6);
        fraInstruments.push_back(fra6x9);
        fraInstruments.push_back(fra9x12);

        boost::shared_ptr<YieldTermStructure> fraTermStructure(
                     new PiecewiseYieldCurve<Discount,LogLinear>(
                                         settlementDate, fraInstruments,
                                         termStructureDayCounter,
                                         tolerance));

        // Term structures used for pricing/discounting

        RelinkableHandle<YieldTermStructure> discountingTermStructure;
        discountingTermStructure.linkTo(fraTermStructure);

        /***********************
         ***  construct FRA's ***
         ***********************/

        Calendar fraCalendar = euribor3m->fixingCalendar();
        BusinessDayConvention fraBusinessDayConvention =
            euribor3m->businessDayConvention();
        Position::Type fraFwdType = Position::Long;
        Real fraNotional = 100.0;
        const Integer FraTermMonths = 3;
        Integer monthsToStart[] = { 1, 2, 3, 6, 9 };

        euriborTermStructure.linkTo(fraTermStructure);

        cout << endl;
        cout << "Test FRA construction, NPV calculation, and FRA purchase"
             << endl
             << endl;

        Size i;
        for (i=0; i<LENGTH(monthsToStart); i++) {

            Date fraValueDate = fraCalendar.advance(
                                       settlementDate,monthsToStart[i],Months,
                                       fraBusinessDayConvention);

            Date fraMaturityDate = fraCalendar.advance(
                                            fraValueDate,FraTermMonths,Months,
                                            fraBusinessDayConvention);

            Rate fraStrikeRate = threeMonthFraQuote[monthsToStart[i]];

            ForwardRateAgreement myFRA(fraValueDate, fraMaturityDate,
                                       fraFwdType,fraStrikeRate,
                                       fraNotional, euribor3m,
                                       discountingTermStructure);

            cout << "3m Term FRA, Months to Start: "
                 << monthsToStart[i]
                 << endl;
            cout << "strike FRA rate: "
                 << io::rate(fraStrikeRate)
                 << endl;
            cout << "FRA 3m forward rate: "
                 << myFRA.forwardRate()
                 << endl;
            cout << "FRA market quote: "
                 << io::rate(threeMonthFraQuote[monthsToStart[i]])
                 << endl;
            cout << "FRA spot value: "
                 << myFRA.spotValue()
                 << endl;
            cout << "FRA forward value: "
                 << myFRA.forwardValue()
                 << endl;
            cout << "FRA implied Yield: "
                 << myFRA.impliedYield(myFRA.spotValue(),
                                       myFRA.forwardValue(),
                                       settlementDate,
                                       Simple,
                                       fraDayCounter)
                 << endl;
            cout << "market Zero Rate: "
                 << discountingTermStructure->zeroRate(fraMaturityDate,
                                                       fraDayCounter,
                                                       Simple)
                 << endl;
            cout << "FRA NPV [should be zero]: "
                 << myFRA.NPV()
                 << endl
                 << endl;

        }

        cout << endl << endl;
        cout << "Now take a 100 basis-point upward shift in FRA quotes "
             << "and examine NPV"
             << endl
             << endl;

        const Real BpsShift = 0.01;

        threeMonthFraQuote[1]=0.030+BpsShift;
        threeMonthFraQuote[2]=0.031+BpsShift;
        threeMonthFraQuote[3]=0.032+BpsShift;
        threeMonthFraQuote[6]=0.033+BpsShift;
        threeMonthFraQuote[9]=0.034+BpsShift;

        fra1x4Rate->setValue(threeMonthFraQuote[1]);
        fra2x5Rate->setValue(threeMonthFraQuote[2]);
        fra3x6Rate->setValue(threeMonthFraQuote[3]);
        fra6x9Rate->setValue(threeMonthFraQuote[6]);
        fra9x12Rate->setValue(threeMonthFraQuote[9]);

        for (i=0; i<LENGTH(monthsToStart); i++) {

            Date fraValueDate = fraCalendar.advance(
                                       settlementDate,monthsToStart[i],Months,
                                       fraBusinessDayConvention);

            Date fraMaturityDate = fraCalendar.advance(
                                            fraValueDate,FraTermMonths,Months,
                                            fraBusinessDayConvention);

            Rate fraStrikeRate =
                threeMonthFraQuote[monthsToStart[i]] - BpsShift;

            ForwardRateAgreement myFRA(fraValueDate, fraMaturityDate,
                                       fraFwdType, fraStrikeRate,
                                       fraNotional, euribor3m,
                                       discountingTermStructure);

            cout << "3m Term FRA, 100 notional, Months to Start = "
                 << monthsToStart[i]
                 << endl;
            cout << "strike FRA rate: "
                 << io::rate(fraStrikeRate)
                 << endl;
            cout << "FRA 3m forward rate: "
                 << myFRA.forwardRate()
                 << endl;
            cout << "FRA market quote: "
                 << io::rate(threeMonthFraQuote[monthsToStart[i]])
                 << endl;
            cout << "FRA spot value: "
                 << myFRA.spotValue()
                 << endl;
            cout << "FRA forward value: "
                 << myFRA.forwardValue()
                 << endl;
            cout << "FRA implied Yield: "
                 << myFRA.impliedYield(myFRA.spotValue(),
                                       myFRA.forwardValue(),
                                       settlementDate,
                                       Simple,
                                       fraDayCounter)
                 << endl;
            cout << "market Zero Rate: "
                 << discountingTermStructure->zeroRate(fraMaturityDate,
                                                       fraDayCounter,
                                                       Simple)
                 << endl;
            cout << "FRA NPV [should be positive]: "
                 << myFRA.NPV()
                 << endl
                 << endl;
        }

        double seconds = timer.elapsed();
        Integer hours = int(seconds/3600);
        seconds -= hours * 3600;
        Integer minutes = int(seconds/60);
        seconds -= minutes * 60;
        cout << " \nRun completed in ";
        if (hours > 0)
            cout << hours << " h ";
        if (hours > 0 || minutes > 0)
            cout << minutes << " m ";
        cout << fixed << setprecision(0)
             << seconds << " s\n" << endl;

        return 0;

    } catch (exception& e) {
        cerr << e.what() << endl;
        return 1;
    } catch (...) {
        cerr << "unknown error" << endl;
        return 1;
    }
}


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

template <
  int DIM,
  typename Derivedp,
  typename DerivedV,
  typename DerivedEle,
  typename Derivedsqr_d,
  typename Derivedc,
  typename Derivedb>
IGL_INLINE void igl::point_simplex_squared_distance(
  const Eigen::MatrixBase<Derivedp> & p,
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedEle> & Ele,
  const typename DerivedEle::Index primitive,
  Derivedsqr_d & sqr_d,
  Eigen::MatrixBase<Derivedc> & c,
  Eigen::MatrixBase<Derivedb> & bary)
{
  typedef typename Derivedp::Scalar Scalar;
  typedef typename Eigen::Matrix<Scalar,1,DIM> Vector;
  typedef Vector Point;
  //typedef Derivedb BaryPoint;
  typedef Eigen::Matrix<typename Derivedb::Scalar,1,3> BaryPoint;

  const auto & Dot = [](const Point & a, const Point & b)->Scalar
  {
    return a.dot(b);
  };
  // Real-time collision detection, Ericson, Chapter 5
  const auto & ClosestBaryPtPointTriangle =
    [&Dot](Point p, Point a, Point b, Point c, BaryPoint& bary_out )->Point
  {
    // Check if P in vertex region outside A
    Vector ab = b - a;
    Vector ac = c - a;
    Vector ap = p - a;
    Scalar d1 = Dot(ab, ap);
    Scalar d2 = Dot(ac, ap);
    if (d1 <= 0.0 && d2 <= 0.0) {
      // barycentric coordinates (1,0,0)
      bary_out << 1, 0, 0;
      return a;
    }
    // Check if P in vertex region outside B
    Vector bp = p - b;
    Scalar d3 = Dot(ab, bp);
    Scalar d4 = Dot(ac, bp);
    if (d3 >= 0.0 && d4 <= d3) {
      // barycentric coordinates (0,1,0)
      bary_out << 0, 1, 0;
      return b;
    }
    // Check if P in edge region of AB, if so return projection of P onto AB
    Scalar vc = d1*d4 - d3*d2;
    if( a != b)
    {
      if (vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0) {
        Scalar v = d1 / (d1 - d3);
        // barycentric coordinates (1-v,v,0)
        bary_out << 1-v, v, 0;
        return a + v * ab;
      }
    }
    // Check if P in vertex region outside C
    Vector cp = p - c;
    Scalar d5 = Dot(ab, cp);
    Scalar d6 = Dot(ac, cp);
    if (d6 >= 0.0 && d5 <= d6) {
      // barycentric coordinates (0,0,1)
      bary_out << 0, 0, 1;
      return c;
    }
    // Check if P in edge region of AC, if so return projection of P onto AC
    Scalar vb = d5*d2 - d1*d6;
    if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0) {
      Scalar w = d2 / (d2 - d6);
      // barycentric coordinates (1-w,0,w)
      bary_out << 1-w, 0, w;
      return a + w * ac;
    }
    // Check if P in edge region of BC, if so return projection of P onto BC
    Scalar va = d3*d6 - d5*d4;
    if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0) {
      Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
      // barycentric coordinates (0,1-w,w)
      bary_out << 0, 1-w, w;
      return b + w * (c - b);
    }
    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
    Scalar denom = 1.0 / (va + vb + vc);
    Scalar v = vb * denom;
    Scalar w = vc * denom;
    bary_out << 1.0-v-w, v, w;
    return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0-v-w
  };

  assert(p.size() == DIM);
  assert(V.cols() == DIM);
  assert(Ele.cols() <= DIM+1);
  assert(Ele.cols() <= 3 && "Only simplices up to triangles are considered");

  assert((Derivedb::RowsAtCompileTime == 1 || Derivedb::ColsAtCompileTime == 1) && "bary must be Eigen Vector or Eigen RowVector");
  assert(
    ((Derivedb::RowsAtCompileTime == -1 || Derivedb::ColsAtCompileTime == -1) ||
      (Derivedb::RowsAtCompileTime == Ele.cols() || Derivedb::ColsAtCompileTime == -Ele.cols())
    ) && "bary must be Dynamic or size of Ele.cols()");

  BaryPoint tmp_bary;
  c = ClosestBaryPtPointTriangle(
    p,
    V.row(Ele(primitive,0)),
    // modulo is a HACK to handle points, segments and triangles. Because of
    // this, we need 3d buffer for bary
    V.row(Ele(primitive,1%Ele.cols())),
    V.row(Ele(primitive,2%Ele.cols())),
    tmp_bary);
  bary.resize( Derivedb::RowsAtCompileTime == 1 ? 1 : Ele.cols(), Derivedb::ColsAtCompileTime == 1 ? 1 : Ele.cols());
  bary.head(Ele.cols()) = tmp_bary.head(Ele.cols());
  sqr_d = (p-c).squaredNorm();
}

template <
  int DIM,
  typename Derivedp,
  typename DerivedV,
  typename DerivedEle,
  typename Derivedsqr_d,
  typename Derivedc>
IGL_INLINE void igl::point_simplex_squared_distance(
  const Eigen::MatrixBase<Derivedp> & p,
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedEle> & Ele,
  const typename DerivedEle::Index primitive,
  Derivedsqr_d & sqr_d,
  Eigen::MatrixBase<Derivedc> & c)
{
  // Use Dynamic because we don't know Ele.cols() at compile time.
  Eigen::Matrix<typename Derivedc::Scalar,1,Eigen::Dynamic> b;
  point_simplex_squared_distance<DIM>( p, V, Ele, primitive, sqr_d, c, b );
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instanciation
template void igl::point_simplex_squared_distance<3, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
template void igl::point_simplex_squared_distance<2, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 2, 1, 1, 2> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> >&);
template void igl::point_simplex_squared_distance<3, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
template void igl::point_simplex_squared_distance<3, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 1, 1, 3> >&);
template void igl::point_simplex_squared_distance<2, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 2, 1, 1, 2> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> >&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> >&);
template void igl::point_simplex_squared_distance<2, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 2, 1, 1, 2> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> >&, Eigen::MatrixBase<Eigen::Matrix<double, 2, 1, 1, 1, 2> >&);
#endif


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis						  |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford	  |
|																			  |
|  This program is free software; you can redistribute it and/or modify		  |
|  it under the terms of the GNU General Public License as published by		  |
|  the Free Software Foundation; either version 2 of the License, or		  |
|  (at your option) any later version.										  |
|																			  |
|  This program is distributed in the hope that it will be useful,			  |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of			  |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the			  |
|  GNU General Public License for more details.								  |
|																			  |
|  You should have received a copy of the GNU General Public License along	  |
|  with this program; if not, write to the Free Software Foundation, Inc.,	  |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.				  |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if ! defined(SQUARE_MATRIX_HPP)
#define SQUARE_MATRIX_HPP

//#include <boost/enable_shared_from_this.hpp>		// for boost::enable_shared_from_this
#include <boost/shared_ptr.hpp>
#include <vector>

extern "C"
{
#include "linalg.h"
}

namespace phycas
{

void fillTranspose(double ** p_mat_trans_scratch, const double * const *p_mat, unsigned dim);

class SquareMatrix
	{
	public:
										SquareMatrix();
										SquareMatrix(unsigned sz, double value);
										SquareMatrix(const SquareMatrix & other);
										~SquareMatrix();
        void                            Clear();
		void							CreateMatrix(unsigned sz, double value);
		void							Identity();
		void							ScalarMultiply(double scalar);
		void							Subtract(const SquareMatrix & other);
		void							SetToScalar(double scalar);
		unsigned						GetDimension() const;
		double                          Trace() const;
		double * *						GetMatrixAsRawPointer() const;
		double *						operator[](unsigned i) const;
		void							MatrixToString(std::string & s, std::string fmt) const;
		void							Fill(double value);
        std::string                     GetStringRepresentation() const;
        std::string                     GetFormattedStringRepresentation(std::string fmt) const;
        double                          GetElement(unsigned i, unsigned j) const;
        void                            SetElement(unsigned i, unsigned j, double v);
        void                            AddToElement(unsigned i, unsigned j, double v);
        std::vector<double>             GetMatrix() const;
        void                            SetMatrix(unsigned sz, std::vector<double>);
        double                          LogProdMainDiag() const;
        double                          LogDeterminant() const;
        SquareMatrix *                  Duplicate() const;
        SquareMatrix *                  Power(double p) const;
        SquareMatrix *                  LUDecomposition() const;
        SquareMatrix *                  CholeskyDecomposition() const;
        SquareMatrix *                  Inverse() const;
        SquareMatrix *                  LeftMultiplyMatrix(SquareMatrix & matrixOnLeft) const;
        SquareMatrix *                  RightMultiplyMatrix(SquareMatrix & matrixOnRight) const;
        std::vector<double>             LeftMultiplyVector(const std::vector<double> & vectorOnLeft) const;
        std::vector<double>             RightMultiplyVector(const std::vector<double> & vectorOnRight) const;

	protected:
		static unsigned					k;		//temporary
		unsigned						id;		//temporary
		double * *						m;		/**< the two-dimensional matrix of doubles */
		unsigned						dim;	/**< the dimension of the matrix */
        //mutable std::vector<double>     work;   /**< used as workspace whenever a reference to a vector needs to be returned */
	};

typedef boost::shared_ptr<SquareMatrix> SquareMatrixShPtr;

}	// namespace phycas
#endif


//==================================================
// cubicBezier
//==================================================
#define BOOST_PYTHON_STATIC_LIB
#define BOOST_NUMPY_STATIC_LIB
#include <Python.h>
#include <valarray>
#include <iostream>
#include <boost/python.hpp>
#include <boost/shared_ptr.hpp>

/*
 ◆開発環境
 [OS]: macOS 10.14~
 [c++] gcc version 9.2.0
 [boost] 1.71.0
 [Python] v2.7.16
 */

typedef std::valarray<double> darray;
typedef std::valarray<darray> d2array;
namespace py = boost::python;
namespace cubicBezier
{
    darray cubicBezier(darray p0, darray p1, darray p2, darray p3, double u, bool restraint=true)
    {
        darray _p0 (2);
        darray _p1 (2);
        darray _p2 (2);
        darray _p3 (2);
        _p0 = p0; //# start-point
        _p1 = p1; //# start-point weighted
        _p2 = p2; //# end-point weighted
        _p3 = p3; //# end-point

        if(restraint)
        { // p0, p1, p2, p3のx値が互い違いになっている場合の為の処理。
            // p0[0]==3, p1[0]==1の様にp0がp1より先のx地点にある場合。
            double scale;
            double btKey;
            double vx_A;
            double vx_B;
            btKey = _p3[0] - _p0[0];
            vx_A = _p1[0] - _p0[0];
            vx_B = _p3[0] - _p2[0];
            scale = btKey/(vx_A + vx_B);
            if(scale<1)
            {
                darray vec_A (2);
                darray vec_B (2);
                vec_A = _p1 - p0;
                vec_B = _p2 - p3;
                //vecB*scale;

                _p1 = vec_A*scale; //vec_A apply ([=](double v) (return v*sclae;))
                _p2 = vec_B*scale; //vec_B apply ([=](double v) (return v;))

                _p1 = _p1+_p0;
                _p2 = _p2+_p3;
            }
        }
        darray _p4 (2);
        darray _p5 (2);
        darray _p6 (2);
        darray _p7 (2);
        darray _p8 (2);
        darray _p9 (2);

        // 計算
        _p4 = (_p1 - _p0)*u +_p0; //
        _p5 = (_p2 - _p1)*u +_p1; //
        _p6 = (_p3 - _p2)*u +_p2; //
        _p7 = (_p5 - _p4)*u +_p4; //
        _p8 = (_p6 - _p5)*u +_p5; //
        _p9 = (_p8 - _p7)*u +_p7; //
        return _p9;
    }

    d2array sampling_cubicBezier_t(d2array points, int count=100)
    {
    /*
     cubicBezierのポイント４点を用いて４点間のcubicBezierの離散化したカーブ(point群)を生成する。
     */
    double u;
    darray p;
    d2array rList(count);
    for(int t=0; t <count; t++)
        {
            u = double(t) / double(count-1);
            p = cubicBezier::cubicBezier(points[0], points[1], points[2], points[3], u);
            rList[t] = p;
        }
    return rList;
    }

}

//==================================================
// convert
//==================================================
d2array convert_list_to_d2array(py::list array)
{
    // array is expected as a d2array (referenced-code)
    int count = py::len(array);
    d2array rArray(count);
    py::list val;

    for(int i=0; i<count; i++)
        {
            val = py::extract<py::list>(array[i]);
            int mCount = py::len(val);
            rArray[i] = darray(mCount);
            for(int m=0; m<mCount; m++)
                {
                rArray[i][m] = py::extract<double>(val[m]);
                }

        }
    return rArray;
}

py::list convert_array_to_list(d2array array)
{
    // array is expected as a d2array //(referencerd-code)
    int count = array.size();
    py::list rArray;
    for(int i=0; i<count; i++)
        {
            py::list temp;
            int mCount = array[i].size();
            for(int m=0; m<mCount; m++)
                {
                temp.append(array[i][m]);
                }
            rArray.append(temp);
        }
    return  rArray;
}

//==================================================
// python command
//==================================================
py::list py_sampling_cubicBezier(py::list points, int count=100)
{
           d2array _points(count);
           _points = convert_list_to_d2array(points);
           d2array tmpArray;

           tmpArray = cubicBezier::sampling_cubicBezier_t(_points, count);
           return convert_array_to_list(tmpArray);
}

py::list py_cubicBezier(py::list p0, py::list p1, py::list p2, py::list p3, double u, bool restraint=true )
{
    darray _p0(2);
    darray _p1(2);
    darray _p2(2);
    darray _p3(2);
    _p0[0] = py::extract<double>(p0[0]);
    _p0[1] = py::extract<double>(p0[1]); //# start-point
    _p1[0] = py::extract<double>(p1[0]);
    _p1[1] = py::extract<double>(p1[1]); //# start-point(weighted)
    _p2[0] = py::extract<double>(p2[0]);
    _p2[1] = py::extract<double>(p2[1]); //# end-point(weigted)
    _p3[0] = py::extract<double>(p3[0]);
    _p3[1] = py::extract<double>(p3[1]); //# end-point

    darray points = cubicBezier::cubicBezier(_p0, _p1, _p2, _p3, u, restraint);
    py::list rValue;
    for(auto& v:points)
       {
        rValue.append(v);
       }
    return rValue;
}

BOOST_PYTHON_MODULE(lib)
{
           def("cubicBezier", &py_cubicBezier);
           def("sampling_cubicBezier", &py_sampling_cubicBezier);
}


/**
 * \file
 * \author Thomas Fischer
 * \date   2011-03-17
 * \brief  Implementation of the AngleSkewMetric class.
 *
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "AngleSkewMetric.h"

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

#include "MathLib/MathTools.h"
#include "MeshLib/Node.h"

using namespace boost::math::double_constants;

namespace MeshLib
{
namespace
{
template <unsigned long N>
std::tuple<double, double> getMinMaxAngle(
    std::array<MeshLib::Node, N> const& nodes)
{
    double min_angle(two_pi);
    double max_angle(0.0);

    for (decltype(N) i = 0; i < N; ++i)
    {
        const double angle(MathLib::getAngle(nodes[i], nodes[(i + 1) % N],
                                             nodes[(i + 2) % N]));
        min_angle = std::min(angle, min_angle);
        max_angle = std::max(angle, max_angle);
    }
    return {min_angle, max_angle};
}

double checkTriangle(Element const& elem)
{
    std::array const nodes = {*elem.getNode(0), *elem.getNode(1),
                              *elem.getNode(2)};
    auto const& [min_angle, max_angle] = getMinMaxAngle(nodes);
    return std::max((max_angle - third_pi) / two_thirds_pi,
                    (third_pi - min_angle) / third_pi);
}

double checkQuad(Element const& elem)
{
    std::array const nodes = {*elem.getNode(0), *elem.getNode(1),
                              *elem.getNode(2), *elem.getNode(3)};
    auto const& [min_angle, max_angle] = getMinMaxAngle(nodes);

    return std::max((max_angle - half_pi) / half_pi,
                    (half_pi - min_angle) / half_pi);
}

double checkTetrahedron(Element const& elem)
{
    std::array<double, 4> min;
    std::array<double, 4> max;
    for (auto face_number = 0; face_number < 4; ++face_number)
    {
        std::unique_ptr<Element const> face{elem.getFace(face_number)};
        std::array const nodes = {*face->getNode(0), *face->getNode(1),
                                  *face->getNode(2)};
        std::tie(min[face_number], max[face_number]) = getMinMaxAngle(nodes);
    }

    double const min_angle = *std::min_element(min.begin(), min.end());
    double const max_angle = *std::max_element(max.begin(), max.end());

    return std::max((max_angle - third_pi) / two_thirds_pi,
                    (third_pi - min_angle) / third_pi);
}

double checkHexahedron(Element const& elem)
{
    std::array<double, 6> min;
    std::array<double, 6> max;
    for (auto face_number = 0; face_number < 6; ++face_number)
    {
        std::unique_ptr<Element const> face{elem.getFace(face_number)};
        std::array const nodes = {*face->getNode(0), *face->getNode(1),
                                  *face->getNode(2), *face->getNode(3)};
        std::tie(min[face_number], max[face_number]) = getMinMaxAngle(nodes);
    }

    double const min_angle = *std::min_element(min.begin(), min.end());
    double const max_angle = *std::max_element(max.begin(), max.end());

    return std::max((max_angle - half_pi) / half_pi,
                    (half_pi - min_angle) / half_pi);
}

double checkPrism(Element const& elem)
{
    // face 0: triangle (0,1,2)
    std::unique_ptr<Element const> f0{elem.getFace(0)};
    std::array const nodes_f0 = {*f0->getNode(0), *f0->getNode(1),
                                 *f0->getNode(2)};
    auto const& [min_angle_tri0, max_angle_tri0] = getMinMaxAngle(nodes_f0);

    // face 4: triangle (3,4,5)
    std::unique_ptr<Element const> f4{elem.getFace(4)};
    std::array const nodes_f4 = {*f4->getNode(0), *f4->getNode(1),
                                 *f4->getNode(2)};
    auto const& [min_angle_tri1, max_angle_tri1] = getMinMaxAngle(nodes_f4);

    auto const min_angle_tri = std::min(min_angle_tri0, min_angle_tri1);
    auto const max_angle_tri = std::max(max_angle_tri0, max_angle_tri1);

    double const tri_criterion(
        std::max((max_angle_tri - third_pi) / two_thirds_pi,
                 (third_pi - min_angle_tri) / third_pi));

    std::array<double, 3> min;
    std::array<double, 3> max;
    for (int i = 1; i < 4; ++i)
    {
        std::unique_ptr<Element const> f{elem.getFace(i)};
        std::array const nodes = {*f->getNode(0), *f->getNode(1),
                                  *f->getNode(2), *f->getNode(3)};
        std::tie(min[i - 1], max[i - 1]) = getMinMaxAngle(nodes);
    }

    double const min_angle_quad = *std::min_element(min.begin(), min.end());
    double const max_angle_quad = *std::max_element(max.begin(), max.end());

    double const quad_criterion(std::max((max_angle_quad - half_pi) / half_pi,
                                         (half_pi - min_angle_quad) / half_pi));

    return std::min(tri_criterion, quad_criterion);
}

}  // end unnamed namespace

void AngleSkewMetric::calculateQuality()
{
    for (auto const e : _mesh.getElements())
    {
        switch (e->getGeomType())
        {
            case MeshElemType::LINE:
                _element_quality_metric[e->getID()] = -1.0;
                break;
            case MeshElemType::TRIANGLE:
                _element_quality_metric[e->getID()] = checkTriangle(*e);
                break;
            case MeshElemType::QUAD:
                _element_quality_metric[e->getID()] = checkQuad(*e);
                break;
            case MeshElemType::TETRAHEDRON:
                _element_quality_metric[e->getID()] = checkTetrahedron(*e);
                break;
            case MeshElemType::HEXAHEDRON:
                _element_quality_metric[e->getID()] = checkHexahedron(*e);
                break;
            case MeshElemType::PRISM:
                _element_quality_metric[e->getID()] = checkPrism(*e);
                break;
            default:
                break;
        }
    }
}

}  // end namespace MeshLib


#include "model.h"
#include <math.h>
#include <time.h>
#include <boost/random.hpp>

Model::Model()
{
}

void Model::init(double dt, int m, int n, int nh, double sigma, double sv0)
{
    this->dt = pow(10, dt);
    this->nh = nh;
    this->n = n;
    this->m = m;
    if (nh % 2 == 0)
        this->N = nh*nh - nh/2;
    else
        this->N = nh*nh - (nh-1)/2;
    this->sigma = sigma;
    this->sv0 = sv0;

    update();

    p.resize(2, N);
    q.resize(2, N);
    f1.resize(2, N);
    f2.resize(2, N);
    f3.resize(2, N);
    f4.resize(2, N);
    v1.resize(2, N);
    v2.resize(2, N);
    v3.resize(2, N);
    v4.resize(2, N);

    typedef boost::mt19937                     ENG;    // Mersenne Twister
    typedef boost::normal_distribution<double> DIST;   // Normal Distribution
    typedef boost::variate_generator<ENG,DIST> GEN;    // Variate generator

    ENG  eng;
    DIST dist(0, sv0);
    GEN  gen(eng, dist);

    gen.engine().seed(time(NULL) + ::getpid());
    gen.distribution().reset();

    Eigen::Vector2d
            qi(sigma - 0.5, sigma - 0.5),
            n1(sigma, 0),
            n2(sigma/2, sigma*sqrt(3)/2);

    for (int i=0; i<N; ++i) {
        p(0,i) = gen();
        p(1,i) = gen();
        q.col(i) = qi;
        qi += n1;
        if (qi[0] > nh*sigma - 0.25)
            qi += n2 - nh*n1;
    }
}

void Model::update()
{
    this->mass = 1.0;
    this->boxW = sigma * (nh + 1) - 1;
    this->boxH = sigma * ((nh - 1) * sqrt(3)/2 + 2) - 1;
    this->packingFractionHCP = (N * M_PI / 4) / (boxW * boxH) / (M_PI / (2*sqrt(3)));
    if (m == 0)
        this->cutoff = pow(1e-10, -1./(n+1.));
    else
        this->cutoff = pow((n/(double)m-1.)*1e-10, -1./(m+1.));
}

void Model::forces(const Eigen::Matrix2Xd &p, const Eigen::Matrix2Xd &q,
                   Eigen::Matrix2Xd &f, Eigen::Matrix2Xd &v)
{
    Eigen::Vector2d rij, fij;
    double r;

    for (int i=0; i<N; ++i)
        f.col(i) << 0, 0;

    for (int i=0; i<N; ++i) {
        fij << n*pow(q(0,i)+0.5, -n-1) - m*pow(q(0,i)+0.5, -m-1) - \
               n*pow(boxW-q(0,i)+0.5, -n-1) + m*pow(boxW-q(0,i)+0.5, -m-1),
               n*pow(q(1,i)+0.5, -n-1) - m*pow(q(1,i)+0.5, -m-1) - \
               n*pow(boxH-q(1,i)+0.5, -n-1) + m*pow(boxH-q(1,i)+0.5, -m-1);
        f.col(i) = f.col(i) + fij;

        for (int j=i+1; j<N; ++j) {
            rij = q.col(j) - q.col(i);
            r = rij.norm();
            if (r > cutoff)
                continue;
            fij = (n*pow(r, -n-1) - m*pow(r, -m-1)) * rij.normalized();
            f.col(j) = f.col(j) + fij;
            f.col(i) = f.col(i) - fij;
        }
    }

    v = p;
}

void Model::rk4step()
{
    forces(p,             q,             f1, v1);
    forces(p + 0.5*dt*f1, q + 0.5*dt*v1, f2, v2);
    forces(p + 0.5*dt*f2, q + 0.5*dt*v2, f3, v3);
    forces(p + dt*f3,     q + dt*v3,     f4, v4);

    p += dt * (f1 + 2*(f2 + f3) + f4) / 6.0;
    q += dt * (v1 + 2*(v2 + v3) + v4) / 6.0;

    t += dt;
}


#include "aikido/planner/vectorfield/VectorFieldUtil.hpp"

#include <Eigen/Geometry>
#include <dart/optimizer/Solver.hpp>
#include <dart/optimizer/nlopt/NloptSolver.hpp>

#include "aikido/common/algorithm.hpp"
#include "aikido/trajectory/Spline.hpp"

namespace aikido {
namespace planner {
namespace vectorfield {

namespace {

/// A function class that defines an objective. The objective measures
/// the difference between a desired twist and Jacobian * joint velocities.
///
class DesiredTwistFunction : public dart::optimizer::Function
{
public:
  using Twist = Eigen::Vector6d;
  using Jacobian = dart::math::Jacobian;

  /// Constructor.
  ///
  /// \param[in] twist A desired twist.
  /// \param[in] jacobian System Jacobian.
  DesiredTwistFunction(const Twist& twist, const Jacobian& jacobian)
    : dart::optimizer::Function("DesiredTwistFunction")
    , mTwist(twist)
    , mJacobian(jacobian)
  {
    // Do nothing
  }

  /// Implementation inherited.
  /// Evaluating an objective by a state value.
  ///
  /// \param[in] qd Joint velocities.
  /// \return Objective value.
  double eval(const Eigen::VectorXd& qd) override
  {
    return 0.5 * (mJacobian * qd - mTwist).squaredNorm();
  }

  /// Implementation inherited.
  /// Evaluating gradient of an objective by a state value.
  /// \param[in] qd Joint velocities.
  /// \param[out] grad Gradient of a defined objective.
  void evalGradient(
      const Eigen::VectorXd& qd, Eigen::Map<Eigen::VectorXd> grad) override
  {
    grad = mJacobian.transpose() * (mJacobian * qd - mTwist);
  }

protected:
  /// Twist.
  Twist mTwist;

  /// Jacobian of Meta Skeleton.
  Jacobian mJacobian;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace

//==============================================================================
bool computeJointVelocityFromTwist(
    Eigen::VectorXd& jointVelocity,
    const Eigen::Vector6d& desiredTwist,
    const dart::dynamics::MetaSkeletonPtr metaSkeleton,
    const dart::dynamics::ConstBodyNodePtr bodyNode,
    double jointLimitPadding,
    const Eigen::VectorXd& jointVelocityLowerLimits,
    const Eigen::VectorXd& jointVelocityUpperLimits,
    bool enforceJointVelocityLimits,
    double stepSize)
{
  using dart::math::Jacobian;
  using dart::optimizer::Problem;
  using dart::optimizer::Solver;
  using Eigen::VectorXd;

  // Use LBFGS to find joint angles that won't violate the joint limits.
  const Jacobian jacobian = metaSkeleton->getWorldJacobian(bodyNode);

  const std::size_t numDofs = metaSkeleton->getNumDofs();

  jointVelocity = Eigen::VectorXd::Zero(numDofs);
  VectorXd positions = metaSkeleton->getPositions();
  VectorXd initialGuess = metaSkeleton->getVelocities();
  VectorXd positionLowerLimits = metaSkeleton->getPositionLowerLimits();
  VectorXd positionUpperLimits = metaSkeleton->getPositionUpperLimits();
  VectorXd velocityLowerLimits = jointVelocityLowerLimits;
  VectorXd velocityUpperLimits = jointVelocityUpperLimits;

  const auto problem = std::make_shared<Problem>(numDofs);
  if (enforceJointVelocityLimits)
  {
    for (std::size_t i = 0; i < numDofs; ++i)
    {
      const double position = positions[i];
      const double positionLowerLimit = positionLowerLimits[i];
      const double positionUpperLimit = positionUpperLimits[i];
      const double velocityLowerLimit = velocityLowerLimits[i];
      const double velocityUpperLimit = velocityUpperLimits[i];

      if (position + stepSize * velocityLowerLimit
          <= positionLowerLimit + jointLimitPadding)
      {
        velocityLowerLimits[i] = 0.0;
      }

      if (position + stepSize * velocityUpperLimit
          >= positionUpperLimit - jointLimitPadding)
      {
        velocityUpperLimits[i] = 0.0;
      }

      initialGuess[i] = common::clamp(
          initialGuess[i], velocityLowerLimits[i], velocityUpperLimits[i]);
    }
    problem->setLowerBounds(velocityLowerLimits);
    problem->setUpperBounds(velocityUpperLimits);
  }

  problem->setInitialGuess(initialGuess);
  problem->setObjective(dart::common::make_aligned_shared<DesiredTwistFunction>(
      desiredTwist, jacobian));

#if DART_VERSION_AT_LEAST(6, 9, 0)
  dart::optimizer::NloptSolver solver(
      problem, dart::optimizer::NloptSolver::LD_LBFGS);
#else
  dart::optimizer::NloptSolver solver(problem, nlopt::LD_LBFGS);
#endif
  if (!solver.solve())
  {
    return false;
  }
  jointVelocity = problem->getOptimalSolution();
  return true;
}

//==============================================================================
Eigen::Vector6d computeGeodesicTwist(
    const Eigen::Isometry3d& fromTrans, const Eigen::Isometry3d& toTrans)
{
  using dart::math::logMap;
  Eigen::Isometry3d relativeTrans = fromTrans.inverse() * toTrans;
  Eigen::Vector3d relativeTranslation
      = fromTrans.linear() * relativeTrans.translation();
  Eigen::Vector3d axisAngles = logMap(relativeTrans.linear());
  Eigen::Vector3d relativeAngles = fromTrans.linear() * axisAngles;
  Eigen::Vector6d geodesicTwist;
  geodesicTwist << relativeAngles, relativeTranslation;
  return geodesicTwist;
}

//==============================================================================
Eigen::Vector4d computeGeodesicError(
    const Eigen::Isometry3d& fromTrans, const Eigen::Isometry3d& toTrans)
{
  using dart::math::logMap;
  Eigen::Isometry3d relativeTrans = fromTrans.inverse() * toTrans;
  Eigen::Vector3d relativeTranslation
      = fromTrans.linear() * relativeTrans.translation();
  Eigen::Vector3d axisAngles = logMap(relativeTrans.linear());
  Eigen::Vector4d geodesicError;
  geodesicError << axisAngles.norm(), relativeTranslation;
  return geodesicError;
}

//==============================================================================
double computeGeodesicDistance(
    const Eigen::Isometry3d& fromTrans,
    const Eigen::Isometry3d& toTrans,
    double r)
{
  Eigen::Vector4d error = computeGeodesicError(fromTrans, toTrans);
  error[0] = r * error[0];
  return error.norm();
}

} // namespace vectorfield
} // namespace planner
} // namespace aikido


/*
Metaheuristic Optimization Using Population-Based Simulated Annealing.

Copyright (c) 2021 Gabriele Gilardi

    func            Function to minimize
    LB              Lower boundaries of the search space
    UB              Upper boundaries of the search space
    nPop            Number of agents (population)
    epochs          Number of iterations
    nMove           Number of neighbours of a state evaluated at each epoch
    T0              Initial temperature
    alphaT          Temperature reduction rate
    sigma0          Initial standard deviation used to search the neighboroud
                    of a state (given as a fraction of the search space)
    alphaS          Standard deviation reduction rate
    prob            Probability the dimension of a state is changed
    IntVar          List of indexes specifying which variable should be treated
                    as integer
    normalize       Specifies if the search space should be normalized
    args            Tuple containing any parameter that needs to be passed to
                    the function

    Dimensions:
    (nVar)          LB, UB, LB_orig, UB_orig, sigma, best_pos
    (nPop, nVar)    agent_pos, neigh_pos, agent_pos_orig, neigh_pos_orig
    (nPop)          agent_cost, neigh_cost
    (epochs)        F
    (0-nVar)        IntVar
*/

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

using namespace std;
using namespace Eigen;

/* Structure used to pass the parameters */
struct Parameters {
    int nPop;
    int epochs;
    int nMove;
    double T0;
    double alphaT;
    double sigma0;
    double alphaS;
    double prob;
    bool normalize;
    ArrayXi IntVar;
    ArrayXXd args;
    int seed;
};

/* Structure used to return the results */
struct Results {
    double best_cost;
    ArrayXXd best_pos;
    ArrayXd F;
    double T;
    ArrayXXd sigma;
};

/* Minimize a function using simulated annealing */
Results sa(ArrayXd (*func)(ArrayXXd, ArrayXXd), ArrayXXd LB, ArrayXXd UB,
           Parameters p)
{
    int nVar, nIntVar, idx;
    double T, prob_swap, delta, best_cost;
    Results res;

    /*Eigen declarations*/
    Index r_min, c_min;
    ArrayXd agent_cost, neigh_cost, F;
    ArrayXXd LBe, UBe, LBe_orig, UBe_orig, sigma, agent_pos, neigh_pos,
             agent_pos_orig, neigh_pos_orig, best_pos, rn, flips;

    /* Random generator and probability distributions */
    mt19937 generator(p.seed);
    uniform_real_distribution<double> unif(0.0, 1.0);
    normal_distribution<double> norm(0.0, 1.0);

    nVar = LB.size();
    nIntVar = p.IntVar.size();

    /* Create boundaries for all agents*/
    LBe = LB.replicate(p.nPop, 1);
    UBe = UB.replicate(p.nPop, 1);

    /* Normalize search space */
    if (p.normalize) {
        LBe_orig = LBe;
        UBe_orig = UBe;
        LBe.setZero(p.nPop, nVar);
        UBe.setOnes(p.nPop, nVar);
    }

    T = p.T0;                           // Temperature
    sigma = p.sigma0 * (UBe - LBe);     // Standard deviation of each dimension

    /* Initial position of each agent */
    rn.setZero(p.nPop, nVar);
    for (int i=0; i<p.nPop; i++) {
        for (int j=0; j<nVar; j++) {
            rn(i, j) = unif(generator);
        }
    }
    agent_pos = LBe + rn * (UBe - LBe);

    /* Correct for any integer variable */
    for (int j=0; j<nIntVar; j++) {
        idx = p.IntVar(j);
        agent_pos.col(idx) = round(agent_pos.col(idx));
    }

    /* Initial cost of each agent */
    if (p.normalize) {
        agent_pos_orig = LBe_orig + agent_pos * (UBe_orig - LBe_orig);
        agent_cost = func(agent_pos_orig, p.args);
    }
    else {
        agent_cost = func(agent_pos, p.args);
    }

    /* Initial (overall) best position/cost */
    best_cost = agent_cost.minCoeff(&r_min, &c_min);
    best_pos = agent_pos.row(r_min);

    /* Main loop (T = const) */
    neigh_pos.setZero(p.nPop, nVar);
    F.setZero(p.epochs);
    for (int epoch=0; epoch<p.epochs; epoch++) {

        /* Sub-loop (search the neighboroud of a state) */
        for (int move=0; move<p.nMove; move++) {

            /* Randomly decide in which dimension to search */
            for (int i=0; i<p.nPop; i++) {
                for (int j=0; j<nVar; j++) {
                    rn(i, j) = unif(generator);
                }
            }
            flips = (rn <= p.prob).cast<double>();

            /* Create each agent's neighbours */
            for (int i=0; i<p.nPop; i++) {
                for (int j=0; j<nVar; j++) {
                    rn(i, j) = norm(generator);
                }
            }
            neigh_pos = agent_pos + flips * rn * sigma;

            /* Correct for any integer variable */
            for (int j=0; j<nIntVar; j++) {
                idx = p.IntVar(j);
                neigh_pos.col(idx) = round(neigh_pos.col(idx));
            }

            /* Impose position boundaries */
            neigh_pos = neigh_pos.max(LBe);
            neigh_pos = neigh_pos.min(UBe);

            for (int j=0; j<nIntVar; j++) {
                idx = p.IntVar(j);
                neigh_pos.col(idx) = neigh_pos.col(idx).max(ceil(LBe.col(idx)));
                neigh_pos.col(idx) = neigh_pos.col(idx).min(floor(UBe.col(idx)));
            }

            /* Evaluate the cost of each agent's neighbour */
            if (p.normalize) {
                neigh_pos_orig = LBe_orig + neigh_pos * (UBe_orig - LBe_orig);
                neigh_cost = func(neigh_pos_orig, p.args);
            }
            else {
                neigh_cost = func(neigh_pos, p.args);
            }

            /* Decide if each agent will change its state */
            for (int i=0; i<p.nPop; i++) {

                /* Swap states if the neighbour state is better ... */
                if (neigh_cost(i) <= agent_cost(i)) {
                    agent_cost(i) = neigh_cost(i);
                    agent_pos.row(i) = neigh_pos.row(i);
                }

                /* ... or decide probabilistically */
                else {

                    /* Acceptance probability */
                    delta = (neigh_cost(i) - agent_cost(i)) / agent_cost(i);
                    prob_swap = exp(-delta / T);

                    /* Randomly swap states */
                    if (unif(generator) <= prob_swap) {
                        agent_cost(i) = neigh_cost(i);
                        agent_pos.row(i) = neigh_pos.row(i);
                    }
                }

                /* Update the (overall) best position/cost */
                best_cost = agent_cost.minCoeff(&r_min, &c_min);
                best_pos = agent_pos.row(r_min);
            }
        }

        /* Save the best cost for this epoch */
        F(epoch) = best_cost;

        /* Cooling scheduling */
        T = p.alphaT * T;

        /* Random neighboroud search schedule */
        sigma = p.alphaS * sigma;
    }

    /* De-normalize */
    if (p.normalize) {
        best_pos = LB + best_pos * (UB - LB);
        sigma = sigma * (UBe_orig - LBe_orig);
    }

    /* Copy solution */
    res.best_cost = best_cost;
    res.T = T;
    res.best_pos = best_pos;
    res.sigma = sigma.row(0);
    res.F = F;

    return res;
}


#include <stdio.h>
#include <iostream>
#include <math.h>
#include <string>
#include <Eigen/Dense>
#include "ros/ros.h"

#include <yaml-cpp/yaml.h>
#include <ros/package.h>

#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/JointState.h>
#include <robot_localization_msgs/HeadTransform.h>

using namespace ros;
using namespace std;
using namespace Eigen;

float angle_torso_pitch=0.0;
float angle_torso_yaw=0.0;
float angle_head_yaw=0.0;
float angle_head_pitch=0.0;

void jointCallback(const sensor_msgs::JointState::ConstPtr& msg);

bool RobotYamlRead(string path, float* length_pelvis2torsopitch, float* length_torsopitch2torsoyaw, float* length_torsoyaw2headyaw, float* length_headyaw2headpitch, float* length_headpitch2camera);
bool ParticleYamlRead(string path, float& unit);

Matrix4f makeYawMatrix(float& theta);
Matrix4f makePitchMatrix(float& theta);
Matrix4f makeTransMatrix(Vector3f& position);
Matrix4f makeTransMatrix(Vector3f& position, float& unit);

void printMatrixValue(Matrix4f& matrix);

void insertMatrix2Message(Matrix4f& matrix, std_msgs::Float32MultiArray& msg);
void insertMatrix2Message(Matrix4f& matrix, robot_localization_msgs::HeadTransform& msg, int flag); // flag=0 -> original, flag=1 -> unit

int main(int argc, char** argv)
{
    //read setting
    string yaml_robot_path = package::getPath("robot_localization_data") + "/data/kin_dyn_3.yaml";
    string yaml_particle_path = package::getPath("robot_localization_data") + "/data/particle_setting.yaml";

    float length_pelvis2torsopitch[3];
    float length_torsopitch2torsoyaw[3];

    float length_torsoyaw2headyaw[3];
    float length_headyaw2headpitch[3];
    float length_headpitch2camera[3];

    float unit;

    if( !RobotYamlRead(yaml_robot_path, length_pelvis2torsopitch, length_torsopitch2torsoyaw, length_torsoyaw2headyaw, length_headyaw2headpitch, length_headpitch2camera) ) exit(0);
    if( !ParticleYamlRead(yaml_particle_path, unit) ) exit(0);

    Vector3f V_length_pelvis2torsopitch(length_pelvis2torsopitch[0], length_pelvis2torsopitch[1], length_pelvis2torsopitch[2]);
    Vector3f V_length_torsopitch2torsoyaw(length_torsopitch2torsoyaw[0], length_torsopitch2torsoyaw[1], length_torsopitch2torsoyaw[2]);
    Vector3f V_length_torsoyaw2headyaw(length_torsoyaw2headyaw[0],length_torsoyaw2headyaw[1],length_torsoyaw2headyaw[2]);
    Vector3f V_length_headyaw2headpitch(length_headyaw2headpitch[0],length_headyaw2headpitch[1],length_headyaw2headpitch[2]);
    Vector3f V_length_headpitch2camera(length_headpitch2camera[0], length_headpitch2camera[1], length_headpitch2camera[2]);

    //start ros
    ros::init(argc, argv, "robot_localization_head_receive3");
    ros::NodeHandle n;
    ros::Rate loop_rate(100);

    ros::Publisher headtransform_pub = n.advertise<std_msgs::Float32MultiArray>("/alice/camera_transform",10);
    ros::Publisher custom_headtransform_pub = n.advertise<robot_localization_msgs::HeadTransform>("/robot_localization/head_transform",10);
    ros::Subscriber joint_sub = n.subscribe("/robotis/present_joint_states",10,jointCallback);

    while(ros::ok())
    {
        std_msgs::Float32MultiArray headtransform_msg;
        robot_localization_msgs::HeadTransform custom_headtransform_msg;

        //real world
        Matrix4f t_pelvis2torsopitch = makeTransMatrix(V_length_pelvis2torsopitch);
        Matrix4f r_pelvis2torsopitch = Matrix4f::Identity();

        Matrix4f t_torsopitch2torsoyaw = makeTransMatrix(V_length_torsopitch2torsoyaw);
        Matrix4f r_torsopitch2torsoyaw = makePitchMatrix(angle_torso_pitch);

        Matrix4f t_torsoyaw2headyaw = makeTransMatrix(V_length_torsoyaw2headyaw);
        Matrix4f r_torsoyaw2headyaw = makeYawMatrix(angle_torso_yaw);

        Matrix4f t_headyaw2headpitch = makeTransMatrix(V_length_headyaw2headpitch);
        Matrix4f r_headyaw2headpitch = makeYawMatrix(angle_head_yaw);

        Matrix4f t_headpitch2camera = makeTransMatrix(V_length_headpitch2camera);
        Matrix4f r_headpitch2camera = makePitchMatrix(angle_head_pitch);

        Matrix4f TR01 = r_pelvis2torsopitch * t_pelvis2torsopitch;
        Matrix4f TR12 = r_torsopitch2torsoyaw * t_torsopitch2torsoyaw;
        Matrix4f TR23 = r_torsoyaw2headyaw * t_torsoyaw2headyaw;
        Matrix4f TR34 = r_headyaw2headpitch * t_headyaw2headpitch;
        Matrix4f TR45 = r_headpitch2camera * t_headpitch2camera;

        Matrix4f Transform_pelvis2head = TR01 * TR12 * TR23 * TR34 * TR45;  // Transform matrix real world = TR

        //virtual world
        Matrix4f t_pelvis2torsopitch_V = makeTransMatrix(V_length_pelvis2torsopitch, unit);
        Matrix4f t_torsopitch2torsoyaw_V = makeTransMatrix(V_length_torsopitch2torsoyaw, unit);
        Matrix4f t_torsoyaw2headyaw_V = makeTransMatrix(V_length_torsoyaw2headyaw, unit);
        Matrix4f t_headyaw2headpitch_V = makeTransMatrix(V_length_headyaw2headpitch, unit);
        Matrix4f t_headpitch2camera_V = makeTransMatrix(V_length_headpitch2camera, unit);

        Matrix4f TV01 = r_pelvis2torsopitch * t_pelvis2torsopitch_V;
        Matrix4f TV12 = r_torsopitch2torsoyaw * t_torsopitch2torsoyaw_V;
        Matrix4f TV23 = r_torsoyaw2headyaw * t_torsoyaw2headyaw_V;
        Matrix4f TV34 = r_headyaw2headpitch * t_headyaw2headpitch_V;
        Matrix4f TV45 = r_headpitch2camera * t_headpitch2camera_V;

        Matrix4f unit_Transform_pelvis2head = TV01 * TV12 * TV23 * TV34 * TV45;   // Transform matrix virtual world = TV

        insertMatrix2Message(Transform_pelvis2head, headtransform_msg);

        insertMatrix2Message(Transform_pelvis2head, custom_headtransform_msg, 0);
        insertMatrix2Message(unit_Transform_pelvis2head, custom_headtransform_msg, 1);

        headtransform_pub.publish(headtransform_msg);
        custom_headtransform_pub.publish(custom_headtransform_msg);

        cout << "-----------------------------------------------" << endl;
        cout << "waist_pitch : " << angle_torso_pitch * 180 / M_PI << endl;
        cout << "waist_yaw : " << angle_torso_yaw * 180 / M_PI << endl;
        cout << "head_yaw : " << angle_head_yaw * 180 / M_PI << endl;
        cout << "head_pitch : " << angle_head_pitch * 180 / M_PI << endl;
        cout << "-----------------------------------------------" << endl;
        printMatrixValue(Transform_pelvis2head);
        cout << "-----------------------------------------------" << endl << endl;

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

void jointCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
    int torso_pitch_index, torso_yaw_index, head_yaw_index, head_pitch_index;
    for(int i=0; i<(msg->name).size(); i++)
    {
        if(msg->name[i] == "waist_pitch") torso_pitch_index = i;
        if(msg->name[i] == "waist_yaw"  ) torso_yaw_index = i;
        if(msg->name[i] == "head_yaw"   ) head_yaw_index = i;
        if(msg->name[i] == "head_pitch" ) head_pitch_index = i;
    }

    angle_torso_pitch = msg->position[torso_pitch_index];
    angle_torso_yaw = msg->position[torso_yaw_index];
    angle_head_yaw = msg->position[head_yaw_index];
    angle_head_pitch = msg->position[head_pitch_index];
}

bool RobotYamlRead(string path, float* length_pelvis2torsopitch, float* length_torsopitch2torsoyaw, float* length_torsoyaw2headyaw, float* length_headyaw2headpitch, float* length_headpitch2camera)
{
    YAML::Node yaml_node;
    try
    {
        yaml_node = YAML::LoadFile(path.c_str());

        length_pelvis2torsopitch[0]   = yaml_node["torso_p"]["relative_position"][0].as<float>();
        length_pelvis2torsopitch[1]   = yaml_node["torso_p"]["relative_position"][1].as<float>();
        length_pelvis2torsopitch[2]   = yaml_node["torso_p"]["relative_position"][2].as<float>();

        length_torsopitch2torsoyaw[0] = yaml_node["torso_y"]["relative_position"][0].as<float>();
        length_torsopitch2torsoyaw[1] = yaml_node["torso_y"]["relative_position"][1].as<float>();
        length_torsopitch2torsoyaw[2] = yaml_node["torso_y"]["relative_position"][2].as<float>();

        length_torsoyaw2headyaw[0]    = yaml_node["head_y"]["relative_position"][0].as<float>();
        length_torsoyaw2headyaw[1]    = yaml_node["head_y"]["relative_position"][1].as<float>();
        length_torsoyaw2headyaw[2]    = yaml_node["head_y"]["relative_position"][2].as<float>();

        length_headyaw2headpitch[0]  = yaml_node["head_p"]["relative_position"][0].as<float>();
        length_headyaw2headpitch[1]  = yaml_node["head_p"]["relative_position"][1].as<float>();
        length_headyaw2headpitch[2]  = yaml_node["head_p"]["relative_position"][2].as<float>();

        length_headpitch2camera[0]   = yaml_node["cam"]["relative_position"][0].as<float>();
        length_headpitch2camera[1]   = yaml_node["cam"]["relative_position"][1].as<float>();
        length_headpitch2camera[2]   = yaml_node["cam"]["relative_position"][2].as<float>();
    }
    catch(const exception& e)
    {
        ROS_ERROR("fail to read robot yaml file");
        return false;
    }
    return true;
}

bool ParticleYamlRead(string path, float& unit)
{
    YAML::Node yaml_node;
    try
    {
        yaml_node = YAML::LoadFile(path.c_str());
        unit = yaml_node["ETCSetting"]["unit"].as<float>();
    }
    catch(const exception& e)
    {
        ROS_ERROR("fail to read particle yaml file");
        return false;
    }
    return true;
}

void insertMatrix2Message(Matrix4f& matrix, std_msgs::Float32MultiArray& msg)
{
    for(int i=0; i<4; i++)
        for(int j=0; j<4; j++)
            msg.data.push_back(matrix(i,j));
}

void printMatrixValue(Matrix4f& matrix){
    vector<float> data;
    for(int i=0; i<4; i++){
        for(int j=0; j<4; j++){
            data.push_back(matrix(i,j));
            printf("%3.3f  ", data.back());
        }
        cout << endl;
    }
}

void insertMatrix2Message(Matrix4f& matrix, robot_localization_msgs::HeadTransform& msg, int flag)
{
    switch(flag)
    {
        case 0:
            for(int i=0; i<4; i++)
                for(int j=0; j<4; j++)
                    msg.Tdata.push_back(matrix(i,j));
            break;
        case 1:
            for(int i=0; i<4; i++)
                for(int j=0; j<4; j++)
                    msg.Udata.push_back(matrix(i,j));
            break;
        default:
            break;
    }
}

Matrix4f makeYawMatrix(float& theta)
{
    Matrix4f matrix;
    matrix(0,0)=cos(theta); matrix(0,1)=-sin(theta); matrix(0,2)=0; matrix(0,3)=0;
    matrix(1,0)=sin(theta); matrix(1,1)= cos(theta); matrix(1,2)=0; matrix(1,3)=0;
    matrix(2,0)=0;          matrix(2,1)=0;           matrix(2,2)=1; matrix(2,3)=0;
    matrix(3,0)=0;          matrix(3,1)=0;           matrix(3,2)=0; matrix(3,3)=1;

    return matrix;
}

Matrix4f makePitchMatrix(float& theta)
{
    Matrix4f matrix;
    matrix(0,0)= cos(theta); matrix(0,1)=0; matrix(0,2)=sin(theta); matrix(0,3)=0;
    matrix(1,0)=0;           matrix(1,1)=1; matrix(1,2)=0;          matrix(1,3)=0;
    matrix(2,0)=-sin(theta); matrix(2,1)=0; matrix(2,2)=cos(theta); matrix(2,3)=0;
    matrix(3,0)=0;           matrix(3,1)=0; matrix(3,2)=0;          matrix(3,3)=1;

    return matrix;
}

Matrix4f makeTransMatrix(Vector3f& position)
{
    Matrix4f matrix;
    matrix(0,0)=1; matrix(0,1)=0; matrix(0,2)=0; matrix(0,3)=position(0);
    matrix(1,0)=0; matrix(1,1)=1; matrix(1,2)=0; matrix(1,3)=position(1);
    matrix(2,0)=0; matrix(2,1)=0; matrix(2,2)=1; matrix(2,3)=position(2);
    matrix(3,0)=0; matrix(3,1)=0; matrix(3,2)=0; matrix(3,3)=1;

    return matrix;
}

Matrix4f makeTransMatrix(Vector3f& position, float& unit)
{
    Matrix4f matrix;
    matrix(0,0)=1; matrix(0,1)=0; matrix(0,2)=0; matrix(0,3)=position(0)*unit;
    matrix(1,0)=0; matrix(1,1)=1; matrix(1,2)=0; matrix(1,3)=position(1)*unit;
    matrix(2,0)=0; matrix(2,1)=0; matrix(2,2)=1; matrix(2,3)=position(2)*unit;
    matrix(3,0)=0; matrix(3,1)=0; matrix(3,2)=0; matrix(3,3)=1;

    return matrix;
}