/* Copyright (C) 2013 Ion Torrent Systems, Inc. All Rights Reserved */

#include <iostream>
#include <algorithm>
#include "PcaSpline.h"
#include "Utils.h"
#include <malloc.h>
//#define EIGEN_USE_MKL_ALL 1
#include <Eigen/Dense>
#include <Eigen/LU>

using namespace std;
using namespace Eigen;

/**
 * Create the basis splines for order requested at a particular value of x.
 * From "A Practical Guide To Splines - Revised Edition" by Carl de Boor p 111
 * the algorithm for BSPLVB. The value of the spline coefficients can be calculated
 * based on a cool recursion. This is a simplified and numerically stable algorithm
 * that seems to be the basis of most bspline implementations including gsl, matlab,etc.
 * @param all_knots - Original knots vector augmented with order number of endpoints
 * @param n_knots - Total number of knots.
 * @param order - Order or order of polynomials, 4 for cubic splines.
 * @param i - index of value x in knots such that all_knots[i] <= x && all_knots[i+1] > x
 * @param x - value that we want to evaluate spline at.
 * @param b - memory to write our coefficients into, must be at least order long.
 */
void basis_spline_xi(float *all_knots, int n_knots, int order, int i, float x, float *b) {
  b[0] = 1;
  double kr[order];
  double kl[order];
  double term;
  double saved;
  for (int j = 0; j < order - 1; j++) {
    kr[j] = all_knots[i + j + 1] - x; // k right values
    kl[j] = x - all_knots[i - j];     // k left values
    saved = 0;
    for (int r = 0; r <= j; r++) {
      term = b[r] / (kr[r] + kl[j-r]);
      b[r] = saved + kr[r] * term;
      saved = kl[j-r] * term;
    }
    b[j+1] = saved;
  }
}

/**
 * Create the augmented knots vector and fill in matrix Xt with the spline basis vectors
 */
void basis_splines_endreps_local(float *knots, int n_knots, int order, int *boundaries, int n_boundaries,  MatrixXf &Xt) {
  assert(n_boundaries == 2 && boundaries[0] < boundaries[1]);
  int n_rows = boundaries[1] - boundaries[0]; // this is frames so evaluate at each frame we'll need
  int n_cols = n_knots + order;  // number of basis vectors we'll have at end
  Xt.resize(n_cols, n_rows); // swapped to transpose later. This order lets us use it as a scratch space
  Xt.fill(0);
  int n_std_knots = n_knots + 2 * order;
  float std_knots[n_std_knots];

  // Repeat the boundary knots on there to ensure linear out side of knots
  for (int i = 0; i < order; i++) {
    std_knots[i] = boundaries[0];
  }
  // Our original boundary knots here
  for (int i = 0; i < n_knots; i++) {
    std_knots[i+order] = knots[i];
  }
  // Repeat the boundary knots on there to ensure linear out side of knots
  for (int i = 0; i < order; i++) {
    std_knots[i+order+n_knots] = boundaries[1];
  }
  // Evaluate our basis splines at each frame.
  for (int i = boundaries[0]; i < boundaries[1]; i++) {
    int idx = -1;
    // find index such that i >= knots[idx] && i < knots[idx+1]
    float *val = std::upper_bound(std_knots, std_knots + n_std_knots - 1, 1.0f * i);
    idx = val - std_knots - 1;
    assert(idx >= 0);
    float *f = Xt.data() + i * n_cols + idx - (order - 1); //column offset
    basis_spline_xi(std_knots, n_std_knots, order, idx, i, f);
  }
  // Put in our conventional format where each column is a basis vector
  Xt.transposeInPlace();
}

void PcaSpline::FillInKnots(const std::string &strategy, int n_frame, std::vector<float> &knots) {
  if (strategy != "no-knots") {
    if (strategy == "even4") {
      float stride = n_frame / 3.0f;
      knots.push_back(stride);
      knots.push_back(stride * 2.0f);
    }
    else if (strategy == "middle") {
      float stride = n_frame / 2.0f;
      knots.push_back(stride);
    }
    else if (strategy.find("explicit:") == 0) {
      string spec = strategy.substr(9, strategy.length() - 9);
      knots = char2Vec<float>(spec.c_str(), ',');
    }
    else {
      assert(false);
    }
  }
}

int PcaSpline::NumBasisVectors(int n_pca, int n_frames, int n_order, const std::string &knot_strategy) {
  std::vector<float> knots;
  FillInKnots(knot_strategy, n_frames, knots);
  int spline_basis = 0;
  if (knots.size() > 0) {
    spline_basis = knots.size() + n_order;
  }
  return spline_basis + n_pca;
}

void PcaSpline::LossyCompress(int n_wells, int n_frame, float *image,
			      int n_sample_wells, float *image_sample,
			      PcaSplineRegion &compressed) {
  Map<MatrixXf, Aligned> Ysub(image_sample, n_sample_wells, n_frame);
  Map<MatrixXf, Aligned> Y(image, n_wells, n_frame);
  MatrixXf C = Ysub.transpose() * Ysub;
  SelfAdjointEigenSolver<MatrixXf> es;
  es.compute(C);
  MatrixXf Pca_Basis = es.eigenvectors();
  MatrixXf Spline_Basis;
  vector<float> knots;
  FillInKnots(m_knot_strategy, n_frame, knots);
  if (!knots.empty()) {
    int boundaries[2];
    boundaries[0] = 0;
    boundaries[1] = n_frame;
    basis_splines_endreps_local(&knots[0], knots.size(), m_order, boundaries, sizeof(boundaries)/sizeof(boundaries[0]), Spline_Basis);
  }
  assert(compressed.n_basis == m_num_pca + Spline_Basis.cols());
  Map<MatrixXf, Aligned> Basis(compressed.basis_vectors, compressed.n_frames, compressed.n_basis);
  for(int i = 0; i < m_num_pca; i++) {
    Basis.col(i) = Pca_Basis.col(Pca_Basis.cols() - i - 1);
  }
  for (int i = 0; i < Spline_Basis.cols(); i++) {
    Basis.col(i+m_num_pca) = Spline_Basis.col(i);
  }
  MatrixXf SX;
  if (m_tikhonov_reg > 0) {
    MatrixXf diag(Basis.cols(), Basis.cols());
    diag.fill(0);
    for (int i = 0; i < diag.rows(); i++) {
      diag(i,i) = 1.0f * m_tikhonov_reg;
    }
    SX = ((Basis.transpose() * Basis) + (diag.transpose() * diag)).inverse() * Basis.transpose();
  }
  else {
    SX = (Basis.transpose() * Basis).inverse() * Basis.transpose();
  }
  Map<MatrixXf, Aligned> B(compressed.coefficients, n_wells, Basis.cols());
  B = Y * SX.transpose();
}

void PcaSpline::SampleImageMatrix(int n_wells, int n_frames, float * __restrict orig, int *__restrict trcs_state, int target_sample,
				  int &n_sample, float ** __restrict sample) {
  int good_wells = 0;
  int * __restrict state_begin = trcs_state;
  int * __restrict state_end  = trcs_state + n_wells;
  while (state_begin != state_end) {
    if (*state_begin++ >= 0) {
      good_wells++;
    }
  }
  int sample_rate = max(1,good_wells/target_sample);
  int n_sample_rows = (int)(ceil((float)good_wells/sample_rate));
  n_sample = n_sample_rows;
  *sample = (float *) memalign(32, sizeof(float) * n_frames * n_sample_rows);
  for (int i = 0; i < n_frames; i++) {
    float *__restrict col_start = orig + i * n_wells;
    float *__restrict col_end = col_start + n_wells;
    state_begin = trcs_state;
    float *__restrict sample_col_begin = (*sample) + n_sample_rows * i;
    int check_count = 0;
    int good_count = sample_rate - 1;
    while (col_start != col_end) {
      if (*state_begin >= 0 && ++good_count == sample_rate) {
	*sample_col_begin++ = *col_start;
	good_count = 0;
	check_count++;
      }
      col_start++;
    }
    assert(check_count == n_sample_rows);
  }
}

void PcaSpline::LossyUncompress(int n_wells, int n_frame, float *image,
				const PcaSplineRegion &compressed) {
  Map<MatrixXf, Aligned> Yh(image, n_wells, compressed.n_frames);
  Map<MatrixXf, Aligned> Basis(compressed.basis_vectors, compressed.n_frames, compressed.n_basis);
  Map<MatrixXf, Aligned> B(compressed.coefficients, n_wells, compressed.n_basis);
  Yh = B * Basis.transpose();
}


/**
 * @file
 * @author Denise Ratasich
 * @date 14.10.2013
 *
 * @brief Implement sampling from specific distributions.
 *
 * Currently only Gaussian distributions can be choosen for process
 * and covariance noise. Hence only sampling from a Gaussian
 * distribution is implemented.
 */

#include "probability/sampling.h"

#include <stdexcept>
#include <cmath>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real.hpp>

namespace probability
{
  // create the rng only once (otherwise the numbers generated are
  // always the same)
  static boost::mt19937 rng(time(NULL));

  VectorXd sampleNormalDistribution(VectorXd mean, MatrixXd covariance)
  {
    // save last covariance (so Cholesky decomposition needs not to
    // be done every time entering this function)
    static MatrixXd cov;
    static MatrixXd T;			// transform matrix when cov is not diagonal

    // additionally needed for rng
    boost::normal_distribution<> dist(0,1);	// mean = 0, standard deviation = 1
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > random(rng, dist);

    VectorXd sample = VectorXd::Zero(mean.size());

    // check if covariance is diagonal (set diagonal to zero and
    // compare with MatrixXD::Zero)
    if (covariance.isDiagonal())
    {
      // sample each variable independently
      for (int i = 0; i < sample.size(); i++)
	sample[i] = mean[i] + random() * sqrt(covariance(i,i));
    }
    else
    {
      // x ~ N(0,C^-1)
      // C = L*L^T
      // sample x = L^-T * z, where z ~ N(0,I)

      // same covariance as before? (do calculation of L only once!)
      if (cov.rows() != covariance.rows() ||
	  cov.cols() != covariance.cols() ||
	  cov != covariance)
	{
	  cov = covariance;

	  // Cholesky decomposition of inverse covariance to get L;
	  // the covariance must be a symmetric and positive definite
	  // matrix (a covariance has these features, but the user
	  // must configure it so)
	  covariance = covariance.inverse();
	  MatrixXd L = covariance.llt().matrixL();

	  MatrixXd checkLL = L*L.transpose();
	  for (int r = 0; r < covariance.rows(); r++)
	    for (int c = 0; c < covariance.cols(); c++)
	      if (checkLL(r,c) - covariance(r,c)  >  0.000001)
		throw std::runtime_error("sampleNormalDistribution: Cholesky decomposition failed.");

	  T = L.inverse().transpose();
	}

      // sample z: z_i ~ N(0,1)
      for (int i = 0; i < sample.size(); i++)
	sample[i] = random();

      // transform to x (and add mean!)
      sample = mean + T * sample;
    }

    return sample;
  }

  VectorXd sampleUniformDistribution(VectorXd a, VectorXd b)
  {
    VectorXd sample = VectorXd::Zero(a.size());

    if (a.size() != b.size())
      throw std::runtime_error("sampleUniformDistribution: Bounds a and b must have equal sizes.");

    // additionally needed for rng
    boost::uniform_real<> dist(0,1);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> >
      random(rng, dist);

    // sample each variable independently
    for (int i = 0; i < sample.size(); i++)
      sample[i] = a[i] + random() * (b[i]-a[i]);

    return sample;
  }
}


// written for clarity not efficiency.

#include <iostream>
using std::cout;
using std::endl;

#include <boost/array.hpp>
#include <boost/circular_buffer.hpp>

class Subtractive_generator {
private:
    static const int param_i = 55;
    static const int param_j = 24;
    static const int initial_load = 219;
    static const int mod = 1e9;
    boost::circular_buffer<int> r;
public:
    Subtractive_generator(int seed);
    int next();
    int operator()(){return next();}
};

Subtractive_generator::Subtractive_generator(int seed)
:r(param_i)
{
    boost::array<int, param_i> s;
    s[0] = seed;
    s[1] = 1;
    for(int n = 2; n < param_i; ++n){
        int t = s[n-2]-s[n-1];
        if (t < 0 ) t+= mod;
        s[n] = t;
    }

    for(int n = 0; n < param_i; ++n){
	int i = (34 * (n+1)) % param_i;
        r.push_back(s[i]);
    }
    for(int n = param_i; n <= initial_load; ++n) next();
}

int Subtractive_generator::next()
{
    int t = r[0]-r[31];
    if (t < 0) t += mod;
    r.push_back(t);
    return r[param_i-1];
}

int main()
{
    Subtractive_generator rg(292929);

    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;

    return 0;
}


#ifndef LATTICE_HPP
#define LATTICE_HPP

