#pragma once

#include <vector>
#include <Eigen/Dense>
#include <string>

namespace cvgl {

inline double clip(double v, double min, double max)
{
    return (v < min ? min : (v > max ? max : v));
}

inline Eigen::ArrayXd clip(const Eigen::ArrayXd& v, double min, double max)
{
    return v.unaryExpr([&](double x){return clip(x, min, max); });
}

inline double wrap(double x, double x_min, double x_max)
{
    return fmod( fmod((x - x_min), (x_max - x_min)) + (x_max - x_min), (x_max - x_min)) + x_min;
}

inline double scale(double v, double in_min, double in_max, double out_min, double out_max)
{
    const double in_range = in_max - in_min;
    return (( (v - in_min) / (in_range == 0 ? 1 : in_range) ) * (out_max - out_min)) + out_min;
}

inline Eigen::ArrayXd scale(const Eigen::ArrayXd& v, double in_min, double in_max, double out_min, double out_max)
{
    return v.unaryExpr([&](double x){ return scale(x, in_min, in_max, out_min, out_max); });
}

template <typename T>
inline std::vector<T> scale(const std::vector<T>& v, const T in_min, const T in_max, const T out_min, const T out_max)
{
    T in_range = in_max - in_min;
    T out_range = out_max - out_min;
    std::vector<T> ret( v.size() );
    for( auto& q : v )
    {
        ret.emplace_back( ( ((q - in_min) / in_range) * out_range) + out_min );
    }
    return ret;
}

inline double scale_clip(double v, double in_min, double in_max, double out_min, double out_max)
{
    double clip_min = out_min > out_max ? out_max : out_min;
    double clip_max = out_max < out_min ? out_min : out_max;

    return clip( scale( v, in_min, in_max, out_min, out_max), clip_min, clip_max);
}

inline Eigen::ArrayXd scale_clip(const Eigen::ArrayXd& v, double in_min, double in_max, double out_min, double out_max)
{
    double clip_min = out_min > out_max ? out_max : out_min;
    double clip_max = out_max < out_min ? out_min : out_max;

    return clip( scale( v, in_min, in_max, out_min, out_max), clip_min, clip_max);
}

template <typename T>
inline std::vector<T> scale_clip(const std::vector<T>& v, const T in_min, const T in_max, const T out_min, const T out_max)
{
    double clip_min = out_min > out_max ? out_max : out_min;
    double clip_max = out_max < out_min ? out_min : out_max;

    T in_range = in_max - in_min;
    T out_range = out_max - out_min;
    std::vector<T> ret( v.size() );
    for( auto& q : v )
    {
        T val = ( ((q - in_min) / in_range) * out_range) + out_min;
        val = (val < clip_min ? clip_min : (val > clip_max ? clip_max : val));
        ret.emplace_back(val);
    }
    return ret;
}

inline double sum( std::vector<double> &vec )
{
    double _sum = 0;
    for (auto& n : vec){
        _sum += n;
    }
    return _sum;
}

inline int32_t round(double x)
{
    return int32_t(x + 0.5);
}

inline std::vector<double> dur2x( std::vector<double> & vec)
{
    std::vector<double> seq_x{0};
    for( size_t i = 0; i < vec.size()-1; ++i)
    {
        seq_x.emplace_back( seq_x.back() + vec[i] );
    }
    return seq_x;
}

inline Eigen::ArrayXd dur2x( Eigen::ArrayXd & vec)
{
    Eigen::ArrayXd seq_x( vec.size()+1 );
    seq_x(0) = 0;

    for( size_t i = 1; i < vec.size()+1; i++)
    {
        seq_x(i) = seq_x(i-1) + vec[i-1];
    }

    return seq_x;
}

inline Eigen::ArrayXd mtof( const Eigen::ArrayXd & v, double a4 = 440.)
{
    return a4 * pow(2., (v - 69.) / 12.) ;
}

inline double mtof( double v, double a4 = 440.)
{
    return a4 * pow(2., (v - 69.) / 12.) ;
}

inline double ftom( double v, double a4 = 440. )
{
    return 69.0 + (12.0 * log2( v / a4 ));
}

/*
inline Eigen::ArrayXd ftom( const Eigen::ArrayXd & v, double a4 = 440. )
{
    return 69 + (12 * log2( v / a4 ));
}*/

inline double erb( double center_hz, double ratio)
{
    const double half_bandwidth = (24.7 * ((0.00437 * center_hz) + 1)) * 0.5;
    return center_hz + (half_bandwidth * ratio);
}

inline double tanhScale( double x, double in_min, double in_max, double curveScalar = 1 )
{
    const double curveTanh = tanh(curveScalar);
    const double scaled_x = scale( x, in_min, in_max, -1., 1.);
    return scale( tanh( scaled_x * curveTanh ), -curveTanh, curveTanh, 0., 1. );
}

inline double scaleInterval( long step, std::vector<double> scale )
{
    if( step < 0 ) {
        return scale[ long( step % scale.size() ) ] + ( long( ((step+1) / (long)scale.size() ) - 1 ) * 12 );
    }
    else
        return scale[ long( step % scale.size() ) ] + long( (step / scale.size() ) * 12 );
}

inline double dbtoa(double db){ return pow(10., (db / 20.)); }

double ntom( const std::string & note );

double lineLookup( double t, std::vector<double> x, std::vector<double> y );

Eigen::ArrayXd lineLookup( const Eigen::ArrayXd& t, const std::vector<double>& x, const std::vector<double>& y );

template <typename T>
std::vector<T> aseq(T from, T to, T step = 1)
{
    std::vector<T> ret;
    T incr = from;
    while( incr <= to )
    {
        ret.emplace_back(incr);
        incr += step;
    }

    return ret;
}

inline double easeInOutQuad(double x)
{
    return x < 0.5 ? 2 * x * x : 1 - pow(-2 * x + 2, 2) / 2;
}

inline double easeInOutSine(double x)
{
    return -(cos(M_PI * x) - 1) * 0.5;
}

inline double easeInSine(double x)
{
     return 1 - cos((x * M_PI) * 0.5);
}

inline double easeOutSine(double x)
{
     return sin((x * M_PI) * 0.5);
}

inline double easeInExpo(double x)
{
    return x == 0 ? 0 : pow(2, 10 * x - 10);
}

inline double easeOutExpo(double x)
{
    return x == 1 ? 1 : 1 - pow(2, -10 * x);

}

}


#pragma once

#include <Eigen/Dense>
#include <cstddef>

namespace icarus
{
    template<typename T, size_t N>
    struct VarianceEstimator
    {
        VarianceEstimator(size_t sampleSize) :
            mSampleSize(sampleSize),
            mSampleNumber(0)
        {
            mMean.setZero();
            mMoment.setZero();
        }

        void addSample(Eigen::Matrix<T, N, 1> const & sample)
        {
            ++mSampleNumber;
            auto oldMean = mMean;
            mMean += (sample - mMean) / T(mSampleNumber);
            mMoment += (sample - oldMean).cwiseProduct(sample - mMean);
        }

        Eigen::Matrix<T, N, 1> mean() const
        {
            return mMean;
        }

        Eigen::Matrix<T, N, 1> variance() const
        {
            return mMoment / mSampleSize;
        }
    private:
        size_t mSampleSize;
        size_t mSampleNumber;
        Eigen::Matrix<T, N, 1> mMean;
        Eigen::Matrix<T, N, 1> mMoment;
    };
}


#include <iostream>
#include <string>
#include <vector>
#include <iomanip>
#include <boost/algorithm/string/erase.hpp>
#include <boost/algorithm/string/case_conv.hpp>

inline void ignoreExtraInputs() {
	std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
}

inline std::string decimalToBinary(int decInput) {

	int exponent = 0;
	int previousExponent = 0;
	std::string binaryNum = "";

	while (pow(2, exponent) <= decInput) {
		previousExponent = exponent;
		exponent++;
	}
	while (previousExponent >= 0) {
		if (decInput - pow(2, previousExponent) >= 0) {
			decInput -= (int) pow(2, previousExponent);
			binaryNum.push_back('1');
		}
		else {
			binaryNum.push_back('0');
		}
		previousExponent--;
	}
	return binaryNum;
}

/*inline std::vector<std::string> decimalToBinaryReturningArray(int decInput) {

	int exponent = 0;
	int previousExponent = 0;
	std::vector<std::string> binaryNum = {};

	while (pow(2, exponent) <= decInput) {
		previousExponent = exponent;
		exponent++;
	}
	while (previousExponent >= 0) {
		if (decInput - pow(2, previousExponent) >= 0) {
			decInput -= (int)pow(2, previousExponent);
			binaryNum.push_back("1");
		}
		else {
			binaryNum.push_back("0");
		}
		previousExponent--;
	}
	return binaryNum;
}*/

inline std::string nibbleGuaranteeer(std::string sub_h_binInput)
{
	int binaryLength = sub_h_binInput.length();
	std::string newBinarySequ = "";

	if (binaryLength % 4 != 0) {
		for (unsigned int i = 0; i < (unsigned)(4 - (binaryLength % 4)); i++) {
			newBinarySequ.push_back('0');
		}
		binaryLength += (4 - (binaryLength % 4));
		newBinarySequ += sub_h_binInput;
	}
	else {
		newBinarySequ = sub_h_binInput;
	}
	return newBinarySequ;
}

/*inline std::vector<std::string> nibbleGuaranteeer(std::vector<std::string> sub_h_binInput, int requiredSize)
{
	int binaryLength = requiredSize;
	std::vector<std::string> newBinarySequ = {};

	if (binaryLength % 4 != 0) {
		for (unsigned int i = 0; i < (unsigned)(4 - (binaryLength % 4)); i++) {
			newBinarySequ.push_back("0");
		}
		binaryLength += (4 - (binaryLength % 4));
		//newBinarySequ += sub_h_binInput;
		for (unsigned int i = 0; i < sub_h_binInput.size(); i++) {
			newBinarySequ.push_back(sub_h_binInput[i]);
		}
	}
	else {
		newBinarySequ = sub_h_binInput;
	}
	return newBinarySequ;
}*/

inline std::string nibbleSpaceAparter(std::string sub_binInput) {

	int indexModifier = 4;
	int whiteSpaceAddedModifier = 0;

	for (unsigned int i = 0; i < (unsigned)((sub_binInput.length() / 4) - 1); i++) {
		sub_binInput.insert(indexModifier + whiteSpaceAddedModifier, " ");
		indexModifier += 4;
		whiteSpaceAddedModifier++;
	}
	return sub_binInput;
}

inline std::string binaryToHexa(std::string h_binInput) {

	int binaryLength = 0;
	int nibbleIndexBeginModifier = 0;
	int nibbleIndexEndModifier = 4;
	int nibbleCalculatedDecimalValue = 0;
	int nibbleHighestExponentValue = 8;
	std::string hexaValue = "";
	std::vector<std::string> nibbles;

	h_binInput = nibbleGuaranteeer(h_binInput);
	binaryLength = h_binInput.length() / 4;

	for (unsigned int i = 0; i < (unsigned) binaryLength; i++) {
		nibbles.push_back(h_binInput.substr(nibbleIndexBeginModifier, nibbleIndexEndModifier));
		nibbleIndexBeginModifier += 4;
		nibbleIndexEndModifier += 4;
	}

	for (std::string &nibbleValue : nibbles) {
		nibbleCalculatedDecimalValue = 0;
		nibbleHighestExponentValue = 8;
		for (unsigned int i = 0; i < 4; i++) {
			if (nibbleValue[i] == '1') {
				nibbleCalculatedDecimalValue += nibbleHighestExponentValue;
			}
			nibbleHighestExponentValue = (int) ((nibbleHighestExponentValue / 2) + 0.5);
		}
		if (nibbleCalculatedDecimalValue >= 0 && nibbleCalculatedDecimalValue <= 9) {
			hexaValue.push_back(nibbleCalculatedDecimalValue + 48);
		}
		else if (nibbleCalculatedDecimalValue >= 10 && nibbleCalculatedDecimalValue <= 15) {
			hexaValue.push_back(nibbleCalculatedDecimalValue + 55);
		}
	}
	return hexaValue;
}

inline int binaryToDecimal(std::string d_binInput) {

	int nibbleHighestExponentValue = (int) pow(2, d_binInput.length() - 1);
	int nibbleCalculatedDecimalValue = 0;

	for (char &indexValue : d_binInput) {
		if (indexValue == '1') {
			nibbleCalculatedDecimalValue += nibbleHighestExponentValue;
		}
		nibbleHighestExponentValue = (int) ((nibbleHighestExponentValue / 2) + 0.5);
	}
	return nibbleCalculatedDecimalValue;
}

inline int hexaToDecimal(std::string hexaInput) {

	int hexaHighestExponentValue = (int) pow(16, hexaInput.length() - 1);
	int hexaToDecimalValue = 0;

	for (char &indexValue : hexaInput) {
		if (indexValue >= '0' && indexValue <= '9') {
			hexaToDecimalValue += (indexValue - 48) * hexaHighestExponentValue;
		}
		else if (indexValue >= 'A' && indexValue <= 'F') {
			hexaToDecimalValue += (indexValue - 55) * hexaHighestExponentValue;
		}
		else {
			std::cout << "\nSomething went wrong in the 'hexaToDecimal' function\n";
		}
		hexaHighestExponentValue = hexaHighestExponentValue / 16;
	}
	return hexaToDecimalValue;
}

