#pragma once

#include "random_engine.hpp"
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/optional.hpp>

namespace cryptb
{
	class rsa
	{
		// Private key- for decrypting / digital signing
		// DON'T SHARE d WITH THE CLIENT!
		// Tends to be a number with around 2048 bits.
		boost::multiprecision::cpp_int d{ 0 };

		// It's completely safe to share e and N with the entire world.
		// In fact, you should.

		// Public key- for encrypting / verifying digital signature
		// Tends to be a very small number. Choosing the number 3 for example, is common.
		boost::multiprecision::cpp_int e{ 0 };

		// Public key- for everything. N is needed for all operations.
		// Tends to be a number with about 4096 bits.
		boost::multiprecision::cpp_int N{ 0 };

		// A pure mathematical function to solve for d such that:
		// ((e * d) modulo PhiN) == 1
		// e and PhiN must already be coprime.
		static boost::multiprecision::cpp_int findd(const boost::multiprecision::cpp_int& PhiN, const boost::multiprecision::cpp_int& e);

	public:
		rsa(const rsa&) = default;
		rsa(rsa&&) = default;
		rsa& operator=(const rsa&) = default;
		rsa& operator=(rsa&&) = default;

		// Constructor for generating RSA public-private key pair using the given random engine.
		//
		// When "num_bytes_in_prime_number" == 128 that's 2048-bit RSA
		// Should take a second and a half (very expensive function, call on an asynchronous thread).
		//
		// "num_bytes_in_prime_number" must be at least 2
		//
		rsa(random_engine& rand, const int num_bytes_in_prime_number = 128);

		// Constructor for loading RSA public-private key pairs from values
		rsa(boost::multiprecision::cpp_int&& e, boost::multiprecision::cpp_int&& d, boost::multiprecision::cpp_int&& N) :
			e(std::move(e)), d(std::move(d)), N(std::move(N)) {}

		// Private secret key, don't share.
		const boost::multiprecision::cpp_int& get_d() const
		{
			return this->d;
		}

		// Public key, no danger. Allowed to reveal to the entire world.
		const boost::multiprecision::cpp_int& get_e() const
		{
			return this->e;
		}

		// Public key, no danger. Allowed to reveal to the entire world.
		const boost::multiprecision::cpp_int& get_N() const
		{
			return this->N;
		}

		// powm(a, b, c) == power(a, b) modulo c
		// When computing powm in one operation that can reduce the
		// computation time down from millions of years to mere microseconds.
		//
		// Technically powm treats negative exponents differently than power(a, b)
		// but in the function "rsa::findd()" we made sure that the returned
		// d is positive so in our use case we're only dealing with positive numbers.
		//

		// "original_message" should either be a large random number
		// otherwise the entire algorithm will be insecure.
		// That's because if the same clear-text message is sent to e or more recipients in an encrypted way,
		// and the receivers share the same exponent e, but different p, q, and therefore n,
		// then it's easy to decrypt the original clear-text message via the Chinese remainder theorem.
		//
		// Basically: only use the "encrypt" function with an original_message that is a large
		// random number.
		//
		// You should check that:
		// 0 <= "original_message" < N
		// and that:
		// is_valid_public_key(e, N) == true
		// Otherwise the function will return boost::none
		static boost::optional<boost::multiprecision::cpp_int> encrypt(
			const boost::multiprecision::cpp_int& original_message,
			const boost::multiprecision::cpp_int& e,
			const boost::multiprecision::cpp_int& N)
		{
			if (!rsa::is_valid_public_key(e, N) || original_message >= N || original_message < 0)
				return boost::none;
			return static_cast<boost::multiprecision::cpp_int>(boost::multiprecision::powm(original_message, e, N));
		}

		// You should check that:
		// 0 <= "encrypted_message" < this->N
		// Otherwise the function will return boost::none
		boost::optional<boost::multiprecision::cpp_int> decrypt(const boost::multiprecision::cpp_int& encrypted_message)
		{
			if (encrypted_message >= this->N || encrypted_message < 0)
				return boost::none;
			return static_cast<boost::multiprecision::cpp_int>(boost::multiprecision::powm(encrypted_message, this->d, this->N));
		}

		// RSA digital signature.
		// message_hash must be a cryptographic hash of a message
		// and not the message itself.
		// Otherwise the digital signature won't be secure.
		//
		// You should check that:
		// 0 <= "message_hash" < this->N
		// Otherwise the function will return boost::none
		boost::optional<boost::multiprecision::cpp_int> sign(const boost::multiprecision::cpp_int& message_hash)
		{
			// It's the same algorithm. Isn't that convenient!
			return this->decrypt(message_hash);
		}

		// Verify an RSA digital signature.
		static bool is_valid_signature(
			const boost::multiprecision::cpp_int& message_hash,
			const boost::multiprecision::cpp_int& signature_of_hash,
			const boost::multiprecision::cpp_int& e,
			const boost::multiprecision::cpp_int& N)
		{
			// It's the same algorithm. Isn't that convenient!
			const boost::optional<boost::multiprecision::cpp_int> result = rsa::encrypt(signature_of_hash, e, N);
			if (result == boost::none)
				return false;
			// If the signature matches then it's legit.
			return result.get() == message_hash;
		}

		// Recommended to check the validity of public keys taken
		// from an untrusted source.
		// We wouldn't want to store an invalid public key
		// in the database of known public keys.
		// That would cause functions such as rsa::encrypt
		// to return boost::none
		static bool is_valid_public_key(
			const boost::multiprecision::cpp_int& e,
			const boost::multiprecision::cpp_int& N)
		{
			if (e < 2 || N < (2*3))
				return false;
			return true;
		}
	};
}


#define EIGEN_USE_MKL_ALL

#include <iostream>

#include <Eigen/Core>

#include "../misc.hpp"

#include "models/regressors/least_squares_linear_regression.hpp"
#include "models/regressors/ridge_regression.hpp"
#include "models/regressors/optimizable_linear_regressor.hpp"
#include "utils/optimizers/stochastic_gradient_descent.hpp"
#include "utils/loss_functions.hpp"

void benchmark_linear_solvers() {
	Eigen::MatrixXd XN = Eigen::MatrixXd::Random(100, 100000);
	Eigen::RowVectorXd YN = Eigen::RowVectorXd::Random(100000);

	mlt::models::regressors::LeastSquaresLinearRegression<mlt::utils::linear_solvers::SVDSolver> linear_regressor_svd(false);
	benchmark(linear_regressor_svd, XN, YN, 100);
	mlt::models::regressors::LeastSquaresLinearRegression<mlt::utils::linear_solvers::LDLTSolver> linear_regressor_ldlt(false);
	benchmark(linear_regressor_ldlt, XN, YN, 100);
	std::cout << std::endl;
	mlt::models::regressors::LeastSquaresLinearRegression<mlt::utils::linear_solvers::CGSolver> linear_regressor_cg(false);
	benchmark(linear_regressor_cg, XN, YN, 100);
	std::cout << std::endl;

	std::cout << "Diff: " << (linear_regressor_svd.coefficients() - linear_regressor_ldlt.coefficients()).squaredNorm() << std::endl;
	std::cout << "Diff: " << (linear_regressor_svd.coefficients() - linear_regressor_cg.coefficients()).squaredNorm() << std::endl;
	std::cout << "Diff: " << (linear_regressor_ldlt.coefficients() - linear_regressor_cg.coefficients()).squaredNorm() << std::endl;
}

void test_optimizable_linear_regressors() {
	auto samples = 100;
	Eigen::MatrixXd input = Eigen::MatrixXd::Random(3, samples) * 100;
	Eigen::MatrixXd output = Eigen::MatrixXd::Random(2, samples).array();
	output = (output.array() > 0.0).cast<double>();
	output.row(1) = 1 - output.row(0).array();

	mlt::utils::optimizers::StochasticGradientDescent<> sgd;
	mlt::utils::loss_functions::SquaredLoss loss;

	mlt::models::regressors::OptimizableLinearRegressor<mlt::utils::loss_functions::SquaredLoss, mlt::utils::optimizers::StochasticGradientDescent<>> model(loss, sgd, 0, false);
	eval_numerical_gradient(model, Eigen::MatrixXd::Random(2, 3) * 0.05, input, output);

	mlt::models::regressors::OptimizableLinearRegressor<mlt::utils::loss_functions::SquaredLoss, mlt::utils::optimizers::StochasticGradientDescent<>> model2(loss, sgd, 0, true);
	eval_numerical_gradient(model2, Eigen::MatrixXd::Random(2, 4) * 0.05, input, output);
}

void lr_examples() {
	benchmark_linear_solvers();

	Eigen::MatrixXd X1(2, 3);
	Eigen::MatrixXd Y1(1, 3);

	X1.row(0) << 0, 1, 2;
	X1.row(1) << 0, 1, 2;
	Y1 << 0, 1, 2;

	mlt::models::regressors::LeastSquaresLinearRegression<> linear_regressor(false);
	linear_regressor.fit(X1, Y1);

	std::cout << "LinearRegression: " << std::endl;
	std::cout << linear_regressor.coefficients() << std::endl;
	if (linear_regressor.fit_intercept()) {
	std::cout << linear_regressor.intercepts() << std::endl;
	}
	std::cout << linear_regressor.predict(X1.col(0)) << std::endl;

	Eigen::MatrixXd X2(2, 3);
	Eigen::MatrixXd Y2(1, 3);

	X2.row(0) << 0, 0, 1;
	X2.row(1) << 0, 0, 1;
	Y2 << 0, .1, 1;

	mlt::models::regressors::RidgeRegression<> ridge_regressor(0.5, true);
	ridge_regressor.fit(X2, Y2);

	std::cout << "RidgeRegression: " << std::endl;
	std::cout << ridge_regressor.coefficients() << std::endl;
	std::cout << ridge_regressor.intercepts() << std::endl;
	std::cout << ridge_regressor.predict(X2.col(0)) << std::endl;

	mlt::utils::optimizers::StochasticGradientDescent<> grad_descent(10, 2000, 0.001, 1);
	mlt::utils::loss_functions::SquaredLoss loss;
	mlt::models::regressors::OptimizableLinearRegressor<mlt::utils::loss_functions::SquaredLoss, mlt::utils::optimizers::StochasticGradientDescent<>> sgd(loss, grad_descent, 0.5, true);

	sgd.fit(X2, Y2, true);

	std::cout << "OptimizableLinearRegressor<SquaredLoss, SGD>: " << std::endl;
	std::cout << ridge_regressor.coefficients() << std::endl;
	std::cout << ridge_regressor.intercepts() << std::endl;
	std::cout << sgd.predict(X2.col(0)) << std::endl;

	std::cout << "loss with closed form: " << sgd.loss(ridge_regressor.all_coefficients(), X2, Y2) << std::endl;
	std::cout << "loss with SGD: " << sgd.loss(sgd.all_coefficients(), X2, Y2) << std::endl;
}


#include "logreg.hpp"
#include <cmath>
#include <random>
#include <fstream>
#include <Eigen/Dense>
#include <stdio.h>
#include <iostream>

using namespace std;
using namespace Eigen;

mt19937 rng;

lrsgd::lrsgd()
{
	//set training parameters
	num_iter = 20;
	lambda = (float) 1e-4;
	theta = VectorXf::Random(2, 1);
	cost = 0.0;
	grad = VectorXf::Zero(2, 1);

	//generate training data
	generate_data(X, y);

	//learning rate schedule
	tau0 = 10;
	kappa = 1;  //(0.5, 1]
	eta = VectorXf::Zero(num_iter,1);
	for (int i = 0; i < num_iter; i++)
	{
		eta[i] = (float) pow((tau0 + i), (-kappa));
	}
}

VectorXf lrsgd::sigmoid(VectorXf& a)
{
	return (1 + (-1*a.array()).exp()).inverse();  // 1/(1 + exp(-a))
}

void lrsgd::lr_objective(float& cost, VectorXf& grad, VectorXf& theta)
{
	float n = (float) y.size();
	VectorXi y01 = (y.array() + 1) / 2;  //y \in {0, 1}
	VectorXf h = X * theta;
	VectorXf mu = sigmoid(h);

	mu = mu.cwiseMax((float) 1e-7);       //bound away from zero: max(mu, eps)
	mu = mu.cwiseMin((float)(1 - 1e-7));  //bound away from one:  min(mu, 1-eps)

	ArrayXf t1 = y01.array().cast<float>() * mu.array().log();
	ArrayXf t2 = (1 - y01.array().cast<float>()) * ((1 - mu.array()).log());
	cost = -(t1 + t2).sum() / n;  //NLL
	cost += lambda * theta.norm();  //regularizer
	//cout << "cost: " << cost << endl;

	grad = X.transpose() * (mu - y01.cast<float>()) + 2 * lambda * theta;  //gradient of LR objective
	//cout << "grad norm: " << grad.norm() << endl;
}