#include <vector>
#include <memory>
#include <utility>
#include <complex>

#include <Eigen/Dense>

/**  class representing a general lattice structure
	parameters
		- lattice basis vectors
		- which lattice sites are coupled
*/

using namespace Eigen;

//a structure given for making the lattice
struct basis{
	//matrix that contains basis indexes
	std::vector<std::pair<int,double>> pairing;
};

class Lattice{

public:

	//lattice basis vectors
	MatrixXf basis_vectors;

	//closest particle vectors
	//  R = r_aa r_ba r_ca
	//		r_ab r_bb r_cb
	//		r_ac r_bc r_cc
	std::vector<std::shared_ptr<MatrixXf>> R;

	//t_matrix << 0, -t,-t,
	//			-t, 0, -t,
	//			-t, -t, 0;
	MatrixXf t_matrix;

	//dimensions
	int bands;
	int dim;

	//all the kinetic hamiltonians
	std::vector<MatrixXcf> H_kin_matrices;

	//all the k_values
	std::vector<MatrixXf> k_vectors;

	const std::complex<float> IF = std::complex<float>(0.0f, 1.0f);

	//constructor
	Lattice(const MatrixXf& basis, const std::vector<std::shared_ptr<MatrixXf>>& r, const MatrixXf& t_mat) :
		basis_vectors(basis), bands(t_mat.cols()), dim(basis.cols()),
		R(r), t_matrix(t_mat) {}

	//destructor
	~Lattice() {};

	int Bands(){
		return bands;
	}

	int Dim(){
		return dim;
	}

	//calculates the kinetic hamiltonian for a given k
	MatrixXcf KineticHamiltonian(const VectorXf& k){
		MatrixXcf H_kin(bands,bands);
		H_kin = MatrixXcf::Zero(bands,bands);
		for(int l=0; l<R.size(); l++){
			MatrixXf R_iter = *R[l];

			for(int i = 0; i<bands; i++){
				for(int j =0; j< bands; j++){

					//make a complex 1by1 matrix
					MatrixXcf dot_ra(1,1);
					//fill the imaginary part with i*dot(k*r_ab)
					dot_ra.imag() = ( R_iter.block(i*dim,j,dim,1).transpose() * k );

					dot_ra.real() = MatrixXf::Zero(1,1);

					//taking the term to direction -r_ab
					MatrixXcf neg_dot_ra = dot_ra;
					neg_dot_ra.imag() = -neg_dot_ra.imag();

					//summing the exponentials
					MatrixXcf coupling = dot_ra.array().exp() + neg_dot_ra.array().exp();

					//the coupling is multiplied by corresponding t

					H_kin.block(i,j,1,1) += -t_matrix(i,j) * coupling;

				}
			}
		}
		return H_kin;
	}

	//this saves the h_kin_matrics
	void SaveKineticHamiltonians(const float& k_min, const float& k_max){
		int Nk = 20;
		VectorXf kx = VectorXf::LinSpaced(Nk,k_min,k_max);
		VectorXf ky = VectorXf::LinSpaced(Nk,k_min,k_max);

		for(int i = 0; i<Nk ; i++){
			for(int j = 0; j<Nk; j++){
				MatrixXf k(dim,1);
				k << kx(i),ky(j);

				k_vectors.push_back(k);
				H_kin_matrices.push_back(KineticHamiltonian(k) );

			}
		}
	}

	//solves the eigenvalues of one k by selfadjointeigensolver of Eigen package
	//@return: column vector of eigenvalues with smallest eigenvalue first
	MatrixXf EigenvaluesOneK(const VectorXf& k){

		SelfAdjointEigenSolver<MatrixXcf> es(bands);

		es.compute(KineticHamiltonian(k));

		return es.eigenvalues();

	}

	//calculates the dispersion over the first brillouin zone
	//returns a matrix of [kx ky E1 E2 E3]
	MatrixXf CalculateDispersion(const float& k_min, const float& k_max){

		int Nk = 20;
		VectorXf kx = VectorXf::LinSpaced(Nk,k_min,k_max);
		VectorXf ky = VectorXf::LinSpaced(Nk,k_min,k_max);

		MatrixXf k_values(Nk*Nk, dim);

		MatrixXf Energies(Nk*Nk, bands);

		for(int i = 0; i<Nk ; i++){
			for(int j = 0; j<Nk; j++){
				MatrixXf k(1,dim);
				k << kx(i),ky(j);
				k_values.row(i*Nk + j) = k;

				Energies.row(i*Nk + j) = EigenvaluesOneK(k.transpose()).transpose();

			}
		}

		MatrixXf data(Nk*Nk,dim+bands);
		data.leftCols(dim) = k_values;
		data.rightCols(bands) = Energies;
		return data;

	}

};

#endif


#include <gtest/gtest.h>

#include "mfem.hpp"
using namespace mfem;

#include <iostream>
#include <fstream>
#include <chrono>
#include <random>

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

#include "../include/core/config.hpp"
#include "../include/core/error.hpp"

/* Test the H1-error computation,
 * implemented in class ComputeH1Error,
 * for quasi-uniform meshes,
 * generated by _serial_ code.
*/
TEST(Poisson, computeH1Error1) {

    std::string input_dir
            = "../input/poisson_smooth_unitSquare/";

    int lx1 = 2;
    int lx2 = 4;

    const std::string mesh_file1
            = input_dir+"mesh_lx"+std::to_string(lx1);
    const std::string sol_file1
            = input_dir+"sol_lx"+std::to_string(lx1);

    const std::string mesh_file2
            = input_dir+"mesh_lx"+std::to_string(lx2);
    const std::string sol_file2
            = input_dir+"sol_lx"+std::to_string(lx2);

    //std::cout << mesh_file1 << std::endl;
    //std::cout << mesh_file2 << std::endl;

    Mesh mesh1(mesh_file1.c_str());
    Mesh mesh2(mesh_file2.c_str());

    std::ifstream sol_ifs1(sol_file1.c_str());
    std::ifstream sol_ifs2(sol_file2.c_str());

    GridFunction u1(&mesh1, sol_ifs1);
    GridFunction u2(&mesh2, sol_ifs2);

    ComputeH1Error computeH1Error{};

    auto start1 = std::chrono::high_resolution_clock::now();
    auto [errorL21, u2L21, errorH101, u2H101]
            = computeH1Error(u1, u2);
    auto end1 = std::chrono::high_resolution_clock::now();

    auto start2 = std::chrono::high_resolution_clock::now();
    auto [errorL22, u2L22, errorH102, u2H102]
            = computeH1Error.test_slow(u1, u2);
    auto end2 = std::chrono::high_resolution_clock::now();

    double TOL = 1E-8;
    ASSERT_LE((errorL21/u2L21 - errorL22/u2L22), TOL);
    ASSERT_LE((errorH101/u2H101 - errorH102/u2H102), TOL);

    auto duration1 = std::chrono::duration_cast
            <std::chrono::microseconds>(end1 - start1);
    auto duration2 = std::chrono::duration_cast
            <std::chrono::microseconds>(end2 - start2);

    std::cout << "Relative L2 error: "
              << errorL21/u2L21 << "\t"
              << errorL22/u2L22 << std::endl;
    std::cout << "Relative H10 error: "
              << errorH101/u2H101 << "\t"
              << errorH102/u2H102 << std::endl;
    std::cout << "Run times: "
              << "\t fast  " << duration1.count()
              << "\t slow  " << duration2.count()
            << std::endl;
}

/* Test the H1-error computation,
 * implemented in class ComputeH1Error,
 * for meshes with local refinement,
 * generated by _serial_ code.
*/
TEST(Poisson, computeH1Error2) {

    std::string input_dir
            = "../input/"
              "poisson_singular_gammaShaped_bisecRefine/";

    int lx1 = 2;
    int lx2 = 4;

    const std::string mesh_file1
            = input_dir+"mesh_lx"+std::to_string(lx1);
    const std::string sol_file1
            = input_dir+"sol_lx"+std::to_string(lx1);

    const std::string mesh_file2
            = input_dir+"mesh_lx"+std::to_string(lx2);
    const std::string sol_file2
            = input_dir+"sol_lx"+std::to_string(lx2);

    //std::cout << mesh_file1 << std::endl;
    //std::cout << mesh_file2 << std::endl;

    Mesh mesh1(mesh_file1.c_str());
    Mesh mesh2(mesh_file2.c_str());

    std::ifstream sol_ifs1(sol_file1.c_str());
    std::ifstream sol_ifs2(sol_file2.c_str());

    GridFunction u1(&mesh1, sol_ifs1);
    GridFunction u2(&mesh2, sol_ifs2);

    ComputeH1Error computeH1Error{};

    auto start1 = std::chrono::high_resolution_clock::now();
    auto [errorL21, u2L21, errorH101, u2H101]
            = computeH1Error(u1, u2);
    auto end1 = std::chrono::high_resolution_clock::now();

    auto start2 = std::chrono::high_resolution_clock::now();
    auto [errorL22, u2L22, errorH102, u2H102]
            = computeH1Error.test_slow(u1, u2);
    auto end2 = std::chrono::high_resolution_clock::now();

    double TOL1 = 1E-6;
    double TOL2 = 1E-5;
    ASSERT_LE((errorL21/u2L21 - errorL22/u2L22), TOL1);
    ASSERT_LE((errorH101/u2H101 - errorH102/u2H102), TOL2);

    auto duration1 = std::chrono::duration_cast
            <std::chrono::microseconds>(end1 - start1);
    auto duration2 = std::chrono::duration_cast
            <std::chrono::microseconds>(end2 - start2);

    std::cout << "Relative L2 error: "
              << errorL21/u2L21 << "\t"
              << errorL22/u2L22 << std::endl;
    std::cout << "Relative H10 error: "
              << errorH101/u2H101 << "\t"
              << errorH102/u2H102 << std::endl;
    std::cout << "Run times: "
              << "\t fast  " << duration1.count()
              << "\t slow  " << duration2.count()
            << std::endl;
}

/* Test the H1-error computation,
 * implemented in class ComputeH1Error,
 * for quasi-uniform meshes,
 * generated by _parallel_ code.
*/
TEST(Poisson, computeH1Error3) {

    std::string input_dir
            = "../input/poisson_smooth_unitSquare/";

    int lx1 = 2;
    int lx2 = 4;

    const std::string mesh_file1
            = input_dir+"mesh_lx"+std::to_string(lx1);
    const std::string sol_file1
            = input_dir+"sol_lx"+std::to_string(lx1);

    const std::string mesh_file2
            = input_dir+"mesh_lx"+std::to_string(lx2);
    const std::string sol_file2
            = input_dir+"sol_lx"+std::to_string(lx2);

    const std::string mesh_file1p
            = input_dir+"pmesh_lx"+std::to_string(lx1);
    const std::string sol_file1p
            = input_dir+"psol_lx"+std::to_string(lx1);

    const std::string mesh_file2p
            = input_dir+"pmesh_lx"+std::to_string(lx2);
    const std::string sol_file2p
            = input_dir+"psol_lx"+std::to_string(lx2);

    Mesh mesh1(mesh_file1.c_str());
    Mesh mesh2(mesh_file2.c_str());
    Mesh mesh1p(mesh_file1.c_str());
    Mesh mesh2p(mesh_file2.c_str());

    std::ifstream sol_ifs1(sol_file1.c_str());
    std::ifstream sol_ifs2(sol_file2.c_str());
    std::ifstream sol_ifs1p(sol_file1.c_str());
    std::ifstream sol_ifs2p(sol_file2.c_str());

    GridFunction u1(&mesh1, sol_ifs1);
    GridFunction u2(&mesh2, sol_ifs2);
    GridFunction u1p(&mesh1p, sol_ifs1p);
    GridFunction u2p(&mesh2p, sol_ifs2p);

    ComputeH1Error computeH1Error{};

    auto start1 = std::chrono::high_resolution_clock::now();
    auto [errorL21, u2L21, errorH101, u2H101]
            = computeH1Error(u1, u2);
    auto end1 = std::chrono::high_resolution_clock::now();

    bool has_shared_vertices = true;
    auto start2 = std::chrono::high_resolution_clock::now();
    auto [errorL22, u2L22, errorH102, u2H102]
            = computeH1Error(u1p, u2p, has_shared_vertices);
    auto end2 = std::chrono::high_resolution_clock::now();

    double TOL = 1E-8;
    ASSERT_LE((errorL21/u2L21 - errorL22/u2L22), TOL);
    ASSERT_LE((errorH101/u2H101 - errorH102/u2H102), TOL);

    auto duration1 = std::chrono::duration_cast
            <std::chrono::microseconds>(end1 - start1);
    auto duration2 = std::chrono::duration_cast
            <std::chrono::microseconds>(end2 - start2);

    std::cout << "Relative L2 error: "
              << errorL21/u2L21 << "\t"
              << errorL22/u2L22 << std::endl;
    std::cout << "Relative H10 error: "
              << errorH101/u2H101 << "\t"
              << errorH102/u2H102 << std::endl;
    std::cout << "Run times: "
              << "\t fast  " << duration1.count()
              << "\t slow  " << duration2.count()
            << std::endl;
}