/*inline std::string addition(std::string binaryInputOne, std::string binaryInputTwo) {

	std::vector<std::string> indexedBinaryInput_One = {}, indexedBinaryInput_Two = {};
	std::string resultantBinaryValue = "", remainder = "0";
	int binaryLength = -1;

	indexedBinaryInput_One = decimalToBinaryReturningArray(binaryToDecimal(binaryInputOne));
	indexedBinaryInput_Two = decimalToBinaryReturningArray(binaryToDecimal(binaryInputTwo));

	if (indexedBinaryInput_One.size() > indexedBinaryInput_Two.size()) {
		binaryLength = indexedBinaryInput_One.size();
	}
	else {
		binaryLength = indexedBinaryInput_Two.size();
	}
	indexedBinaryInput_One = nibbleGuaranteeer(indexedBinaryInput_One, binaryLength);
	indexedBinaryInput_Two = nibbleGuaranteeer(indexedBinaryInput_Two, binaryLength);

	for (int i = (binaryLength-1); i >= 0; i--) {
		if (indexedBinaryInput_One[i] == "0" && indexedBinaryInput_Two[i] == "0" && remainder == "0") {
			resultantBinaryValue.push_back('0');
			remainder = "0";
		}
		else if ((indexedBinaryInput_One[i] == "1" && indexedBinaryInput_Two[i] == "0" && remainder == "0") ||
				 (indexedBinaryInput_One[i] == "0" && indexedBinaryInput_Two[i] == "1" && remainder == "0") ||
				 (indexedBinaryInput_One[i] == "0" && indexedBinaryInput_Two[i] == "0" && remainder == "1")) {
			resultantBinaryValue.push_back('1');
			remainder = "0";
		}
		else if ((indexedBinaryInput_One[i] == "1" && indexedBinaryInput_Two[i] == "1" && remainder == "0") ||
				 (indexedBinaryInput_One[i] == "1" && indexedBinaryInput_Two[i] == "0" && remainder == "1") ||
				 (indexedBinaryInput_One[i] == "0" && indexedBinaryInput_Two[i] == "1" && remainder == "1")) {
			resultantBinaryValue.push_back('0');
			remainder = "1";
		}
		else if (indexedBinaryInput_One[i] == "1" && indexedBinaryInput_Two[i] == "1" && remainder == "1") {
			resultantBinaryValue.push_back('1');
			remainder = "1";
		}
		else {
			std::cout << "\nThere happens to be something else occurring in the forloop in 'binaryAddition'\n";
		}
	}

	return resultantBinaryValue;
}*/


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt 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 Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef __TEST_FUNCTIONS_HPP__
#define __TEST_FUNCTIONS_HPP__

#define _USE_MATH_DEFINES
#include <cmath>
#include <algorithm>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/assignment.hpp>
#include "bayesopt/bayesopt.hpp"
#include "specialtypes.hpp"

class ExampleOneD: public bayesopt::ContinuousModel
{
public:
  ExampleOneD(bayesopt::Parameters par):
    ContinuousModel(1,par) {}

  double evaluateSample(const vectord& xin)
  {
    if (xin.size() != 1)
      {
	std::cout << "WARNING: This only works for 1D inputs." << std::endl
		  << "WARNING: Using only first component." << std::endl;
      }

    double x = xin(0);
    return (x-0.3)*(x-0.3) + sin(20*x)*0.2;
  };

  bool checkReachability(const vectord &query)
  {return true;};

  void printOptimal()
  {
    std::cout << "Optimal:" << 0.23719 << std::endl;
  }

};

class BraninNormalized: public bayesopt::ContinuousModel
{
public:
  BraninNormalized(bayesopt::Parameters par):
    ContinuousModel(2,par) {}

  double evaluateSample( const vectord& xin)
  {
     if (xin.size() != 2)
      {
	std::cout << "WARNING: This only works for 2D inputs." << std::endl
		  << "WARNING: Using only first two components." << std::endl;
      }

    double x = xin(0) * 15 - 5;
    double y = xin(1) * 15;

    return branin(x,y);
  }

  double branin(double x, double y)
  {
    const double pi = boost::math::constants::pi<double>();
    const double rpi = pi*pi;
    return sqr(y-(5.1/(4*rpi))*sqr(x)
	       +5*x/pi-6)+10*(1-1/(8*pi))*cos(x)+10;
  };

  bool checkReachability(const vectord &query)
  {return true;};

  inline double sqr( double x ){ return x*x; };

  void printOptimal()
  {
    vectord sv(2);
    sv(0) = 0.1238938; sv(1) = 0.818333;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
    sv(0) = 0.5427728; sv(1) = 0.151667;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
    sv(0) = 0.961652; sv(1) = 0.1650;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
  }

};

class ExampleCamelback: public bayesopt::ContinuousModel
{
public:
  ExampleCamelback(bayesopt::Parameters par):
    ContinuousModel(2,par) {}

  double evaluateSample( const vectord& x)
  {
     if (x.size() != 2)
      {
	std::cout << "WARNING: This only works for 2D inputs." << std::endl
		  << "WARNING: Using only first two components." << std::endl;
      }
     double x1_2 = x(0)*x(0);
     double x2_2 = x(1)*x(1);

     double tmp1 = (4 - 2.1 * x1_2 + (x1_2*x1_2)/3) * x1_2;
     double tmp2 = x(0)*x(1);
     double tmp3 = (-4 + 4 * x2_2) * x2_2;
     return tmp1 + tmp2 + tmp3;
  }

  bool checkReachability(const vectord &query)
  {return true;};

  inline double sqr( double x ){ return x*x; };

  void printOptimal()
  {
    vectord sv(2);
    sv(0) = 0.0898; sv(1) = -0.7126;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
    sv(0) = -0.0898; sv(1) = 0.7126;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
  }

};

class ExampleHartmann6: public bayesopt::ContinuousModel
{
public:
  ExampleHartmann6(bayesopt::Parameters par):
    ContinuousModel(6,par), mA(4,6), mC(4), mP(4,6)
  {
    mA <<= 10.0,   3.0, 17.0,   3.5,  1.7,  8.0,
      0.05, 10.0, 17.0,   0.1,  8.0, 14.0,
      3.0,   3.5,  1.7,  10.0, 17.0,  8.0,
      17.0,   8.0,  0.05, 10.0,  0.1, 14.0;

    mC <<= 1.0, 1.2, 3.0, 3.2;

    mP <<= 0.1312, 0.1696, 0.5569, 0.0124, 0.8283, 0.5886,
      0.2329, 0.4135, 0.8307, 0.3736, 0.1004, 0.9991,
      0.2348, 0.1451, 0.3522, 0.2883, 0.3047, 0.6650,
      0.4047, 0.8828, 0.8732, 0.5743, 0.1091, 0.0381;
  }

  double evaluateSample( const vectord& xin)
  {
    double y = 0.0;
    for(size_t i=0; i<4; ++i)
      {
	double sum = 0.0;
	for(size_t j=0;j<6; ++j)
	  {
	    double val = xin(j)-mP(i,j);
	    sum -= mA(i,j)*val*val;
	  }
	y -= mC(i)*std::exp(sum);
      }
    return y;
  };

  bool checkReachability(const vectord &query)
  {return true;};

  inline double sqr( double x ){ return x*x; };

  void printOptimal()
  {
    vectord sv(6);
    sv <<= 0.20169, 0.150011, 0.476874, 0.275332, 0.311652, 0.6573;

    std::cout << "Solution: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
  }

private:
  matrixd mA;
  vectord mC;
  matrixd mP;
};

#endif


#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
#include <boost/program_options.hpp>
#include <filesystem>
#include "CommonCurve.h" //std::left std::setw std::setfill

namespace fs = std::filesystem;
using namespace boost;
namespace po = boost::program_options;
// The vertex of the curve model, template parameters: optimization of variable dimensions and data types
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // Reset
    {
        _estimate << 0,0,0;
    }

    virtual void oplusImpl( const double* update ) // Update
    {
        _estimate += Eigen::Vector3d(update);
    }
    // Save and read: leave blank
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}
};

// Error model template parameters: observation dimension, type, connection vertex type
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // Calculate curve model error
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}
public:
    double _x;  //x value, y value _measurement
};

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

    std::string dataFile = "readDataG2O.txt";
    std::string parameter = "parametersG2O.txt";
    std::string path = "./result/";
    // create directory if not exist
    struct stat info;
    if( stat( path.c_str(), &info ) != 0 )fs::create_directories(path);
    std::string pathRealData = path + za::getCurrentTime() + dataFile;
    std::string pathParameter = path + za::getCurrentTime() + parameter;

    // True parameter value to be estimated
    double a, b, c;

    // Total number of data point
    int N, iterate ;

    // Noise Sigma value
    double w_sigma;
    // Cammand line parser
	try
		{
			po::options_description desc("Allowed options");
			desc.add_options()
				("help,h", "produce help message")
				("first,a", po::value<double>(),
					"first parameter")
                ("second,b", po::value<double>(),
					"second parameter")
				("third,c", po::value<double>(),
					"third parameter")
                ("number,n", po::value<int>(&N)->default_value(100)->implicit_value(500),
					"number of data")
                ("iteration,i", po::value<int>(&iterate)->default_value(100)->implicit_value(200),
					"number of iterations")
				("noise,s", po::value<double>(&w_sigma)->default_value(1)->implicit_value(5),
					"noise added");

			/* Point out that all unknown values should be converted to the value of the "input-file" option.
					* Also use the command_line_parser class instead of parse_command_line */
			po::variables_map vm;
			po::store(po::parse_command_line(argc, argv, desc), vm);
			po::notify(vm);

			if (vm.count("help"))
			{
				std::cout << "Usage: options_description [options]\n";
				std::cout << desc;
				return 0;
			}
 			if (vm.count("first") )
        	{

                a =  vm["first"].as<double>();
        	}
        	else
       		{
            	std::cout << "First parameter not set.\n";
				return -1;
        	}
 			if (vm.count("second") )
        	{

                b =  vm["second"].as<double>();
        	}
        	else
       		{
            	std::cout << "Second parameter not set.\n";
				return -1;
        	}
 			if (vm.count("third") )
        	{

                a =  vm["third"].as<double>();
        	}
        	else
       		{
            	std::cout << "Third parameter not set.\n";
				return -1;
        	}
        }// end of try

        catch(std::exception& e)
		{
			std::cout << e.what() << "\n";
			return 1;
		}

      // OpenCV random number generator
    cv::RNG rng;

    // abc Estimated value of the parameter
    double abc[3] = {0,0,0};

   // set of x and y data
    std::vector<double> x_data, y_data;
    std::ofstream myfile;
    const int nameWidth     = 6;
    const int numWidth      = 8;
    myfile.open (&pathRealData[0]);
    for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
    myfile <<"\n";
    za::printElement("|", 1, myfile);
    za::printElement("N", nameWidth, myfile);
    za::printElement("|", 1, myfile);
    za::printElement("X", nameWidth, myfile);
    za::printElement("|", 1, myfile);
    za::printElement("Y", 10, myfile);
    za::printElement("|", 1, myfile);
    myfile <<"\n";
    for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
    myfile <<"\n";
    std::cout<<"Generating data: \n";

    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        za::printElement("|", 1, myfile);
        za::printElement(i+1, nameWidth, myfile);
        za::printElement("|", 1, myfile);
        za::printElement(x_data[i], nameWidth, myfile);
        za::printElement("|", 1, myfile);
        za::printElement(y_data[i], 10, myfile);
        za::printElement("|", 1, myfile);
        myfile <<"\n";
        for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
        myfile <<"\n";
    }
     myfile.close();
    // Build graph optimization, first set g2o
    // The dimension of the optimized variable for each error term is 3, and the dimension of the error value is 1
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;
    //zouma
    // Linear equation solver
    std::unique_ptr<Block::LinearSolverType> linearSolver
				(new g2o::LinearSolverDense<Block::PoseMatrixType>());

    // Matrix block solver
    std::unique_ptr<Block> solver_ptr (new Block(std::move(linearSolver)));

    // Gradient descent method, choose from GN, LM, DogLeg
    g2o::OptimizationAlgorithmGaussNewton * solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));
    //zouma

    g2o::SparseOptimizer optimizer;     // Graph model
    optimizer.setAlgorithm( solver );   // Set up the solver
    optimizer.setVerbose( true );       // Turn on debug output

    // Add vertices to the graph
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

    // Add edges to the graph
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // Set the connected vertices
        edge->setMeasurement( y_data[i] );      // Observed value
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // Information matrix: the inverse of the covariance matrix
        optimizer.addEdge( edge );
    }

    //Perform optimization
    std::cout<<"start optimization\n";
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(iterate);
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>( t2-t1 );
    std::cout<<"solve time cost = "<<time_used.count()<<" seconds. \n";

    // Output optimized value
    Eigen::Vector3d abc_estimate = v->estimate();
    std::ofstream mySol;
    mySol.open (&pathParameter[0]);
    mySol<<"estimated model a, b, c: "<<abc_estimate.transpose()<<"\n";
    mySol.close();
    return 0;
}