void lrsgd::fit()
{
	VectorXf obj_hist = VectorXf::Zero(num_iter, 1);
	VectorXf theta_norm_hist = VectorXf::Zero(num_iter, 1);

	for (int i = 0; i < num_iter; ++i)
	{
		lr_objective(cost, grad, theta);
		theta = theta - eta[i] * grad;

		cout << "grad: " << endl << grad << endl;
		cout << "theta: " << endl << theta << endl;

		obj_hist[i] = cost;
		theta_norm_hist[i] = theta.norm();
		printf("iteration: %d, cost: %.4f, eta: %.4f, theta_norm: %.4f, grad_norm: %.4f\n", i, obj_hist[i], eta[i], theta.norm(), grad.norm());
	}
}

void lrsgd::generate_data(MatrixXf& X, VectorXi& y)
{
	int n = 32, d = 2;
	X = MatrixXf::Zero(n, 2);
	y = VectorXi::Zero(n, 1);

	Vector2f mu1(1, 1), mu2(-1, -1);
	Vector2f pik(0.4, 0.6), cpik(0.4, 1.0); //cumsum(pik)

	cout << "mu1: " << endl << mu1 << endl;
	cout << "mu2: " << endl << mu2 << endl;

	uniform_real_distribution<float> dis01(0, 1);
	Vector2f xn(0, 0);

	for (int i = 0; i < n; i++)
	{
		float z01 = dis01(rng);
		//cout << "z01: " << z01 << endl;

		if (z01 < cpik[0])
		{
			X.row(i) = xn.setRandom() + mu1;
			y(i) = 1;
		}
		else
		{
			X.row(i) = xn.setRandom() + mu2;
			y(i) = -1;
		}
	}

	cout << "y: " << endl << y << endl;
	cout << "X: " << endl << X << endl;

	ofstream fout("./input.txt");
	if (fout.is_open())
	{
		fout << X.size() << endl;
		fout << X << endl;
		fout << y.size() << endl;
		fout << y << endl;
	}
	else
		cout << "Unable to open fout.\n";
}

int main()
{
    cout << "Binary Logistic Regression\n";
	lrsgd LR = lrsgd();
	LR.fit();
	cout << "Learned weights: " << endl << LR.theta << endl;

	cout << "Predicting on training data: " << endl;
	VectorXf mu = LR.X * LR.theta;
	VectorXf h = LR.sigmoid(mu);
	VectorXf y_pred = 2.0 * (h.array() >= 0.5).cast<float>() - 1.0;
	float y_err = (y_pred - LR.y.cast<float>()).sum() / (float) y_pred.size();
        cout << "LR classification error: " << y_err << endl;

	return 0;
}


//
//  EigenEx2DenseLinearSolve.cpp
//
//
//  Created by Zac Schulwolf on 12/26/16.
//
// Compile by g++ -I "$(brew --prefix eigen)/include/eigen3" EigenEx2DenseLinearSolve.cpp -o EigenEx2LinearSolve
//
// From https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
//

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

using namespace std;
using namespace Eigen;

int main()
{
    //1
    //General Decomposition using ColPivHouseholderQR
    Matrix3f m1; //m1 is 3 by 3 of floats
    Vector3f v1a; //B is 1 by 3 of floats
    m1 << 1,2,3,  4,5,6,  7,8,10;
    v1a << 3, 3, 4;
    cout << "Here is the matrix m1:\n" << m1 << endl;
    cout << "Here is the vector v1a:\n" << v1a << endl;
    Vector3f v1b = m1.colPivHouseholderQr().solve(v1a); //A QR decomposition with column pivoting and solve
    cout << "The solution is:\n" << v1b << endl;
    cout << endl << endl;

    //2
    //Decomposition if matix is positive definite use LLT or LDLT
    Matrix2f m2a, m2b; //using 2 matrices, m2a could also be a vector
    m2a << 2, -1, -1, 3;
    m2b << 1, 2, 3, 1;
    cout << "Here is the matrix m2a:\n" << m2a << endl;
    cout << "Here is the right hand side m2b:\n" << m2b << endl;
    Matrix2f x = m2a.ldlt().solve(m2b);
    cout << "The solution is:\n" << x << endl;
    cout << endl << endl;

    //3
    //Error margin
    MatrixXd m3a = MatrixXd::Random(100,100);
    MatrixXd m3b = MatrixXd::Random(100,50);
    MatrixXd m3c = m3a.fullPivLu().solve(m3b);
    double relative_error = (m3a*m3c - m3b).norm() / m2b.norm(); //norm() is L2 norm
    cout << "The relative error is:\n" << relative_error << endl;
    cout << endl << endl;

    //4
    //eigenvalues() and eginvectors() by SelfAdjointEigenSolver and EigenSolver
    Matrix2f m4a;
    m4a << 1, 2, 2, 3;
    cout << "Here is the matrix m4a:\n" << m4a << endl;
    SelfAdjointEigenSolver<Matrix2f> eigensolver(m4a); //SelfAdjointEigenSolver
    if (eigensolver.info() != Success) abort(); //eigensolver
    cout << "The eigenvalues of m4a are:\n" << eigensolver.eigenvalues() << endl; //eigenvalues()
    cout << "Here's a matrix whose columns are eigenvectors of m4a \n";
    cout << "corresponding to these eigenvalues:\n";
    cout << eigensolver.eigenvectors() << endl; //eigenvecotors()
    cout << endl << endl;

    //5
    //Inverse and Determinant
    Matrix3f m5;
    m5 << 1, 2, 1, 2, 1, 0, -1, 1, 2;
    cout << "Here is the matrix m5:\n" << m5 << endl;
    cout << "The determinant of m5 is " << m5.determinant() << endl;
    cout << "The inverse of m5 is:\n" << m5.inverse() << endl;
    cout << endl << endl;

    //6
    //Least squares solving using JocobiSVD
    MatrixXf m6 = MatrixXf::Random(3, 2);
    cout << "Here is the matrix m6:\n" << m6 << endl;
    VectorXf v6 = VectorXf::Random(3);
    cout << "Here is the right hand side b:\n" << v6 << endl;
    cout << "The least-squares solution is:\n";
    cout << m6.jacobiSvd(ComputeThinU | ComputeThinV).solve(v6) << endl;
    cout << endl << endl;

    //7
    //Separating the computation from the construction
    Matrix2f m7a, m7b;
    LLT<Matrix2f> llt;
    m7a << 2, -1, -1, 3;
    m7b << 1, 2, 3, 1;
    cout << "Here is the matrix m7a:\n" << m7a << endl;
    cout << "Here is the right hand side m7b:\n" << m7b << endl;
    cout << "Computing LLT decomposition..." << endl;
    llt.compute(m7a);
    cout << "The solution is:\n" << llt.solve(m7b) << endl;
    m7a(1,1)++;
    cout << "The matrix m7a is now:\n" << m7a << endl;
    cout << "Computing LLT decomposition..." << endl;
    llt.compute(m7a);
    cout << "The solution is now:\n" << llt.solve(m7b) << endl;
    cout << endl << endl;

    //8
    //Rank-revealing decompositions
    Matrix3f m8a;
    m8a << 1, 2, 5, 2, 1, 4, 3, 0, 3;
    cout << "Here is the matrix m8a:\n" << m8a << endl;
    FullPivLU<Matrix3f> lu_decomp(m8a);
    cout << "The rank of m8a is " << lu_decomp.rank() << endl;
    cout << "Here is a matrix whose columns form a basis of the null-space of m8a:\n"
    << lu_decomp.kernel() << endl;
    cout << "Here is a matrix whose columns form a basis of the column-space of m8a:\n"
    << lu_decomp.image(m8a) << endl; // yes, have to pass the original m8a

    Matrix2d m8b;
    m8b << 2, 1,
    2, 0.9999999999;
    FullPivLU<Matrix2d> lu(m8b);
    cout << "By default, the rank of m8b is found to be " << lu.rank() << endl;
    lu.setThreshold(1e-5);
    cout << "With threshold 1e-5, the rank of m8b is found to be " << lu.rank() << endl;
    cout << endl << endl;

}


/**
 * @file    euler080.cpp
 * @author  Marvin Smith
 * @date    5/9/2015
*/

// C++ Standard Libraries
#include <cmath>
#include <iomanip>
#include <iostream>

// Boost Libraries
#include <boost/multiprecision/cpp_dec_float.hpp>

// Common Libraries
#include "../common/StringUtilities.hpp"

using namespace std;
using namespace boost::multiprecision;
typedef number<cpp_dec_float<200>> cpp_dec_float_200;

/**
 * @brief Main Function
*/
int main( int argc, char* argv[] )
{
    // Misc Values
    int root;
    int max_value = 100;
    int64_t sum = 0;
    int64_t counter;

    // Iterate from 2 - 100
    for( int n=2; n<=max_value; n++ )
    {
        // Check if rational or irrational
        root = std::sqrt(n);
        if( n != root*root )
        {
            // Compute the square root
            cpp_dec_float_200 value = boost::multiprecision::sqrt( cpp_dec_float_200(n) );

            // Convert to a string
            std::string value_str = num2str(value, 200);

            // Sum the digits
            counter = 0;
            for( int j=0; j<value_str.size() && counter < 100; j++ )
            {
                if( value_str[j] != '.' ){
                    sum += (value_str[j] - '0');
                    counter++;
                }
            }
        }
    }

    // Print Result
    std::cout << sum << std::endl;

    // Exit
    return 0;
}


#include "gaussianXd.h"
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;

namespace filters

{
    void mean(MatrixXd& data, VectorXd& mean){
        // X_bar = X_transpose * j/N ; where J is a N*1 vector of ones and N is the totl number of data points
        int N = data.rows();
        const VectorXd J = VectorXd::Ones(N);
        mean = data.transpose() * J / N;
    }
    double mean(VectorXd& data){
        float N = data.rows();
        auto sum = data.sum();
        return sum/N;
    }

    void covariance(MatrixXd& data, MatrixXd& covariance){
        // cov = X_transpose * (I - J/n) * X
        // I = Identity
        // J = Ones matrix

        int N = data.rows();
        MatrixXd J = MatrixXd::Ones(N, N);
        MatrixXd I = MatrixXd::Identity(N, N);
        covariance = (data.transpose() * (I - (J/N)) * data)/(N-1);
    }

    void covariance(VectorXd& data1, VectorXd& data2, MatrixXd& cov){
        // construct a hstack
        MatrixXd M(data1.rows(), data1.cols() + data2.cols());
        M << data1, data2;

        covariance(M, cov);
    }

    MultivariateGaussian::MultivariateGaussian(MatrixXd& data) : data_(data)
    {
        mean(data, mean_);
        covariance(data, covariance_);

    }

    MultivariateGaussian::MultivariateGaussian(VectorXd& mean, MatrixXd& cov) : mean_(mean), covariance_(cov)
    {}

    VectorXd MultivariateGaussian::get_mean(){
        return mean_;
    }

    MatrixXd MultivariateGaussian::get_covariance(){
        return covariance_;
    }

    double MultivariateGaussian::probability(VectorXd& x){
        MatrixXd in = (x-mean_).transpose() * covariance_.inverse() * (x-mean_) * (-0.5);
        int N = x.rows();
        double denom = std::sqrt(std::pow(2*M_PI, N) * covariance_.norm());

        std::cout << in << std::endl;

        std:: cout << std::exp(in.value()) << std::endl;
        return 0.0;
    }
}


#ifndef __MATRIX_UTIL_HH__
#define __MATRIX_UTIL_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Matrix utility functions)
LIBRARY DEPENDENCY:
      ((../../src/matrix/utility.cpp))
*******************************************************************************/
#include <armadillo>

/** Returns polar from cartesian coordinates.
 * magnitude = POLAR(0,0) = |V|
 * azimuth   = POLAR(1,0) = atan2(V2,V1)
 * elevation = POLAR(2,0) = atan2(-V3,sqrt(V1^2+V2^2)
 * Example: POLAR = VEC.pol_from_cart();
 */
arma::vec3 pol_from_cart(arma::vec3 in);

/// @return the angle between two 3x1 vectors
double angle(arma::vec3 VEC1, arma::vec3 VEC2);

/// @return skew symmetric matrix of a Vector3
arma::mat33 skew_sym(arma::vec3 vec);

/// @return the T.M. of the psivg -> thtvg sequence
arma::mat33 build_psivg_thtvg_TM(const double &psivg, const double &thtvg);

/// @return the Euler T.M. of the psi->tht->phi sequence
arma::mat33 build_psi_tht_phi_TM(const double &psi, const double &tht, const double &phi);