/* Test the H1-error computation,
 * implemented in class ComputeH1Error,
 * for meshes with local refinement,
 * generated by _serial_ code.
*/
TEST(Poisson, computeH1Error4) {

    std::string input_dir
            = "../input/"
              "poisson_singular_gammaShaped_bisecRefine/";

    int lx1 = 2;
    int lx2 = 4;

    const std::string mesh_file1
            = input_dir+"mesh_lx"+std::to_string(lx1);
    const std::string sol_file1
            = input_dir+"sol_lx"+std::to_string(lx1);

    const std::string mesh_file2
            = input_dir+"mesh_lx"+std::to_string(lx2);
    const std::string sol_file2
            = input_dir+"sol_lx"+std::to_string(lx2);

    const std::string mesh_file1p
            = input_dir+"pmesh_lx"+std::to_string(lx1);
    const std::string sol_file1p
            = input_dir+"psol_lx"+std::to_string(lx1);

    const std::string mesh_file2p
            = input_dir+"pmesh_lx"+std::to_string(lx2);
    const std::string sol_file2p
            = input_dir+"psol_lx"+std::to_string(lx2);

    Mesh mesh1(mesh_file1.c_str());
    Mesh mesh2(mesh_file2.c_str());
    Mesh mesh1p(mesh_file1.c_str());
    Mesh mesh2p(mesh_file2.c_str());

    std::ifstream sol_ifs1(sol_file1.c_str());
    std::ifstream sol_ifs2(sol_file2.c_str());
    std::ifstream sol_ifs1p(sol_file1.c_str());
    std::ifstream sol_ifs2p(sol_file2.c_str());

    GridFunction u1(&mesh1, sol_ifs1);
    GridFunction u2(&mesh2, sol_ifs2);
    GridFunction u1p(&mesh1p, sol_ifs1p);
    GridFunction u2p(&mesh2p, sol_ifs2p);

    ComputeH1Error computeH1Error{};

    auto start1 = std::chrono::high_resolution_clock::now();
    auto [errorL21, u2L21, errorH101, u2H101]
            = computeH1Error(u1, u2);
    auto end1 = std::chrono::high_resolution_clock::now();

    bool has_shared_vertices = true;
    auto start2 = std::chrono::high_resolution_clock::now();
    auto [errorL22, u2L22, errorH102, u2H102]
            = computeH1Error(u1p, u2p, has_shared_vertices);
    auto end2 = std::chrono::high_resolution_clock::now();

    double TOL = 1E-8;
    ASSERT_LE((errorL21/u2L21 - errorL22/u2L22), TOL);
    ASSERT_LE((errorH101/u2H101 - errorH102/u2H102), TOL);

    auto duration1 = std::chrono::duration_cast
            <std::chrono::microseconds>(end1 - start1);
    auto duration2 = std::chrono::duration_cast
            <std::chrono::microseconds>(end2 - start2);

    std::cout << "Relative L2 error: "
              << errorL21/u2L21 << "\t"
              << errorL22/u2L22 << std::endl;
    std::cout << "Relative H10 error: "
              << errorH101/u2H101 << "\t"
              << errorH102/u2H102 << std::endl;
    std::cout << "Run times: "
              << "\t fast  " << duration1.count()
              << "\t slow  " << duration2.count()
            << std::endl;
}


//  GAMBIT: Global and Modular BSM Inference Tool
//  *********************************************
///  \file
///
///  Multivariate Gaussian prior
///
///  *********************************************
///
///  Authors (add name and date if you modify):
///
///  \author Ben Farmer
///  (benjamin.farmer@monash.edu.au)
///  \date 2013 Dec
///
///  \author Gregory Martinez
///  (gregory.david.martinez@gmail.com)
///  \date Feb 2014
///
///  \author Andrew Fowlie
///    (andrew.j.fowlie@qq.com)
///  \date August 2020
///
///  *********************************************

#ifndef __PRIOR_GAUSSIAN_HPP__
#define __PRIOR_GAUSSIAN_HPP__

#include <algorithm>
#include <cmath>
#include <string>
#include <unordered_map>
#include <vector>

#include "gambit/ScannerBit/cholesky.hpp"
#include "gambit/ScannerBit/priors.hpp"
#include "gambit/Utils/yaml_options.hpp"

#include <boost/math/special_functions/erf.hpp>

namespace Gambit
{
  namespace Priors
  {
    /**
     * @brief  Multi-dimensional Gaussian prior
     *
     * Defined by a covariance matrix and mean.
     *
     * If the covariance matrix is diagonal, it may instead be specified by the square-roots of its
     * diagonal entries, denoted \f$\sigma\f$.
     */
    class Gaussian : public BasePrior
    {
     private:
      std::vector <double> mu;
      mutable Cholesky col;

     public:
      // Constructor defined in gaussian.cpp
      Gaussian(const std::vector<std::string>&, const Options&);

      /** @brief Transformation from unit interval to the Gaussian */
      void transform(const std::vector <double> &unitpars, std::unordered_map<std::string, double> &outputMap) const
      {
        std::vector<double> vec(unitpars.size());

        auto v_it = vec.begin();
        for (auto elem_it = unitpars.begin(), elem_end = unitpars.end(); elem_it != elem_end; elem_it++, v_it++)
        {
          *v_it = M_SQRT2 * boost::math::erf_inv(2. * (*elem_it) - 1.);
        }

        col.ElMult(vec);

        v_it = vec.begin();
        auto m_it = mu.begin();
        for (auto str_it = param_names.begin(), str_end = param_names.end(); str_it != str_end; str_it++)
        {
          outputMap[*str_it] = *(v_it++) + *(m_it++);
        }
      }

      std::vector<double> inverse_transform(const std::unordered_map<std::string, double> &physical) const override
      {
        // subtract mean
        std::vector<double> central;
        for (int i = 0, n = this->size(); i < n; i++)
        {
          central.push_back(physical.at(param_names[i]) - mu[i]);
        }

        // invert rotation by Cholesky matrix
        std::vector<double> rotated = col.invElMult(central);

        // now diagonal; invert Gaussian CDF
        std::vector<double> u;
        for (const auto& v : rotated)
        {
          u.push_back(0.5 * (boost::math::erf(v / M_SQRT2) + 1.));
        }
        return u;
      }

      double operator()(const std::vector<double> &vec) const
      {
        static double norm = 0.5 * std::log(2. * M_PI * std::pow(col.DetSqrt(), 2));
        return -0.5 * col.Square(vec, mu) - norm;
      }
    };

    LOAD_PRIOR(gaussian, Gaussian)

  }  // namespace Priors
}  // namespace Gambit

#endif  // __PRIOR_GAUSSIAN_HPP__


#include <boost/ut.hpp>

using namespace boost::ut;

unsigned int Factorial(unsigned int number) {
  if(number == 0 or number == 1) return 1;
  return number * Factorial(number - 1);
}

suite errors = []{
  expect(Factorial(1) == 1_i);
  expect(Factorial(0) == 1_i);
};


// Copyright (C) 2009  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../stl_checked.h"
#include "../array.h"
#include "../rand.h"
#include <dlib/string.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.matrix_chol");

    dlib::rand rnd;

// ----------------------------------------------------------------------------------------

    template <typename mat_type>
    const matrix<typename mat_type::type> symm(const mat_type& m) { return m*trans(m); }

// ----------------------------------------------------------------------------------------

    template <typename type>
    const matrix<type> randmat(long r, long c)
    {
        matrix<type> m(r,c);
        for (long row = 0; row < m.nr(); ++row)
        {
            for (long col = 0; col < m.nc(); ++col)
            {
                m(row,col) = static_cast<type>(rnd.get_random_double());
            }
        }

        return m;
    }

    template <typename type, long NR, long NC>
    const matrix<type,NR,NC> randmat()
    {
        matrix<type,NR,NC> m;
        for (long row = 0; row < m.nr(); ++row)
        {
            for (long col = 0; col < m.nc(); ++col)
            {
                m(row,col) = static_cast<type>(rnd.get_random_double());
            }
        }

        return m;
    }

// ----------------------------------------------------------------------------------------

    template <typename matrix_type>
    void test_cholesky ( const matrix_type& m)
    {
        typedef typename matrix_type::type type;
        const type eps = 10*max(abs(m))*sqrt(std::numeric_limits<type>::epsilon());
        dlog << LDEBUG << "test_cholesky():  " << m.nr() << " x " << m.nc() << "  eps: " << eps;
        print_spinner();

        cholesky_decomposition<matrix_type> test(m);

        // none of the matrices we should be passing in to test_cholesky() should be non-spd.
        DLIB_TEST(test.is_spd() == true);

        type temp;
        DLIB_TEST_MSG( (temp= max(abs(test.get_l()*trans(test.get_l()) - m))) < eps,temp);

        {
            matrix<type> mat = chol(m);
            DLIB_TEST_MSG( (temp= max(abs(mat*trans(mat) - m))) < eps,temp);
        }

        matrix<type> m2;
        matrix<type,0,1> col;

        m2 = identity_matrix<type>(m.nr());
        DLIB_TEST_MSG(equal(m*test.solve(m2), m2,eps),max(abs(m*test.solve(m2)- m2)));
        m2 = randmat<type>(m.nr(),5);
        DLIB_TEST_MSG(equal(m*test.solve(m2), m2,eps),max(abs(m*test.solve(m2)- m2)));
        m2 = randmat<type>(m.nr(),1);
        DLIB_TEST_MSG(equal(m*test.solve(m2), m2,eps),max(abs(m*test.solve(m2)- m2)));
        col = randmat<type>(m.nr(),1);
        DLIB_TEST_MSG(equal(m*test.solve(col), col,eps),max(abs(m*test.solve(m2)- m2)));

        // now make us a non-spd matrix
        if (m.nr() > 2)
        {
            matrix<type> sm(lowerm(m));
            sm(1,1) = 0;

            cholesky_decomposition<matrix_type> test2(sm);
            DLIB_TEST_MSG(test2.is_spd() == false,  test2.get_l());

            cholesky_decomposition<matrix_type> test3(sm*trans(sm));
            DLIB_TEST_MSG(test3.is_spd() == false,  test3.get_l());

            sm = sm*trans(sm);
            sm(1,1) = 5;
            sm(1,0) -= 1;
            cholesky_decomposition<matrix_type> test4(sm);
            DLIB_TEST_MSG(test4.is_spd() == false,  test4.get_l());
        }

    }

// ----------------------------------------------------------------------------------------

    void matrix_test_double()
    {

        test_cholesky(uniform_matrix<double>(1,1,1) + 10*symm(randmat<double>(1,1)));
        test_cholesky(uniform_matrix<double>(2,2,1) + 10*symm(randmat<double>(2,2)));
        test_cholesky(uniform_matrix<double>(3,3,1) + 10*symm(randmat<double>(3,3)));
        test_cholesky(uniform_matrix<double>(4,4,1) + 10*symm(randmat<double>(4,4)));
        test_cholesky(uniform_matrix<double>(15,15,1) + 10*symm(randmat<double>(15,15)));
        test_cholesky(uniform_matrix<double>(101,101,1) + 10*symm(randmat<double>(101,101)));

        typedef matrix<double,0,0,default_memory_manager, column_major_layout> mat;
        test_cholesky(mat(uniform_matrix<double>(101,101,1) + 10*symm(randmat<double>(101,101))));
    }

// ----------------------------------------------------------------------------------------

    void matrix_test_float()
    {

        test_cholesky(uniform_matrix<float>(1,1,1) + 2*symm(randmat<float>(1,1)));
        test_cholesky(uniform_matrix<float>(2,2,1) + 2*symm(randmat<float>(2,2)));
        test_cholesky(uniform_matrix<float>(3,3,1) + 2*symm(randmat<float>(3,3)));

        typedef matrix<float,0,0,default_memory_manager, column_major_layout> mat;
        test_cholesky(mat(uniform_matrix<float>(3,3,1) + 2*symm(randmat<float>(3,3))));
    }

// ----------------------------------------------------------------------------------------

    class matrix_tester : public tester
    {
    public:
        matrix_tester (
        ) :
            tester ("test_matrix_chol",
                    "Runs tests on the matrix cholesky component.")
        {
            rnd.set_seed(cast_to_string(time(0)));
        }

        void perform_test (
        )
        {
            dlog << LINFO << "seed string: " << rnd.get_seed();

            dlog << LINFO << "begin testing with double";
            matrix_test_double();
            dlog << LINFO << "begin testing with float";
            matrix_test_float();
        }
    } a;

}