//
//  strassen.cpp
//  Jacobian
//
//  Created by David Freifeld
//  Copyright © 2020 David Freifeld. All rights reserved.
//
//  Description:
//  Experimental benchmarking of Strassen's Algorithm vs plain Eigen matrix multiply.
//  While naive multiplication is O(n^3), Strassen's Algorithm for matrix multiply is O(n^2.8074)
//  which means significant differences will begin to manifest for large matrices. All faster algorithms are galactic.
//

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

// Not remotely extensible, just initial test.
Eigen::MatrixXf strassen_mul(Eigen::MatrixXf x, Eigen::MatrixXf y)
{
  int power = 0;
  int largest;
  if (x.rows() >= x.cols() || x.rows() >= y.cols() || x.rows() >= y.rows()) largest = x.rows();
  else if (x.cols() >= x.rows() || x.cols() >= y.cols() || x.cols() >= y.rows()) largest = x.cols();
  else if (y.cols() >= x.rows() || y.cols() >= x.cols() || y.cols() >= y.rows()) largest = y.cols();
  else if (y.rows() >= x.rows() || y.rows() >= x.cols() || y.rows() >= y.cols()) largest = y.rows();
  for (; pow(2, power) < largest; power++) {
    std::cout << pow(2, power) << " " << power <<"\n";
  }
  std::cout << "POWER: " << power << " LARGEST: " << largest << "\n";
  Eigen::MatrixXf a ((int)pow(2,power), (int)pow(2,power));
  Eigen::MatrixXf b ((int)pow(2,power), (int)pow(2,power));
  a.block(0,0,x.rows(), x.cols()) = x;
  b.block(0,0,y.rows(), y.cols()) = y;
  std::cout << "\nINIT\n" << a << "\n\n" << b << "\n\n\n";
  std::cout << "\nEIGEN_VER\n" << a*b << "\n\n\n";
  //Eigen::MatrixXf a ()
  int block_len = largest/2;
  Eigen::MatrixXf result ((int)pow(2,power), (int)pow(2,power));

  Eigen::MatrixXf m1 = ((a.block(0,0, block_len, block_len)) + a.block(a.rows()-block_len,a.cols()-block_len, block_len, block_len)) * (b.block(0,0, block_len, block_len) + b.block(b.rows()-block_len,b.cols()-block_len, block_len, block_len));
  Eigen::MatrixXf m2 = (a.block(a.rows()-block_len, 0, block_len, block_len) + a.block(a.rows()-block_len,a.cols()-block_len, block_len, block_len)) * (b.block(0,0, block_len, block_len));
  Eigen::MatrixXf m3 = a.block(0,0, block_len, block_len) * (b.block(0,b.cols()-block_len, block_len, block_len) - b.block(b.rows()-block_len,b.cols()-block_len, block_len, block_len));
  Eigen::MatrixXf m4 = a.block(a.rows()-block_len,a.cols()-block_len, block_len, block_len) * (b.block(b.rows()-block_len,0, block_len, block_len) - b.block(0,0, block_len, block_len));
  Eigen::MatrixXf m5 = (a.block(0, 0, block_len, block_len) + a.block(0,a.cols()-block_len, block_len, block_len)) * (b.block(b.rows()-block_len,b.cols()-block_len, block_len, block_len));
  Eigen::MatrixXf m6 = (a.block(a.rows()-block_len,0, block_len, block_len) - a.block(0,0, block_len, block_len)) * (b.block(0,0, block_len, block_len) + b.block(0,b.cols()-block_len, block_len, block_len));
  Eigen::MatrixXf m7 = (a.block(0,a.cols()-block_len, block_len, block_len) - a.block(a.rows()-block_len,a.cols()-block_len, block_len, block_len)) * (b.block(a.rows()-block_len,0, block_len, block_len) + b.block(b.rows()-block_len,b.cols()-block_len, block_len, block_len));

  //  std::cout << m1 + m4 - m5 + m7 << "\n\n" << m3+m5 << "\n\n" << m2+m4 << "\n\n" << m1-m2+m3+m6;

  result.block(0,0, block_len, block_len) =  m1 + m4 - m5 + m7;
  result.block(0,result.cols()-block_len, block_len, block_len) =  m3 + m5;
  result.block(result.rows()-block_len,0, block_len, block_len) =  m2 + m4;
  result.block(result.rows()-block_len,result.cols()-block_len, block_len, block_len) =  m1 -m2 + m3 + m6;
  std::cout << "\nSTRASSEN_VER\n" << result << "\n\n\n";
  return result;
}

int main()
{
  srand((unsigned int) time(0));
  int sz;
  std::cin >> sz;
  Eigen::MatrixXf a = Eigen::MatrixXf::Random(sz, 3);
  Eigen::MatrixXf b = Eigen::MatrixXf::Random(3, sz);

  auto eigen_begin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXf product = a * b;
  auto eigen_end = std::chrono::high_resolution_clock::now();

  auto strassen_begin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXf sproduct = strassen_mul(a, b);
  auto strassen_end = std::chrono::high_resolution_clock::now();

  /* std::cout << "EIGEN: " << std::chrono::duration_cast<std::chrono::nanoseconds>(eigen_end - eigen_begin).count() / pow(10,9) << " STRASSEN: " << std::chrono::duration_cast<std::chrono::nanoseconds>(strassen_end - strassen_begin).count() / pow(10,9) << "\n"; */
  /* std::cout << "A:\n" << a << "\nB:\n" << b << "\nEigen:\n" << product << "\nStrassen:\n" << sproduct << "\n"; */
}


#pragma once

#include <Eigen/Core>
using namespace Eigen;

#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

namespace samson::so3
{
    auto so3(const Vector3dual &v) -> Matrix3dual
    {
        Matrix3dual X;
        X << 0.0, -v(2), v(1), v(2), 0.0, -v(0), -v(1), v(0), 0.0;
        return X;
    }

    auto so3(const Matrix3dual &X) -> Vector3dual
    {
        Vector3dual v;
        v << X(2, 1), X(0, 2), X(1, 0);
        return v;
    }

    auto to_axis_angle(const Vector3dual &v) -> std::pair<Vector3dual, dual>
    {
        dual theta = v.norm();
        Vector3dual k = v.normalized();
        return std::make_pair(k, theta);
    }

    auto exp(const Vector3dual &v) -> Matrix3dual
    {
        auto [k, th] = to_axis_angle(v);
        // std::cout << (th) << std::endl;;
        Matrix3dual K = so3(k);
        return Matrix3dual::Identity() + sin(th) * K + (1.0 - cos(th)) * K * K;
    }

    auto exp(const Matrix3dual &X) -> Matrix3dual
    {
        Vector3dual v = so3(X);
        return exp(v);
    }

    auto from_axis_angle(const Vector3dual &axis, dual angle)
    {
        Vector3dual x = axis * angle;
        Matrix3dual X = so3(x);
        return exp(X);
    }

} // namespace samson::so3


#include "SlamShuffler.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/integer/mod_inverse.hpp>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2019
{
namespace Day22
{

SlamShuffler::SlamShuffler(std::vector<ShuffleInstruction> instructions, BigNumber deckSize, BigNumber numIterations)
    : m_instructions{std::move(instructions)}
    , m_deckSize{deckSize}
    , m_numIterations{numIterations}
{

}

BigNumber SlamShuffler::getCardAtPosition(BigNumber position)
{
    BigNumber iterOneResult = getWhereCardComesFrom(position);
    BigNumber iterTwoResult = getWhereCardComesFrom(iterOneResult);

    // Subtract the following equations:
    // slope * position + offset = iterOneResult
    // slope * iterOneResult + offset = iterTwoResult
    // -> slope = (iterOneResult - iterTwoResult) / (position - iterOneResult)
    BigNumber slope = (iterOneResult - iterTwoResult) * boost::integer::mod_inverse(position - iterOneResult + m_deckSize, m_deckSize) % m_deckSize;
    BigNumber offset = (iterOneResult - slope * position + m_deckSize * position * slope) % m_deckSize;

    // For 2 iterations:
    // result = slope * (slope * position + offset) + offset
    // For m_numIterations iterations:
    // (slope^m_numIterations) * position + (slope^(m_numIterations - 1))) * offset + (slope^(m_numIterations - 2))) * offset + ... + offset
    // (slope^m_numIterations) * position + (slope^m_numIterations - 1) / (slope - 1) * B
    BigNumber slopeToTheNumberOfIterations = boost::multiprecision::powm(slope, m_numIterations, m_deckSize);
    BigNumber firstTerm = slopeToTheNumberOfIterations * position;
    BigNumber secondTerm = (slopeToTheNumberOfIterations - 1) * boost::integer::mod_inverse(slope - 1 + m_deckSize, m_deckSize) * offset;

    return (firstTerm + secondTerm) % m_deckSize;
}

std::vector<unsigned> SlamShuffler::getAllCardsAfterSingleIteration()
{
    std::vector<unsigned> allCards;

    for (int i = 0; i < m_deckSize; ++i)
    {
        auto cardAtPosition = getWhereCardComesFrom(i);
        allCards.push_back(unsigned{cardAtPosition});
    }

    return allCards;
}

BigNumber SlamShuffler::getWhereCardComesFrom(BigNumber position) const
{
    for (auto instructionIter = m_instructions.crbegin(); instructionIter != m_instructions.crend(); ++instructionIter)
    {
        position = executeInReverse(*instructionIter, position);
    }

    return position;
}

BigNumber SlamShuffler::executeInReverse(const ShuffleInstruction& instruction, BigNumber position) const
{
    if (instruction.type == DEAL_INTO_NEW_STACK)
    {
        return m_deckSize - 1 - position;
    }
    else if (instruction.type == CUT)
    {
        return (position + instruction.arg + m_deckSize) % m_deckSize;
    }
    else
    {
        BigNumber inverse = boost::integer::mod_inverse(BigNumber{instruction.arg}, m_deckSize);
        return inverse * position % m_deckSize;
    }
}

}
}
}


#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

MatrixXcd bl_bicggr(const MatrixXcd& A, const MatrixXcd& B, const double& tol, const int& itermax)
{
// Block BiCGGR [Tadano etal 2009 JSIAM letters]
  double Bnorm= B.norm();
  MatrixXcd X= MatrixXcd::Zero(B.rows(),B.cols()); // Initial guess of X (zeros)
  MatrixXcd R= B-A*X;
  MatrixXcd P= R;
  MatrixXcd V= A*R;
  MatrixXcd W= V;
  MatrixXcd R0til= R; //MatrixXcd::Random(n,L);
  MatrixXcd R0til_H= R0til.adjoint();
  for(int k= 0; k < itermax; ++k){
    MatrixXcd alfa= (R0til_H*V).fullPivLu().solve(R0til_H*R);
    complex<double> qsi= (W.adjoint()*R).trace()/(W.adjoint()*W).trace();
    MatrixXcd S= P-qsi*V;
    MatrixXcd U= S*alfa;
    MatrixXcd Y= A*U;
    X= X+qsi*R+U;
    MatrixXcd Rnew= R-qsi*W-Y;
    double err= Rnew.norm()/Bnorm;
    cout << "bl_bicggr: " << "iter= " << k << " relative err= " << err << endl;
    if(err < tol) break;
    W= A*Rnew;
    MatrixXcd gamma = (R0til_H*R).fullPivLu().solve(R0til_H*Rnew/qsi);
    R=Rnew;
    P= R+U*gamma;
    V= W+Y*gamma;

  }
  if((A*X-B).norm()/Bnorm > 10*tol){
      cerr << "bl_bicggr did not converge to solution within error tolerance !" << endl;
     // exit(EXIT_FAILURE);
  }

  return X;
}


#include "rmsd_align.hpp"
#include <Eigen/Dense>

#include <iostream> // delete me