arma::vec4 Matrix2Quaternion(arma::mat33 Matrix_in);
arma::mat33 Quaternion2Matrix(arma::vec4 Quaternion_in);
arma::vec4 Quaternion_conjugate(arma::vec4 Quaternion_in);
arma::vec4 Quaternion_cross(arma::vec4 Quaternion_in1, arma::vec4 Quaternion_in2);
void Quaternion2Euler(arma::vec4 Quaternion_in, double &Roll, double &Pitch, double &Yaw);
arma::vec4 Euler2Quaternion(double Roll, double Pitch, double Yaw);
arma::vec4 QuaternionMultiply(arma::vec4 Q_in1, arma::vec4 Q_in2);
arma::vec4 QuaternionInverse(arma::vec4 Q_in);
arma::vec4 QuaternionTranspose(arma::vec4 Q_in);
arma::vec3 QuaternionRotation(arma::vec4 Q_in, arma::vec3 V_in);
arma::mat33 cross_matrix(arma::vec3 in);
arma::mat33 TMX(double ang);
arma::mat33 TMY(double ang);
arma::mat33 TMZ(double ang);

#define STORE_MAT33(dest, src) \
    do { \
        auto cpy = src; \
        trans(cpy); \
        double *in = cpy.memptr(); \
        memcpy(dest, in, sizeof(dest)); \
    } while (0);

#define STORE_VEC(dest, src) \
    do { \
        double *in = src.memptr(); \
        memcpy(dest, in, sizeof(dest)); \
    } while (0);

#define GRAB_VAR(x) [&]() { return x; }
#define GRAB_VEC3(x) [&]() { return arma::vec3(x); }
#define GRAB_MAT33(x) [&]() { return arma::mat33((const double *)(&x)); }

#endif  // __MATRIX_UTIL_HH__


/*
Reed Solomon coding
*/

#ifndef ReedSolomon
#define ReedSolomon

#include <boost/operators.hpp>
#include <iostream>
#include <ostream>
#include <vector>
#include <cassert>
#include "polynomial.hpp"

using namespace std;

template<class GFE,class Dft>
class RScode{
	public:
	RScode(){};
	RScode(unsigned n_, unsigned k_,const GFE& primitive_el,Dft dft_){
		n = n_; k = k_; a = primitive_el; dftd = dft_;
		n_u = n_;
		k_u = k_;
	}
		RScode(unsigned n_, unsigned k_,const GFE& primitive_el,Dft dft_,unsigned n_u_){
		n = n_; k = k_; a = primitive_el; dftd = dft_;
		n_u = n_u_;
		k_u = k + n_u - n;
	}
	unsigned n;
	unsigned n_u;
	unsigned k_u;
	unsigned k;
	GFE a; // primitive element
	Dft dftd; // class providing Fourier transform
	typedef GFE Symbol; // Symbols of the code

	// GF: coefficients of the RS code, GF(p^m)
	// infvec: vector with k entries, containing the information
	vector<GFE>& RSencode(const vector<GFE>& infvec, vector<GFE>& c);

	// endode a shortened RS code
	// infvec: contains the information, length k
	// n: the length of cw
	// n_u: length of the underlying RS code
	vector<GFE>& RS_shortened_encode(const vector<GFE>& infvec, vector<GFE>& c);

	// GF: coefficients of the RS code, GF(p^m)
	// infvec: vector with k entries, containing the information
	vector<GFE>& RS_systematic_encode(const vector<GFE>& infvec, vector<GFE>& c);

	// decoding when the first n-k entries of the Fourier transform of c are equal zero
	// c: received vector
	// crec: recovered vector
	// retured is a pair, where pair.fist is the number of erasures and pair.second the number of errors
	pair<unsigned,unsigned> RSdecode(vector<GFE>& crec, const vector<GFE>& c);

	// decode a shortened RS code
	// infvec: contains the information, length k
	// n: the length of cw
	// n_u: length of the underlying RS code
	pair<unsigned,unsigned> RS_shortened_decode(vector<GFE>& infvec, vector<GFE>& c);

	// decode when information is encoded in the spectrum of c, that is C
	pair<unsigned,unsigned> RS_decode_spec(vector<GFE>& infvec, const vector<GFE>& c);
};

template<class GFE, class Dft >
vector<GFE>& RScode<GFE,Dft>::RSencode(const vector<GFE>& infvec, vector<GFE>& c){
	assert(infvec.size() == k);

	// encode
	c.resize(n); // c is the codeword
	vector<GFE> C(n); // Fourier transform of codeword
	// first n-k entries are zero
	for(unsigned i=0;i<n-k;++i) C[i] = GFE(0);
	// next k entries contain information
	for(unsigned i=0;i<k;++i) {
		C[i+n-k] = infvec[i];
	}
	dftd.idft(c,C);
	return c;
}

template<class GFE,class Dft>
//vector<GFE>& RS_shortened_encode(const vector<GFE>& infvec, vector<GFE>& c, const GFE& a, unsigned k, unsigned n, unsigned n_u){
vector<GFE>& RScode<GFE,Dft>::RS_shortened_encode(const vector<GFE>& infvec, vector<GFE>& c){
	vector<GFE> extrazeros(n_u-n, GFE(0));
	vector<GFE> infvec_u = infvec;
	// append zeros to the information vector
	infvec_u.insert(infvec_u.end(), extrazeros.begin(), extrazeros.end());

	// systematic encoding
	RS_systematic_encode(infvec_u,c);//a, k+n_u-n,n_u);
	// the last nu-n entries are zero, so erase them
	c.resize(n);
	return c;
}

// GF: coefficients of the RS code, GF(p^m)
// infvec: vector with k entries, containing the information
template<class GFE,class Dft>
vector<GFE>& RScode<GFE,Dft>::RS_systematic_encode(const vector<GFE>& infvec, vector<GFE>& c){
	assert(infvec.size() == k_u);

	// compute the generator polynomial
	polynomial<GFE> gp = polynomial<GFE>(1);
	GFE ai = GFE(a);
	for(unsigned i=1;i <= n_u-k_u; ++i){
		vector<GFE> prov(2);
		prov[0] = GFE(0) - ai;
		prov[1] = GFE(1);
		gp *= polynomial<GFE>(prov);
		ai *= a;
	}
	// generate message polynomial
	polynomial<GFE> mp(infvec);

	// construct the polynomial x^(n-k)
	vector<GFE> xnmkv(n_u-k_u+1,GFE(0));
	xnmkv[n_u-k_u] = GFE(1);
	polynomial<GFE> xnmk(xnmkv);

	polynomial<GFE> sp = mp*xnmk; // the codeword polynomial

	// obtain s_r(x) = p(x)*x^(n-k) mod g(x) as res.second
	pair<polynomial<GFE>, polynomial<GFE> > res = divide<GFE>(sp ,gp);

	// c(x) = p(x)*x^(n-k) - s_r(x)
	sp -= res.second;

	// make sure the cw has length n

	c = sp.poly;
	unsigned oldsize = c.size();
	c.resize(n_u);
	for(unsigned i=oldsize;i<n_u;++i) c[i]=GFE(0);
	return c;
}

// decoding when the first n-k entries of the Fourier transform of c are equal zero
// c: received vector
// crec: recovered vector
// retured is a pair, where pair.first is the number of erasurs and pair.second the number of errors
template<class GFE, class Dft>
pair<unsigned,unsigned> RScode<GFE,Dft>::RScode<GFE,Dft>::RSdecode(vector<GFE>& crec, const vector<GFE>& c){

	pair<unsigned, unsigned> erctr;
	// codeword received
	crec = c; // this will be the recoverd vector

	GFE am = a.inverse(); // am is the inverse of primitive element
	assert(n_u == crec.size());
	unsigned d = n_u-k_u+1;

	// the positions of the erasures
	vector<unsigned> er_pos(n_u);
	unsigned j=0;
	for(unsigned i=0;i<n_u;++i)
		if(crec[i].isempty()) {
			er_pos[j] = i;
			j++;
			crec[i] = GFE(0); // set to zero
		}
	er_pos.resize(j);

	erctr.first = j;
	// compute the erasure locator polynomial
	polynomial<GFE> elp = polynomial<GFE>(1);
	for(unsigned i=0;i<er_pos.size(); ++i){
		vector<GFE> prov(2);
		/////// here I lose time when computing the powers..
		prov[0] = GFE(0) - pow(am,er_pos[i]);
		prov[1] = GFE(1);
		//prov[0] = GFE(1);
		//prov[1] = GFE(0) - pow(a,er_pos[i]);
		elp *= polynomial<GFE>(prov);
	}

	// obtain Crec from the received vector crec
	vector<GFE> Crec(n_u);
	dftd.dft(crec,Crec);

	// compute syndrome from received C
	vector<GFE> syndv(n_u-k_u); //syndrome vector
	for(unsigned i=1; i<= n_u-k_u  ;++i)
		syndv[i-1] = Crec[i];
	polynomial<GFE> synd(syndv);

	synd *= elp;
	synd.poly.resize(n_u-k_u); // mod x^{n-k}

	//// solving the key equation by Euclid's method

	// construct polynomial x^(d-1)
	vector<GFE> xdm1(d,GFE(0));
	xdm1[d-1] = GFE(1);

	polynomial<GFE> rem1 = xdm1;
	polynomial<GFE> rem2 = synd;
	polynomial<GFE> aux1 = polynomial<GFE>(0);
	polynomial<GFE> aux2 = polynomial<GFE>(1);
	//polynomial<GFE> aux2 = elp;//polynomial<GFE>(1);

	while(! (rem2.degree() < (d-1+j)/2  )  ){
		pair<polynomial<GFE>, polynomial<GFE> > res = divide<GFE>(rem1,rem2);
		polynomial<GFE> aux_new = polynomial<GFE>(0) - res.first*aux2 + aux1;
		// prepare for the next step
		rem1 = rem2;
		rem2 = res.second;
		aux1 = aux2;
		aux2 = aux_new;
	}

	// aux2 is the error locator
	// rem2 is the errata evaluator polynomial

	erctr.second = aux2.degree();

	//cout << "erasures: " << erctr.first << " errors: " << erctr.second << endl;

	// obtain the errata locator as the product of the error evaluator and the errata evaluator
	elp *= aux2;

    // Get the lowest coefficient in the polynomial elp
	polynomial<GFE> A0 = polynomial<GFE>(elp.poly[0]);
	// rem2 is polynomial Omega
	pair<polynomial<GFE>, polynomial<GFE> > res_tmp= divide<GFE>(rem2, A0);
	pair<polynomial<GFE>, polynomial<GFE> > res_tmp2= divide<GFE>(elp, A0);
	polynomial<GFE> Omega = res_tmp.first;
	elp = res_tmp2.first;
	//// Forney's algorithm to find the error values

	polynomial<GFE> elpder = elp;
	elpder.derive(); // derivative of the error locator polynomial

	// compute the error values

	//vector<GFE> err_val(n);
	GFE ami = GFE(1);
	for(unsigned i=0;i<n_u; ++i){
		if( elp.evaluate(ami) == GFE(0)  ){
			// e_j = a^i \Gamma(a^-i) / \Lambda'(a^-i)
			//err_val[i] = GFE(0) -  ai*( rem2.evaluate(ami)/ elpder.evaluate(ami) );
			//cout << "errata at: " << i << endl;
			// correct the error
			//cout << "corrected at " << i << endl;
			GFE elpderval = elpder.evaluate(ami);
			if(! elpderval.iszero()){
				crec[i] += ( Omega.evaluate(ami)/ elpderval);
			}else{ // otherwise something did go wrong, we cannot divide by zero..
				erctr.second = n_u; // this marks an decoding error
			}
		}
		ami *= am;
	}
	return erctr;
}

//
template<class GFE, class Dft>
pair<unsigned,unsigned> RScode<GFE,Dft>::RS_shortened_decode(vector<GFE>& infvec, vector<GFE>& c){

	vector<GFE> extrazeros(n_u-n, GFE(0));
	// append the zeros to the received vector
	c.insert(c.end(), extrazeros.begin(), extrazeros.end());

	vector<GFE> crec(n_u);
	pair<unsigned,unsigned> erctr = RSdecode(crec,c);

	// copy the information; recall that c is endoded systematically..
	infvec = vector<GFE>(crec.begin()+n-k, crec.begin()+n );
	infvec.resize(k);
	return erctr;
}

//
template<class GFE, class Dft>
pair<unsigned,unsigned> RScode<GFE,Dft>::RS_decode_spec(vector<GFE>& infvec, const vector<GFE>& c){

	pair<unsigned,unsigned> erctr;

	assert(n == c.size());
	vector<GFE> crec; // the recovered codeword
	cout << "start RS decode.. " << endl;
	erctr = RSdecode(crec,c);
	cout << "..end RS decode.. " << endl;

	vector<GFE> Crec(n);
	dftd.dft(crec,Crec); // obtain C from the recovered codeword

	// recover the information from C
	infvec.resize(k);
	for(unsigned i=0;i<k;++i) infvec[i] = Crec[i+n-k];

	return erctr;
}