#include "functions/transpose.hh"
#include <boost/test/unit_test.hpp>
#include "data/matrix.hh"

BOOST_AUTO_TEST_CASE(tranpose_test) {
  using namespace manifolds;
  auto m1 = GetRowMatrix<3, 2>(1, 2, 3, 4, 5, 6);
  auto m2 = GetColMatrix<2, 3>(1, 2, 3, 4, 5, 6);
  auto check = GetMatrix<2, 3>(1, 3, 5, 2, 4, 6);
  BOOST_CHECK_EQUAL(check, m2);
  BOOST_CHECK_EQUAL(transpose(m1), m2);
}


#include <frovedis.hpp>
#include <frovedis/matrix/blas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>
#include "../../rmse.hpp"

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // creating a colmajor matrix local from file
    colmajor_matrix_local<float> cm1 (
           make_rowmajor_matrix_local_load<float>("./sample_4x4"));
    auto cm2 = cm1;

    // slicing rows
    auto row1 = make_row_vector<float> (cm1,0);
    auto row2 = make_row_vector<float> (cm1,1);
    auto row3 = make_row_vector<float> (cm1,2);
    auto row4 = make_row_vector<float> (cm1,3);

    // row3-of-cm1 = 5*cm2*row1-of-cm1
    gemv<float>(cm2,row1,row3,'N',5.0);

    // row4-of-cm1 = 2*trans(cm2)*row2-of-cm1 + 3*row4-of-cm1
    gemv<float>(cm2,row2,row4,'T',2.0,3.0);

    double tol = 0.01;
    colmajor_matrix_local<float> ref (
           make_rowmajor_matrix_local_load<float> ("./ref_4x4"));
    BOOST_CHECK (calc_rms_err<float> (cm1.val, ref.val) < tol);
}


#include <itomp_cio_planner/util/exponential_map.h>
#include <boost/math/special_functions/sinc.hpp>

namespace itomp_cio_planner
{
namespace exponential_map
{

Eigen::Vector3d RotationToExponentialMap(const Eigen::Matrix3d& matrix, const Eigen::Vector3d* close_to)
{
    if (close_to == NULL)
        return QuaternionToExponentialMap(Eigen::Quaterniond(matrix));
    else
    {
        Eigen::Quaterniond q0 = Eigen::Quaterniond(matrix);
        Eigen::Vector3d exp_map0 = QuaternionToExponentialMap(q0);

        Eigen::Quaterniond q1 = q0;
        q1.coeffs() = -q0.coeffs();
        Eigen::Vector3d exp_map1 = QuaternionToExponentialMap(q1);

        if ((exp_map0 - *close_to).norm() <= (exp_map1 - *close_to).norm())
            return exp_map0;
        else
            return exp_map1;
    }
}

Eigen::Matrix3d ExponentialMapToRotation(const Eigen::Vector3d& exponential_rotation)
{
	return ExponentialMapToQuaternion(exponential_rotation).toRotationMatrix();
}

Eigen::Vector3d QuaternionToExponentialMap(const Eigen::Quaterniond& quaternion)
{
	Eigen::Vector3d vec = quaternion.vec();
    if (vec.norm() < ITOMP_EPS)
		return Eigen::Vector3d::Zero();

	double theta = 2.0 * std::acos(quaternion.w());
	vec.normalize();
	return theta * vec;
}

Eigen::Quaterniond ExponentialMapToQuaternion(const Eigen::Vector3d& exponential_rotation)
{
	double angle = 0.5 * exponential_rotation.norm();
	Eigen::Quaterniond quaternion;
	quaternion.w() = std::cos(angle);
	quaternion.vec() = 0.5 * boost::math::sinc_pi(angle) * exponential_rotation;
	return quaternion;
}

}
}


// Implementing the class that is defined in the header file: Option.hpp
//
// (c) Sudhansh Dua

#include "EuropeanOption.hpp"
#include <cmath>
#include <boost/math/distributions.hpp>

using namespace std;
using namespace boost::math;

//	Gaussian functions using boost libraries
double EuropeanOption::N(double x) const
{
	normal_distribution<> Standard_normal(0.0, 1.0);
	return cdf(Standard_normal, x);
}

double EuropeanOption::n(double x) const
{
	normal_distribution<> Standard_normal(0.0, 1.0);
	return pdf(Standard_normal, x);
}

//	Kernel functions
double EuropeanOption::CallPrice() const
{
	return ::CallPrice(S, K, T, r, sig, b);
}

double EuropeanOption::PutPrice() const
{
	return ::PutPrice(S, K, T, r, sig, b);
}

double EuropeanOption::CallDelta() const
{
	return ::CallDelta(S, K, T, r, sig, b);
}

double EuropeanOption::PutDelta() const
{
	return ::PutDelta(S, K, T, r, sig, b);
}

double EuropeanOption::CallGamma() const
{
	return ::CallGamma(S, K, T, r, sig, b);
}

double EuropeanOption::PutGamma() const
{
	return ::PutGamma(S, K, T, r, sig, b);
}

double EuropeanOption::CallVega() const
{
	return ::CallVega(S, K, T, r, sig, b);
}

double EuropeanOption::PutVega() const
{
	return ::PutVega(S, K, T, r, sig, b);
}

double EuropeanOption::CallTheta() const
{
	return ::CallTheta(S, K, T, r, sig, b);
}

double EuropeanOption::PutTheta() const
{
	return ::PutTheta(S, K, T, r, sig, b);
}

double EuropeanOption::CallRho() const
{
	return ::CallRho(S, K, T, r, sig, b);
}

double EuropeanOption::PutRho() const
{
	return ::PutRho(S, K, T, r, sig, b);
}

double EuropeanOption::CallCoc() const
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	return T * S * exp((b - r) * T) * N(d1);
}

double EuropeanOption::PutCoc() const
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	return -T * S * exp((b - r) * T) * N(-d1);
}

//	Initialising all the default values
void EuropeanOption::init()
{
	//	Default values
	r = 0.03;
	sig = 0.2;
	K = 100;
	S = 95;				//	Default stock price
	T = 1;
	b = r;				//	Black - Scholes(1973) stock option model : b = r
	type = "C";			//	Call option as the default
}

void EuropeanOption::copy(const EuropeanOption& option)
{
	S = option.S;
	K = option.K;
	T = option.T;
	r = option.r;
	sig = option.sig;
	b = option.b;
	type = option.type;
}

//	Constructors and destructor
EuropeanOption::EuropeanOption() : Option()						//	Default constructor
{
	init();
}

EuropeanOption::EuropeanOption(const EuropeanOption& option) : Option(option)		//	Copy constructor
{
	copy(option);
}

//	Constructor that accepts values
EuropeanOption::EuropeanOption(const double& S1, const double& K1, const double& T1, const double& r1,
	const double& sig1, const double& b1, const string type1) : Option(), S(S1), K(K1), T(T1), r(r1), sig(sig1), b(b1), type(type1) {}

EuropeanOption::~EuropeanOption() {}							//	Destructor

//	Assignment operator
EuropeanOption& EuropeanOption::operator = (const EuropeanOption& option)
{
	if (this == &option)
	{
		return *this;
	}
	Option::operator = (option);
	copy(option);
	return *this;
}

//	Functions that calculate option price and sensitivities
double EuropeanOption::Price() const
{
	if (type == "C")
	{
		return CallPrice();
	}
	else
	{
		return PutPrice();
	}
}

double EuropeanOption::Delta() const
{
	if (type == "C")
	{
		return CallDelta();
	}
	else
	{
		return PutDelta();
	}
}

double EuropeanOption::Gamma() const
{
	if (type == "C")
	{
		return CallGamma();
	}
	else
	{
		return PutGamma();
	}
}

double EuropeanOption::Vega() const
{
	if (type == "C")
	{
		return CallVega();
	}
	else
	{
		return PutVega();
	}
}

double EuropeanOption::Theta() const
{
	if (type == "C")
	{
		return CallTheta();
	}
	else
	{
		return PutTheta();
	}
}

double EuropeanOption::Rho() const
{
	if (type == "C")
	{
		return CallRho();
	}
	else
	{
		return PutRho();
	}
}

//	Functions that calculates the Cost of carry
double EuropeanOption::Coc() const
{
	if (type == "C")
	{
		return CallCoc();
	}
	else
	{
		return PutCoc();
	}
}

// Modifier functions
void EuropeanOption::toggle()				//	Change the option type
{
	type = ((type == "C") ? "P" : "C");
}

//	Global Functions
double CallDelta(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);
	return exp((b - r) * T) * cdf(standard_normal, d1);

}

double PutDelta(const double S, const double K, const double T, const double r, const double sig, const double b)
{

	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	return exp((b - r) * T) * (cdf(standard_normal, d1) - 1.0);
}

double CallGamma(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	return (pdf(standard_normal, d1) * exp((b - r) * T)) / (S * sig * sqrt(T));
}

double PutGamma(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	return CallGamma(S, K, T, r, sig, b);
}

double CallVega(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	return (S * sqrt(T) * exp((b - r) * T) * pdf(standard_normal, d1));
}

double PutVega(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	return CallVega(S, K, T, r, sig, b);
}

double CallTheta(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	double t1 = (S * sig * exp((b - r) * T) * pdf(standard_normal, d1)) / (2 * sqrt(T));
	double t2 = ((b - r) * S * exp((b - r) * T) * cdf(standard_normal, d1));
	double t3 = (r * K * exp(-r * T) * cdf(standard_normal, d1));
	return -(t1 + t2 + t3);
}

double PutTheta(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	double t1 = (S * sig * exp((b - r) * T) * pdf(standard_normal, d1)) / (2 * sqrt(T));
	double t2 = ((b - r) * S * exp((b - r) * T) * cdf(standard_normal, d1));
	double t3 = (r * K * exp(-r * T) * cdf(standard_normal, d1));
	return t2 + t3 - t1;
}

double CallRho(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	if (b != 0.0)
	{
		return T * K * exp(-r * T) * cdf(standard_normal, d2);
	}
	else
	{
		return -T * CallPrice(S, K, T, r, sig, b);
	}
}