namespace timemachine {

/*
Optimally align x2 onto x1. In particular, x2 is shifted so that its centroid is placed
at the same position as the of x1's centroid. x2 is also rotated so that the RMSD
is minimized.
*/
void rmsd_align_cpu(const int N, const double *x1_raw, const double *x2_raw, double *x2_aligned_raw) {

    Eigen::MatrixXd x1(N, 3);
    Eigen::MatrixXd x2(N, 3);

    for (int i = 0; i < N; i++) {
        x1(i, 0) = x1_raw[i * 3 + 0];
        x1(i, 1) = x1_raw[i * 3 + 1];
        x1(i, 2) = x1_raw[i * 3 + 2];

        x2(i, 0) = x2_raw[i * 3 + 0];
        x2(i, 1) = x2_raw[i * 3 + 1];
        x2(i, 2) = x2_raw[i * 3 + 2];
    }

    Eigen::Vector3d x1_centroid = x1.colwise().mean();
    Eigen::Vector3d x2_centroid = x2.colwise().mean();
    Eigen::Vector3d translation = x2_centroid - x1_centroid;

    // shift to the center
    Eigen::MatrixXd x1_centered = x1.rowwise() - x1_centroid.transpose();
    Eigen::MatrixXd x2_centered = x2.rowwise() - x2_centroid.transpose();

    // compute correlations
    Eigen::MatrixXd c = x2_centered.transpose() * x1_centered;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(c, Eigen::ComputeFullU | Eigen::ComputeFullV);
    auto s = svd.singularValues();

    Eigen::MatrixXd u = svd.matrixU();
    Eigen::MatrixXd v = svd.matrixV();
    Eigen::MatrixXd v_t = v.transpose();

    bool is_reflection = u.determinant() * v_t.determinant() < 0.0;
    if (is_reflection) {
        for (int i = 0; i < 3; i++) {
            u(i, 2) = -u(i, 2);
        }
    }

    Eigen::MatrixXd rotation = u * v_t;

    // x2 is centered
    Eigen::MatrixXd x2_rot = x2_centered * rotation;
    Eigen::MatrixXd x2_aligned = x2_rot.rowwise() - (translation.transpose() - x2_centroid.transpose());

    for (int i = 0; i < N; i++) {
        x2_aligned_raw[i * 3 + 0] = x2_aligned(i, 0);
        x2_aligned_raw[i * 3 + 1] = x2_aligned(i, 1);
        x2_aligned_raw[i * 3 + 2] = x2_aligned(i, 2);
    }
}

} // namespace timemachine


//EuropeanOption.cpp

//Modification date: 6/17/15

#include "EuropeanOption.hpp"
#include "GlobalFunctions.hpp"
#include <cmath>
#include <iostream>
#include <boost/math/distributions/normal.hpp>
using namespace std;

//Operator Overloads
EuropeanOption& EuropeanOption::operator = (const EuropeanOption& source)
{
	if (this == &source)							//check if current object and argument object are same object
		return *this;

	Option::operator =(source);

	T = source.T;

	return *this;
}

EuropeanOption& EuropeanCallOption::operator = (const EuropeanOption& source)
{
	if (this == &source)							//check if current object and argument object are same object
		return *this;

	EuropeanOption::operator=(source);
	return *this;
}

EuropeanOption& EuropeanPutOption::operator = (const EuropeanOption& source)
{
	if (this == &source)							//check if current object and argument object are same object
		return *this;

	EuropeanOption::operator=(source);
	return *this;
}

void EuropeanCallOption::Print() const{
	cout << "Call Option data: T=" << T << ", K=" << K << ", sig=" << sig << ", r=" <<r << ", S=" << S << "\n";
	double C = CallPrice(S, K, T, r, sig, b); double P = PutPrice(S, K, T, r, sig, b);
	cout << "Black Scholes Formulae: C=" << C << ", P=" << P << "\n";
	cout << "Put Call Parity: C=" << CallPricePCP(P, S, K, T, r, b) << ", P=" << PutPricePCP(C, S, K, T, r, b) << "\n";
	cout << "Delta(Call)=" << CallDelta(S, K, T, r, sig, b) << "\n";
	cout << "Gamma=" << GammaGF(S, K, T, r, sig, b) << ", Vega=" << VegaGF(S, K, T, r, sig, b) << "\n";		//Gamma and Vega are the same for call and put option, given the parameters are.
	cout << "Theta(Call)=" << CallTheta(S, K, T, r, sig, b) << "\n";
	cout << "Rho(Call)=" << CallRho(S, K, T, r, sig, b) << endl;

	if (PutCallParity(C, P))						//calling on either type of option, as long as Option Data is the same.
		cout << "Put Call Parity is Satisfied\n";
	else
		cout << "Arbitrage oportunity is detected!!!!\n";

}

void EuropeanPutOption::Print()const {
	cout << "Put Option data: T=" << T << ", K=" << K << ", sig=" << sig << ", r=" << r << ", S=" << S << "\n";
	double C = CallPrice(S, K, T, r, sig, b); double P = PutPrice(S, K, T, r, sig, b);
	cout << "Black Scholes Formulae: C=" << C << ", P=" << P << "\n";
	cout << "Put Call Parity: C=" << CallPricePCP(P, S, K, T, r, b) << ", P=" << PutPricePCP(C, S, K, T, r, b) << "\n";
	cout << "Delta(Put)=" << PutDelta(S, K, T, r, sig, b) << "\n";
	cout << "Gamma=" << GammaGF(S, K, T, r, sig, b) << ", Vega=" << VegaGF(S, K, T, r, sig, b) << "\n";		//Gamma and Vega are the same for call and put option, given the parameters are.
	cout << "Theta(Put)=" << PutTheta(S, K, T, r, sig, b) << "\n";
	cout << "Rho(Put)=" << CallRho(S, K, T, r, sig, b) << endl;

	if (PutCallParity(C, P))						//calling on either type of option, as long as Option Data is the same.
		cout << "Put Call Parity is Satisfied\n";
	else
		cout << "Arbitrage oportunity is detected!!!!\n";

}

//Pricing option functions

std::vector<double> EuropeanCallOption::Price(double LowerLimit, double UpperLimit, int Num, FunctionPointer Ptr)
{
	vector<double> mesh = MeshArray(LowerLimit, UpperLimit, Num);

	vector<double> result;
	result.reserve(mesh.size());

	for (vector<double>::iterator it = mesh.begin(); it != mesh.end(); ++it)
	{
		result.push_back((this->*Ptr)(*it));		//Calculate result using function pointer. Current value of vector element is taken as an argument.
	}
	return result;
}

std::vector<double> EuropeanPutOption::Price(double LowerLimit, double UpperLimit, int Num, FunctionPointer Ptr)
{
	vector<double> mesh = MeshArray(LowerLimit, UpperLimit, Num);

	vector<double> result;
	result.reserve(mesh.size());

	for (vector<double>::iterator it = mesh.begin(); it != mesh.end(); ++it)
	{
		result.push_back((this->*Ptr)(*it));
	}
	return result;
}

bool EuropeanOption::PutCallParity(double C, double P)const
{
	static const double epsilon = 0.00001;

	if ((C + K*exp(-r*T) - P - S*exp((b - r)*T)) > epsilon)
	{
		return false;
	}
	else
	{
		return true;
	}
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2017 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/Alpha_complex.h>
#include <gudhi/Rips_complex.h>
#include <gudhi/distance_functions.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>
#include <gudhi/Points_off_io.h>
#include <gudhi/Bottleneck.h>

#include <CGAL/Epick_d.h>

#include <boost/program_options.hpp>

#include <string>
#include <vector>
#include <limits>  // infinity
#include <utility>  // for pair
#include <algorithm>  // for transform

// Types definition
using Simplex_tree = Gudhi::Simplex_tree<Gudhi::Simplex_tree_options_fast_persistence>;
using Filtration_value = Simplex_tree::Filtration_value;
using Rips_complex = Gudhi::rips_complex::Rips_complex<Filtration_value>;
using Field_Zp = Gudhi::persistent_cohomology::Field_Zp;
using Persistent_cohomology = Gudhi::persistent_cohomology::Persistent_cohomology<Simplex_tree, Field_Zp >;
using Kernel = CGAL::Epick_d< CGAL::Dynamic_dimension_tag >;
using Point_d = Kernel::Point_d;
using Points_off_reader = Gudhi::Points_off_reader<Point_d>;

void program_options(int argc, char * argv[]
                     , std::string & off_file_points
                     , Filtration_value & threshold
                     , int & dim_max
                     , int & p
                     , Filtration_value & min_persistence);

static inline std::pair<double, double> compute_root_square(std::pair<double, double> input) {
  return std::make_pair(std::sqrt(input.first), std::sqrt(input.second));
}

int main(int argc, char * argv[]) {
  std::string off_file_points;
  Filtration_value threshold;
  int dim_max;
  int p;
  Filtration_value min_persistence;

  program_options(argc, argv, off_file_points, threshold, dim_max, p, min_persistence);

  Points_off_reader off_reader(off_file_points);

  // --------------------------------------------
  // Rips persistence
  // --------------------------------------------
  Rips_complex rips_complex(off_reader.get_point_cloud(), threshold, Gudhi::Euclidean_distance());

  // Construct the Rips complex in a Simplex Tree
  Simplex_tree rips_stree;

  rips_complex.create_complex(rips_stree, dim_max);
  std::cout << "The Rips complex contains " << rips_stree.num_simplices() << " simplices and has dimension "
            << rips_stree.dimension() << " \n";

  // Sort the simplices in the order of the filtration
  rips_stree.initialize_filtration();

  // Compute the persistence diagram of the complex
  Persistent_cohomology rips_pcoh(rips_stree);
  // initializes the coefficient field for homology
  rips_pcoh.init_coefficients(p);
  rips_pcoh.compute_persistent_cohomology(min_persistence);

  // rips_pcoh.output_diagram();

  // --------------------------------------------
  // Alpha persistence
  // --------------------------------------------
  Gudhi::alpha_complex::Alpha_complex<Kernel> alpha_complex(off_reader.get_point_cloud());

  Simplex_tree alpha_stree;
  alpha_complex.create_complex(alpha_stree, threshold * threshold);
  std::cout << "The Alpha complex contains " << alpha_stree.num_simplices() << " simplices and has dimension "
            << alpha_stree.dimension() << " \n";

  // Sort the simplices in the order of the filtration
  alpha_stree.initialize_filtration();

  // Compute the persistence diagram of the complex
  Persistent_cohomology alpha_pcoh(alpha_stree);
  // initializes the coefficient field for homology
  alpha_pcoh.init_coefficients(p);
  alpha_pcoh.compute_persistent_cohomology(min_persistence * min_persistence);

  // alpha_pcoh.output_diagram();

  // --------------------------------------------
  // Bottleneck distance between both persistence
  // --------------------------------------------
  double max_b_distance {};
  for (int dim = 0; dim < dim_max; dim ++) {
    std::vector< std::pair< Filtration_value , Filtration_value > > rips_intervals;
    std::vector< std::pair< Filtration_value , Filtration_value > > alpha_intervals;
    rips_intervals = rips_pcoh.intervals_in_dimension(dim);
    alpha_intervals = alpha_pcoh.intervals_in_dimension(dim);
    std::transform(alpha_intervals.begin(), alpha_intervals.end(), alpha_intervals.begin(), compute_root_square);

    double bottleneck_distance = Gudhi::persistence_diagram::bottleneck_distance(rips_intervals, alpha_intervals);
    std::cout << "In dimension " << dim << ", bottleneck distance = " << bottleneck_distance << std::endl;
    if (bottleneck_distance > max_b_distance)
      max_b_distance = bottleneck_distance;
  }
  std::cout << "================================================================================" << std::endl;
  std::cout << "Bottleneck distance is " << max_b_distance << std::endl;

  return 0;
}

void program_options(int argc, char * argv[]
                     , std::string & off_file_points
                     , Filtration_value & threshold
                     , int & dim_max
                     , int & p
                     , Filtration_value & min_persistence) {
  namespace po = boost::program_options;
  po::options_description hidden("Hidden options");
  hidden.add_options()
      ("input-file", po::value<std::string>(&off_file_points),
       "Name of an OFF file containing a point set.\n");

  po::options_description visible("Allowed options", 100);
  visible.add_options()
      ("help,h", "produce help message")
      ("max-edge-length,r",
       po::value<Filtration_value>(&threshold)->default_value(std::numeric_limits<Filtration_value>::infinity()),
       "Maximal length of an edge for the Rips complex construction.")
      ("cpx-dimension,d", po::value<int>(&dim_max)->default_value(1),
       "Maximal dimension of the Rips complex we want to compute.")
      ("field-charac,p", po::value<int>(&p)->default_value(11),
       "Characteristic p of the coefficient field Z/pZ for computing homology.")
      ("min-persistence,m", po::value<Filtration_value>(&min_persistence),
       "Minimal lifetime of homology feature to be recorded. Default is 0. Enter a negative value to see zero length intervals");

  po::positional_options_description pos;
  pos.add("input-file", 1);

  po::options_description all;
  all.add(visible).add(hidden);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).
            options(all).positional(pos).run(), vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("input-file")) {
    std::cout << std::endl;
    std::cout << "Compute the persistent homology with coefficient field Z/pZ \n";
    std::cout << "of a Rips complex defined on a set of input points.\n \n";
    std::cout << "The output diagram contains one bar per line, written with the convention: \n";
    std::cout << "   p   dim b d \n";
    std::cout << "where dim is the dimension of the homological feature,\n";
    std::cout << "b and d are respectively the birth and death of the feature and \n";
    std::cout << "p is the characteristic of the field Z/pZ used for homology coefficients." << std::endl << std::endl;

    std::cout << "Usage: " << argv[0] << " [options] input-file" << std::endl << std::endl;
    std::cout << visible << std::endl;
    exit(-1);
  }
}