#endif


#include <Eigen/Sparse>
#include <Eigen/Eigenvalues>

#include <cmath>

#include <iomanip>
#include <iostream>

#define PI  M_PI

using vector = Eigen::VectorXd;

//! \brief Golub-Welsh implementation 5.3.35
//! \param[in] n number of Gauss nodes
//! \param[out] w weights
//! \param[out] x nodes for interval [-1,1]
void golubwelsh(int n, vector & w, vector & x) {
    // TODO: implement Golub-Welsh
}

//! \brief Compute \int_a^b f(x) dx \approx \sum w_i f(x_i) (with scaling of w and x)
//! \tparam func template type for function handle f (e.g. lambda func.)
//! \param[in] f integrand
//! \param[in] w weights
//! \param[in] x nodes for interval [-1,1]
//! \param[in] a left boundary in [a,b]
//! \param[in] b right boundary in [a,b]
//! \return Approximation of integral \int_a^b f(x) dx
template <class func>
double quad(func&& f, const vector & w, const vector & x, double a, double b) {
    // TODO: implement generic quadrature
    // WARNING: careful scaling
}

//! \brief Compute \int_{-infty}^\infty f(x) dx using transformation x = cot(t)
//! \tparam func template type for function handle f (e.g. lambda func.)
//! \param[in] n number of Gauss points
//! \param[in] f integrand
//! \return Approximation of integral \int_{-infty}^\infty f(x) dx
template <class func>
double quadinf(int n, func&& f) {
    // TODO: implement tranformation of f and call to quad
}

int main() {
    // TODO: test integration of h with 1 to 100 Gaussian points
}

Nummerical Methods for CSE/PS11/solutions_ps11/quadinf_template.cpp


/* Simple program for solving the two-dimensional diffusion
   equation or Poisson equation using Jacobi's iterative method
   Note that this program does not contain a loop over the time
   dependence.
*/

#include <iostream>
#include <iomanip>
#include <armadillo>
using namespace std;
using namespace arma;

int JacobiSolver(int, double, double, mat &, mat &, double);

int main(int argc, char * argv[]){
  int Npoints = 40;
  double ExactSolution;
  double dx = 1.0/(Npoints-1);
  double dt = 0.25*dx*dx;
  double tolerance = 1.0e-14;
  mat A = zeros<mat>(Npoints,Npoints);
  mat q = zeros<mat>(Npoints,Npoints);

  // setting up an additional source term
  for(int i = 0; i < Npoints; i++)
    for(int j = 0; j < Npoints; j++)
      q(i,j) = -2.0*M_PI*M_PI*sin(M_PI*dx*i)*sin(M_PI*dx*j);

  int itcount = JacobiSolver(Npoints,dx,dt,A,q,tolerance);

  // Testing against exact solution
  double sum = 0.0;
  for(int i = 0; i < Npoints; i++){
    for(int j=0;j < Npoints; j++){
      ExactSolution = -sin(M_PI*dx*i)*sin(M_PI*dx*j);
      sum += fabs((A(i,j) - ExactSolution));
    }
  }
  cout << setprecision(5) << setiosflags(ios::scientific);
  cout << "Jacobi: L2 Error is " << sum/Npoints << " in " << itcount << " iterations" << endl;
}

// Function for setting up the iterative Jacobi solver
int JacobiSolver(int N, double dx, double dt, mat &A, mat &q, double abstol)
{
  int MaxIterations = 100000;
  mat Aold = zeros<mat>(N,N);

  double D = dt/(dx*dx);

  for(int i=1;  i < N-1; i++)
    for(int j=1; j < N-1; j++)
      Aold(i,j) = 1.0;

  // Boundary Conditions -- all zeros
  for(int i=0; i < N; i++){
    A(0,i) = 0.0;
    A(N-1,i) = 0.0;
    A(i,0) = 0.0;
    A(i,N-1) = 0.0;
  }
  // Start the iterative solver
  for(int k = 0; k < MaxIterations; k++){
    for(int i = 1; i < N-1; i++){
      for(int j=1; j < N-1; j++){
	A(i,j) = dt*q(i,j) + Aold(i,j) +
	  D*(Aold(i+1,j) + Aold(i,j+1) - 4.0*Aold(i,j) +
	     Aold(i-1,j) + Aold(i,j-1));
      }
    }
    double sum = 0.0;
    for(int i = 0; i < N;i++){
      for(int j = 0; j < N;j++){
	sum += (Aold(i,j)-A(i,j))*(Aold(i,j)-A(i,j));
	Aold(i,j) = A(i,j);
      }
    }
    if(sqrt (sum) <abstol){
      return k;
    }
  }
  cerr << "Jacobi: Maximum Number of Interations Reached Without Convergence\n";
  return MaxIterations;
}


/**
 *  @file    diffusion.hpp
 *  @brief   Collections of diffusion problem, definitions.
 *  @author  Francois Roy
 *  @date    12/01/2019
 */
#ifndef DIFFUSION_H
#define DIFFUSION_H

#include <string>
#include <Eigen/Core>
#include "spdlog/spdlog.h"
#include "numerical/fdm/fdm.hpp"

namespace bench{
namespace diffusion{

typedef Eigen::VectorXd Vec;
typedef Eigen::ArrayXd Arr;

/**
* Computes the 1D diffusion problem defined in @ref FDMDiffusionA::reference()
* using the finite difference method and computes the L2-norm of the error
* between the exact and computed solution.
*
* @return The L2-norm of the error between the computed and exact solution.
* @see numerical::fdm::Parameters
* @see numerical::fdm::FDMesh
* @see numerical::fdm::SparseSolver
*/
double fdm_diffusion_a();
/**
* Computes the 1D diffusion problem defined in @ref FDMDiffusionB::reference()
* using the finite difference method and computes the L2-norm of the error
* between the exact and computed solution.
*
* @return The L2-norm of the error between the computed and exact solution.
* @see numerical::fdm::Parameters
* @see numerical::fdm::FDMesh
* @see numerical::fdm::SparseSolver
*/
double fdm_diffusion_b();
/**
* Computes the 1D diffusion problem defined in @ref FDMDiffusionC::reference()
* using the finite difference method and computes the L2-norm of the error
* between the exact and computed solution.
*
* @return The L2-norm of the error between the computed and exact solution.
* @see numerical::fdm::Parameters
* @see numerical::fdm::FDMesh
* @see numerical::fdm::SparseSolver
*/
double fdm_diffusion_c();
/**
* Computes the 1D diffusion problem defined in @ref FDMDiffusionD::reference()
* using the finite difference method and computes the L2-norm of the error
* between the exact and computed solution.
*
* @return The L2-norm of the error between the computed and exact solution.
* @see numerical::fdm::Parameters
* @see numerical::fdm::FDMesh
* @see numerical::fdm::SparseSolver
*/
double fdm_diffusion_d();
/**
* Computes the 1D diffusion problem defined in @ref FDMDiffusionE::reference()
* using the finite difference method and computes the L2-norm of the error
* between the exact and computed solution.
*
* @return The L2-norm of the error between the computed and exact solution.
* @see numerical::fdm::Parameters
* @see numerical::fdm::FDMesh
* @see numerical::fdm::SparseSolver
*/
double fdm_diffusion_e();

/**
* This class provides tools to compute the finite difference problem
* defined in @ref FDMDiffusionA::reference().
*
* The class is derived form the @ref numerical::fdm::FDProblem class and
* overrides the member function @ref numerical::fdm::FDProblem::left(),
* @ref numerical::fdm::FDProblem::right(),
* @ref numerical::fdm::FDProblem::initial_value(),
* @ref numerical::fdm::FDProblem::source(), and
* @ref numerical::fdm::FDProblem::reference().
*/
class FDMDiffusionA : public numerical::fdm::Problem<double>{
public:
    FDMDiffusionA(numerical::fdm::Parameters<double>* p):
        numerical::fdm::Problem<double>(p){
        }
    double left(double y, double z, double t);
    double right(double y, double z, double t);
    double source(const Eigen::Matrix<double, 3, 1>& x, double t);
    /**
    * Reference solution for the 1D diffusion problem a, defined as:
    *
    * \f[
    *    \frac{\partial u}{\partial t} = \alpha \frac{\partial^2 u}
    *                                 {\partial x^2}
    *                               + f(x,t)\quad x \in (0, L),~t \in (0, T]
    * \f]
    *
    * with homogeneous Dirichlet boundary condition \f$u(0, t) = 0\f$ and
    * \f$ u(L, t) = 0\f$ for \f$t>0\f$.
    *
    * The diffusion coefficient, \f$ \alpha(L, t)\f$ is constant and
    * uniform over the interval (line).
    *
    * We define the source term and initial condition with a reference
    * (manufactured) solution that respects the boundary conditions at the
    * extremities of the interval:
    *
    * \f[
    *    u(x, t) = 5tx\left(L-x\right)
    * \f]
    *
    * We get the source term substituting the manufactured solution in the
    * diffusion equation, i.e.:
    *
    * \f[
    *    f(x, t) = 5x\left(L-x\right)+10\alpha t
    * \f]
    *
    * The initial condition is simply set to the manufactured solution at
    * \f$t=0\f$:
    *
    * \f[
    *    u(x, 0) = u_0 = 0
    * \f]
    *
    * @param x The spatial mesh node coordinates.
    * @param t The temporal mesh node.
    * @return The reference solution.
    */
    double reference(const Eigen::Matrix<double, 3, 1>& x, double t);
};

/**
* This class provides tools to compute the finite difference problem
* defined in @ref FDMDiffusionB::reference().
*
* The class is derived form the @ref numerical::fdm::FDProblem class and
* overrides the member function @ref numerical::fdm::FDProblem::left(),
* @ref numerical::fdm::FDProblem::right(),
* @ref numerical::fdm::FDProblem::initial_value(),
* @ref numerical::fdm::FDProblem::source(), and
* @ref numerical::fdm::FDProblem::reference().
*/
class FDMDiffusionB : public numerical::fdm::Problem<double>{
public:
    FDMDiffusionB(numerical::fdm::Parameters<double>* p):
        numerical::fdm::Problem<double>(p){
        }
    double left(double y, double z, double t);
    double right(double y, double z, double t);
    double source(const Eigen::Matrix<double, 3, 1>& x, double t);
    /**
    * Reference solution for the 1D diffusion problem b, defined as:
    *
    * \f[
    *    \frac{\partial u}{\partial t} = \alpha \frac{\partial^2 u}
    *                                 {\partial x^2}
    *                                 + f(x,t)\quad x \in (0, L),~t \in (0, T]
    * \f]
    *
    * with homogeneous Neumann boundary condition
    * \f$\left. \frac{\partial}{\partial x}u(x, t)\right|_{x=0} = 0\f$ and
    * \f$\left. \frac{\partial}{\partial x}u(x, t)\right|_{x=L} = 0\f$.
    *
    * The diffusion coefficient, \f$ \alpha(L, t)\f$ is constant and
    * uniform over the interval (line).
    *
    * We define the source term and initial condition with a reference
    * (manufactured) solution that respects the boundary conditions at the
    * extremities of the interval:
    *
    * \f[
    *    \frac{\partial}{\partial x}u(x, t) = 5tx\left(L-x\right)
    * \f]
    *
    * which leads to
    *
    * \f[
    *    u(x, t) = 5tx\left(\frac{Lx}{2}-\frac{x^2}{3}\right)
    * \f]
    *
    * We get the source term substituting the manufactured solution in the
    * diffusion equation, i.e.:
    *
    * \f[
    *    f(x, t) = 5x\left(\frac{Lx}{2}-\frac{x^2}{3}\right)-
    *      5\alpha t\left(L-2x\right)
    * \f]
    *
    * The initial condition is simply set to the manufactured solution at
    * \f$t=0\f$:
    *
    * \f[
    *    u(x, 0) = u_0 = 0
    * \f]
    *
    * @param x The spatial mesh node coordinates.
    * @param t The temporal mesh node.
    * @return The reference solution.
    */
    double reference(const Eigen::Matrix<double, 3, 1>& x, double t);
};

/**
* This class provides tools to compute the finite difference problem
* defined in @ref FDMDiffusionC::reference().
*
* The class is derived form the @ref numerical::fdm::FDProblem class and
* overrides the member function @ref numerical::fdm::FDProblem::left(),
* @ref numerical::fdm::FDProblem::right(),
* @ref numerical::fdm::FDProblem::initial_value(),
* @ref numerical::fdm::FDProblem::source(), and
* @ref numerical::fdm::FDProblem::reference().
*/
class FDMDiffusionC : public numerical::fdm::Problem<double>{
public:
    FDMDiffusionC(numerical::fdm::Parameters<double>* p):
        numerical::fdm::Problem<double>(p){
        }
    double left(double y, double z, double t);
    double right(double y, double z, double t);
    double source(const Eigen::Matrix<double, 3, 1>& x, double t);
    /**
    * Reference solution for the 1D diffusion problem c, defined as:
    *
    * \f[
    *    \frac{\partial u}{\partial t} = \alpha \frac{\partial^2 u}
    *                                 {\partial x^2}
    *                                 + f(x,t)\quad x \in (0, L),~t \in (0, T]
    * \f]
    *
    * with non-homogeneous Neumann boundary condition
    * \f$\mathbf{\hat{n}}\cdot\left. \frac{\partial}{\partial x}
    *  u(x, t)\right|_{x=0} = -5tL\f$ and
    * \f$\mathbf{\hat{n}}\cdot\left. \frac{\partial}{\partial x}
    *  u(x, t)\right|_{x=L} = -5tL\f$.
    *
    * The diffusion coefficient, \f$ \alpha(L, t)\f$ is constant and
    * uniform over the interval (line).
    *
    * We define the source term and initial condition with a reference
    * (manufactured) solution that respects the boundary conditions at the
    * extremities of the interval:
    *
    * \f[
    *    \frac{\partial}{\partial x}u(x, t) = 5t\left(L-2x\right)
    * \f]
    *
    * which leads to
    *
    * \f[
    *    u(x, t) = 5tx\left(L-x\right)
    * \f]
    *
    * We get the source term substituting the manufactured solution in the
    * diffusion equation, i.e.:
    *
    * \f[
    *    f(x, t) = 5x\left(L-x\right)+10\alpha t
    * \f]
    *
    * The initial condition is simply set to the manufactured solution at
    * \f$t=0\f$:
    *
    * \f[
    *    u(x, 0) = u_0 = 0
    * \f]
    *
    * @param x The spatial mesh node coordinates.
    * @param t The temporal mesh node.
    * @return The reference solution.
    */
    double reference(const Eigen::Matrix<double, 3, 1>& x, double t);
};

/**
* This class provides tools to compute the finite difference problem
* defined in @ref FDMDiffusionD::reference().
*
* The class is derived form the @ref numerical::fdm::FDProblem class and
* overrides the member function @ref numerical::fdm::FDProblem::left(),
* @ref numerical::fdm::FDProblem::right(),
* @ref numerical::fdm::FDProblem::initial_value(),
* @ref numerical::fdm::FDProblem::source(), and
* @ref numerical::fdm::FDProblem::reference().
*/
class FDMDiffusionD : public numerical::fdm::Problem<double>{
public:
    FDMDiffusionD(numerical::fdm::Parameters<double>* p):
        numerical::fdm::Problem<double>(p){
        }
    double left(double y, double z, double t);
    double right(double y, double z, double t);
    double source(const Eigen::Matrix<double, 3, 1>& x, double t);
    /**
    * Reference solution for the 1D diffusion problem d, defined as:
    *
    * \f[
    *    \frac{\partial u}{\partial t} = \alpha \frac{\partial^2 u}
    *                                 {\partial x^2}
    *                                 + f(x,t)\quad x \in (0, L),~t \in (0, T]
    * \f]
    *
    * with non-homogeneous Dirichlet boundary condition
    * \f$u(0, t) = 5tL^2/4\f$ and
    * \f$\mathbf{\hat{n}}\cdot\left. \alpha(u)\frac{\partial}{\partial x}
    *  u(x, t)\right|_{x=L} = g\f$.
    *
    * The diffusion coefficient, \f$ \alpha(u)=u\f$.
    *
    * We define the source term and initial condition with a reference
    * (manufactured) solution that respects the boundary conditions at the
    * extremities of the interval:
    *
    * \f[
    *    u(x, t) = 5t\left(x-\frac{L}{2}\right)^2
    * \f]
    *
    * We get the source term substituting the manufactured solution in the
    * diffusion equation, i.e.:
    *
    * \f[
    *    f(x, t) = 5\left(x-\frac{L}{2}\right)^2-
    *      10\alpha t
    * \f]
    *
    * The initial condition is simply set to the manufactured solution at
    * \f$t=0\f$:
    *
    * \f[
    *    u(x, 0) = u_0 = 0
    * \f]
    *
    * @param x The spatial mesh node coordinates.
    * @param t The temporal mesh node.
    * @return The reference solution.
    */
    double reference(const Eigen::Matrix<double, 3, 1>& x, double t);
};

/**
* This class provides tools to compute the finite difference problem
* defined in @ref FDMDiffusionE::reference().
*
* The class is derived form the @ref numerical::fdm::FDProblem class and
* overrides the member function @ref numerical::fdm::FDProblem::left(),
* @ref numerical::fdm::FDProblem::right(),
* @ref numerical::fdm::FDProblem::initial_value(),
* @ref numerical::fdm::FDProblem::source(), and
* @ref numerical::fdm::FDProblem::reference().
*/
class FDMDiffusionE : public numerical::fdm::Problem<double>{
public:
    FDMDiffusionE(numerical::fdm::Parameters<double>* p):
        numerical::fdm::Problem<double>(p){
        }
    double left(double y, double z, double t);
    double right(double y, double z, double t);
    double source(const Eigen::Matrix<double, 3, 1>& x, double t);
    /**
    * Reference solution for the nonlinear 1D diffusion problem e, defined as:
    *
    * \f[
    *    \frac{\partial u}{\partial t} = \frac{\partial}{\partial x}
    *        \left(\alpha(u)\frac{\partial u}{\partial x}\right)
    *        + f(x,t)\quad x \in (0, L),~t \in (0, T]
    * \f]
    *
    * with non-homogeneous mixed Dirichlet/Neumann boundary conditions
    * \f$u(0, t) = u_D\f$ and \f$u(L, t)= 5tL^2/4\f$.
    *
    * The diffusion coefficient, \f$ \alpha(L, t)\f$ is constant and
    * uniform over the interval (line).
    *
    * We define the source term and initial condition with a reference
    * (manufactured) solution that respects the boundary conditions at the
    * extremities of the interval:
    *
    * \f[
    *    u(x, t) = 5t\left(x-\frac{L}{2}\right)^2
    * \f]
    *
    * We get the source term substituting the manufactured solution in the
    * diffusion equation, i.e.:
    *
    * \f[
    *    f(x, t) = 5\left(x-\frac{L}{2}\right)^2-
    *      10\alpha t
    * \f]
    *
    * The initial condition is simply set to the manufactured solution at
    * \f$t=0\f$:
    *
    * \f[
    *    u(x, 0) = u_0 = 0
    * \f]
    *
    * @param x The spatial mesh node coordinates.
    * @param t The temporal mesh node.
    * @return The reference solution.
    */
    double reference(const Eigen::Matrix<double, 3, 1>& x, double t);
};

}  // namespace diffusion
}  // namespace bench