double PutRho(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	if (b != 0.0)
	{
		return T * K * exp(-r * T) * cdf(standard_normal, -d2);
	}
	else
	{
		return -T * PutPrice(S, K, T, r, sig, b);
	}
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_random.h>
#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/big/big_cholesky.h>

#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/symmetric.hpp>

namespace ublas = boost::numeric::ublas;

#include <cassert>
#include <limits>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

/**
* Make a immutable triangular adaptor from a matrix
*
* \usage:

A = triangular< lower >(B);
A = triangular(B, lower());

*/
template < class TYPE, class MATRIX >
ublas::triangular_adaptor<const MATRIX, TYPE>
triangular(const MATRIX & A, const TYPE& uplo = TYPE())
{
  return ublas::triangular_adaptor<const MATRIX, TYPE>(A);
}

/**
* Make a immutable banded adaptor from a matrix
*
* \usage:

A = banded(B, lower, upper);

*/
template < class MATRIX >
ublas::banded_adaptor<const MATRIX>
banded(const MATRIX & A, const size_t lower, const size_t upper)
{
  return ublas::banded_adaptor<const MATRIX>(A, lower, upper);
}

/**
* Make a immutable symmetric adaptor from a matrix
*
* \usage:

A = symmetric< lower >(B);
A = symmetric(B, lower());

*/
template < class TYPE, class MATRIX >
ublas::symmetric_adaptor<const MATRIX, TYPE>
symmetric(const MATRIX & A, const TYPE& uplo = TYPE())
{
  return ublas::symmetric_adaptor<const MATRIX, TYPE>(A);
}

/**
* Fill lower triangular matrix L
*/
template < class MATRIX >
void fill_symm(MATRIX & L, const size_t bands = std::numeric_limits<size_t>::max() )
{
  typedef typename MATRIX::size_type size_type;

  assert(L.size1() == L.size2());

  size_type size = L.size1();
  for (size_type i=0; i<size; i++)
  {
    for (size_type j = ((i>bands)?(i-bands):0); j<i; j++)
    {
      L(i,j) = 1 + (1.0 + i)/(1.0 + j) + 1.0 / (0.5 + i - j);
    }
    L(i,i) = 1+i+size;
  }

  return;
}

template<typename matrix_type,typename vector_type>
void test(matrix_type const & A, vector_type  & x, vector_type const & b, vector_type const & y)
{
  using std::min;

  typedef typename matrix_type::value_type real_type;
  typedef typename matrix_type::size_type  size_type;

  real_type const tol = boost::numeric_cast<real_type>(0.01);

  size_type const m = A.size1();
  size_type const n = A.size2();
  matrix_type L;
  L.resize(m,n);

  // Dense versions
  OpenTissue::math::big::cholesky_decompose(A,L);
  x = b;
  OpenTissue::math::big::cholesky_solve(L, x, ublas::lower());
  for(size_type i = 0; i < x.size();++i)
    BOOST_CHECK_CLOSE( real_type( x(i) ), real_type( y(i) ), tol );
  x.clear();

  L.assign(A);
  OpenTissue::math::big::cholesky_decompose(L);
  x = b;
  OpenTissue::math::big::cholesky_solve(L, x, ublas::lower());
  for(size_type i = 0; i < x.size();++i)
    BOOST_CHECK_CLOSE( real_type( x(i) ), real_type( y(i) ), tol );
  x.clear();

  // Easy Dense version
  L.assign(A);
  OpenTissue::math::big::cholesky_solve(L,x,b);
  for(size_type i = 0; i < x.size();++i)
    BOOST_CHECK_CLOSE( real_type( x(i) ), real_type( y(i) ), tol );
  x.clear();

  // sparse version
  ublas::compressed_matrix<double> C;
  C.resize(m,n,false);
  C.assign(A);
  OpenTissue::math::big::incomplete_cholesky_decompose(C);
  x = b;
  OpenTissue::math::big::cholesky_solve(C, x, ublas::lower());
  for(size_type i = 0; i < x.size();++i)
    BOOST_CHECK_CLOSE( real_type( x(i) ), real_type( y(i) ), tol );
  x.clear();

  // Easy sparse version
  C.assign(A);
  OpenTissue::math::big::cholesky_solve(C,x,b);
  for(size_type i = 0; i < x.size();++i)
    BOOST_CHECK_CLOSE( real_type( x(i) ), real_type( y(i) ), tol );

}

BOOST_AUTO_TEST_SUITE(opentissue_math_big_cholesky);

BOOST_AUTO_TEST_CASE(Gunter_Winkler_Konstantin_Kutzkow_Test)
{
  size_t size = 10;
  {
    // use dense matrix
    ublas::matrix<double, ublas::row_major> A (size, size);
    ublas::matrix<double, ublas::row_major> T (size, size);
    ublas::matrix<double, ublas::row_major> L (size, size);

    A = ublas::zero_matrix<double>(size, size);

    ublas::vector<double> b (size);
    ublas::vector<double> x (size);
    ublas::vector<double> y (size);

    std::fill(y.begin(), y.end(), 1.0);

    fill_symm(T);
    A = ublas::prod(T, trans(T));
    b = prod( A, y);
    size_t res = OpenTissue::math::big::cholesky_decompose(A, L);

    BOOST_CHECK( res == 0u );

    x = b;
    OpenTissue::math::big::cholesky_solve(L, x, ublas::lower());
    //std::cout << res << ": "
    //          << ublas::norm_inf(L-T) << " "
    //          << ublas::norm_2(x-y) << " "
    //          << " (deco: " << watch1() << " sec)"
    //          << " (prod: " << watch2() << " sec)"
    //          << " (solve: " << watch3() << " sec)"
    //          << " " << size
    //          << std::endl;
  }

  {
    // use dense triangular matrices
    ublas::triangular_matrix<double, ublas::lower, ublas::row_major> A (size, size);
    ublas::triangular_matrix<double, ublas::lower, ublas::row_major> T (size, size);
    ublas::triangular_matrix<double, ublas::lower, ublas::row_major> L (size, size);

    A = ublas::zero_matrix<double> (size, size) ;
    A = triangular<ublas::lower>( ublas::zero_matrix<double> (size, size) );
    A = triangular( ublas::zero_matrix<double> (size, size), ublas::lower() );

    ublas::vector<double> b (size);
    ublas::vector<double> x (size);
    ublas::vector<double> y (size);

    std::fill(y.begin(), y.end(), 1.0);

    fill_symm(T);
    A = triangular<ublas::lower>( ublas::prod(T, trans(T)) );
    b = prod( symmetric<ublas::lower>(A), y);
    size_t res = OpenTissue::math::big::cholesky_decompose(A, L);

    BOOST_CHECK( res == 0u );

    x = b;
    OpenTissue::math::big::cholesky_solve(L, x, ublas::lower());
    //std::cout << res << ": "
    //          << ublas::norm_inf(L-T) << " "
    //          << ublas::norm_2(x-y) << " "
    //          << " (deco: " << watch1() << " sec)"
    //          << " (prod: " << watch2() << " sec)"
    //          << " (solve: " << watch3() << " sec)"
    //          << " " << size
    //          << std::endl;
  }
  {
    // use banded matrices matrix
    typedef ublas::banded_matrix<double, ublas::row_major> MAT;

    size_t bands = std::min<size_t>( size, 50 );
    MAT A (size, size, bands, 0);
    MAT T (size, size, bands, 0);
    MAT L (size, size, bands, 0);

    A = ublas::zero_matrix<double> (size, size) ;
    A = banded( ublas::zero_matrix<double> (size, size), bands, 0 );

    ublas::vector<double> b (size);
    ublas::vector<double> x (size);
    ublas::vector<double> y (size);

    std::fill(y.begin(), y.end(), 1.0);

    fill_symm(T, bands);
    A = banded( ublas::prod(T, trans(T)), bands, 0 );
    b = prod( symmetric<ublas::lower>(A), y);
    size_t res = OpenTissue::math::big::cholesky_decompose(A, L);

    BOOST_CHECK( res == 0u );

    x = b;
    OpenTissue::math::big::cholesky_solve(L, x, ublas::lower());
    //std::cout << res << ": "
    //          << ublas::norm_inf(L-T) << " "
    //          << ublas::norm_2(x-y) << " "
    //          << " (deco: " << watch1() << " sec)"
    //          << " (prod: " << watch2() << " sec)"
    //          << " (solve: " << watch3() << " sec)"
    //          << " " << size
    //          << std::endl;
  }
}

BOOST_AUTO_TEST_CASE(random_test_case)
{
  typedef ublas::matrix<double>            matrix_type;
  typedef ublas::vector<double>            vector_type;
  typedef vector_type::size_type           size_type;

  for(size_type tst=0;tst<1000;++tst)
  {
    size_type N = 10;

    matrix_type A;
    A.resize(N,N,false);

    vector_type x;
    x.resize(N,false);

    vector_type b;
    b.resize(N,false);

    vector_type y;
    y.resize(N,false);

    matrix_type R;
    R.resize(N,N,false);

    OpenTissue::math::Random<double> value(0.0,1.0);
    for(size_t i=0;i<R.size1();++i)
    {
      b(i) = value();
      x(i) = value();
      y(i) = value();
      for(size_t j=0;j<R.size2();++j)
        R(i,j) = value();
    }
    ublas::noalias(A) = ublas::prod( ublas::trans(R), R );
    // forcing A to become PD matrix (it should be non-singular at all times)
    for(size_t i=0;i<R.size1();++i)
      A(i,i) += 0.5;

    ublas::noalias(b) = ublas::prod(A,x);
    y.assign(x);

    x.clear();
    test(A,x,b,y);
  }

}

BOOST_AUTO_TEST_SUITE_END();


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, rising_factorial) {
  using stan::math::rising_factorial;

  EXPECT_FLOAT_EQ(120, rising_factorial(4.0,3));
  EXPECT_FLOAT_EQ(360, rising_factorial(3.0,4));
  EXPECT_THROW(rising_factorial(-1, 4),std::domain_error);
}

TEST(MathFunctions, rising_factorial_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::rising_factorial(4.0, nan));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::rising_factorial(nan, 3.0));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::rising_factorial(nan, nan));
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/trace.hpp
 *
 * \brief Compute the trace of a matrix.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_TRACE_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_TRACE_HPP

#include <algorithm>
#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>

namespace boost { namespace numeric { namespace ublasx {

namespace ublas = ::boost::numeric::ublas;

/**
 * \brief Compute the trace of a given matrix.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param A The input matrix expression.
 * \return The trace of \a A.
 *
 * The trace of a m-by-n matrix \f$A\f$ is defined as
 * \f[
 *   \operatorname{tr}(A) = \sum_{i=1}^k a_{ii}
 * \f]
 * where \f$k=\min(m,n)\f$.
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename ublas::matrix_traits<MatrixExprT>::value_type trace(ublas::matrix_expression<MatrixExprT> const& A)
{
    typedef typename ublas::matrix_traits<MatrixExprT>::value_type value_type;
    typedef typename ublas::matrix_traits<MatrixExprT>::size_type size_type;

    size_type n = ::std::min(num_rows(A), num_columns(A));
    value_type res = 0;

    for (size_type i = 0; i < n; ++i)
    {
        res += A()(i,i);
    }

    return res;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_TRACE_HPP


#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <Eigen/Dense>
#include <algorithm>
#include <ctime>
#include <fstream>
#include <iostream>
#include <math.h>
#include <random>
#include <string>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned, K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Delaunay::Face_handle Face_handle;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> MapM3Xd;
typedef Eigen::Ref<Matrix3Xd> RefM3Xd;

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

  // We expect three command line arguments:
  // 1. Path to the data file
  // 2. Output directory where output files will be created
  if (argc < 3) {
    std::cout << "Usage: ./calcVolume <data_file> <output_dir>" << std::endl;
    return 0;
  }

  clock_t t = clock();

  // Open the data file
  std::ifstream inputfile(argv[1]);
  if (!inputfile.is_open()) {
    std::cout << "Unable to open file " << argv[1] << std::endl;
    return 0;
  }

  //**********************************************************************//
  //

  auto outputFile = std::string(argv[2]) + "/volume.dat";
  std::ofstream vol_file(outputFile);

  if (!vol_file) {
    std::cout << "Failed to create output file " << outputFile << std::endl;
    return 0;
  }

  //**********************************************************************//
  // Now read the first( or the header ) line of the data file
  // The first line contains N, gamma and beta information
  std::string line, ignore;
  size_t N;
  double_t gamma = 0, beta = 0;
  std::getline(inputfile, line);
  std::istringstream header(line);
  header >> ignore >> N >> ignore >> gamma >> ignore >> beta;
  int lmax = std::floor(std::sqrt(N) - 1);

  // Write column names in the output files
  vol_file << "volume" << std::endl;
  vol_file.precision(7);

  //**********************************************************************//
  // Some useful lambda functions
  //

  // A function to read N rows of a 3xN matrix from a stringstream
  auto readMatrix3Xd = [](std::istringstream &ss, RefM3Xd X) {
    auto N = X.cols();
    size_t valCount = 0;
    while (valCount < 3 * N && ss.good()) {
      std::string value;
      std::getline(ss, value, ',');
      X(valCount % 3, valCount / 3) = std::stod(value);
      valCount++;
    }
  };
  //**********************************************************************//

  //***************************** MAIN LOOP ******************************//
  // Skip the first row which is just zero temperature data
  std::getline(inputfile, line);

  // Now iterate over the remaining rows
  auto rowCount = 1;
  while (std::getline(inputfile, line)) {

    std::istringstream rowStream(line);

    // We will extract 3*N doubles representing particle positions
    // from the stream and ignore the 3*N rotation vector components
    Matrix3Xd Xt(3, N), X0(3, N);
    readMatrix3Xd(rowStream, Xt);

    // Project the points to a unit sphere
    X0 = Xt.colwise().normalized();

    // Rotate all points of the shell so that the 0th point is along z-axis
    Vector3d c = X0.col(0);
    double_t cos_t = c(2);
    double_t sin_t = std::sin(std::acos(cos_t));
    Vector3d axis;
    axis << c(1), -c(0), 0.;
    axis.normalize();
    Matrix3d rotMat, axis_cross, outer;
    axis_cross << 0., -axis(2), axis(1), axis(2), 0., -axis(0), -axis(1),
        axis(0), 0.;
    outer.noalias() = axis * axis.transpose();
    rotMat =
        cos_t * Matrix3d::Identity() + sin_t * axis_cross + (1 - cos_t) * outer;
    Matrix3Xd rPts(3, N);
    rPts = rotMat * X0;

    // Calculate the stereographic projections
    Vector3d p0;
    p0 << 0, 0, -1.0; // Point on the plane of projection
    c = rPts.col(0);  // The point from which we are projecting

    MapM3Xd l0(&(rPts(0, 1)), 3, N - 1);
    Matrix3Xd l(3, N - 1), proj(3, N - 1);
    l = (l0.colwise() - c).colwise().normalized(); // dirns of projections
    for (auto j = 0; j < N - 1; ++j) {
      proj.col(j) = ((p0(2) - l0(2, j)) / l(2, j)) * l.col(j) + l0.col(j);
    }

    // Insert the projected points in a CGAL vertex_with_info vector
    std::vector<std::pair<Point, unsigned>> verts;
    for (auto j = 0; j < N - 1; ++j) {
      verts.push_back(std::make_pair(Point(proj(0, j), proj(1, j)), j + 1));
    }

    // Triangulate
    Delaunay dt(verts.begin(), verts.end());

    // Iterate over the triangles to calculate volume
    auto volume = 0.0;
    for (auto ffi = dt.finite_faces_begin(); ffi != dt.finite_faces_end();
         ++ffi) {
      auto i = ffi->vertex(0)->info();
      auto j = ffi->vertex(2)->info();
      auto k = ffi->vertex(1)->info();
      volume += 0.166666667 * (Xt.col(i).dot(Xt.col(j).cross(Xt.col(k))));
    }

    // Iterate over infinite faces
    Face_circulator fc = dt.incident_faces(dt.infinite_vertex()), done(fc);
    if (fc != 0) {
      do {
        auto i = dt.is_infinite(fc->vertex(0)) ? 0 : fc->vertex(0)->info();
        auto j = dt.is_infinite(fc->vertex(2)) ? 0 : fc->vertex(2)->info();
        auto k = dt.is_infinite(fc->vertex(1)) ? 0 : fc->vertex(1)->info();
        volume += 0.166666667 * (Xt.col(i).dot(Xt.col(j).cross(Xt.col(k))));
      } while (++fc != done);
    }
    vol_file << volume << std::endl;
    rowCount++;
  }
  inputfile.close();
  vol_file.close();