/*! \file demo_Hoaglin.cpp
  \brief Demonstration of Boxplot quartile options. Boxplots appear different depending the choice of definition for the quartile.

  \details
    "Some Implementations of the Boxplot"
    Michael Frigge, David C. Hoaglin and Boris Iglewicz
    The American Statistician, Vol. 43, No. 1 (Feb., 1989), pp. 50-54
    discusses the design of the boxplot.

    However the plot of their example data shown below shows the considerable variation in the appearance of the same data,
    using different definitions of quartiles used in various popular statistics packages.

    One obvious conclusion is that you should not expect boxplots to look the same when using more than one program.

    Boost.Plot provides 5 popular definitions for the quartiles.
    This should allow the user to produce plots that look similar to boxplots from most statistics plotting program.
    To confuse matter further, most have their own default definition *and* options to chose other definitions:
    these options are shown below as type, method, PCTLDEF.

    The interquartile range is calculated using the 1st \& 3rd sample quartiles,
    but there are various ways to calculate those quartiles, summarised in
    Rob J. Hyndman and Yanan Fan, 1996, "Sample Quantiles in Statistical Packages",
    The American Statistician 50(4):361-365, (1996).

    The interquartile range, often called IQR is quartile 3 (p = 3/4) - quartile 1 (1/4).
    The median is the 2nd quartile (p = 2/4 = 1/2).

    Five of Hyndman and Fan's sample quantile definitions have a particularly simple common form
    selected according to which definition of m is chosen in function quantiles.
    This is implemented in function quantiles by parameter `HF_definition`:

      double quantile(vector<double>& data, double p, int HF_definition = 8);

    The default definition is that recommended by Hyndman and Fan, or
    users can select which definition is used for all boxplots, or individual data series as shown in the example below.

       my_boxplot.quartile_definition(5); // All plots

       my_boxplot.plot.quartile_definition(7); // Just this data series plot.

    Hyndman and Fan definitions 4 to 8 are used by the following packages:

    * #4 SAS (PCTLDEF=1), R (type=4), Maple (method=3)
    * #5 R (type=5), Maple (method=4), Wolfram Mathematica quartiles.
    * #6 Minitab, SPSS, BMDP, JMP, SAS (PCTLDEF=4), R(type=6), Maple (method=5).
    * #7 Excel, S-Plus, R (type=7[default]), Maxima, Maple (method=6).
    * #8 H&F 8: R (type=8), Maple (method=7[default]).

    Some observations on the various options are:

    * #4 Often a moderate interquartile range.

    * #5 Symmetric linear interpolation: a common choice when the data represent a sample
    from a continuous distribution and you want an unbiased estimate of the quartiles of that distribution.

    * #6 This "half" sample excludes the sample median (k observations) for odd n (=2*k+1).
    This will tend to be a better estimate for the population quartiles,
    but will tend to give quartile estimates that are a bit too far
    from the center of the whole sample (too wide an interquartile range).

    * #7 Smallest interquartile range, so flags most outliers.
    For a continuous distribution,
    this will tend to give too narrow an interquartile range,
    since there will tend to be a small fraction of the population beyond the extreme
    sample observations. In particular, for odd n (=2*k+1), Excel calculates the
    1st (3rd) quartile as the median of the lower (upper) "half" of the sample
    including the sample median (k+1 observations).

    * #8 recommended by H&F because it is
    approximately median-unbaised estimate regardless of distribution
    and thus suitable for continuous and discrete distributions.
    which gives quartiles between those reported by Minitab and Excel.
    This approach is approximately median unbiased for continuous distributions.
    Slightly higher interquartile range than definition 7.

    The 'fences' beyond which points are regarded as outliers, or extreme outliers,
    are a multiplying factor, usually called k, and usually 1.5 * interquartile range,
    and 3 * interquartile range as recommended by Hoaglin et al.

  \author Paul A Bristow
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2008

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

// An example to demonstrate the implementation of boxplot.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.

// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[demo_Hoaglin_1

/*`

"Some Implementations of the Boxplot"
Michael Frigge, David C. Hoaglin and Boris Iglewicz
The American Statistician, Vol. 43, No. 1 (Feb., 1989), pp. 50-54
discusses the design of the boxplot.

However the plot of their example data shown below shows the considerable variation in the appearance of the same data,
using different definitions of quartiles used in various popular statistics packages.

One obvious conclusion is that you should not expect boxplots to look the same when using more than one program.

Boost.Plot provides 5 popular definitions for the quartiles.
This should allow the user to produce plots that look similar to boxplots from most statistics plotting program.
To confuse matter further, most have their own default definition *and* options to chose other definitions:
these options are shown below as type, method, PCTLDEF.

The interquartile range is calculated using the 1st & 3rd sample quartiles,
but there are various ways to calculate those quartiles, summarised in
Rob J. Hyndman and Yanan Fan, 1996, "Sample Quantiles in Statistical Packages",
The American Statistician 50(4):361-365, (1996).

The interquartile range, often called IQR is quartile 3 (p = 3/4) - quartile 1 (1/4).
The median is the 2nd quartile (p = 2/4 = 1/2).

Five of Hyndman and Fan's sample quantile definitions have a particularly simple common form
selected according to which definition of m is chosen in function quantiles.
This is implemented in function quantiles by parameter `HF_definition`:

  double quantile(vector<double>& data, double p, int HF_definition = 8);

The default definition is that recommended by Hyndman and Fan, or
users can select which definition is used for all boxplots, or individual data series as shown in the example below.

   my_boxplot.quartile_definition(5); // All plots

   my_boxplot.plot.quartile_definition(7); // Just this data series plot.

Hyndman and Fan definitions 4 to 8 are used by the following packages:

* #4 SAS (PCTLDEF=1), R (type=4), Maple (method=3)
* #5 R (type=5), Maple (method=4), Wolfram Mathematica quartiles.
* #6 Minitab, SPSS, BMDP, JMP, SAS (PCTLDEF=4), R(type=6), Maple (method=5).
* #7 Excel, S-Plus, R (type=7[default]), Maxima, Maple (method=6).
* #8 H&F 8: R (type=8), Maple (method=7[default]).

Some observations on the various options are:

* #4 Often a moderate interquartile range.

* #5 Symmetric linear interpolation: a common choice when the data represent a sample
from a continuous distribution and you want an unbiased estimate of the quartiles of that distribution.

* #6 This "half" sample excludes the sample median (k observations) for odd n (=2*k+1).
This will tend to be a better estimate for the population quartiles,
but will tend to give quartile estimates that are a bit too far
from the center of the whole sample (too wide an interquartile range).

* #7 Smallest interquartile range, so flags most outliers.
For a continuous distribution,
this will tend to give too narrow an interquartile range,
since there will tend to be a small fraction of the population beyond the extreme
sample observations. In particular, for odd n (=2*k+1), Excel calculates the
1st (3rd) quartile as the median of the lower (upper) "half" of the sample
including the sample median (k+1 observations).

* #8 recommended by H&F because it is
approximately median-unbaised estimate regardless of distribution
and thus suitable for continuous and discrete distributions.
which gives quartiles between those reported by Minitab and Excel.
This approach is approximately median unbiased for continuous distributions.
Slightly higher interquartile range than definition 7.

The 'fences' beyond which points are regarded as outliers, or extreme outliers,
are a multiplying factor, usually called k, and usually 1.5 * interquartile range,
and 3 * interquartile range as recommended by Hoaglin et al.

*/

#include <vector>
using std::vector;
#include <cmath>
using ::sin;
//#include <boost/assert.hpp> // for BOOST_ASSERT
#include <boost/svg_plot/svg_boxplot.hpp>

#include <boost/svg_plot/quantile.hpp>
using boost::svg::quantile;

// double boost::svg::quantile(vector<double>& data, double p, int HF_definition);
// Estimate pth quantile of data using one of 5 definitions.
// Default HF_definition is the recommendation of Hyndman and Fan, definition #8.

#include <boost/array.hpp>
  using boost::array;

#include <iostream>
using std::cout;
using std::endl;
//] [demo_Hoaglin_1]