#endif  // DIFFUSION_H


#include "ExponentialForm.hpp"
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;

Polynomial<double> ExponentialForm::taylorExpand(int degree) const {
  VectorXd coefs = VectorXd::Zero(degree+1);
  coefs(0) = m_a + m_c;

  double factorial = 1.0;
  for (int d=1; d < degree + 1; d++) {
    factorial *= d;
    coefs(d) = m_a * std::pow(m_b, d) / factorial;
  }

  return Polynomial<double>(coefs);
}

double ExponentialForm::value(double t) const {
  return m_a * exp(m_b * t) + m_c;
}


//test_mcr.cpp

//compute Monte Carlo sim

#include <iostream>
#include <string>
#include <fstream>
#include <sstream>
#include <vector>
#include<memory>

#include <Eigen/Dense>

#include "compute_returns_eigen.h"
#include "compute_var.h"
#include "path.h"
#include "mc_engine.h"
#include "rng.h"
#include "pca.h"

using namespace Eigen;
using namespace std;

void readCSV(std::istream &input, std::vector< std::vector<std::string> > &output)
//https://www.gamedev.net/topic/444193-c-how-to-load-in-a-csv-file/
{
	std::string csvLine;
	// read every line from the stream
	while( std::getline(input, csvLine) )
	{
		std::istringstream csvStream(csvLine);
		std::vector<std::string> csvColumn;
		std::string csvElement;
		// read every element from the line that is seperated by commas
		// and put it into the vector or strings
		while( std::getline(csvStream, csvElement, ',') )
		{
			csvColumn.push_back(csvElement);
		}
		output.push_back(csvColumn);
	}
}

int main()
{

    try{

    // Read daily index level for major and emerging markets
    // daily series obtained from Yahoo! Finance through
	//https://www.quandl.com

	std::fstream file("/home/mrnoname/Documents/VaR/data/StockIndexData.csv", ios::in);
	if(!file.is_open())
	{
		std::cout << "File not found!\n";
		return 1;
	}
	// typedef to save typing for the following object
	typedef std::vector< std::vector<std::string> > csvVector;
	csvVector csvData;

	readCSV(file, csvData);

    //test
    for(size_t i = 0;i < 5; ++i){
        for(size_t j = 0;j < csvData[i].size();++j){
            cout << csvData[i][j] << '\t';

        }

        cout << endl;
    }
    cout << endl;

    // Remove lines with missing values

    size_t n(csvData.size() - 1);
    size_t m(csvData[0].size() - 1);

    Mat _prices;
    _prices.resize(m,Vec(n-1062));

    for(size_t i = 1062;i < n;++i){
        for(size_t j = 1;j < csvData[i].size();++j){
            std::string tmp = csvData[i][j];
            if(tmp.empty()){
                _prices[j-1][i-1062] = 99999.;
            }
            else{
                _prices[j-1][i-1062] = std::stod(tmp);
            }
        }
    }

    std::vector<std::string> indexNames(csvData[0].size() - 1);

    for(size_t i = 1;i < csvData[0].size();++i){
        indexNames[i-1] = csvData[0][i];
    }

	//Remove missing values to compute trailling returns
    //Asynchornous time series. Shift to the next value
    Mat prices;
    prices.resize(m,Vec(0));

	for(size_t i = 0;i < _prices.size();++i){
        for(size_t j = 0;j < _prices[i].size();++j){
            if(!((_prices[i][j] == 99999) || (_prices[i][j] == 0)))
                prices[i].push_back(_prices[i][j]);
        }
	}

    std::shared_ptr<ComputeReturn> cr(new ComputeReturn(prices,1,252,true));
	// 252 / 4 = 63 - 3 months
    // 4 * 252 = 1008 use 4 years of data to compute mean, and std dev

    //-------------------------------------------------------------------------
    // single case

	// Simulate stock rtn using AR(1)xGARCH(1,1) through brute force Monte-Carlo
    //

	AR1xGARCH11 process (-0.0003114, -0.0693, 0.01854, 0.10150, 0.88374); // DJIA

	rng _rng;

	MCEngine<rng,AR1xGARCH11> engine(_rng, process);

	engine.setValues(cr->getReturns(1),cr->getStdDev(1));

	Vec sim(10);

	sim = engine.DoSimulation(10,Gaussian);

    for(auto& i : sim)   cout << i << endl;

    engine.setValues(cr->getReturns(0),cr->getRollingStdDev(0).back());

    Vec sim1(10);

    sim1 = engine.DoSimulation(10,Gaussian);

    cout << endl; for(auto& j : sim1)   cout << j << endl;

    // --------------------------------------------------------------
    // Multiple stock returns

    Eigen::MatrixXd C = cr->getVarCov();

    Eigen::MatrixXd A( C.llt().matrixL() );

    std::vector<AR1xGARCH11> processes(7);

    processes[0] = AR1xGARCH11(-0.0003114, -0.0693, 0.01854, 0.10150, 0.88374); // DJIA
    processes[1] = AR1xGARCH11(-0.0003515,-0.0729,0.01979, 0.09502, 0.89028); // GSPC
    processes[2] = AR1xGARCH11(-0.0004741,-0.1041,0.02788, 0.08605, 0.89754); // NDX
    processes[3] = AR1xGARCH11(-1.37e-17, 0.,0.02614, 0.09003, 0.89950); // GDAXI
    processes[4] = AR1xGARCH11(2.348e-17,0.,0.02476, 0.08731, 0.90240); // FCHI
    processes[5] = AR1xGARCH11(2.468e-17,0.,0.02987, 0.07847, 0.91352); // SSEC
    processes[6] = AR1xGARCH11(5.382e-18,0.,2.166e+00, 4.902e-01, 4.990e-15); // SENSEX

    MCEngine<rng,AR1xGARCH11> engine1(_rng, processes, A, Cholesky);

    Mat d = cr->getReturns();

    Vec b;

	for(size_t i = 0;i < m;++i) b.push_back(cr->getRollingStdDev(i).back());

    engine1.setValues(d,b);

    Mat sim2(7,Vec(10));

    sim2 = engine1.DoMultiSimulation(10,Gaussian);

    cout << endl;
    for(size_t i =0;i < sim2.size();++i){
        for(size_t j =0;j < sim2[i].size();++j)
            cout << sim2[i][j] << '\t';
        cout << endl;
    }

    //-----------------------------------------------------------------
    // test PCA case

    // convert to req format
    vector<float> vec;

	n = C.rows();
	m = C.cols();

    for(size_t i = 0;i < n;++i)
        for(size_t j = 0;j < m;++j)
            vec.push_back(C(i,j));

	std::shared_ptr<Pca> pca(new Pca());

  	int init_result = pca->Calculate(vec, n, m);

  	if(init_result == 1) cout << "correl mat positive semi-definite " << endl;

    vector<float> scores = pca->scores(); //Rotated data

  	unsigned int kaiser = pca->kaiser(); //Kaiser criterion 99%

    unsigned int nrows = pca->nrows();

	Eigen::MatrixXd PC(nrows, kaiser);

	for(size_t i = 0;i < nrows;++i)
		for(size_t j = 0;j < kaiser;++j)
			PC(i,j) = scores[j + kaiser*i];

    MCEngine<rng,AR1xGARCH11> engine2(_rng, processes, PC, pc);

    engine2.setValues(d,b);

    Mat sim3(7,Vec(10));

    sim3 = engine2.DoMultiSimulation(10,Gaussian);

    cout << endl;
    for(size_t i =0;i < sim2.size();++i){
        for(size_t j =0;j < sim2[i].size();++j)
            cout << sim3[i][j] << '\t';
        cout << endl;
    }

    return 0;

    } catch (const std::exception& e) { // caught by reference to base
        std::cout << " a standard exception was caught, with message '"
                  << e.what() << "'\n";
    }

}