  std::cout << "Time elapsed = " << ((float)(clock() - t)) / CLOCKS_PER_SEC
            << " seconds." << std::endl;

  return 1;
}


#include <doctest/doctest.h>

#define EIGEN_INITIALIZE_MATRICES_BY_ZERO

#include <Eigen/Core>
#include <ode/ode.hpp>

TEST_CASE("Coupled Test")
{
  using method_type               = ode::explicit_method<ode::forward_euler_tableau<float>>;
  using second_order_problem_type = ode::higher_order_initial_value_problem<float, Eigen::Vector3f, 2>;
  using coupled_problem_type      = second_order_problem_type::coupled_type;

  constexpr auto alpha = 1.0f;

  const auto second_order_problem = second_order_problem_type
  {
    0.0f,                                                                              /* t0 */
    std::array {Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 0.0f)}, /* y0, y'0 */
    [&] (const float h, const Eigen::Vector3f& y, const Eigen::Vector3f& dydt)         /* y'' = f(t, y, y') */
    {
      return 2.0f * h * dydt + 2.0f * alpha * y; /* Hermite differential equation */
    }
  };

  auto iterator = ode::coupled_fixed_step_iterator<method_type, coupled_problem_type>(second_order_problem.to_coupled_first_order(), 1.0f /* h */);
  for (auto i = 0; i < 1000; ++i)
    ++iterator;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/PolyMesh_ArrayKernelT.hh>

#include <CGAL/boost/graph/graph_traits_PolyMesh_ArrayKernelT.h>
#include <CGAL/Polygon_mesh_processing/compute_normal.h>

#include <iostream>
#include <fstream>
#include <boost/foreach.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_3 Point;
typedef K::Vector_3 Vector;

typedef OpenMesh::PolyMesh_ArrayKernelT< > Mesh;

typedef boost::graph_traits<Mesh>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Mesh>::face_descriptor face_descriptor;

int main(int argc, char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/eight.off";

  Mesh mesh;
  OpenMesh::IO::read_mesh(mesh, filename);

  CGAL::OM_pmap<Mesh, face_descriptor, Vector> fnormals(mesh);
  CGAL::OM_pmap<Mesh, vertex_descriptor, Vector> vnormals(mesh);
  mesh.add_property(fnormals.handle());
  mesh.add_property(vnormals.handle());

  Vector v(0, 0, 0);
  BOOST_FOREACH(vertex_descriptor vd, vertices(mesh)){
      put(vnormals, vd, v);
    }
  BOOST_FOREACH(face_descriptor fd, faces(mesh)){
      put(fnormals, fd, v);
    }
  CGAL::Polygon_mesh_processing::compute_normals
    (mesh,
     vnormals,
     fnormals,
     CGAL::Polygon_mesh_processing::parameters::vertex_point_map(get(CGAL::vertex_point, mesh)).
     geom_traits(K()));

  std::cout << "Face normals :" << std::endl;
  BOOST_FOREACH(face_descriptor fd, faces(mesh)){
    std::cout << fnormals[fd] << std::endl;
  }
  std::cout << "Normals at vertices :" << std::endl;
  BOOST_FOREACH(vertex_descriptor vd, vertices(mesh)){
    std::cout << vnormals[vd] << std::endl;
  }
  return 0;
}


#ifndef PYBNESIAN_UTIL_MATH_CONSTANTS_HPP
#define PYBNESIAN_UTIL_MATH_CONSTANTS_HPP

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

namespace util {

template <typename T>
inline auto constexpr pi = boost::math::constants::pi<T>();

template <typename T>
inline auto constexpr root_two = boost::math::constants::root_two<T>();

template <typename T>
inline auto constexpr one_div_root_two = boost::math::constants::one_div_root_two<T>();

template <typename T>
inline auto constexpr nan = std::numeric_limits<T>::quiet_NaN();

namespace detail {

double constexpr sqrtNewtonRaphson(double x, double curr, double prev) {
    return curr == prev ? curr : sqrtNewtonRaphson(x, 0.5 * (curr + x / curr), curr);
}

double constexpr sqrt_constexpr(double x) { return sqrtNewtonRaphson(x, x, 0); }

}  // namespace detail

inline auto constexpr machine_tol = detail::sqrt_constexpr(std::numeric_limits<double>::epsilon());

}  // namespace util

#endif  // PYBNESIAN_UTIL_MATH_CONSTANTS_HPP


//
// Created by kazem on 2020-05-18.
//

#include "includes/sympiler_cholesky.h"
#include <Eigen/Core>
#include <Eigen/Sparse>

 namespace sympiler
 {
  //  Solving Ax=b
  // Inputs:
  //   H  n by n sparse Hessian matrix **lower triangle only** (see
  //     .triangularView<Eigen::Lower>() )
  //   q  n by 1 vector
  // Outputs:
  //   x  n by 1 solution vector
  // Returns sympiler exit flag
  //
  int sympiler_spd_linsolve(
    // Pass inputs by copy so we get non-const and casted data
    Eigen::SparseMatrix<double,Eigen::ColMajor,int> A,
    Eigen::Matrix<double,Eigen::Dynamic,1> b,
    Eigen::Matrix<double,Eigen::Dynamic,1> & x){
   assert(A.isApprox(A.triangularView<Eigen::Lower>(),0) &&
          "P should be lower triangular");
   assert(A.isCompressed());
   assert(A.rows()==A.cols());
   assert(A.rows()==b.rows());

   auto *H = new sym_lib::parsy::CSC;
   H->nzmax = A.nonZeros();
   H->ncol= H->nrow=A.rows();
   H->p = A.outerIndexPtr();
   H->i = A.innerIndexPtr();
   H->x = A.valuePtr();
   H->stype=-1;
   H->xtype=SYMPILER_REAL;
   H->packed=TRUE;
   H->nz = NULL;
   H->sorted = TRUE;

   auto *sym_chol = new sym_lib::parsy::SolverSettings(H,b.data());
   sym_chol->ldl_variant = 1;
   sym_chol->solver_mode = 0;
   sym_chol->symbolic_analysis();
   sym_chol->numerical_factorization();
   double *sol = sym_chol->solve_only();
   //lbl->compute_norms();
   //std::cout<<"residual: "<<lbl->res_l1<<"\n";

   x = Eigen::Map< Eigen::Matrix<double,Eigen::Dynamic,1> >(
     sol,A.rows(),1);

   // Exitflag TODO
   int exitflag = 0;
   delete sym_chol;
   delete H;
   return exitflag;
  }

  sym_lib::parsy::CSC* sympiler_csc_format(
    // Pass inputs by copy so we get non-const and casted data
    Eigen::SparseMatrix<double,Eigen::ColMajor,int> A) {
   assert(A.isApprox(A.triangularView<Eigen::Lower>(), 0) &&
          "P should be lower triangular");
   assert(A.isCompressed());
   assert(A.rows() == A.cols());

   auto *H = new sym_lib::parsy::CSC;
   H->nzmax = A.nonZeros();
   H->ncol = H->nrow = A.rows();
   H->p = A.outerIndexPtr();
   H->i = A.innerIndexPtr();
   H->x = A.valuePtr();
   H->stype = -1;
   H->xtype = SYMPILER_REAL;
   H->packed = TRUE;
   H->nz = NULL;
   H->sorted = TRUE;
   return H;
  }

  sym_lib::parsy::SolverSettings* sympiler_chol_symbolic(
    // Pass inputs by copy so we get non-const and casted data
    sym_lib::parsy::CSC *A){

   auto *sym_chol = new sym_lib::parsy::SolverSettings(A);
   sym_chol->ldl_variant = 1;
   sym_chol->solver_mode = 0;
   sym_chol->symbolic_analysis();
   return sym_chol;
  }

  sym_lib::parsy::SolverSettings* sympiler_chol_symbolic(
    // Pass inputs by copy so we get non-const and casted data
    sym_lib::parsy::CSC *A, int num_threads, sym_lib::parsy::SYM_ORDER  so, int mode){

   auto *sym_chol = new sym_lib::parsy::SolverSettings(A);
   sym_chol->ldl_variant = mode;
   sym_chol->num_thread = num_threads;
   sym_chol->solver_mode = 0;
   sym_chol->sym_order = so;
   sym_chol->symbolic_analysis();
   return sym_chol;
  }

 }


//
// Created by jianping on 17-9-1.
//

#ifndef DEPTHPROBABILITY_ESTIMATOR_HPP
#define DEPTHPROBABILITY_ESTIMATOR_HPP

#include <ceres/ceres.h>
#include <Eigen/Dense>
#include <algorithm>
#include <math.h>

class DistributionEstimator {
public:
    struct distributionCostFunctor {

        distributionCostFunctor(double ob_x,double min, double max)
        :_ob_x(ob_x),_min(min),_max(max)
        {

        }

        template <typename T>
        bool operator()(const T* const params, T* residual) const {
            T part_normal = T(1.0)/(T(std::sqrt(2.0*M_PI))*params[2])
                            *ceres::exp(-(T(_ob_x)-params[1])*(T(_ob_x)-params[1])/(T(2)*params[2]*params[2]))*params[0];
            T part_uniform = T(1)/(_max-_min)*(T(1.0)-params[0]);
            residual[0] = -ceres::log(part_normal+part_uniform);
            if(params[0] < T(0.3) || params[0]>T(1))
            {
                return false;
            }
            return true;
        }

        double _ob_x;
        double _min;
        double _max;

    };

public:
    DistributionEstimator(const std::vector<double>& data)
    :_data(data)
    {
        _min = *std::min_element(_data.begin(),_data.end());
        _max = *std::max_element(_data.begin(),_data.end());

        //init
        params.resize(3);
        params[0] = 1;
        params[1] = std::accumulate(_data.begin(),_data.end(),0.0)/_data.size();
        params[2] = 0;
        for (int i = 0; i < _data.size(); ++i) {
            params[2] += (_data[i]-params[1])*(_data[i]-params[1]);
        }
        params[2]/=_data.size();
        params[2] = std::sqrt(params[2]);
        std::cout<<" mu:"<<params[1]<<std::endl;
        std::cout<<" sigma:"<<params[2]<<std::endl;
    }