int main()
{
  using namespace boost::svg;
  try
  {
//[demo_Hoaglin_2]
  // 11 values from Hoaglin et al page 50.
  const boost::array<double, 11> Hoaglin_data = {53., 56., 75., 81., 82., 85., 87., 89., 95., 99., 100.};
  //                                                       q1           median           q3

  vector<double> Hoaglin(Hoaglin_data.begin(), Hoaglin_data.end());
  for (int def = 4; def <= 8; def++)
  { // All the F&Y definitions of quartiles.
    double q1 = quantile(Hoaglin, 0.25, def); // 75
    double q2 = quantile(Hoaglin, 0.5, def); // 85
    double q3 = quantile(Hoaglin, 0.75, def); // 95
    cout << "Hoaglin definition #" << def << ", q1 " << q1
      << ", q2 " << q2 << ", q3 " << q3 << ", IQR " << q3 - q1 << endl;
  } // for

  // Same data copied for different data series.
  vector<double> Hoaglin4(Hoaglin_data.begin(), Hoaglin_data.end());
  vector<double> Hoaglin5(Hoaglin_data.begin(), Hoaglin_data.end());
  vector<double> Hoaglin6(Hoaglin_data.begin(), Hoaglin_data.end());
  vector<double> Hoaglin7(Hoaglin_data.begin(), Hoaglin_data.end());
  vector<double> Hoaglin8(Hoaglin_data.begin(), Hoaglin_data.end());

  svg_boxplot H_boxplot;

  /*`Show the quartile definition default.
*/
    cout << "Default boxplot.quartile_definition() = " << H_boxplot.quartile_definition() << endl; // 8

/*` Add title, labels, range etc to the whole boxplot:
*/
  H_boxplot  // Title and axes labels.
    .title("Hoaglin Example Data")
    .x_label("Boxplot")
    .y_label("Value")
    .y_range(45, 115)  // Y-Axis range.
    .y_minor_tick_length(2)
    .y_major_interval(10);

/*`Add a few setting to the plot including setting quartile definition (though is actually same as the default 8),
and show that the value is stored.
*/
    svg_boxplot& b = H_boxplot.median_values_on(true)
    .outlier_values_on(true)
    .extreme_outlier_values_on(true)
    .quartile_definition(8);
/*`Show the quartile definition just assigned:
*/
    cout << "boxplot.quartile_definition() = " << b.quartile_definition() << endl; // 8

/*`Add a data series container, and labels, to the plot using the whole boxplot quartile definition set.
*/
    H_boxplot.plot(Hoaglin_data, "default_8");

/*`Add another data series container, and the labels, to the plot, and select a *different* quartile definition.
*/

    svg_boxplot_series& d4 =
    H_boxplot.plot(Hoaglin4, "def #4")
    .whisker_length(4.)
    .quartile_definition(4);

/*`Show the quartile definition just assigned to the this data series.
*/
  cout << "boxplot_series.quartile_definition() = " << d4.quartile_definition() << endl; // 4

/*`Add yet more data series container, and the labels, to the plot, and select a *different* quartile definition for each.
*/    H_boxplot.plot(Hoaglin5, "def #5")
    .whisker_length(5.)
    .quartile_definition(5);

    H_boxplot.plot(Hoaglin6, "def #6")
    .whisker_length(6.)
    .quartile_definition(6);

    H_boxplot.plot(Hoaglin6, "def #7")
    .whisker_length(7.)
    .quartile_definition(7);

    H_boxplot.plot(Hoaglin6, "def #8")
    .whisker_length(8.)
    .quartile_definition(8);

/*`Write the entire SVG plot to a file.
*/
  H_boxplot.write("demo_Hoaglin.svg");
//] [demo_Hoaglin_2]
  }
  catch(const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n  " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*

Output:

//[demo_Hoaglin_output

``Autorun "j:\Cpp\SVG\debug\demo_Hoaglin.exe"
Hoaglin definition #4, q1 70.25, q2 83.5, q3 90.5, IQR 20.25
Hoaglin definition #5, q1 76.5, q2 85, q3 93.5, IQR 17
Hoaglin definition #6, q1 75, q2 85, q3 95, IQR 20
Hoaglin definition #7, q1 78, q2 85, q3 92, IQR 14
Hoaglin definition #8, q1 76, q2 85, q3 94, IQR 18
Default boxplot.quartile_definition() = 8
boxplot.quartile_definition() = 8
boxplot_series.quartile_definition() = 4
``
//] [demo_Hoaglin_output]

*/


#include <cassert>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "geometry/matrix.h"

namespace gca {

  int determinant_sign(const ublas::permutation_matrix<std ::size_t>& pm) {
    int pm_sign=1;
    std::size_t size = pm.size();
    for (std::size_t i = 0; i < size; ++i)
      if (i != pm(i))
	pm_sign *= -1.0; // swap_rows would swap a pair of rows here, so we change sign
    return pm_sign;
  }

  double determinant(matrix& m) {
    ublas::permutation_matrix<size_t> pm(m.size1());
    double det = 1.0;
    if( lu_factorize(m,pm) ) {
      det = 0.0;
    } else {
      for(int i = 0; i < m.size1(); i++)
	det *= m(i,i); // multiply by elements on diagonal
      det = det * determinant_sign( pm );
    }
    return det;
  }

  double determinant(const matrix& m) {
    matrix l = m;
    return determinant(l);
  }

  matrix inverse(matrix& a) {
    matrix a_inv = ublas::identity_matrix<double>(a.size1());
    ublas::permutation_matrix<size_t> pm(a.size1());
    int res = lu_factorize(a, pm);
    if (!res) {
      lu_substitute(a, pm, a_inv);
    } else {
      std::cout << a << std::endl;
      std::cout << "Singular matrix!" << std::endl;
      assert(false);
    }
    return a_inv;
  }

  matrix inverse(const matrix& a) {
    matrix b = a;
    return inverse(b);
  }

  using namespace gca;

  matrix
  plane_basis_rotation(const point at, const point bt, const point ct,
		       const point apt, const point bpt, const point cpt) {
    matrix a(3, 3);
    a(0, 0) = at.x;
    a(1, 0) = at.y;
    a(2, 0) = at.z;

    a(0, 1) = bt.x;
    a(1, 1) = bt.y;
    a(2, 1) = bt.z;

    a(0, 2) = ct.x;
    a(1, 2) = ct.y;
    a(2, 2) = ct.z;

    matrix b(3, 3);
    b(0, 0) = apt.x;
    b(1, 0) = apt.y;
    b(2, 0) = apt.z;

    b(0, 1) = bpt.x;
    b(1, 1) = bpt.y;
    b(2, 1) = bpt.z;

    b(0, 2) = cpt.x;
    b(1, 2) = cpt.y;
    b(2, 2) = cpt.z;

    auto a_inv = inverse(a);
    return prod(b, a_inv);
  }

  vec
  to_vector(const point p) {
    vec v(3);
    v(0) = p.x;
    v(1) = p.y;
    v(2) = p.z;
    return v;
  }

  point from_vector(ublas::vector<double> v) {
    return point(v(0), v(1), v(2));
  }

  vec
  plane_basis_displacement(const matrix& r,
			   const point u1, const point u2, const point u3,
			   const point q1, const point q2, const point q3,
			   const point p1, const point p2, const point p3) {
    vec uv1 = to_vector(u1);
    vec uv2 = to_vector(u2);
    vec uv3 = to_vector(u3);

    vec qv1 = to_vector(q1);
    vec qv2 = to_vector(q2);
    vec qv3 = to_vector(q3);

    vec pv1 = to_vector(p1);
    vec pv2 = to_vector(p2);
    vec pv3 = to_vector(p3);

    vec s(3);
    s(0) = inner_prod(qv1, uv1) - inner_prod(prod(r, pv1), uv1);
    s(1) = inner_prod(qv2, uv2) - inner_prod(prod(r, pv2), uv2);
    s(2) = inner_prod(qv3, uv3) - inner_prod(prod(r, pv3), uv3);

    matrix u(3, 3);
    u(0, 0) = uv1(0);
    u(0, 1) = uv1(1);
    u(0, 2) = uv1(2);

    u(1, 0) = uv2(0);
    u(1, 1) = uv2(1);
    u(1, 2) = uv2(2);

    u(2, 0) = uv3(0);
    u(2, 1) = uv3(1);
    u(2, 2) = uv3(2);

    auto u_inv = inverse(u);

    return prod(u_inv, s);
  }

  point times_3(const matrix m, const point p) {
    auto v = to_vector(p);
    return from_vector(prod(m, v));
  }
}


/**
 *@file main.cpp
 */

#include <fstream>
#include <armadillo>
#include "Poly.h"
#include "Basis.h"
#include "Density.h"
#include "time.h"

/**
 * @mainpage Calcul et tracé de la densité d'un système nucléaire
 *# Densité nucléaire locale
 *\f[\rho(\mathbf{r})\equiv \sum_a \sum_b \rho_{ab}\psi_a(\mathbf{r})\psi^*_b(\mathbf{r})\f]
 *avec \f[\rho_{ab}\f] une matrice donnée
 *et \f[\psi_a(\mathbf{r})\f] la fonction d'onde solution de l'équation de Schrödinger 3D
\f[
\psi_{m,n,n_z}(r_\perp, \theta, z)
        \equiv
    Z(z, n_z)
    .
    R(r_\perp, m, n)
    .
         e^{im\theta}
\f]
 * # Equation de Schrödinger
 * \f[\hat{H}_{(z)}\psi_n(z) = E_n\psi_n(z)\f]
 */

/**
 *La fonction principale
 *
 *Cette fonction a pour but de calculer les données nécessaires pour tracer les grraphes
 *
 *@return la fonction créer des fichier .txt et retourne 0
 */

int main()
{
    clock_t start, finish;
    double duration;

    Basis basis(1.935801664793151,      2.829683956491218,     14,     1.3);

    Density density(basis);

    std::cout<< "===Calcul de la fonction d'onde===" << std::endl;
    //Calcul de la densité 2D
    arma::vec rVals_psi = arma::linspace(-10,10,100);
    arma::vec zVals_psi = arma::linspace(-10,10,100);
    arma::mat psi = basis.basisFunc(0, 0, 0, zVals_psi, rVals_psi);
    Utils::matToFile(rVals_psi, "rVals_psi000.txt");
    Utils::matToFile(zVals_psi, "zVals_psi000.txt");
    Utils::matToFile(psi%psi, "Psi000.txt");

    psi = basis.basisFunc(0, 0, 1, zVals_psi, rVals_psi);
    Utils::matToFile(rVals_psi, "rVals_psi001.txt");
    Utils::matToFile(zVals_psi, "zVals_psi001.txt");
    Utils::matToFile(psi%psi, "Psi001.txt");

    psi = basis.basisFunc(0, 1, 1, zVals_psi, rVals_psi);
    Utils::matToFile(rVals_psi, "rVals_psi011.txt");
    Utils::matToFile(zVals_psi, "zVals_psi011.txt");
    Utils::matToFile(psi%psi, "Psi011.txt");

    psi = basis.basisFunc(1, 0, 1, zVals_psi, rVals_psi);
    Utils::matToFile(rVals_psi, "rVals_psi101.txt");
    Utils::matToFile(zVals_psi, "zVals_psi101.txt");
    Utils::matToFile(psi%psi, "Psi101.txt");
    std::cout<< "Psi.py pour le plot Psi" << std::endl;

    std::cout<< "===Calcul de la densité 2D===" << std::endl;
    //Calcul de la densité 2D
    arma::vec rVals_2D = arma::linspace(-10,10,100);
    arma::vec zVals_2D = arma::linspace(-10,10,100);
    arma::mat R = density.calcDensity1(rVals_2D, zVals_2D);
    Utils::matToFile(rVals_2D, "rVals.txt");
    Utils::matToFile(zVals_2D, "zVals.txt");
    Utils::matToFile(R, "plot2d.txt");
    std::cout<< "Density.py pour le plot 2D" << std::endl;

    std::cout<< "===Calcul de la densité 3D===" << std::endl;
    //Calcul de la densité 3D
    arma::vec zVals = arma::linspace(-20,20,64);

    arma::mat rVals;

    if (!rVals.load("rVals3d.txt"))
    {
        std::cout<< "=================================" << std::endl;
        std::cout<< "ERREUR: exécuter python rVals3d.py" << std::endl;
        std::cout<< "=================================" << std::endl;
    }

    start = clock();
    arma::cube results = arma::zeros(rVals.n_rows,rVals.n_cols,zVals.n_rows);
    arma::mat result = arma::zeros(rVals.n_rows,zVals.n_rows);

    for (uint i =0; i<rVals.n_cols; i++)
    {
        density.calcDensity1(rVals.col(i), zVals);
        for (uint k=0; k<results.n_slices; k++)
            results.slice(k).col(i) = result.col(k);
        finish = clock();
        duration = (double)(finish - start) / CLOCKS_PER_SEC;
        std::cout<< i <<" :" << duration << "s" << std::endl;
    }

    finish = clock();
    duration = (double)(finish - start) / CLOCKS_PER_SEC;
    std::cout<< duration << "s Temps de calcul de la densité 3D" << std::endl;

    //Enregistrement des données dans un fichier
    std::ofstream out("plot3d.raw");
    if (out.is_open())
    {
        out << Utils::cubeToRaw(results) << std::endl;
        out.close();
    }
    std::cout << "La matrice est stockée dans " << "plot3d.raw" << std::endl;

    //Analyse des performances des calculs
    std::cout<< "====Performances des calculs===" << std::endl;

    rVals = arma::linspace(-10,10,10);
    zVals = arma::linspace(-10,10,10);

    start = clock();

    arma::mat resOp = density.calcDensityOp(rVals, zVals);
    finish = clock();
    duration = (double)(finish - start) / CLOCKS_PER_SEC;
    std::cout<< duration << "s après l'optimisation 1" << std::endl;

    start = clock();
    arma::mat res2 = density.calcDensity1(rVals, zVals);
    finish = clock();
    duration = (double)(finish - start) / CLOCKS_PER_SEC;
    std::cout<< duration << "s après l'optimisation 2" << std::endl;

    start = clock();
    arma::mat res1 = density.calcDensity(rVals, zVals);
    finish = clock();
    duration = (double)(finish - start) / CLOCKS_PER_SEC;
    std::cout<< duration << "s sans Optimization" << std::endl;

    std::cout << "Différence Op 1: " << arma::norm(res1 - resOp) << std::endl;
    std::cout << "Différence Op 2: " << arma::norm(res1 - res2) << std::endl;

    return 0;
}


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

#pragma once

// eigen
#include <Eigen/Core>

// curves
#include "curves/polynomial_splines_traits.hpp"

namespace curves {

/*
 * This class is the implementation of a scalar polinomial spline s(t) function of a scalar t.
 * The spline is define as
 *    s(t) = an*t^n + ... + a1*t + a0 = sum(ai*t^i)
 *
 *  The spline coefficients are stored in a standard container as
 *    alpha = [an ... a1 a0]
 */
template <int splineOrder_>
class PolynomialSpline {
 public:

  static constexpr unsigned int splineOrder = splineOrder_;
  static constexpr unsigned int coefficientCount = splineOrder + 1;

  using SplineImplementation = spline_traits::spline_rep<double, splineOrder>;
  using SplineCoefficients = typename SplineImplementation::SplineCoefficients;
  using EigenTimeVectorType = Eigen::Matrix<double, 1, coefficientCount>;
  using EigenCoefficientVectorType = Eigen::Matrix<double, coefficientCount, 1>;

  PolynomialSpline() :
    duration_(0.0),
    didEvaluateCoeffs_(false),
    coefficients_()
  {

  }

  PolynomialSpline(const SplineCoefficients& coefficients, double duration) :
    duration_(duration),
    didEvaluateCoeffs_(true),
    coefficients_(coefficients)
  {

  }

  PolynomialSpline(SplineCoefficients&& coefficients, double duration) :
    duration_(duration),
    didEvaluateCoeffs_(true),
    coefficients_(std::forward<SplineCoefficients>(coefficients))
  {

  }

  virtual ~PolynomialSpline() {

  }

  PolynomialSpline(PolynomialSpline &&) = default;
  PolynomialSpline& operator=(PolynomialSpline &&) = default;

  PolynomialSpline(const PolynomialSpline&) = default;
  PolynomialSpline& operator=(const PolynomialSpline&) = default;

  //! Get the coefficients of the spline.
  const SplineCoefficients& getCoefficients() const {
    return coefficients_;
  }

  //! Compute the coefficients of the spline.
  bool computeCoefficients(const SplineOptions& options) {
    SplineImplementation::compute(options, coefficients_);
    duration_ = options.tf_;
    return true;
  }

  //! Set the coefficients and the duration of the spline.
  void setCoefficientsAndDuration(const SplineCoefficients& coefficients, double duration) {
    coefficients_ = coefficients;
    duration_ = duration;
  }

  //! Set the coefficients and the duration of the spline.
  void setCoefficientsAndDuration(const EigenCoefficientVectorType& coefficients, double duration) {
    for (unsigned int k=0; k<coefficientCount; k++) {
      coefficients_[k] = coefficients(k);
    }
    duration_ = duration;
  }

  //! Get the spline evaluated at time tk.
  constexpr double getPositionAtTime(double tk) const {
    return std::inner_product(coefficients_.begin(), coefficients_.end(),
                              SplineImplementation::tau(std::max(0.0, std::min(tk, duration_))).begin(), 0.0);
  }

  //! Get the first derivative of the spline evaluated at time tk.
  constexpr double getVelocityAtTime(double tk) const {
    return std::inner_product(coefficients_.begin(), coefficients_.end(),
                              SplineImplementation::dtau(std::max(0.0, std::min(tk, duration_))).begin(), 0.0);
  }

  //! Get the second derivative of the spline evaluated at time tk.
  constexpr double getAccelerationAtTime(double tk) const {
    return std::inner_product(coefficients_.begin(), coefficients_.end(),
                              SplineImplementation::ddtau(std::max(0.0, std::min(tk, duration_))).begin(), 0.0);
  }

  //! Get the time vector tau evaluated at time tk.
  static inline void getTimeVector(Eigen::Ref<EigenTimeVectorType> timeVec, double tk) {
    timeVec = Eigen::Map<EigenTimeVectorType>(SplineImplementation::tau(tk).data());
  }

  //! Get the first derivative of the time vector tau evaluated at time tk.
  static inline void getdTimeVector(Eigen::Ref<EigenTimeVectorType> dtimeVec, double tk) {
    dtimeVec = Eigen::Map<EigenTimeVectorType>(SplineImplementation::dtau(tk).data());
  }

  //! Get the second derivative of the time vector tau evaluated at time tk.
  static inline void getddTimeVector(Eigen::Ref<EigenTimeVectorType> ddtimeVec, double tk) {
    ddtimeVec = Eigen::Map<EigenTimeVectorType>(SplineImplementation::ddtau(tk).data());
  }

  //! Get the time vector tau evaluated at zero.
  static inline void getTimeVectorAtZero(Eigen::Ref<EigenTimeVectorType> timeVec) {
    timeVec = Eigen::Map<const EigenTimeVectorType>((SplineImplementation::tauZero).data());
  }

  //! Get the first derivative of the time vector tau evaluated at zero.
  static inline void getdTimeVectorAtZero(Eigen::Ref<EigenTimeVectorType> dtimeVec) {
    dtimeVec = Eigen::Map<const EigenTimeVectorType>((SplineImplementation::dtauZero).data());
  }

  //! Get the second derivative of the time vector tau evaluated at zero.
  static inline void getddTimeVectorAtZero(Eigen::Ref<EigenTimeVectorType> ddtimeVec) {
    ddtimeVec = Eigen::Map<const EigenTimeVectorType>((SplineImplementation::ddtauZero).data());
  }

  //! Get the duration of the spline in seconds.
  double getSplineDuration() const {
    return duration_;
  }

 protected:
  //! The duration of the spline in seconds.
  double duration_;

  //! True if the coefficents were computed at least once.
  bool didEvaluateCoeffs_;

  /*
   * s(t) = an*t^n + ... + a1*t + a0
   * splineCoeff_ = [an ... a1 a0]
   */
  SplineCoefficients coefficients_;
};

} /* namespace */


/**
 * @file taylorode.cc
 * @brief NPDE homework TaylorODE
 * @author ?, Philippe Peter
 * @date 24.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "taylorode.h"

#include <Eigen/Core>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

namespace TaylorODE {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector2d PredPreyModel::f(const Eigen::Vector2d& y) const {
  return {(alpha1_ - beta1_ * y(1)) * y(0), (beta2_ * y(0) - alpha2_) * y(1)};
}

Eigen::Vector2d PredPreyModel::df(const Eigen::Vector2d& y,
                                  const Eigen::Vector2d& z) const {
  Eigen::Matrix2d Df;
  Df << alpha1_ - beta1_ * y(1), -beta1_ * y(0), beta2_ * y(1),
      -alpha2_ + beta2_ * y(0);
  return Df * z;
}

Eigen::Vector2d PredPreyModel::d2f(const Eigen::Vector2d& y,
                                   const Eigen::Vector2d& z) const {
  Eigen::Matrix2d H1, H2;
  H1 << 0, -beta1_, -beta1_, 0;
  H2 << 0, beta2_, beta2_, 0;
  return {z.transpose() * H1 * z, z.transpose() * H2 * z};
}

std::vector<Eigen::Vector2d> SolvePredPreyTaylor(const PredPreyModel& model,
                                                 double T,
                                                 const Eigen::Vector2d& y0,
                                                 unsigned int M) {
  std::vector<Eigen::Vector2d> res;
  res.reserve(M + 1);

  Eigen::Vector2d y = y0;
  double h = T / M;
  res.push_back(y);

  for (unsigned int k = 0; k < M; ++k) {
    // evaluate terms for taylor step.
    auto fy = model.f(y);
    auto dfyfy = model.df(y, fy);
    auto df2yfy = model.df(y, dfyfy);
    auto d2fyfy = model.d2f(y, fy);

    // evaluate taylor expansion to compute update
    y = y + h * fy + 0.5 * h * h * dfyfy +
        1.0 / 6.0 * h * h * h * (df2yfy + d2fyfy);

    // save new state:
    res.push_back(y);
  }
  return res;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
double TestCvgTaylorMethod() {
  // initialize parameters for the model:
  double T = 10;               // final time
  Eigen::Vector2d y0(100, 5);  // initial condition
  Eigen::Vector2d yex(0.319465882659820,
                      9.730809352326228);  // reference solution
  double alpha1 = 3.0;
  double alpha2 = 2.0;
  double beta1 = 0.1;
  double beta2 = 0.1;
  PredPreyModel model(alpha1, alpha2, beta1, beta2);

  // Initialize parameters for the convergence study
  unsigned int M0 = 128;    // Minimum number of timesteps
  unsigned int numRef = 8;  // Number of refinements

  // Convergence study
  Eigen::ArrayXd error(numRef);
  Eigen::ArrayXd M(numRef);
  for (unsigned int i = 0; i < numRef; ++i) {
    M(i) = std::pow(2, i) * M0;
    auto res = SolvePredPreyTaylor(model, T, y0, M(i));
    error(i) = (res.back() - yex).norm();
  }

  PrintErrorTable(M, error);

  // calculate linear regression line: log(error) ~ c0 + c1*log(M)
  Eigen::MatrixXd A(numRef, 2);
  A.col(0) = Eigen::VectorXd::Ones(numRef);
  A.col(1) = M.log();
  Eigen::VectorXd logError = error.log();
  Eigen::Vector2d coeffs = A.householderQr().solve(logError);

  // estimated convergence rate: -c1
  return -coeffs(1);
}
/* SAM_LISTING_END_2 */

void PrintErrorTable(const Eigen::ArrayXd& M, const Eigen::ArrayXd& error) {
  std::cout << std::setw(15) << "M" << std::setw(15) << "error" << std::setw(15)
            << "rate" << std::endl;

  for (unsigned int i = 0; i < M.size(); ++i) {
    std::cout << std::setw(15) << M(i) << std::setw(15) << error(i);
    if (i > 0) {
      std::cout << std::setw(15) << std::log2(error(i - 1) / error(i));
    }
    std::cout << std::endl;
  }
}

}  // namespace TaylorODE


/**
 * @file semimprk.cc
 * @brief NPDE homework SemImpRK code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include "semimprk.h"

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

#include "polyfit.h"

namespace SemImpRK {

/* SAM_LISTING_BEGIN_0 */
double cvgRosenbrock() {
  double cvgRate = 0.0;
  // TO DO: (13-2.d) Use polyfit() to estimate the rate of convergence
  // for solveRosenbrock().
  // Final time
  const double T = 10.0;
  // Mesh sizes h=2^{-k} for k in K.
  const Eigen::ArrayXd K = Eigen::ArrayXd::LinSpaced(7, 4, 10);

  // Initial data
  Eigen::Vector2d y0(1., 1.);
  // Parameter and useful matrix for f
  const double lambda = 1;
  Eigen::Matrix2d R;
  R << 0.0, -1.0, 1.0, 0.0;

  // Function and its Jacobian
  auto f = [&R, &lambda](Eigen::Vector2d y) {
    return R * y + lambda * (1.0 - y.squaredNorm()) * y;
  };
  auto df = [&lambda](Eigen::Vector2d y) {
    double x = 1 - y.squaredNorm();
    Eigen::Matrix2d J;
    J << lambda * x - 2 * lambda * y(0) * y(0), -1 - 2 * lambda * y(1) * y(0),
        1 - 2 * lambda * y(1) * y(0), lambda * x - 2 * lambda * y(1) * y(1);
    return J;
  };

  // Reference mesh size
  const int N_ref = 10 * std::pow(2, 12);
  // Reference solution
  std::vector<Eigen::VectorXd> solref = solveRosenbrock(f, df, y0, N_ref, T);

  Eigen::ArrayXd Error(K.size());
  std::cout << std::setw(15) << "N" << std::setw(16) << "maxerr\n";
  // Main loop: loop over all meshes
  for (unsigned int i = 0; i < K.size(); ++i) {
    // h = 2^{-k} => N = T*h = T*2^k
    int N = T * std::pow(2, K[i]);
    // Get solution
    std::vector<Eigen::VectorXd> sol = solveRosenbrock(f, df, y0, N, T);
    // Compute error
    double maxerr = 0;
    for (unsigned int j = 0; j < sol.size(); ++j) {
      maxerr =
          std::max(maxerr, (sol.at(j) - solref.at((j * N_ref) / N)).norm());
    }

    Error[i] = maxerr;
    std::cout << std::setw(15) << N << std::setw(16) << maxerr << std::endl;
  }
  // Use log(N)=log(T*2^k)=log(T)+log(2)*k to get natural logarithm of N.
  cvgRate = -polyfit(std::log(2.0) * K, Error.log(), 1)(0);
  return cvgRate;
}
/* SAM_LISTING_END_0 */

}  // namespace SemImpRK