/**
 * @file laxwendroffscheme.cc
 * @brief NPDE homework "LaxWendroffScheme" code
 * @author Oliver Rietmann
 * @date 29.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "laxwendroffscheme.h"

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

namespace LaxWendroffScheme {

namespace Constant {
constexpr double e = 2.71828182845904523536;
constexpr double pi = 3.14159265358979323846;
}  // namespace Constant

constexpr double Square(double x) { return x * x; }

/**
 * @brief Computes the right-hand side according to the Lax-Wendroff scheme.
 * @param mu mu^(k-1) (i.e. mu at timestep k-1)
 * @param gamma tau / h, where tau = timestep size and h = spatial meshwidth
 * @return mu^(k) (i.e. mu at timestep k)
 */
/* SAM_LISTING_BEGIN_0 */
Eigen::VectorXd LaxWendroffRhs(const Eigen::VectorXd &mu, double gamma) {
  int N = mu.size();
  Eigen::VectorXd result(N);

#if SOLUTION
  auto f = [](double x) { return std::exp(x); };
  auto df2 = [](double x) { return Square(std::exp(x)); };

  // Lax-Wendroff fully discrete evolution \prbeqref{eq:12} for
  // $\cob{f(u)=e^u}$. Store the values of $f(x)$ and $(f'(x))^2$ from the
  // previous iteration:

  // We extend $\texttt{mu}$ to the left by $\texttt{mu(0)}$:
  double mu_left = mu(0);

  // $f'\left(\tfrac{1}{2}\left(\mu_j+\mu_{j-1}\right)\right)^2$:
  double df2_old = df2(0.5 * (mu(0) + mu_left));

  // $f'\left(\tfrac{1}{2}\left(\mu_{j+1}+\mu_j\right)\right)^2$:
  double df2_new = df2(0.5 * (mu(1) + mu(0)));

  double f_old = f(mu_left);  // $f\left(\mu_{j-1}\right)$
  double f_mid = f(mu(0));    // $f\left(\mu_j\right)$
  double f_new = f(mu(1));    // $f\left(\mu_{j+1}\right)$

  result(0) = mu(0) - 0.5 * gamma * (f_new - f_old) +
              0.5 * Square(gamma) *
                  (df2_new * (mu(1) - mu(0)) - df2_old * (mu(0) - mu_left));

  for (int j = 1; j < N - 1; ++j) {
    df2_old = df2_new;
    df2_new = df2(0.5 * (mu(j + 1) + mu(j)));
    f_old = f_mid;
    f_mid = f_new;
    f_new = f(mu(j + 1));
    result(j) =
        mu(j) - 0.5 * gamma * (f_new - f_old) +
        0.5 * Square(gamma) *
            (df2_new * (mu(j + 1) - mu(j)) - df2_old * (mu(j) - mu(j - 1)));
  }

  // We extend $\texttt{mu}$ to the right by $\texttt{mu(N-1)}$:
  double mu_right = mu(N - 1);

  df2_old = df2_new;
  df2_new = df2(0.5 * (mu_right + mu(N - 1)));

  f_old = f_mid;
  f_new = f(mu_right);

  result(N - 1) = mu(N - 1) - 0.5 * gamma * (f_new - f_old) +
                  0.5 * Square(gamma) *
                      (df2_new * (mu_right - mu(N - 1)) -
                       df2_old * (mu(N - 1) - mu(N - 2)));
#else
  //====================
  // Your code goes here
  //====================
#endif

  return result;
}

Eigen::VectorXd solveLaxWendroff(const Eigen::VectorXd &u0, double T,
                                 unsigned int M) {
  double gamma = 1.0 / Constant::e;
  Eigen::VectorXd mu = u0;
  // Main timestepping loop
  for (int j = 0; j < M; ++j) mu = LaxWendroffRhs(mu, gamma);
  return mu;
}

/* SAM_LISTING_END_0 */