    void estimate();
    std::vector<double> params;//pi mu sigma
private:
    const std::vector<double>& _data;
    double _min;
    double _max;

};

#endif //DEPTHPROBABILITY_ESTIMATOR_HPP


#ifndef MOD_EXPONENTIATION_HPP
#define MOD_EXPONENTIATION_HPP

#include <boost/multiprecision/cpp_int.hpp>

#include "big_int_type.hpp"

/*
 * Para ser realmente segura, a potenciação modular não deve deixar rastros da chave.
 * https://crypto.stackexchange.com/questions/75408/efficient-function-algorithm-method-to-do-modular-exponentiation
 */

// Mod pow in constant time with conditional copy
big_int mod_pow(big_int base_, big_int power_, big_int n_);

// long long mod_pow_fast(long long base, long long power, long long n);

// long long mod_pow_const_time(long long base, long long power, long long n);

#endif


#include <fstream>
#include <iostream>
#include <vector>
#include <cmath>
#include <boost/tuple/tuple.hpp>

#include "gnuplot-iostream.h"
#include "casadi/casadi.hpp"

#include <unistd.h>
inline void mysleep(unsigned millis) {
  ::usleep(millis * 1000);
}

void demo_animation() {

    Gnuplot gp;

    std::cout << "Press Ctrl-C to quit (closing gnuplot window doesn't quit)." << std::endl;

    gp << "set yrange [-1:1]\n";

    const int N = 1000;
    std::vector<double> pts(N);

    double theta = 0;
    while(1) {
        for(int i=0; i<N; i++) {
            double alpha = (static_cast<double>(i)/N-0.5) * 10;
            pts[i] = sin(alpha*8.0 + theta) * exp(-alpha*alpha/2.0);
        }

        gp << "plot '-' binary" << gp.binFmt1d(pts, "array") << "with lines notitle\n";
        gp.sendBinary1d(pts);
        gp.flush();

        theta += 0.2;
        mysleep(100);
    }
}

void demo_basic() {
  Gnuplot gp;
  // For debugging or manual editing of commands:
  //Gnuplot gp(std::fopen("plot.gnu", "w"));
  // or
  //Gnuplot gp("tee plot.gnu | gnuplot -persist");

  std::vector<std::pair<double, double>> xy_pts_A;
  for(double x=-2; x<2; x+=0.01) {
    double y = x*x*x;
    xy_pts_A.emplace_back(x, y);
  }

  std::vector<std::pair<double, double>> xy_pts_B;
  for(double alpha=0; alpha<1; alpha+=1.0/24.0) {
    double theta = alpha*2.0*3.14159;
    xy_pts_B.emplace_back(cos(theta), sin(theta));
  }

  gp << "set xrange [-2:2]\nset yrange [-2:2]\n";
  gp << "plot '-' with lines title 'cubic', '-' with points title 'circle'\n";
  gp.send1d(xy_pts_A);
  gp.send1d(xy_pts_B);

}

void demo_tmpfile() {
    Gnuplot gp;

    std::vector<std::pair<double, double>> xy_pts_A;
    for(double x=-2; x<2; x+=0.01) {
        double y = x*x*x;
        xy_pts_A.emplace_back(x, y);
    }

    std::vector<std::pair<double, double>> xy_pts_B;
    for(double alpha=0; alpha<1; alpha+=1.0/24.0) {
        double theta = alpha*2.0*3.14159;
        xy_pts_B.emplace_back(cos(theta), sin(theta));
    }

    gp << "set xrange [-2:2]\nset yrange [-2:2]\n";
    // Data will be sent via a temporary file.  These are erased when you call
    // gp.clearTmpfiles() or when gp goes out of scope.  If you pass a filename
    // (i.e. `gp.file1d(pts, "mydata.dat")`), then the named file will be created
    // and won't be deleted.
    //
    // Note: you need std::endl here in order to flush the buffer.  The send1d()
    // function flushes automatically, but we're not using that here.
    gp << "plot" << gp.file1d(xy_pts_A) << "with lines title 'cubic',"
        << gp.file1d(xy_pts_B) << "with points title 'circle'" << std::endl;

}

int main()
{
  std::cout << "Hello" << std::endl;

  demo_basic();
 // demo_tmpfile();
  demo_animation();

  return 0;
}


#include <armadillo>

int main()
{

    size_t dim = 5;
    double fl = 0.567;

    arma::mat em = arma::eye<arma::mat>(dim, dim);
    em.print("em");

    // type is arma::SizeMat?
    // auto s = arma::size(em);
    // s.print("s");

    arma::mat em2 = arma::eye<arma::mat>(arma::size(em));
    em2.print("em2");

    arma::mat em3 = fl * arma::eye<arma::mat>(arma::size(em));
    em3.print("em3");

    return 0;

}


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

using namespace Eigen;
using namespace std;

/*
 * c++ version of
 *     function [H,C,B] = manipulatorDynamics(obj,q,qd)
 */

template<typename DerivedQ>
void manipulatorDynamics(const mxArray *pobj, const MatrixBase<DerivedQ>& q, const MatrixBase<DerivedQ>& qd,
                         mxArray **plhs)
{
  // keep it readable:
  double m1 = mxGetScalar(mxGetProperty(pobj, 0, "m1"));
  double m2 = mxGetScalar(mxGetProperty(pobj, 0, "m2"));
  double l1 = mxGetScalar(mxGetProperty(pobj, 0, "l1"));
  double g = mxGetScalar(mxGetProperty(pobj, 0, "g"));
  double lc1 = mxGetScalar(mxGetProperty(pobj, 0, "lc1"));
  double lc2 = mxGetScalar(mxGetProperty(pobj, 0, "lc2"));
  double b1 = mxGetScalar(mxGetProperty(pobj, 0, "b1"));
  double b2 = mxGetScalar(mxGetProperty(pobj, 0, "b2"));
  double I1 = mxGetScalar(mxGetProperty(pobj, 0, "Ic1")) + m1 * lc1 * lc1;
  double I2 = mxGetScalar(mxGetProperty(pobj, 0, "Ic2")) + m2 * lc2 * lc2;
  double m2l1lc2 = m2 * l1 * lc2;  // occurs often!

  auto c2 = cos(q(1));
  auto s1 = sin(q(0)), s2 = sin(q(1));
  auto s12 = sin(q(0) + q(1));

  auto h12 = I2 + m2l1lc2 * c2;

  typedef typename DerivedQ::Scalar Scalar;
  Matrix<Scalar, 2, 2> H;
  Matrix<Scalar, 2, 1> C;
  Matrix<Scalar, 2, 1> B;

  H << I1 + I2 + m2 * l1 * l1 + 2 * m2l1lc2 * c2, h12, h12, I2;

  //C = [ -2*m2l1lc2*s(2)*qd(2), -m2l1lc2*s(2)*qd(2); m2l1lc2*s(2)*qd(1), 0 ];
  //G = g*[ m1*lc1*s(1) + m2*(l1*s(1)+lc2*s12); m2*lc2*s12 ];

  C << -2 * m2l1lc2 * s2 * qd(1) * qd(0) + -m2l1lc2 * s2 * qd(1) * qd(1), m2l1lc2 * s2 * qd(0) * qd(0);

  // add in G terms
  C(0) += g * m1 * lc1 * s1 + g * m2 * (l1 * s1 + lc2 * s12);
  C(1) += g * m2 * lc2 * s12;

  // damping terms
  C(0) += b1 * qd(0);
  C(1) += b2 * qd(1);

  // input matrix
  // multiplying by q(0) just to get the size of the derivatives vector right; Matlab TaylorVar operations will complain otherwise. We could handle this case on the Matlab side instead.
  B << 0.0 * q(0), 1.0;

  plhs[0] = eigenToMatlabGeneral<2, 2>(H);
  plhs[1] = eigenToMatlabGeneral<2, 1>(C);
  plhs[2] = eigenToMatlabGeneral<2, 1>(B);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  const mxArray *pobj = prhs[0];

  if (mxIsDouble(prhs[1]) && mxIsDouble(prhs[2])) {
    auto q = matlabToEigenMap<2, 1>(prhs[1]);
    auto qd = matlabToEigenMap<2, 1>(prhs[2]);
    manipulatorDynamics(pobj, q, qd, plhs);
  } else if (isa(prhs[1], "TrigPoly") && isa(prhs[2], "TrigPoly")) {
    auto q = trigPolyToEigen(prhs[1]);
    auto qd = trigPolyToEigen(prhs[2]);
    manipulatorDynamics(pobj, q, qd, plhs);
  } else if (isa(prhs[1], "TaylorVar") && isa(prhs[2], "TaylorVar")) {
    auto q = taylorVarToEigen<Dynamic, 1>(prhs[1]);
    auto qd = taylorVarToEigen<Dynamic, 1>(prhs[2]);
    manipulatorDynamics(pobj, q, qd, plhs);
  } else {
    mexErrMsgIdAndTxt("Drake:AcrobotPLantCpp:UnknownType",
                      "don't know how to handle the datatypes passed in for q and/or qd (yet)");
  }
}


/*! \file main.cpp
    \brief The main file performing the spectral numerical integration.

    In this file, we perform the computation from the PDF.
*/

#include <iostream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <chrono>
#include <fstream>

#include <Eigen/Dense>

#include "chebyshev_differentiation.h"
#include "spectral_integration_utilities.h"

#include "tictoc.h"

//  Number od admitted strain fields and number of mode per strain field
static constexpr unsigned int na = 3;  //  Kirkhoff rod
static constexpr unsigned int ne = 3;  // dimesion of qe

//  Number of Chebyshev nodes
static constexpr unsigned int number_of_chebyshev_nodes = 11;

/*!
 * \brief getA Compute the matrix A for the system x' = Ax + b
 * \param t_qe The current generalized strains coordinates
 * \tparam t_state_dimension The dimension of the state x
 * \tparam t_number_of_chebyshev_nodes The number of Chebyshev nodes (which also account for the first one)
 * \tparam t_na The number of allowed strain coordinates
 * \tparam t_ne The number of modes per strain coordinate
 * \return
 */
template<unsigned int t_state_dimension, unsigned int t_number_of_chebyshev_nodes, unsigned int t_na, unsigned int t_ne>
Eigen::MatrixXd getA(const Eigen::Matrix<double, ne*ne, 1> &t_qe)
{

    //  Define the Chebyshev points on the unit circle
    const auto x = ComputeChebyshevPoints<t_number_of_chebyshev_nodes>();

    //  Definition of the problem dymension
    static constexpr unsigned int problem_dimension = t_state_dimension * t_number_of_chebyshev_nodes;

    std::array<Eigen::Matrix<double, t_state_dimension, t_state_dimension>, t_number_of_chebyshev_nodes> A_stack;

    Eigen::Vector3d K;
    Eigen::Matrix<double, t_state_dimension, t_state_dimension> A_at_chebyshev_point;
    for(unsigned int i=0; i<x.size(); i++){

        //  Extract the curvature from the strain
        K = Phi<t_na, t_ne>(x[i])*t_qe;

        //  Compute the A matrix of Q' = 1/2 A(K) Q
        A_at_chebyshev_point <<      0, -K(0),  -K(1),  -K(2),
                                  K(0),     0,   K(2),  -K(1),
                                  K(1), -K(2),      0,   K(0),
                                  K(2),  K(1),  -K(0),      0;

        A_stack[i] = 0.5*A_at_chebyshev_point;
    }

    //  Declare the matrix for the system Ax = b
    Eigen::Matrix<double, problem_dimension, problem_dimension> A = Eigen::Matrix<double, problem_dimension, problem_dimension>::Zero();
    A.setConstant(0);

    //  Define a vector containing the indexes of the top left corners of the blocks composing the matrix A
    Eigen::VectorXi block_indexes = Eigen::VectorXi::LinSpaced(t_state_dimension, 0, t_number_of_chebyshev_nodes*(t_state_dimension-1));

    //  Populate this matrix with all the elements in the right order
    for(unsigned int chebyshev_point=0; chebyshev_point<t_number_of_chebyshev_nodes; chebyshev_point++){

        //  Get the current set of indexes for the coefficients of the matrix A at the current chebyshev point
        A(block_indexes, block_indexes) = A_stack[chebyshev_point] ;

        block_indexes += Eigen::VectorXi::Constant(t_state_dimension, 1, 1);
    }

    return A;
}

/*!
 * \brief writeToFile writes a Eigen matrix into file
 * \param t_name    name of the file
 * \param t_matrix  the Eigen matrix to write into the file
 * \param t_relative_path_from_build the relative path from the build folder to the file location. Default is none so the file is written in the build directory)
 * \param t_format  the specification for writing. (Default in column major allignment, with comma column separator and 8 digits precision)
 */
void writeToFile(std::string t_name,
                 const Eigen::MatrixXd &t_matrix,
                 std::string t_relative_path_from_build="",
                 const Eigen::IOFormat &t_format=Eigen::IOFormat(16, 0, ","));

Eigen::MatrixXd GetQuaternions(const Eigen::Vector4d &t_initial_state, const Eigen::Matrix<double, ne*ne, 1> &t_qe)
{
    /*  The state dimension and the number of nodes are known. The state dimension will not change
     *  (a quaternion will remain a quaternion, a twist will remain a twist ecc..) and the number of
     *  nodes are not changed at runtime and neither between runs. Typically, once a satisfactory number
     *  of nodes has been found, it is kept for the whole lifetime of the project.
     *
     *  Thus, we can enforce this concept with constexpr classifier so that the values are known at
     *  compile time and we can use templated function for our matrices and vector, which are a bit faster.
     */
        //  Dimension of the state
        constexpr unsigned int state_dimension = 4;

        //  Problem size is the total number of elements
        constexpr unsigned int prob_dimension = state_dimension * number_of_chebyshev_nodes;
        //  The subset of unknows in the problem
        constexpr unsigned int unknow_state_dimension = state_dimension * (number_of_chebyshev_nodes - 1);

    /*  These are a set of type definition in order to have a more neat algorithm in
     *  terms of matrix and vector dymensions
     */
        typedef Eigen::Matrix<double, prob_dimension, prob_dimension> MatrixNpNp;
        typedef Eigen::Matrix<double, prob_dimension, 1> VectorNp;

        typedef Eigen::Matrix<double, prob_dimension, state_dimension> MatrixNpNs;

        typedef Eigen::Matrix<double, unknow_state_dimension, unknow_state_dimension> MatrixNuNu;
        typedef Eigen::Matrix<double, unknow_state_dimension, 1> VectorNu;

        typedef Eigen::Matrix<double, number_of_chebyshev_nodes, number_of_chebyshev_nodes> MatrixNchebNcheb;

        typedef Eigen::Matrix<double, number_of_chebyshev_nodes, state_dimension, Eigen::ColMajor> MatrixNchebNs;

    /*  In this first part, we precompute the matrices P and Dn. In fact, once we know the state dimension and the
     *  number of nodes, these matrices are constant and thus me can compute them beforehand
     *
     *  P.S. As we know the number of nodes, the matrix Phi can be computed at every chebyshev point to be stacked
     *  into a vector. As a result, instead of computing the matrices, we directly access their value in the vector position.
     *  This can actually be something more than 200 times faster.
     *
     */

        const MatrixNpNp  P = getP<state_dimension, number_of_chebyshev_nodes>();

        const MatrixNchebNcheb Dn = getDn<number_of_chebyshev_nodes>();
        const MatrixNpNp D = Eigen::KroneckerProduct(Eigen::MatrixXd::Identity(state_dimension, state_dimension), Dn);

    /*  In this part we compute the matrix A and the vector b.
     *  In this case, the elements of the matrix A depends on the strain. As we change the strains during
     *  simulation, we have to compute the components of A at runtime.
     *
     *  Similarly, the vector b contains the values not related to the derivating variable, but that depends on other parameters.
     *  For example when computing r' = R(Q)*Γ it does not depend on r but only on Q and Γ.
     *
     *  We can interpret everything before this point as the setup and everything after as the actual run-time operations.
     *
     *  In the following there are some matrices and operation that could be moved in the setup. However I choose to left them
     *  there for clarity, but feel free to move where you thing it's better.
     *
     *  The following is the translation into C++ of the equations presented in the PDF
     *
     */
        //tictoc.tic();
        const MatrixNpNp A = getA<state_dimension, number_of_chebyshev_nodes, na, ne>(t_qe);
        //tictoc.toc("Time to compute A : ");

        const VectorNp b = Eigen::Matrix<double, prob_dimension, 1>::Zero();

        //  Apply transformation of initial condition onto ODE's matrices

        const MatrixNpNp Ap = P.transpose() * A * P;
        const MatrixNpNp Dp = P.transpose() * D * P;    //  Can be moved in setup
        const VectorNp bp   = P * b;

        //  Compute the ivp
        const MatrixNpNs D_IT = Dp.block<prob_dimension, state_dimension>(0, 0);    //  Can be moved in setup
        const MatrixNpNs A_IT = Ap.block<prob_dimension, state_dimension>(0, 0);
        const VectorNp b_IT = ( D_IT - A_IT ) * t_initial_state;

        //  Obtain the section related to the unknows of the problem
        const MatrixNuNu D_NN = Dp.block<unknow_state_dimension, unknow_state_dimension>(state_dimension, state_dimension);
        const MatrixNuNu A_NN = Ap.block<unknow_state_dimension, unknow_state_dimension>(state_dimension, state_dimension);
        const VectorNu ivp = b_IT.block<unknow_state_dimension, 1>(state_dimension, 0);
        const VectorNu b_NN   = bp.block<unknow_state_dimension, 1>(state_dimension, 0);

        //  Finally compute the states at the unknows Chebyshev points
        const VectorNu X_NN = (D_NN - A_NN).inverse() * (b_NN - ivp);

        //  We now stack together the initial state on top of the other we just compute
        //  Then we need to map back to a more readable stack of states
        const VectorNp X_tilde = P * (VectorNp() << t_initial_state, X_NN).finished();

        //  Then we write the element row-wise
        const MatrixNchebNs X_stack = Eigen::Map<const MatrixNchebNs>(X_tilde.data());

        return X_stack;

}

Eigen::MatrixXd IntegratePositions(const Eigen::Vector3d &t_initial_state,
                                   const Eigen::Matrix<double, ne*ne, 1> &t_qe,
                                   const Eigen::MatrixXd &t_quaternions_stack)
{
    /*  The state dimension and the number of nodes are known. The state dimension will not change
     *  (a quaternion will remain a quaternion, a twist will remain a twist ecc..) and the number of
     *  nodes are not changed at runtime and neither between runs. Typically, once a satisfactory number
     *  of nodes has been found, it is kept for the whole lifetime of the project.
     *
     *  Thus, we can enforce this concept with constexpr classifier so that the values are known at
     *  compile time and we can use templated function for our matrices and vector, which are a bit faster.
     */
        //  Dimension of the state
        constexpr unsigned int state_dimension = 3;

        //  Problem size is the total number of elements
        constexpr unsigned int prob_dimension = state_dimension * number_of_chebyshev_nodes;
        //  The subset of unknows in the problem
        constexpr unsigned int unknow_state_dimension = state_dimension * (number_of_chebyshev_nodes - 1);

    /*  These are a set of type definition in order to have a more neat algorithm in
     *  terms of matrix and vector dymensions
     */
        typedef Eigen::Matrix<double, prob_dimension, prob_dimension> MatrixNpNp;
        typedef Eigen::Matrix<double, prob_dimension, 1> VectorNp;

        typedef Eigen::Matrix<double, prob_dimension, state_dimension> MatrixNpNs;

        typedef Eigen::Matrix<double, unknow_state_dimension, unknow_state_dimension> MatrixNuNu;
        typedef Eigen::Matrix<double, unknow_state_dimension, 1> VectorNu;

        typedef Eigen::Matrix<double, number_of_chebyshev_nodes, number_of_chebyshev_nodes> MatrixNchebNcheb;

        typedef Eigen::Matrix<double, number_of_chebyshev_nodes, state_dimension, Eigen::ColMajor> MatrixNchebNs;

    MatrixNchebNs positions_stack;

    return positions_stack;
}

int main()
{
    tictoc tictoc;

    //  Const curvature strain field
    Eigen::Matrix<double, ne*na, 1> qe;
    //  Here we give some value for the strain
    qe <<   0,
            0,
            0,
            1.2877691307032,
           -1.63807499160786,
            0.437406679142598,
            0,
            0,
            0;

//  Let's deal with quaternion

    //  Define the initial state
    const Eigen::Vector4d initial_state(1, 0, 0, 0); // Quaternion case
    const auto ruaternions_stack = GetQuaternions(initial_state, qe);

//  Let's deal with positions
     const Eigen::Vector3d initial_position(0, 0, 0);
     const auto positions_stack = IntegratePositions(initial_position, qe, ruaternions_stack);

    Eigen::Quaterniond q;
    Eigen::Matrix3d R = q.toRotationMatrix();

    std::cout<< "X_stack = " << std::endl << ruaternions_stack <<std::endl << std::endl;

    writeToFile("Q_stack", ruaternions_stack);

    return 0;

    /*
     * Workflow
     *
     * 1) Read through, test some values qe Cheb points
     *
     * 2) Move the computations in main into a function IntegrateQuaternion (it returns quaternion stack)
     *
     * 3) Extend to positions (stack of r)
     *
     */

}

void writeToFile(std::string t_name,
                 const Eigen::MatrixXd &t_matrix,
                 std::string t_relative_path_from_build,
                 const Eigen::IOFormat &t_format)
{
    if(not t_relative_path_from_build.empty()){
        //  Ensure relative path ends with a backslash only if a path is given
        if(not t_relative_path_from_build.ends_with('/'))
            t_relative_path_from_build.append("/");
    }

    //  Ensure it ends with .csv
    if(t_name.find(".csv") == std::string::npos)
        t_name.append(".csv");

    //  The file will be created in the location given by the realtive path and with the given name
    const auto file_name_and_location = t_relative_path_from_build + t_name;

    //  Create file in given location with given name
    std::ofstream file(file_name_and_location.c_str());

    //  Put matrix in this file
    file << t_matrix.format(t_format);

    //  Close the file
    file.close();
 }


/*
 * @Description: ceres residual block for lidar frontend relative pose measurement
 * @Author: Ge Yao
 * @Date: 2020-11-29 15:47:49
 */
#ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_RELATIVE_POSE_HPP_
#define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_RELATIVE_POSE_HPP_

#include <ceres/ceres.h>

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

#include <sophus/so3.hpp>

#include "glog/logging.h"

namespace sliding_window {

class FactorPRVAGRelativePose : public ceres::SizedCostFunction<6, 15, 15> {
public:
	static const int INDEX_P = 0;
	static const int INDEX_R = 3;