#pragma once

#include "perceive/foundation.hpp"
#include "perceive/geometry/vector.hpp"
#include <Eigen/Dense>

namespace perceive
{
// ------------------------------------------------------------------- is-finite

bool is_finite(const Vector2r& M);
bool is_finite(const Vector3r& M);
bool is_finite(const Vector4r& M);
bool is_finite(const Matrix3r& M);
bool is_finite(const Matrix34r& M);
bool is_finite(const MatrixXr& M);

// ------------------------------------------------------------------- normalize

inline Vector3r& normalize(Vector3r& X)
{
   auto norm_inv = 1.0 / X.norm();
   X *= norm_inv;
   return X;
}

inline Vector3r normalized(const Vector3r& X)
{
   auto Y(X);
   normalize(Y);
   return Y;
}

// ------------------------------------------------------------------------- str

std::string str(const Vector2r& M);
std::string str(const Vector3r& M);
std::string str(const Vector4r& M);
std::string str(const Matrix3r& M);
std::string str(const Matrix34r& M);
std::string str(const MatrixXr& M);

std::string str(std::string name, const Matrix3r& M);
std::string str(std::string name, const Matrix34r& M);
std::string str(std::string name, const MatrixXr& M);

// ------------------------------------------------------------------------- SVD

real svd_thin(const MatrixXr& M, VectorXr& out);
real svd_thin(const Matrix3r& M, Vector3r& out);
// real svd_thin(const Matrix3d& M, Vector3d& out);
// real svd_thin(const MatrixXd& M, VectorXd& out);
real svd_thin(const MatrixXr& M, Vector6r& out);
void svd_UV(const MatrixXr& M, MatrixXr& U, MatrixXr& V);
void svd_UV(const Matrix3r& M, Matrix3r& U, Matrix3r& V);
// void svd_UV(const MatrixXd& M, MatrixXd& U, MatrixXd& V);
void svd_UDV(const MatrixXr& M, MatrixXr& U, VectorXr& D, MatrixXr& V);
void svd_UDV(const Matrix3r& M, Matrix3r& U, Vector3r& D, Matrix3r& V);
void svd_UDV(const Matrix3r& M, Matrix3r& U, Matrix3r& D, Matrix3r& V);
// void svd_UDV(const MatrixXd& M, MatrixXd& U, VectorXd& D, MatrixXd& V);

void svd_UDV(const Eigen::Matrix<std::complex<long double>, 3, 3>& M,
             Eigen::Matrix<std::complex<long double>, 3, 3>& U,
             Eigen::Matrix<std::complex<long double>, 3, 1>& D,
             Eigen::Matrix<std::complex<long double>, 3, 3>& V);
void svd_UDV(const Eigen::Matrix<std::complex<double>, 3, 3>& M,
             Eigen::Matrix<std::complex<double>, 3, 3>& U,
             Eigen::Matrix<std::complex<double>, 3, 1>& D,
             Eigen::Matrix<std::complex<double>, 3, 3>& V);
void svd_UDV(const Eigen::Matrix<std::complex<float>, 3, 3>& M,
             Eigen::Matrix<std::complex<float>, 3, 3>& U,
             Eigen::Matrix<std::complex<float>, 3, 1>& D,
             Eigen::Matrix<std::complex<float>, 3, 3>& V);

void svd_UDV(
    const Eigen::
        Matrix<std::complex<long double>, Eigen::Dynamic, Eigen::Dynamic>& M,
    Eigen::Matrix<std::complex<long double>, Eigen::Dynamic, Eigen::Dynamic>& U,
    Eigen::Matrix<std::complex<long double>, Eigen::Dynamic, 1>& D,
    Eigen::Matrix<std::complex<long double>, Eigen::Dynamic, Eigen::Dynamic>&
        V);

void svd_UDV(
    const Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic>&
        M,
    Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic>& U,
    Eigen::Matrix<std::complex<double>, Eigen::Dynamic, 1>& D,
    Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic>& V);

void svd_UDV(
    const Eigen::Matrix<std::complex<float>, Eigen::Dynamic, Eigen::Dynamic>& M,
    Eigen::Matrix<std::complex<float>, Eigen::Dynamic, Eigen::Dynamic>& U,
    Eigen::Matrix<std::complex<float>, Eigen::Dynamic, 1>& D,
    Eigen::Matrix<std::complex<float>, Eigen::Dynamic, Eigen::Dynamic>& V);

// double bdcsvd_thin(const MatrixXd& M, VectorXd& out);
real bdcsvd_thin(const MatrixXr& M, VectorXr& out);

template<typename T> struct CentreEigenVectorResult
{
   T C;                           // centre
   vector<std::pair<T, real>> Es; // {eigenvectors, eigenvalues}
};

template<typename InputIt>
auto calc_centre_and_eigenvectors(InputIt begin, InputIt end)
    -> CentreEigenVectorResult<
        typename std::iterator_traits<InputIt>::value_type>;

// For best results, centre and scale the data before performing svd-3d-UDV
struct SVD3DRet
{
   Matrix3r U, V;
   Vector3r D;
   Matrix3r Dm() const noexcept; // `D` as a Diagonal Matrix
   string to_string() const noexcept;
   friend string str(const SVD3DRet& o) noexcept { return o.to_string(); }
   Vector3r eigen_vector(int ind) const noexcept;
   Quaternion rot_vec() const noexcept;
};

template<typename InputIt> SVD3DRet svd_3d_UDV(InputIt start, InputIt finish);

// ------------------------------------------------------------ condition-number

real condition_number(const MatrixXr& M);
real condition_number(const Matrix3r& M);

// ----------------------------------------------------------------- matrix rank

template<typename T> inline unsigned matrix_rank(const T& M)
{
   Eigen::FullPivLU<T> lu(M);
   return lu.rank();
}

// ------------------------------------------------------------------ null space
template<typename T> inline T matrix_kernel(const T& M)
{
   Eigen::FullPivLU<T> lu(M);
   T out = lu.kernel();
   return out;
}

// -------------------------------------------------------- Cross-product Matrix

void to_cross_product_matrix(const Vector3& X, Matrix3r& M);
void to_cross_product_matrix(const Vector3r& X, Matrix3r& M);
Matrix3r make_cross_product_matrix(const Vector3& X);
Matrix3r make_cross_product_matrix(const Vector3r& X);

// --------------------------------------------------- Quadratic with Constraint

// Minimize xGtGx such that Hx = h
//  * G must have more rows than columns.
//  * G.cols() == H.cols()
MatrixXr minimize_with_constraint(const MatrixXr& G, const MatrixXr& H, real h);

//
//
//
//
//
//
//
//
//                                Implementations
//
//
//
//
//
//
//

template<typename InputIt>
auto calc_centre_and_eigenvectors(InputIt begin, InputIt end)
    -> CentreEigenVectorResult<
        typename std::iterator_traits<InputIt>::value_type>
{
   using T   = typename std::iterator_traits<InputIt>::value_type;
   using Ret = CentreEigenVectorResult<T>;

   Ret out;
   const int sz = std::distance(begin, end);
   const int N  = (sz == 0) ? -1 : begin->size();
   if(sz < N) return out;

   // Calculate the centre
   T C;
   for(auto i = 0; i < sz; ++i) C(i) = 0.0;
   for(auto ii = begin; ii != end; ++ii) { C += *ii; }
   C /= sz;

   // Create matrices
   MatrixXr A(sz, N);
   int pos = 0;
   for(auto ii = begin; ii != end; ++ii, ++pos) {
      for(auto j = 0; j < N; ++j) A(pos, j) = ii->operator()(j) - C(j);
   }

   MatrixXr At  = A.transpose();
   MatrixXr AtA = At * A;

   MatrixXr U, V;
   VectorXr D;
   svd_UDV(A, U, D, V);

   Expects(V.rows() == N);
   Expects(V.cols() == N);

   out.C = C;
   out.Es.resize(N);
   for(auto i = 0; i < N; ++i) {
      auto& E  = out.Es[i];
      E.second = D(i);
      for(auto j = 0; j < N; ++j) { E.first(j) = V(j, i); }
   }

   return out;
}

// -------------------------------------------------------------------svd-3d-UDV
// For best results, center and scale the data first.
template<typename InputIt> SVD3DRet svd_3d_UDV(InputIt start, InputIt finish)
{
   using T = typename std::iterator_traits<InputIt>::value_type;

   const size_t n_rows     = size_t(std::distance(start, finish));
   constexpr size_t n_cols = 3;
   Expects(n_rows >= n_cols);

   auto M  = MatrixXr(n_rows, n_cols);
   int row = 0;
   for(auto ii = start; ii != finish; ++ii) {
      const T& o = *ii;
      Expects(o.size() == n_cols);
      for(int col = 0; col < int(n_cols); ++col) M(row, col) = real(o(col));
      ++row;
   }

   MatrixXr Mt  = M.transpose();
   MatrixXr MtM = Mt * M;

   SVD3DRet ret;
   svd_UDV(MtM, ret.U, ret.D, ret.V);

   return {ret};
}

} // namespace perceive


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>