/* SAM_LISTING_BEGIN_2 */
// Build spatial grid
Eigen::VectorXd getXValues(double T, unsigned int M) {
  double tau = T / M;
  double h = Constant::e * tau;
  int j_max = (int)(std::ceil((3.0 * T + 1.0) / h) + 0.5);
  int j_min = (int)(std::floor(-3.0 * T / h) - 0.5);
  unsigned int N = j_max - j_min + 1;
  return Eigen::VectorXd::LinSpaced(N, j_min * h, j_max * h);
}
Eigen::VectorXd numexpLaxWendroffRP(const Eigen::VectorXi &M) {
  const double T = 1.0;
  const int M_size = M.size();
  Eigen::VectorXd error(M_size);
  // Initial values for the Riemann problem
  auto u_initial = [](double x) { return 0.0 <= x ? 1.0 : 0.0; };
  // Exact solution \prbeqref{eq:solrp} at time $T = 1.0$
  auto u_exact = [](double x) {
    return (x <= 1.0) ? 0.0 : ((Constant::e <= x) ? 1.0 : std::log(x));
  };
#if SOLUTION
  for (int i = 0; i < M_size; ++i) {
    Eigen::VectorXd x = getXValues(T, M(i));
    Eigen::VectorXd u0 = x.unaryExpr(u_initial);
    Eigen::VectorXd uT = solveLaxWendroff(u0, T, M(i));

    double tau = T / M(i);
    double h = Constant::e * tau;
    error(i) = h * (x.unaryExpr(u_exact) - uT).lpNorm<1>();
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return error;
}
/* SAM_LISTING_END_2 */

/**
 * @brief Evaluates the discrete function u at position x by linear
 * interpolation
 * @param u descrete function values at spatial positions y
 * @param y vector of same length as u, representing the nodes of u
 * @return best linear interpolation of u at spacial position x
 */
double eval(const Eigen::VectorXd &u, const Eigen::VectorXd &y, double x) {
  int N = y.size();
  double a = y(0);
  double b = y(N - 1);

  if (x <= a) return u(0);
  if (b <= x) return u(N - 1);

  double lambda = (x - a) / (b - a);
  int k0 = (int)(lambda * (N - 1));
  int k1 = k0 + 1;

  lambda = (x - y(k0)) / (y(k1) - y(k0));
  return lambda * u(k1) + (1.0 - lambda) * u(k0);
}

/* SAM_LISTING_BEGIN_9 */
double smoothU0(double x) {
  return (x < 0.0)
             ? 0.0
             : ((1.0 < x) ? 1.0 : Square(std::sin(0.5 * Constant::pi * x)));
}
Eigen::VectorXd referenceSolution(const Eigen::VectorXd &x) {
  double T = 1.0;
  // Reference solution on a very fine mesh
  unsigned int M = 3200;

  Eigen::VectorXd y = getXValues(T, M);
  Eigen::VectorXd u0 = y.unaryExpr(&smoothU0);
  Eigen::VectorXd u = solveLaxWendroff(u0, T, M);
  int N = x.size();
  Eigen::VectorXd u_ref(N);
  // The vector u is larger than u_ref. Use eval() from above the "evaluate" u
  // at the positions x(i) and thus obtain the reference solution u_ref.
#if SOLUTION
  for (int i = 0; i < N; ++i) {
    u_ref(i) = eval(u, y, x(i));
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return u_ref;
}
/* SAM_LISTING_END_9 */

/* SAM_LISTING_BEGIN_1 */
Eigen::VectorXd numexpLaxWendroffSmoothU0(const Eigen::VectorXi &M) {
  const double T = 1.0;
  const int M_size = M.size();
  Eigen::VectorXd error(M_size);

#if SOLUTION
  for (int i = 0; i < M_size; ++i) {
    Eigen::VectorXd x = getXValues(T, M(i));
    Eigen::VectorXd u0 = x.unaryExpr(&smoothU0);
    Eigen::VectorXd uT = solveLaxWendroff(u0, T, M(i));

    double tau = T / M(i);
    double h = Constant::e * tau;
    error(i) = h * (referenceSolution(x) - uT).lpNorm<1>();
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return error;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_7 */
Eigen::VectorXd solveGodunov(const Eigen::VectorXd &u0, double T,
                             unsigned int M) {
  double tau = T / M;
  double h = Constant::e * tau;
  unsigned int N = u0.size();
  Eigen::VectorXd mu = u0;

#if SOLUTION
  for (int i = 0; i < M; ++i) {
    for (int j = N - 1; 0 < j; --j) {
      mu(j) = mu(j) - tau / h * (std::exp(mu(j)) - std::exp(mu(j - 1)));
    }
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return mu;
}

/* SAM_LISTING_END_7 */

/* SAM_LISTING_BEGIN_8 */
Eigen::VectorXd numexpGodunovSmoothU0(const Eigen::VectorXi &M) {
  const double T = 1.0;
  const int M_size = M.size();
  Eigen::VectorXd error(M_size);

#if SOLUTION
  for (int i = 0; i < M_size; ++i) {
    Eigen::VectorXd x = getXValues(T, M(i));
    Eigen::VectorXd u0 = x.unaryExpr(&smoothU0);
    Eigen::VectorXd uT = solveGodunov(u0, T, M(i));

    double tau = T / M(i);
    double h = Constant::e * tau;
    error(i) = h * (referenceSolution(x) - uT).lpNorm<1>();
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return error;
}
/* SAM_LISTING_END_8 */

}  // namespace LaxWendroffScheme


//rng.cpp

#include<memory>
#include<math.h>
#include<cmath>

#include <boost/math/distributions.hpp>

#include "rng.h"

using namespace std;

copulaRng::copulaRng(unsigned int _d, rng& _rng_)
:d(_d)
{
    _rng = shared_ptr<GenFromDistr<rng>>(new GenFromDistr<rng>(_rng_));
}

// Gaussian
Vec copulaRng::getGaussiancopula(Eigen::MatrixXd C){

	boost::math::normal_distribution<> dist(0.,1.);

	//d dimension normal(0, cov)
	Eigen::VectorXd X = _rng->getXGaussian(d );

	Eigen::MatrixXd A( C.llt().matrixL() );

	Eigen::VectorXd Z = X.transpose() * A ;

	Vec sample;

	for(size_t i = 0;i < d;++i)
        sample.push_back(boost::math::cdf(dist,Z(i)));

	return sample;
}

// Studen's t-distr
Vec copulaRng::getStudentcopula(Eigen::MatrixXd C, double v){

	boost::math::students_t_distribution<> dist(v);

	//d dimension t-distr(0, cov, degree of freedom) central

	//1. Find the Cholesky matrix of Sigma -> L
	Eigen::MatrixXd L( C.llt().matrixL() );

	//2. Simulate a vector of n N(0,1) iid -> Y
	Eigen::VectorXd Y = _rng->getXGaussian(d );

	//3. Simulate a Chi-Square(v) -> S
	double S = _rng->getOneChisquare(v);
	//4. Compute vector Z = sqrt(v/S) * L * Y
	Eigen::VectorXd Z = L * Y;
	Z *= sqrt(v/S);

	//5. Finally, U = cdf_student(Z)
	Vec sample;

	for(size_t i = 0;i < d;++i)
		sample.push_back(boost::math::cdf(dist,Z(i)));

	return sample;
}

// Clayton
Vec copulaRng::getClaytoncopula(double gamma){

	double X = _rng->getOneGamma(1./gamma, 1.); //1 dimension gamma_dist(1./gamma, 1.)

	Eigen::VectorXd U = _rng->getXUniform(d); //d dimension uniform(0,1)

	Vec sample;

	for(size_t i = 0;i < d;++i)
		sample.push_back(pow(1.- log(U(i)/X) , -1./gamma));

	return sample;
}

// Gumbel
Vec copulaRng::getGumbelcopula(double gamma){

	double theta = -pow(-cos(M_PI/(2. * gamma)),gamma);

	//1 dimension stable_dist(1./theta, 1.,gamma,0.)
	double X = getOneStableDist<shared_ptr<GenFromDistr<rng>>>(_rng,1./gamma, 1.,theta,0.);

	//d dimension uniform(0,1)
	Eigen::VectorXd U = _rng->getXUniform(d);

	Vec sample;

	for(size_t i = 0;i < d;++i)
        sample.push_back(exp(-pow(abs(log(abs(U(i)/X))) , 1./gamma)));

	return sample;
}


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

#include <iostream>
#include <boost/math/special_functions/relative_difference.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/debug_adaptor.hpp>

//[scoped_precision_1

/*`
All our precision changing examples are based around `mpfr_float`.
However, in order to make running this example a little easier to debug,
we'll use `debug_adaptor` throughout so that the values of all variables
can be displayed in your debugger of choice:
*/

using mp_t = boost::multiprecision::debug_adaptor_t<boost::multiprecision::mpfr_float>;

/*`
Our first example will investigate calculating the Bessel J function via it's well known
series representation:

[$../bessel2.svg]

This simple series suffers from catastrophic cancellation error
near the roots of the function, so we'll investigate slowly increasing the precision of
the calculation until we get the result to N-decimal places.  We'll begin by defining
a function to calculate the series for Bessel J, the details of which we'll leave in the
source code:
*/
mp_t calculate_bessel_J_as_series(mp_t x, mp_t v, mp_t* err)
//<-
{
   mp_t sum        = pow(x / 2, v) / tgamma(v + 1);
   mp_t abs_sum    = abs(sum);
   mp_t multiplier = -x * x / 4;
   mp_t divider    = v + 1;
   mp_t term       = sum;
   mp_t eps        = boost::math::tools::epsilon<mp_t>();
   unsigned   m          = 1;

   while (fabs(term / abs_sum) > eps)
   {
      term *= multiplier;
      term /= m;
      term /= divider;
      ++divider;
      ++m;
      sum += term;
      abs_sum += abs(term);
   }
   if (err)
      *err = eps * fabs(abs_sum / sum);
   /*
   std::cout << eps << std::endl;
   if (err)
      std::cout << *err << std::endl;
   std::cout << abs_sum << std::endl;
   std::cout << sum << std::endl;
   */
   return sum;
}
//->

/*`
Next come some simple helper classes, these allow us to modify the current precision and precision-options
via scoped objects which will put everything back as it was at the end.  We'll begin with the class to
modify the working precision:
*/
struct scoped_mpfr_precision
{
   unsigned saved_digits10;
   scoped_mpfr_precision(unsigned digits10) : saved_digits10(mp_t::thread_default_precision())
   {
      mp_t::thread_default_precision(digits10);
   }
   ~scoped_mpfr_precision()
   {
      mp_t::thread_default_precision(saved_digits10);
   }
   void reset(unsigned digits10)
   {
      mp_t::thread_default_precision(digits10);
   }
   void reset()
   {
      mp_t::thread_default_precision(saved_digits10);
   }
};
/*`
And a second class to modify the precision options:
*/
struct scoped_mpfr_precision_options
{
   boost::multiprecision::variable_precision_options saved_options;
   scoped_mpfr_precision_options(boost::multiprecision::variable_precision_options opts) : saved_options(mp_t::thread_default_variable_precision_options())
   {
      mp_t::thread_default_variable_precision_options(opts);
   }
   ~scoped_mpfr_precision_options()
   {
      mp_t::thread_default_variable_precision_options(saved_options);
   }
   void reset(boost::multiprecision::variable_precision_options opts)
   {
      mp_t::thread_default_variable_precision_options(opts);
   }
};
/*`
We can now begin writing a function to calculate J[sub v](z) to a specified precision.
In order to keep the logic as simple as possible, we'll adopt a ['uniform precision computing] approach,
which is to say, within the body of the function, all variables are always at the same working precision.
*/
mp_t Bessel_J_to_precision(mp_t v, mp_t x, unsigned digits10)
{
   //
   // Begin by backing up digits10:
   //
   unsigned saved_digits10 = digits10;
   //
   //
   // Start by defining 2 scoped objects to control precision and associated options.
   // We'll begin by setting the working precision to the required target precision,
   // and since all variables will always be of uniform precision, we can tell the
   // library to ignore all precision control by setting variable_precision_options::assume_uniform_precision:
   //
   scoped_mpfr_precision           scoped(digits10);
   scoped_mpfr_precision_options   scoped_opts(boost::multiprecision::variable_precision_options::assume_uniform_precision);

   mp_t            result;
   mp_t            current_error{1};
   mp_t            target_error {std::pow(10., -static_cast<int>(digits10))};

   while (target_error < current_error)
   {
      //
      // Everything must be of uniform precision in here, including
      // our input values, so we'll begin by setting their precision:
      //
      v.precision(digits10);
      x.precision(digits10);
      //
      // Calculate our approximation and error estimate:
      //
      result = calculate_bessel_J_as_series(x, v, &current_error);
      //
      // If the error from the current approximation is too high we'll need
      // to loop round and try again, in this case we use the simple heuristic
      // of doubling the working precision with each loop.  More refined approaches
      // are certainly available:
      //
      digits10 *= 2;
      scoped.reset(digits10);
   }
   //
   // We now have an accurate result, but it may have too many digits,
   // so lets round the result to the requested precision now:
   //
   result.precision(saved_digits10);
   //
   // To maintain uniform precision during function return, lets
   // reset the default precision now:
   //
   scoped.reset(saved_digits10);
   return result;
}

/*`
So far, this is all well and good, but there is still a potential trap for the unwary here,
when the function returns the variable [/result] may be copied/moved either once or twice
depending on whether the compiler implements the named-return-value optimisation.  And since this
all happens outside the scope of this function, the precision of the returned value may get unexpected
changed - and potentially with different behaviour once optimisations are turned on!

To prevent these kinds of unintended consequences, a function returning a value with specified precision
must either:

* Be called in a /uniform-precision-environment/, with the current working precision, the same as both
the returned value and the variable to which the result will be assigned.
* Be called in an environment that has one of the following set:
   * variable_precision_options::preserve_source_precision
   * variable_precision_options::preserve_component_precision
   * variable_precision_options::preserve_related_precision
   * variable_precision_options::preserve_all_precision

In the case of our example program, we use a /uniform-precision-environment/ and call the function
with the value of `6541389046624379 / 562949953421312` which happens to be near a root of J[sub v](x)
and requires a high-precision calculation to obtain low relative error in the result of
`-9.31614245636402072613249153246313221710284959883647822724e-15`.

You will note in the example we have so far that there are a number of unnecessary temporaries
created: we pass values to our functions by value, and we call the `.precision()` member function
to change the working precision of some variables - something that requires a reallocation internally.
We'll now make our example just a little more efficient, by removing these temporaries, though in the
process, we'll need just a little more control over how mixed-precision arithmetic behaves.

It's tempting to simply define the function that calculates the series to take arguments
by constant reference like so:

*/

mp_t calculate_bessel_J_as_series_2(const mp_t& x, const mp_t& v, mp_t* err)
//<-
{
   mp_t sum        = pow(x / 2, v) / tgamma(v + 1);
   mp_t abs_sum    = abs(sum);
   mp_t multiplier = -x * x / 4;
   mp_t divider    = v + 1;
   mp_t term       = sum;
   mp_t eps        = boost::math::tools::epsilon<mp_t>();
   unsigned   m          = 1;

   while (fabs(term / abs_sum) > eps)
   {
      term *= multiplier;
      term /= m;
      term /= divider;
      ++divider;
      ++m;
      sum += term;
      abs_sum += abs(term);
   }
   if (err)
      *err = eps * fabs(abs_sum / sum);

   return sum;
}
//->

/*`
And to then pass our arguments to it, without first altering their precision to match
the current working default.  However, imagine that `calculate_bessel_J_as_series_2`
calculates x[super 2] internally, what precision is the result?  If it's the same as the
precision of /x/, then our calculation will loose precision, since we really want the result
calculated to the full current working precision, which may be significantly higher than
that of our input variables.  Our new version of Bessel_J_to_precision therefore uses
`variable_precision_options::preserve_target_precision` internally, so that expressions
containing only the low-precision input variables are calculated at the precision of
(at least) the target - which will have been constructed at the current working precision.

Here's our revised code:

*/

mp_t Bessel_J_to_precision_2(const mp_t& v, const mp_t& x, unsigned digits10)
{
   //
   // Begin with 2 scoped objects, one to manage current working precision, one to
   // manage mixed precision arithmetic.  Use of variable_precision_options::preserve_target_precision
   // ensures that expressions containing only low-precision input variables are evaluated at the precision
   // of the variable they are being assigned to (ie current working precision).
   //
   scoped_mpfr_precision scoped(digits10);
   scoped_mpfr_precision_options scoped_opts(boost::multiprecision::variable_precision_options::preserve_target_precision);

   mp_t                    result;
   mp_t            current_error{1};
   mp_t            target_error{std::pow(10., -static_cast<int>(digits10))};

   while (target_error < current_error)
   {
      //
      // The assignment here, rounds the high precision result
      // returned by calculate_bessel_J_as_series_2, to the precision
      // of variable result: ie to the target precision we specified in
      // the function call.  This is only the case because we have
      // variable_precision_options::preserve_target_precision set.
      //
      result = calculate_bessel_J_as_series_2(x, v, &current_error);

      digits10 *= 2;
      scoped.reset(digits10);
   }
   //
   // There may be temporaries created when we return, we must make sure
   // that we reset the working precision and options before we return.
   // In the case of the options, we must preserve the precision of the source
   // object during the return, not only here, but in the calling function too:
   //
   scoped.reset();
   scoped_opts.reset(boost::multiprecision::variable_precision_options::preserve_source_precision);
   return result;
}

/*`
In our final example, we'll look at a (somewhat contrived) case where we reduce the argument
by N * PI, in this case we change the mixed-precision arithmetic options several times,
depending what it is we are trying to achieve at that moment in time:

*/
mp_t reduce_n_pi(const mp_t& arg)
{
   //
   // We begin by estimating how many multiples of PI we will be reducing by,
   // note that this is only an estimate because we're using low precision
   // arithmetic here to get a quick answer:
   //
   unsigned n = static_cast<unsigned>(arg / boost::math::constants::pi<mp_t>());
   //
   // Now that we have an estimate for N, we can up the working precision and obtain
   // a high precision value for PI, best to play safe and preserve the precision of the
   // source here.  Though note that expressions are evaluated at the highest precision
   // of any of their components: in this case that's the current working precision
   // returned by boost::math::constants::pi, and not the precision of arg.
   // However, should this function be called with assume_uniform_precision set
   // then all bets are off unless we do this:
   //
   scoped_mpfr_precision            scope_1(mp_t::thread_default_precision() * 2);
   scoped_mpfr_precision_options    scope_2(boost::multiprecision::variable_precision_options::preserve_source_precision);

   mp_t reduced = arg - n * boost::math::constants::pi<mp_t>();
   //
   // Since N was only an estimate, we may have subtracted one PI too many,
   // correct if that's the case now:
   //
   if (reduced < 0)
      reduced += boost::math::constants::pi<mp_t>();
   //
   // Our variable "reduced" now has the correct answer, but too many digits precision,
   // we can either call its .precision() member function, or assign to a new variable
   // with variable_precision_options::preserve_target_precision set:
   //
   scope_1.reset();
   scope_2.reset(boost::multiprecision::variable_precision_options::preserve_target_precision);
   mp_t result = reduced;
   //
   // As with previous examples, returning the result may create temporaries, so lets
   // make sure that we preserve the precision of result.  Note that this isn't strictly
   // required if the calling context is always of uniform precision, but we can't be sure
   // of our calling context:
   //
   scope_2.reset(boost::multiprecision::variable_precision_options::preserve_source_precision);
   return result;
}

/*`
And finally... we need to mention `preserve_component_precision`, `preserve_related_precision` and `preserve_all_precision`.

These form a hierarchy, with each inheriting the properties of those before it.
`preserve_component_precision` is used when dealing with complex or interval numbers, for example if we have:

   mpc_complex val = some_expression;

And `some_expression` contains scalar values of type `mpfr_float`, then the precision of these is ignored unless
we specify at least `preserve_component_precision`.

`preserve_related_precision` we'll somewhat skip over - it extends the range of types whose precision is
considered within an expression to related types - for example all instantiations of `number<mpfr_float_backend<N>, ET>`
when dealing with expression of type `mpfr_float`.  However, such situations are - and should be -  very rare.

The final option - `preserve_all_precision` - is used to preserve the precision of all the types in an expression,
for example:

   // calculate 2^1000 - 1:
   mpz_int i(1);
   i <<= 1000;
   i -= 1;

   mp_t f = i;

The final assignment above will round the value in /i/ to the current working precision, unless `preserve_all_precision`
is set, in which case /f/ will end up with sufficient precision to store /i/ unchanged.

*/

//]

int main()
{
   mp_t::thread_default_precision(50);
   mp_t x{6541389046624379uLL};
   x /= 562949953421312uLL;
   mp_t v{2};
   mp_t err;
   mp_t J = Bessel_J_to_precision(v, x, 50);

   std::cout << std::setprecision(50) << J << std::endl;
   std::cout << J.precision() << std::endl;

   mp_t expected{"-9.31614245636402072613249153246313221710284959883647822724e-15"};
   std::cout << boost::math::relative_difference(expected, J) << std::endl;

   J = Bessel_J_to_precision_2(v, x, 50);
   std::cout << boost::math::relative_difference(expected, J) << std::endl;
   std::cout << J.precision() << std::endl;

   std::cout << reduce_n_pi(boost::math::float_next(boost::math::float_next(5 * boost::math::constants::pi<mp_t>()))) << std::endl;

   return 0;
}


#include <iostream>
#include <chrono>
#include <random>
#include <map>

#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

#include "matplotlibcpp.h"

using namespace std;
using namespace Eigen;
namespace plt = matplotlibcpp;

double g(double x, double a, double b, double c)
{
  // return a*x*x+b*x+c;
  return std::exp(a * x * x + b * x + c);
}

int main(int argc, char **argv) {
  std::cout << "Gauss-Newton \n";

  double aa = 1.0, bb = 2.0, cc = 1.0;
  double a = 2.0, b = -1.0, c = 5.5;

  double obs_sigma = 1.0;
  std::default_random_engine generator;
  std::normal_distribution<double> obs_noise_distrib(0.0, obs_sigma);

  int N = 100;
  std::vector<double> x_data(N), y_data(N);
  for (int i = 0; i < N; ++i)
  {
    x_data[i] = static_cast<double>(i) / 100.0;
    y_data[i] = g(x_data[i], aa, bb, cc) + obs_noise_distrib(generator);

  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

  // Optimize
  int max_iter = 10000;
  for (int it = 0; it < max_iter; ++it)
  {
    // std::cout << "iter " << it << " : ";
    // std::cout << "abc = " << a << " " << b << " " << c << "\n";
    Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
    Eigen::Vector3d e = Eigen::Vector3d::Zero();
    double total_err = 0.0;
    for (int i = 0; i < N; ++i)
    {
      double x = x_data[i];
      double gx = g(x, a, b, c);
      double err = y_data[i] - gx;
      total_err += err * err;

      // Compute derivative of f(x) (F(x) = 1/2 sum(||f(x)||^2)
      Eigen::Vector3d J(0.0, 0.0, 0.0);
      J[0] = -x * x * gx; // df/da
      J[1] = -x * gx;  // df/db
      J[2] = -gx;   // df/dc

      // left part of the normal equation
      H += J * J.transpose();
      // right part
      e += -J * err;
    }
    // std::cout << "H = " << H << "\n";
    // std::cout << "total error: " << total_err << "\n";

    // solve Hx = e
    Eigen::Vector3d delta_x = H.inverse() * e;
    // Eigen::Vector3d delta_x = H.ldlt().solve(e); // second version

    // std::cout << "delta_x = " << delta_x.transpose() << "\n";

    if (delta_x.norm() < 1e-4)
      break;

    a += delta_x[0];
    b += delta_x[1];
    c += delta_x[2];
  }
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  std::cout << "final = " << a << " " << b << " " << c << "\n";

  vector<double> final_y_data(N);
  for (int i = 0; i < N; ++i)
  {
    final_y_data[i] = g(x_data[i], a, b, c);
  }

  plt::scatter(x_data, y_data);
  std::map<std::string, std::string> parameters;
  parameters["c"] = "red";
  plt::scatter(x_data, final_y_data, 1.0, parameters);
  plt::show();

  return 0;
}


/**
 * Configured with: --prefix=/Library/Developer/CommandLineTools/usr --with-gxx-include-dir=/usr/include/c++/4.2.1
 * Apple LLVM version 9.1.0 (clang-902.0.39.1)
 * Target: x86_64-apple-darwin17.5.0
 * Thread model: posix
*/

#include <iostream> //std::cout
#include <iostream> //formatting
#include <vector> //Container
#include <boost/rational.hpp> // Rationals
#include <boost/multiprecision/cpp_int.hpp> //1024bit precision

typedef boost::rational<boost::multiprecision::int1024_t> rational; // reduce boilerplate

rational bernulli(size_t n){

     auto out = std::vector<rational>();

     for(size_t m=0;m<=n;m++){
         out.emplace_back(1,(m+1)); // automatically constructs object
         for (size_t j = m;j>=1;j--){
             out[j-1] = rational(j) * (out[j-1]-out[j]);
         }
     }
     return out[0];
 }

int main() {
    for(size_t n = 0; n <= 60;n+=n>=2?2:1){
        auto b = bernulli(n);
        std::cout << "B("<<std::right<<std::setw(2)<<n<<") = ";
        std::cout << std::right<<std::setw(44)<<b.numerator();
        std::cout << " / " << b.denominator() <<std::endl;
    }

    return 0;
}


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_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 <g2o/solvers/cholmod/linear_solver_cholmod.h>

#include <sophus/se3.h>
#include <sophus/so3.h>

typedef Eigen::Matrix<double, 6, 6> Matrix6d;

/**
 * Give an approximation( J_R^{-1} ) of the error
 */
Matrix6d JRInv(Sophus::SE3 error)
{
    Matrix6d Jacobian;
    Jacobian.block(0, 0, 3, 3) = Sophus::SO3::hat(error.so3().log());
    Jacobian.block(0, 3, 3, 3) = Sophus::SO3::hat(error.translation()); // * Sophus::SO3::hat(error.so3().log());
    Jacobian.block(3, 0, 3, 3) = Eigen::Matrix3d::Zero();
    Jacobian.block(3, 3, 3, 3) = Sophus::SO3::hat(error.so3().log());

    Jacobian = Jacobian*0.5 + Matrix6d::Identity();

    return Jacobian;
}

/**
 * vertex of lie algebra
 */
typedef Eigen::Matrix<double, 6, 1> Vector6d;
class VertexSE3LieAlgebra : public g2o::BaseVertex<6, Sophus::SE3>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    bool read(std::istream& is)
    {
        double data[7];
        for (int i = 0; i < 7; i ++) {
            is >> data[i];
        }

        setEstimate(Sophus::SE3(
            Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
            Eigen::Vector3d(data[0], data[1], data[2])
        ));

        return true;
    }

    bool write(std::ostream &os) const
    {
        os << id() << " ";
        Eigen::Quaterniond q = _estimate.unit_quaternion();
        os << _estimate.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " <<q.coeffs()[3] << std::endl;

        return true;
    }

    virtual void setToOriginImpl()
    {
        _estimate = Sophus::SE3();
    }

    /* update */
    virtual void oplusImpl(const double * update)
    {
        Sophus::SE3 up(
            Sophus::SO3(update[3], update[4], update[5]),
            Eigen::Vector3d(update[0], update[1], update[2])
        );
        _estimate = up * _estimate;
    }
};

class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, Sophus::SE3, VertexSE3LieAlgebra, VertexSE3LieAlgebra>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    bool read(std::istream& is)
    {
        double data[7];
        for (int i = 0; i < 7; i++) {
            is >> data[i];
        }

        Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
        q.normalize();
        setMeasurement(Sophus::SE3(q, Eigen::Vector3d(data[0], data[1], data[2])));

        for (int i = 0; i < information().rows() && is.good(); i ++) {
            for (int j = i; j < information().cols() && is.good(); j ++) {
                is >> information()(i, j);
                if (i != j) {
                    information()(j, i) = information()(i, j);
                }
            }
        }

        return true;
    }

    bool write(std::ostream& os) const
    {
        VertexSE3LieAlgebra* v1 = static_cast<VertexSE3LieAlgebra*>(_vertices[0]);
        VertexSE3LieAlgebra* v2 = static_cast<VertexSE3LieAlgebra*>(_vertices[1]);

        os << v1->id() << " " << v2->id() << " ";

        Sophus::SE3 m = _measurement;
        Eigen::Quaterniond q = m.unit_quaternion();
        os << m.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << q.coeffs()[3] << " ";

        // information matrix
        for (int i = 0; i < information().rows(); i++)
            for (int j = i; j < information().cols(); j++)
                os << information()(i, j) << " ";

        os << std::endl;

        return true;
    }

    // Compute Error
    virtual void computeError()
    {
        Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*>(_vertices[0]))->estimate();
        Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*>(_vertices[1]))->estimate();
        _error = (_measurement.inverse() * v1.inverse() * v2).log();
    }

    // compute Jacobian
    virtual void linearizeOplus()
    {
        Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*>(_vertices[0]))->estimate();
        Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*>(_vertices[1]))->estimate();
        Matrix6d J = JRInv(Sophus::SE3::exp(_error));

        // try J ~= I ?
        _jacobianOplusXi = - J * v2.inverse().Adj();
        _jacobianOplusXj = J * v2.inverse().Adj();
    }
};