  FactorPRVAGRelativePose(void) {};

  void SetMeasurement(const Eigen::VectorXd &m) {
		m_ = m;
	}

  void SetInformation(const Eigen::MatrixXd &I) {
    I_ = I;
  }

  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    //
    // parse parameters:
    //
    // a. pose i
    Eigen::Map<const Eigen::Vector3d>     pos_i(&parameters[0][INDEX_P]);
    Eigen::Map<const Eigen::Vector3d> log_ori_i(&parameters[0][INDEX_R]);
    const Sophus::SO3d                    ori_i = Sophus::SO3d::exp(log_ori_i);

    // b. pose j
    Eigen::Map<const Eigen::Vector3d>     pos_j(&parameters[1][INDEX_P]);
    Eigen::Map<const Eigen::Vector3d> log_ori_j(&parameters[1][INDEX_R]);
    const Sophus::SO3d                    ori_j = Sophus::SO3d::exp(log_ori_j);

    //
    // parse measurement:
    //
		const Eigen::Vector3d     &pos_ij = m_.block<3, 1>(INDEX_P, 0);
		const Eigen::Vector3d &log_ori_ij = m_.block<3, 1>(INDEX_R, 0);
    const Sophus::SO3d         ori_ij = Sophus::SO3d::exp(log_ori_ij);

    //
    // TODO: get square root of information matrix:
    //

    //
    // TODO: compute residual:
    //

    //
    // TODO: compute jacobians:
    //
    if ( jacobians ) {
      // compute shared intermediate results:

      if ( jacobians[0] ) {
        // implement computing:
      }

      if ( jacobians[1] ) {
        // implement computing:
      }
    }

    //
    // TODO: correct residual by square root of information matrix:
    //

    return true;
  }

private:
  static Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d &w) {
      Eigen::Matrix3d J_r_inv = Eigen::Matrix3d::Identity();

      double theta = w.norm();

      if ( theta > 1e-5 ) {
          Eigen::Vector3d k = w.normalized();
          Eigen::Matrix3d K = Sophus::SO3d::hat(k);

          J_r_inv = J_r_inv
                    + 0.5 * K
                    + (1.0 - (1.0 + std::cos(theta)) * theta / (2.0 * std::sin(theta))) * K * K;
      }

      return J_r_inv;
  }

  Eigen::VectorXd m_;
  Eigen::MatrixXd I_;
};

} // namespace sliding_window

#endif // LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_RELATIVE_POSE_HPP_


#pragma once

#include <Eigen/Core>
using namespace Eigen;

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

#include <samson/so3.hpp>

namespace samson::se3
{

    using Vector6dual = Matrix<dual, 6, 1, 0, 6, 1>;
    using Matrix4dual = Matrix<dual, 4, 4, 0, 4, 4>;

    auto se3(const Vector6dual &v) -> Matrix4dual
    {
        Matrix4dual X;
        X << 0.0, -v(2), v(1), v(3),
            v(2), 0.0, -v(0), v(4),
            -v(1), v(0), 0.0, v(5),
            0.0, 0.0, 0.0, 0.0;
        return X;
    }

    auto se3(const Matrix4dual &X) -> Vector6dual
    {
        Vector6dual v;
        v << X(2, 1), X(0, 2), X(1, 0), X(0, 3), X(1, 3), X(2, 3);
        return v;
    }

    auto exp(Vector6dual &x) -> Matrix4dual
    {
        using samson::so3::from_axis_angle;
        using samson::so3::so3;

        Vector3dual w = x.head<3>();
        Vector3dual v = x.tail<3>();

        dual theta = w.norm();
        Matrix4dual ret = Matrix4dual::Identity();
        if (theta == 0.0)
        {
            ret = Matrix4dual::Identity();
            ret.block<3, 1>(0, 3) = v;
            return ret;
        }

        Vector3dual k = w.normalized();
        dual sin_theta = sin(theta);
        dual one_minus_cos_theta = 1.0 - cos(theta);
        Matrix3dual K = so3(k);
        Matrix3dual KK = K * K;

        Matrix3dual I = Matrix3dual::Identity();

        ret.block<3, 3>(0, 0) = from_axis_angle(k, theta);
        ret.block<3, 1>(0, 3) = (I * theta + one_minus_cos_theta * K + (theta - sin_theta) * KK) * (v / theta);

        return ret;
    }

    auto exp(Matrix4dual &X) -> Matrix4dual
    {
        Vector6dual x = se3(X);
        return exp(x);
    }
} // namespace samson::se3