#include <adept_source.h>
#include <adept_arrays.h>
using adept::adouble;
using adept::aMatrix;
using adept::aVector;

using adept::Vector;

extern int enzyme_const;
template<typename Return, typename... T>
Return __enzyme_autodiff(T...);

float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

static double sum(const double *x, size_t n) {
    double res = 0;
    for(int i=0; i<n; i++) {
        res+=x[i];
    }
    return res;
}

static double max(double x, double y) {
    return (x > y) ? x : y;
}

static adouble amax(adouble x, adouble y) {
    return (x > y) ? x : y;
}

static double logsumexp(const double *__restrict x, size_t n) {
  double A = x[0];
  for(int i=0; i<n; i++) {
    A = max(A, x[i]);
  }
  double sema = 0;
  for(int i=0; i<n; i++) {
    sema += exp(x[i] - A);
  }
  return log(sema) + A;
}

static adouble alogsumexp(const aVector &x, size_t n) {
  adouble A = x[0];
  for(int i=0; i<n; i++) {
    A = amax(A, x[i]);
  }
  adouble ema[n];
  for(int i=0; i<n; i++) {
    ema[i] = exp(x[i] - A);
  }
  adouble sema = 0;
  for(int i=0; i<n; i++)
    sema += ema[i];
  return log(sema) + A;
}

extern "C" {
#include <adBuffer.h>
}

/*
  Differentiation of logsumexp in reverse (adjoint) mode:
   gradient     of useful results: *x logsumexp
   with respect to varying inputs: *x
   RW status of diff variables: *x:incr logsumexp:in-killed
   Plus diff mem management of: x:in
*/
static void logsumexp_b(const double *__restrict x, double *xb, size_t n, double logsumexpb) {
    double A = x[0];
    double Ab = 0.0;
    int branch;
    double logsumexp;
    for (int i = 0; i < n; ++i)
        if (A < x[i]) {
            A = x[i];
            pushControl1b(0);
        } else {
            pushControl1b(1);
            A = A;
        }
    double sema = 0;
    double semab = 0.0;
    for (int i = 0; i < n; ++i)
        sema = sema + exp(x[i] - A);
    semab = logsumexpb/sema;
    Ab = logsumexpb;
    {
      double tempb;
      for (int i = n-1; i > -1; --i) {
          tempb = exp(x[i]-A)*semab;
          xb[i] = xb[i] + tempb;
          Ab = Ab - tempb;
      }
    }
    for (int i = n-1; i > -1; --i) {
        popControl1b(&branch);
        if (branch == 0) {
            xb[i] = xb[i] + Ab;
            Ab = 0.0;
        }
    }
    xb[0] = xb[0] + Ab;
}

adouble alogsumexp2(const aVector &x, size_t n) {
  adouble A = x[0];
  for(int i=0; i<n; i++) {
    A = amax(A, x[i]);
  }
  return adept::log(adept::sum(exp(x - A))) + A;
}

static void adept_sincos(double *input, double *inputp, unsigned long n, unsigned long repeat) {
  {
  struct timeval start, end;
  //gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector inp(n);
  for(int i=0; i<n; i++) inp(i) = input[i];
  memset(inputp, 0, sizeof(double)*n);
  double total = 0;

  gettimeofday(&start, NULL);
  for (int iter = 0; iter < repeat; iter++) {
    stack.new_recording();
    adouble resa = alogsumexp(inp, n);
    stack.pause_recording();
    total += resa.value();
    stack.continue_recording();
  }
  gettimeofday(&end, NULL);

  stack.pause_recording();

  printf("adept forward (recording) %0.6f res'=%f\n", tdiff(&start, &end), total);
  }
  {
  struct timeval start, end;
  //gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector inp(n);
  for(int i=0; i<n; i++) inp(i) = input[i];
  memset(inputp, 0, sizeof(double)*n);

  gettimeofday(&start, NULL);
  for (int iter = 0; iter < repeat; iter++) {
    stack.new_recording();
    adouble resa = alogsumexp(inp, n);
    resa.set_gradient(1.0);
    stack.reverse();
    stack.pause_recording();
    for (int i = 0; i < n; i++) {
        inputp[i] += inp(i).get_gradient();
    }
    stack.continue_recording();
  }
  gettimeofday(&end, NULL);

  stack.pause_recording();

  printf("adept forward reverse %0.6f res'=%f\n", tdiff(&start, &end), sum(inputp, n));
  }
}
static void adept2_sincos(double *input, double *inputp, unsigned long n, unsigned long repeat) {
  {
  struct timeval start, end;
  //gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector inp(n);
  for(int i=0; i<n; i++) inp(i) = input[i];
  memset(inputp, 0, sizeof(double)*n);
  double total = 0;

  gettimeofday(&start, NULL);
  for (int iter = 0; iter < repeat; iter++) {
    stack.new_recording();
    adouble resa = alogsumexp2(inp, n);
    stack.pause_recording();
    total += resa.value();
    stack.continue_recording();
  }
  gettimeofday(&end, NULL);

  stack.pause_recording();

  printf("adept2 forward (recording) %0.6f res'=%f\n", tdiff(&start, &end), total);
  }
  {
  struct timeval start, end;
  //gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector inp(n);
  for(int i=0; i<n; i++) inp(i) = input[i];
  memset(inputp, 0, sizeof(double)*n);

  gettimeofday(&start, NULL);
  for (int iter = 0; iter < repeat; iter++) {
    stack.new_recording();
    adouble resa = alogsumexp2(inp, n);
    resa.set_gradient(1.0);
    stack.reverse();
    stack.pause_recording();
    for (int i = 0; i < n; i++) {
        inputp[i] += inp(i).get_gradient();
    }
    stack.continue_recording();
  }
  gettimeofday(&end, NULL);

  stack.pause_recording();

  printf("adept2 forward reverse %0.6f res'=%f\n", tdiff(&start, &end), sum(inputp, n));
  }
}

static void enzyme_sincos(double *input, double *inputp, unsigned long n, unsigned long repeat) {
    double realinput = input[0];
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);
  double total = 0;
  for(int i=0; i<repeat; i++) {
    input[0] = realinput + (double)i/10000000;
    total += logsumexp(input, n);
  }

  gettimeofday(&end, NULL);
  printf("enzyme forward %0.6f res'=%f\n", tdiff(&start, &end), total);
  }
  {
      input[0] = realinput;
  struct timeval start, end;
  memset(inputp, 0, sizeof(double)*n);

  gettimeofday(&start, NULL);

  for(int i=0; i<repeat; i++) {
    __enzyme_autodiff<void>(logsumexp, input, inputp, n);
  }

  gettimeofday(&end, NULL);
  printf("enzyme forward and reverse %0.6f res'=%f\n", tdiff(&start, &end), sum(inputp, n));
  }
}
static void tapenade_sincos(double *input, double *inputp, unsigned long n, unsigned long repeat) {
    double realinput = input[0];
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);
  double total = 0;
  for(int i=0; i<repeat; i++) {
    input[0] = realinput + (double)i/10000000;
    total += logsumexp(input, n);
  }

  gettimeofday(&end, NULL);
  printf("tapenade forward %0.6f res'=%f\n", tdiff(&start, &end), total);
  }
  {
      input[0] = realinput;
  struct timeval start, end;
  memset(inputp, 0, sizeof(double)*n);

  gettimeofday(&start, NULL);

  for(int i=0; i<repeat; i++) {
    logsumexp_b(input, inputp, n, 1.0);
  }

  gettimeofday(&end, NULL);
  printf("tapenade forward and reverse %0.6f res'=%f\n", tdiff(&start, &end), sum(inputp, n));
  }
}

int main(int argc, char** argv) {
    if (argc < 2) {
        printf("usage %s n repeat\n", argv[0]);
        return 1;
    }
  unsigned long n = atoi(argv[1]);
  unsigned long repeat = atoi(argv[2]);

  double *input = new double[n];
  double *inputp = new double[n];
  for(int i=0; i<n; i++) {
    input[i] = 3.1415926535 / (i+1);
  }

  //adept_sincos(input, inputp, n, repeat);

  adept2_sincos(input, inputp, n, repeat);

  tapenade_sincos(input, inputp, n, repeat);

  enzyme_sincos(input, inputp, n, repeat);
}


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

using namespace std;
using boost::multiprecision::cpp_int;

// Returns n!
cpp_int factorial(cpp_int n) {
	if(n == 0) {
		return 1;
	}
	return n * factorial(n - 1);
}

cpp_int sum_of_digits(cpp_int num) {
	cpp_int sum = 0;
	while(num) {
		sum += num % 10;
		num /= 10;
	}
	return sum;
}

int main(int argc, char *argv[]) {
	cout << sum_of_digits(factorial(100)) << endl;
	return 0;
}