int main(int argc, char** argv)
{
    if (argc != 2) {
        std::cout << "Usage: pose_graph_g2o_lie_algebra sphere.g2o" << std::endl;
        exit(EXIT_FAILURE);
    }

    std::ifstream fin(argv[1]);
    if (!fin) {
        std::cout << "file " << argv[1] << " does not exist." << std::endl;
        exit(EXIT_FAILURE);
    }

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> Block;
    Block::LinearSolverType* linear_solver = new g2o::LinearSolverCholmod<Block::PoseMatrixType>();
    Block* block_solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linear_solver));
    g2o::OptimizationAlgorithmLevenberg* optimization_algorithm_ptr = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(block_solver_ptr));
//     g2o::OptimizationAlgorithmGaussNewton* optimization_algorithm_ptr = new g2o::OptimizationAlgorithmGaussNewton(std::unique_ptr<Block>(block_solver_ptr));
//     g2o::OptimizationAlgorithmDogleg* optimization_algorithm_ptr = new g2o::OptimizationAlgorithmDogleg(std::unique_ptr<Block>(block_solver_ptr));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(optimization_algorithm_ptr);

    int vertexCnt = 0, edgeCnt = 0;     // the number of vertex & edge
    std::vector<VertexSE3LieAlgebra*> vertices;
    std::vector<EdgeSE3LieAlgebra*> edges;

    while (!fin.eof()) {
        std::string name;
        fin >> name;
        if (name == "VERTEX_SE3:QUAT") {
            // vertex
            VertexSE3LieAlgebra* v = new VertexSE3LieAlgebra();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt ++;
            vertices.push_back(v);
            if (index == 0)
                v->setFixed(true);

        } else if (name == "EDGE_SE3:QUAT") {
            // SE3 - SE3 dege
            EdgeSE3LieAlgebra* e = new EdgeSE3LieAlgebra();
            int idx1, idx2;
            fin >> idx1 >> idx2;
            e->setId(edgeCnt ++);
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
            edges.push_back(e);
        }

        if (!fin.good()) break;
    }

    std::cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << std::endl;

    std::cout << "prepare optimizing ..." << std::endl;

    optimizer.setVerbose(true);
    optimizer.initializeOptimization();

    std::cout << "calling optimizing ..." << std::endl;

    optimizer.optimize(30);

    std::cout << "saving optimization results ..." << std::endl;

    /* 因为用了自定义顶点且没有向g2o注册，这里保存自己来实现伪装成 SE3 顶点和边，让 g2o_viewer 可以认出 */
    std::ofstream fout("result_lie.g2o");
    if (!fout.is_open()) {
        std::cout << "result_lie.g2o dose not exist." << std::endl;
        exit(EXIT_FAILURE);
    }

    for (auto v : vertices) {
        fout << "VERTEX_SE3:QUAT ";
        v->write(fout);
    }
    for (auto e : edges) {
        fout << "EDGE_SE3:QUAT ";
        e->write(fout);
    }

    fout.close();

    return 0;
}