#include "alpha.h"

#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <numeric>

#include "operationVector.h" // ceci va chercher le fichier qui contient les operations sur les vecteurs

using namespace std;
using namespace boost::numeric::ublas;

matrix<double> Alpha(matrix<double> Obs, matrix<double> M, std::vector<double> pie) {

// 	Obs est la matrice des observations
// 	M est la matrice de transition
// 	pie est le vecteur de probabilite initiale

	const int m = Obs.size2(); // Nombre detat, de cases
	const int n = Obs.size1(); // Nombre dobservation, duree de la partie

//	matrix Alpha = matrix(NbLigne= n, NbCol= m);  Initialisation de la matric Alpha de tailler n*m
	matrix<double> matAlpha(n, m);
	auto alph = std::vector<double>(m, 0); // Declaration d'un vecteur de longueur m remplie de 0

	for (int i = 0; i < m; ++i) {
		alph[i] = pie[i] * Obs(0, i);
	}

	double log1surc = log(sum(alph)); // J'ai ajouter la fonction sum, qui fait la somme d'une matrice ou d'un vecteur

// 	alph = alph * (1 / std::accumulate(alph.begin(), alph.end(), 0));
	alph = scalarMultiplication(alph, 1.0 / sum(alph));

	for (int i = 0; i < m; ++i) {
		matAlpha(0, i) = log(alph[i]) + log1surc;
	}

	auto Alph = matrix<double>(1, m);

	for (int i = 0 ; i < m ; ++i) {
		Alph(0, i) = alph[i];
	}

	for (int i = 1; i < n ; ++i) { // J'ai changer la partie "i < n, ++j" pour "i < n ; ++i"

		Alph = prod(Alph, M); // prod est le produit matriciel

		for (int j = 0; j < m ; ++j) {
			Alph(0, j) = Alph(0, j) * Obs(i, j);
		}

		log1surc += log(sum(Alph));
		Alph /= sum(Alph);

		for (int j = 0; j < m ; ++j) {
			matAlpha(i, j) = log(Alph(0, j)) + log1surc;
		}

	}

	return matAlpha;

}


// The template and inlines for the -*- C++ -*- finite field classes.
// Initially implemented by Wai-Shing Luk <luk036@gmail.com>
//

/** @file include/GF.hpp
 *  This is a C++ Library header.
 */

#ifndef FUN_GF_HPP
#define FUN_GF_HPP 1

#include <boost/operators.hpp>
#include <cassert>

namespace fun {
/**
 * @defgroup GF Finite (Galois) Field
 * @ingroup arithmetic
 *
 * Classes and functions for (extended) finite field.
 * @{
 */

// Forward declarations.
template <unsigned int _p> struct GF;

/**
 *  finite (Galois) field.
 *
 *  @param  p  Prime number
 */
template <unsigned int _p = 3> struct GF : boost::field_operators<GF<_p>> {
  /// Default constructor.
  constexpr GF() noexcept : _m(0) {}

  ///  Unspecified parameters default to 0.
  explicit GF(unsigned int m) noexcept : _m(m) { assert(_m < _p); }

  // Lets the compiler synthesize the copy constructor
  // GF (const GF<_p>&);
  /// Copy constructor
  GF(const GF<_p> &s) noexcept : _m(s.value()) { assert(_m >= 0); }

  /// Return internal element of finite field.
  constexpr unsigned int value() const noexcept { return _m; }

  // Lets the compiler synthesize the assignment operator
  // GF<_p>& operator= (const GF<_p>&);
  /// Assign this finite field to finite field @a s.
  GF<_p> &operator=(const GF<_p> &s) {
    _m = s.value();
    return *this;
  }

  /// Add @a s to this finite field.
  GF<_p> &operator+=(const GF<_p> &s) {
    _m += s.value();
    _m %= _p;
    assert(_m < _p);
    return *this;
  }

  /// Subtract @a s from this finite field.
  GF<_p> &operator-=(const GF<_p> &s) {
    _m += _p - s.value();
    _m = _m % _p;
    assert(_m < _p);
    return *this;
  }

  /// Multiply @a s to this finite field.
  GF<_p> &operator*=(const GF<_p> &s) {
    _m *= s.value();
    _m %= _p;
    assert(_m < _p);
    return *this;
  }

private:
  unsigned int _m;
};

// Operators:
///  Return new finite field @a r plus @a s.
template <unsigned int _p> inline GF<_p> operator+(GF<_p> r, const GF<_p> &s) {
  return r += s;
}

///  Return new finite field @a r minus @a s.
template <unsigned int _p> inline GF<_p> operator-(GF<_p> r, const GF<_p> &s) {
  return r -= s;
}

//@{
///  Return new finite field @a r times @a a.
template <unsigned int _p> inline GF<_p> operator*(GF<_p> r, const GF<_p> &s) {
  return r *= s;
}
//@}

/// Return @a r.
template <unsigned int _p>
inline constexpr GF<_p> operator+(const GF<_p> &r) noexcept {
  return r;
}

/// Return negation of @a r
template <unsigned int _p> inline GF<_p> operator-(const GF<_p> &r) {
  if (r.value() == 0)
    return r;
  return GF<_p>(_p - r.value());
}

/// Return true if @a r is equal to @a s.
template <unsigned int _p>
inline constexpr bool operator==(const GF<_p> &r, const GF<_p> &s) noexcept {
  return r.value() == s.value();
}

/// Return false if @a r is equal to @a s.
template <unsigned int _p>
inline constexpr bool operator!=(const GF<_p> &r, const GF<_p> &s) noexcept {
  return !(r == s);
}

///  Insertion operator for finite field values.
template <unsigned int _p, class _Stream>
_Stream &operator<<(_Stream &os, const GF<_p> &r) {
  os << '(' << r.value() << " mod " << _p << ')';
  return os;
}
}

#endif


/** @file
 * @ingroup utils
 *
 * Utility mathematical functions that do not fit anywhere else.
 *
 * The type definitions are shorthands for declaring `Eigen::Vector`,
 * `Eigen::Matrix` and `Eigen::Quaternion` objects. At current we assume all
 * slam code development will be using double precision floating points.
 */

#ifndef WAVE_UTILS_MATH_HPP
#define WAVE_UTILS_MATH_HPP

#include <iostream>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace wave {
/** @addtogroup utils
 *  @{ */

#ifndef EIGEN_TYPEDEF
#define EIGEN_TYPEDEF
typedef Eigen::Matrix<double, 1, 1> Vec1;
typedef Eigen::Vector2d Vec2;
typedef Eigen::Vector3d Vec3;
typedef Eigen::Vector4d Vec4;
typedef Eigen::Matrix<double, 5, 1> Vec5;
typedef Eigen::Matrix<double, 6, 1> Vec6;
typedef Eigen::Matrix<double, 12, 1> Vec12;
typedef Eigen::VectorXd VecX;

typedef Eigen::Matrix2d Mat2;
typedef Eigen::Matrix3d Mat3;
typedef Eigen::Matrix4d Mat4;
typedef Eigen::Matrix<double, 5, 5> Mat5;
typedef Eigen::Matrix<double, 6, 6> Mat6;
typedef Eigen::Matrix<double, 12, 12> Mat12;
typedef Eigen::MatrixXd MatX;

typedef Eigen::Matrix<double, 3, 4> Mat34;

typedef Eigen::Matrix<float, 1, 1> Vec1f;
typedef Eigen::Vector2f Vec2f;
typedef Eigen::Vector3f Vec3f;
typedef Eigen::Vector4f Vec4f;
typedef Eigen::Matrix<float, 5, 1> Vec5f;
typedef Eigen::Matrix<float, 6, 1> Vec6f;
typedef Eigen::Matrix<float, 12, 1> Vec12f;
typedef Eigen::VectorXf VecXf;

typedef Eigen::Matrix2f Mat2f;
typedef Eigen::Matrix3f Mat3f;
typedef Eigen::Matrix4f Mat4f;
typedef Eigen::Matrix<float, 5, 5> Mat5f;
typedef Eigen::Matrix<float, 6, 6> Mat6f;
typedef Eigen::Matrix<float, 12, 12> Mat12f;
typedef Eigen::MatrixXf MatXf;

typedef Eigen::Matrix<float, 3, 4> Mat34f;

typedef Eigen::Affine3d Affine3;

typedef Eigen::Quaterniond Quaternion;
#endif

/**
 * Eigen vector comparator
 */
struct VecComparator {
    bool operator()(const VecX &a, const VecX &b) const {
        return std::lexicographical_compare(
          a.data(), a.data() + a.size(), b.data(), b.data() + b.size());
    }
};

/**
 * Eigen matrix comparator
 */
struct MatComparator {
    bool operator()(const MatX &a, const MatX &b) const {
        return std::lexicographical_compare(
          a.data(), a.data() + a.size(), b.data(), b.data() + b.size());
    }
};

/** Generates random integer with a upper bound `ub` and lower bound `lb` using
 * a uniform random distribution. */
int randi(int ub, int lb);

/** Generates random float with a upper bound `ub` and lower bound `lb` using a
 * uniform random distribution. */
double randf(double ub, double lb);

/** Compares two floating point numbers `f1` and `f2` with an error threshold
 * defined by `threshold`.
 * @return
 * - `0`: If `f1` == `f2` or the difference is less then `threshold`
 * - `1`: If `f1` > `f2`
 * - `-1`: If `f1` < `f2`
 */
int fltcmp(double f1, double f2, double threshold = 0.0001);

/** @return the median of `v`. */
double median(std::vector<double> v);

/** Converts degrees to radians. */
double deg2rad(double d);

/** Converts radians to degrees. */
double rad2deg(double r);

/** Reshapes a vector `x` to matrix `y` of size `rows` and `cols` */
void vec2mat(std::vector<double> x, int rows, int cols, MatX &y);

/** Reshapes a matrix to a vector*/
void mat2vec(MatX A, std::vector<double> &x);

/** Wraps `euler_angle` to 180 degrees **/
double wrapTo180(double euler_angle);

/** Wraps `euler_angle` to 360 degrees **/
double wrapTo360(double euler_angle);

/** Convert euler angle to rotation matrix **/
int euler2rot(Vec3 euler, int euler_seq, Mat3 &R);

/** Convert euler angle to quaternion **/
int euler2quat(Vec3 euler, int euler_seq, Quaternion &q);

/** Convert quaternion to euler angles **/
int quat2euler(Quaternion q, int euler_seq, Vec3 &euler);

/** Convert quaternion to rotation matrix **/
int quat2rot(Quaternion q, Mat3 &R);

/** ENU to NWU coordinate system **/
void enu2nwu(const Vec3 &enu, Vec3 &nwu);

/** NED to ENU coordinate system **/
void ned2enu(const Vec3 &ned, Vec3 &enu);

/** NED to NWU coordinate system **/
void ned2nwu(const Quaternion &ned, Quaternion &enu);

/** NWU to ENU coordinate system **/
void nwu2enu(const Vec3 &nwu, Vec3 &enu);

/** NWU to NED coordinate system **/
void nwu2ned(const Quaternion &nwu, Quaternion &ned);

/** NWU to EDN coordinate system **/
void nwu2edn(const Vec3 &nwu, Vec3 &edn);

/** @} group utils */
}  // namespace wave

#endif  // WAVE_UTILS_MATH_HPP


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

namespace ublas = boost::numeric::ublas;
/* Matrix inversion routine.
   Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */

bool InvertMatrix (const ublas::matrix<double>& input, ublas::matrix<double>& inverse)
{
  using namespace boost::numeric::ublas;
  typedef permutation_matrix<std::size_t> pmatrix;
  // create a working copy of the input
  matrix<double> A(input);
  // create a permutation matrix for the LU-factorization
  pmatrix pm(A.size1());
  // perform LU-factorization
  int res = lu_factorize(A,pm);
  if( res != 0 ) return false;
  // create identity matrix of "inverse"
  inverse.assign(ublas::identity_matrix<double>(A.size1()));
  // backsubstitute to get the inverse
  lu_substitute(A, pm, inverse);
  return true;
}

bool InvertMatrixGen(const ublas::matrix<double>& input, ublas::matrix<double>& inverse)
{
  const int n = input.size1();
  gsl_matrix *A = gsl_matrix_alloc(n,n);
  gsl_matrix *V = gsl_matrix_alloc(n,n);
  gsl_vector *S = gsl_vector_alloc(n);
  gsl_vector *work = gsl_vector_alloc(n);

  for(int i = 0; i < n; ++i) {
    for(int j = 0; j < n; ++j) {
      gsl_matrix_set(A,i,j,input(i,j));
      inverse(i,j) = 0.0;
    }
  }

  gsl_linalg_SV_decomp(A,V,S,work);

  const double smax = gsl_vector_get(S,0);
  for(int k = 0; k < n; ++k) {
    const double s = gsl_vector_get(S,k);
    if(s/smax > 1.0e-8) {
      for(int i = 0; i < n; ++i) {
	for(int j = 0; j < n; ++j) {
	  inverse(i,j) += gsl_matrix_get(V,i,k) * gsl_matrix_get(A,j,k) / s;
	}
      }
    }
  }

  gsl_matrix_free(A);
  gsl_matrix_free(V);
  gsl_vector_free(S);
  gsl_vector_free(work);
  return true;
}


#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_traits_2.h>
#include <CGAL/Hyperbolic_octagon_translation.h>

#include <boost/tuple/tuple.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/variate_generator.hpp>

#include <iostream>

typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_traits_2<>           Traits;
typedef Traits::FT                                                              NT;
typedef Traits::Point_2                                                         Point;
typedef Traits::Circle_2                                                        Circle;
typedef Traits::Euclidean_line_2                                                Line;
typedef Traits::Construct_intersection_2                                        Construct_intersection_2;

int main(int /*argc*/, char** /*argv*/)
{
  NT F2(2);

  Line ell1(NT(1),  sqrt(F2 - sqrt(F2)), NT(0));
  Line ell2(NT(1), -sqrt(F2 + sqrt(F2)), NT(0));

  // In the Poincaré disk, all Euclidean lines pass through the origin
  NT sx = NT(0);
  NT sy = NT(0);
  Point sol(sx, sy);

  Point p = Construct_intersection_2()(ell1, ell2);
  std::cout << "line-line intersection: " << p << ", solution is: " << sol << std::endl;
  assert(p == sol);
  std::cout << "The solution is exact!" << std::endl;

  Point pc1(NT(2)/NT(10), NT(5)/NT(10));
  Point pc2(-NT(3)/NT(10), NT(15)/NT(100));
  Point pc3(NT(20)/NT(29), NT(50)/NT(29));
  Circle c1(pc1, pc2, pc3);
  std::pair<Point, Point> ipt = Construct_intersection_2()(ell1, c1);
  Point ip1 = ipt.first, ip2 = ipt.second;

  NT sq(sqrt(NT(2)));
  NT nsq( sqrt( NT(2) - sqrt(NT(2)) ) );
  NT sol1x( nsq*(NT(2438)+NT(1451)*nsq-sqrt(NT(3933846)-NT(31801)*sq+NT(7075076)*nsq))/(-NT(4320)+NT(1440)*sq) );
  NT sol1y( (-NT(2438)-NT(1451)*nsq+sqrt(NT(3933846)-NT(31801)*sq+NT(7075076)*nsq))/(-NT(4320)+NT(1440)*sq) );
  Point solution1(sol1x, sol1y);

  std::cout << "Intersection of circle and line: " << ip1 << " and " << ip2 << std::endl;
  std::cout << "Solution: " << solution1 << std::endl;
  assert(ip1 == solution1 || ip2 == solution1);
  std::cout << "The solution is exact!" << std::endl;

  Point pc4(NT(2)/NT(10), -NT(15)/NT(100));
  Circle c2(pc1, pc4, pc3);

  std::pair<Point, Point> cpt = Construct_intersection_2()(c1, c2);
  ip1 = cpt.first;
  ip2 = cpt.second;
  std::cout << "Intersection of circles: " << ip1 << " and " << ip2 << std::endl;
  assert(ip1 == pc1 || ip2 == pc1);
  std::cout << "The solution is exact!" << std::endl;

  return EXIT_SUCCESS;
}


/*
 * DataSet.cpp
 *
 *  Created on: 2014/06/11
 *      Author: kryozahiro
 */

#include "DataSet.h"

#include <cfloat>
#include <boost/lexical_cast.hpp>
#include "cpputil/GenericIo.h"
using namespace std;
using namespace cpputil;

std::vector<double> DataSet::nguyen(int dimension, const std::vector<double>& input) {
	vector<double> output(1, 0);
	for (int i = 1; i <= dimension; ++i) {
		output[0] += pow(input[0], (double)i);
	}
	return output;
}

std::vector<double> DataSet::rosenbrock(const std::vector<double>& input) {
	vector<double> output(1, 0);
	for (unsigned int i = 0; i < input.size() - 1; ++i) {
		output[0] += 100.0 * pow(input[i + 1] - input[i] * input[i], 2.0) + pow(input[i] - 1.0, 2.0);
	}
	return output;
}

std::vector<double> DataSet::schwefel(const std::vector<double>& input) {
	vector<double> output(1, 418.9829 * input.size());
	for (unsigned int i = 0; i < input.size(); ++i) {
		output[0] -= input[i] * sin(sqrt(abs(input[i])));
	}
	return output;
}

std::vector<double> DataSet::sphere(const std::vector<double>& input) {
	vector<double> output(1, 0);
	for (unsigned int i = 0; i < input.size(); ++i) {
		output[0] += input[i] * input[i];
	}
	return output;
}

std::vector<double> DataSet::threeHumpCamel(const std::vector<double>& input) {
	vector<double> output(1, 0);
	output[0] = 2.0*input[0]*input[0] - 1.05*pow(input[0], 4.0) + pow(input[0], 6.0)/6.0 + input[0]*input[1] + input[1]*input[1];
	return output;
}

DataSet::DataSet() {
}

DataSet::DataSet(const ProgramType& programType) : programType(programType) {
}

DataSet::DataSet(const boost::property_tree::ptree& dataTree, mt19937_64& randomEngine) {
	string name = dataTree.get<string>("Name");
	int dimension = dataTree.get<int>("Dimension");
	int size = dataTree.get<int>("Size");
	if (name.find("Nguyen") != string::npos) {
		programType = ProgramType(DataType(-1, 1, 0.001, 1), DataType(DBL_MIN, DBL_MAX, 0.001, 1));
		addData(randomEngine, size, [=](const vector<double>& input) {
			return nguyen(dimension, input);
		});
	} else if (name.find("Rosenbrock") != string::npos) {
		programType = ProgramType(DataType(-5, 10, 0.001, dimension), DataType(DBL_MIN, DBL_MAX, 0.001, 1));
		addData(randomEngine, size, rosenbrock);
	} else if(name.find("Schwefel") != string::npos) {
		programType = ProgramType(DataType(-500, 500, 0.001, dimension), DataType(DBL_MIN, DBL_MAX, 0.001, 1));
		addData(randomEngine, size, schwefel);
	} else if (name.find("Sphere") != string::npos) {
		programType = ProgramType(DataType(-5.12, 5.12, 0.001, dimension), DataType(DBL_MIN, DBL_MAX, 0.001, 1));
		addData(randomEngine, size, sphere);
	} else if (name.find("ThreeHumpCamel") != string::npos) {
		programType = ProgramType(DataType(-5, 5, 0.001, 2), DataType(DBL_MIN, DBL_MAX, 0.001, 1));
		addData(randomEngine, size, threeHumpCamel);
	} else {
		assert(false);
	}
}

const ProgramType& DataSet::getProgramType() const {
	return programType;
}

int DataSet::getSize() const {
	return data.size();
}

pair<vector<double>, vector<double>>& DataSet::operator[](int i) {
	return data[i];
}

const pair<vector<double>, vector<double>>& DataSet::operator[](int i) const {
	return data[i];
}

vector<double>& DataSet::getInput(int i) {
	return data[i].first;
}

vector<double>& DataSet::getOutput(int i) {
	return data[i].second;
}

void DataSet::addData(const vector<double>& input, const vector<double>& output) {
	data.push_back(make_pair(input, output));
}

string DataSet::toString() const {
	string ret = "DataSet\n";
	for (const pair<vector<double>, vector<double>>& point : data) {
		ret += boost::lexical_cast<string>(point) + "\n";
	}
	return ret;
}


#include <gnuplot-iostream/gnuplot-iostream.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <vector>
using namespace std;

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

This is the program for the bifurcation diagram! :D
The flow is simple: For each r value between 1 and 4 (with
resolution of dr), 100 values of the logistic map are saved
in the solutions vector. The values are chosen far enough
away from the origin to remove the effects of the initial
behavior. The values are paired with the r value they were
calculated at and then sent to the plotter.

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

const int START = 900;
const int CUTOFF = 1000;	// Cutoff value for finding attractors
const double dr = 0.001;
const double x0 = 0.5;		// Initial value

void plotStuff(vector<double> x, vector<double> y){	// Pretty plotting shenanigans
	Gnuplot gp;

	gp << "set xrange [1:4]\n";
	gp << "set yrange [0:1]\n";
	gp << "set term wxt font \"FreeSerif,12\"\n";
	gp << "set title \"End Behavior of the Logistic Function vs. Growth Rate (x_0 = 0.5)\"\n";
	gp << "set xlabel \"r\"\n";
	gp << "set ylabel \"x_n\"\n";
	gp << "set output \"plot.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(x,y));
}

vector<double> getSolutions(double r){
	double x = x0;							// Set up x_0
	vector<double> out;						// All the solutions for this value of r

	for(int i = 0; i < CUTOFF; i++){		// Loop until the cutoff
		if(i >= START){out.push_back(x);}	// Push the x value onto the vector of values, but only after a few (900) iterations to observe only the long-term behavior
		x = r*x*(1-x);						// Perform the logistic map operation x_n+1 = r*x_n*(1-x_n)
	}

	return out;
}

int main(){
	vector<double> rvals;					// R values (there will be lots of repeats since each r value has 100 solutions)
	vector<double> solutions;				// Solutions

	for(double r = 1; r < 4; r += dr){		// between r=1 and r=4
		vector<double> s = getSolutions(r);	// Get the solutions for that value of r

		for(int i = 0; i < s.size(); i++){	// Go through all solutions
			solutions.push_back(s[i]);		// Add them to the main list
			rvals.push_back(r);				// Along with the value of r they were recorded at
		}
	}

	cout << "Done with calculation!\n";		// This is for debug

	plotStuff(rvals,solutions);				// Plot it!
}


/*
 * Main.cpp
 *
 *  Created on: Nov 10, 2015
 *      Author: doncole
 */

#include <stdio.h>
#include <iostream>
#include <Eigen>
#include <Dense>

using namespace std;
using namespace Eigen;

ArrayXXf InitMatrix(ArrayXXf matrix);
ArrayXf InitVector(ArrayXf vector);
ArrayXXf AddRowToMatrix(ArrayXf rowA, ArrayXXf matrixU, int i);
ArrayXf AddNumBToVectorB(float numB,ArrayXf vectorB, int j);

int main(void){

	int squareNum = 7;	//square matrix 7x7 and vector = 7

	ArrayXXf matrixA(squareNum,squareNum);
	ArrayXXf matrixU(squareNum,squareNum);
	ArrayXf vectorB(squareNum);
	ArrayXf rowA(squareNum), rowB(squareNum), solutionX(squareNum);
	float numA, numB;
	//Init the Matrix and Vector
	matrixA = InitMatrix(matrixA);
	vectorB = InitVector(vectorB);
	matrixU = matrixA;

	cout << "Matrix A " << endl << matrixA << endl;
	cout << "Vector B " << endl << vectorB << endl;
	//Solve by G.E.
	//iterate squareNum-1 times
	for (int i = 0; i < squareNum -1; i++)
	{
		rowA = matrixU.row(i);	//Get the row
		numA = rowA[i];	//Get the Rows first number
		rowA = rowA/numA; 	//divide the row by the number
		numB = vectorB[i]/numA;	//divide the Vector by the number
		//numB = vectorB[i];	//copy the number for the rest of the vector

		for (int j = i+1; j< squareNum; j++)
		{
			rowB = rowA;	//get the beginning Row again
//			cout << "Matrix U" << endl << matrixU << endl;	//print the Matrix out
			//Multiply the Row by the Next row's first Integer
			//get the next rows first integer
//			cout << "The next Rows integer" << matrixU.row(j)[i] << endl;
			rowB = rowB * (-1.0) * matrixU.row(j)[i];
			numB = (numB * (-1.0) * matrixU.row(j)[i]);
//			cout << "The num B " << endl << numB << endl;
//			cout << "The row multiplied By" << endl << rowB << endl;
			//Add the row to the next Rows
			matrixU = AddRowToMatrix(rowB, matrixU, j);		//add the row to the rest of the matrix
			vectorB = AddNumBToVectorB(numB, vectorB, j);	//add the number to the rest of the vector
		}
	}
	solutionX[squareNum-1] = vectorB[squareNum-1]/matrixU.row(squareNum-1)[squareNum-1];
//	cout << "Solution X" << endl << solutionX << endl;

	for (int i = squareNum-2; i>-1; i--)
	{
		float sum = 0;
//		cout << "VectorB" << endl << vectorB[i] << endl;
//		matrixU.row(i)/matrixU.row(i)[i];
//		vectorB[i] = vectorB[i]/matrixU.row(i)[i];
//		cout << "Matrix U" << endl << matrixU << endl;
//		cout << "Vector B" << endl << vectorB << endl;

		for (int j = 6; j > i; j--)
		{
			sum = sum + matrixU.row(i)[j] * solutionX[i+1];
			//cout << "sum" << endl << sum << endl;
		}
		solutionX[i] = (vectorB[i] - sum)/matrixU.row(i)[i];
//		cout << "Solution X" << endl << solutionX << endl;
	}
	//Print the outputs
	cout << "Matrix U " << endl << matrixU << endl;
	cout << "Vector B " << endl << vectorB << endl;
	cout << "The Solution Matrix " << endl << solutionX << endl;

	return 0;
}

ArrayXf AddNumBToVectorB(float numB, ArrayXf vectorB, int row)
{
	//vectorB[0] = 100.0;
	while (row < 7)
	{
		vectorB[row] = vectorB[row] + numB;
		row++;
	}
	//print the Vector
	//cout << "VectorB" << endl << vectorB << endl;
	return vectorB;
}

ArrayXXf AddRowToMatrix(ArrayXf rowA, ArrayXXf matrixU, int row)
{
	//we need to construct the matrices
	//By first adding the row to the rest of the matrix
//	ArrayXXf tempArray(7, 7);
//	tempArray = matrixU.array();
	while (row < 7)
	{
		//add to the rest of the rows
		matrixU.row(row) += rowA;
		row++;
	}
	//print the array
	//cout << "MatrixU" << endl << matrixU << endl;
	return matrixU;
}

//Initialize the Given 7x7 Matrix
ArrayXXf InitMatrix(ArrayXXf matrix)
{
	int i= 0, j = 0;
	//First Row
	matrix(i, j++) = .1897;
	matrix(i, j++) = .3784;
	matrix(i, j++) = .6449;
	matrix(i, j++) = .7271;
	matrix(i, j++) = .4449;
	matrix(i, j++) = .1730;
	matrix(i, j++) = .0118;
	j = 0;
	i++;
	//Second Row
	matrix(i, j++) = .1934;
	matrix(i, j++) = .8600;
	matrix(i, j++) = .8180;
	matrix(i, j++) = .3093;
	matrix(i, j++) = .6946;
	matrix(i, j++) = .9797;
	matrix(i, j++) = .8939;
	j = 0;
	i++;
	//Third Row
	matrix(i, j++) = .6822;
	matrix(i, j++) = .8537;
	matrix(i, j++) = .6602;
	matrix(i, j++) = .8385;
	matrix(i, j++) = .6213;
	matrix(i, j++) = .2714;
	matrix(i, j++) = .1991;
	j = 0;
	i++;
	//FourthRow
	matrix(i, j++) = .3028;
	matrix(i, j++) = .5936;
	matrix(i, j++) = .3420;
	matrix(i, j++) = .5681;
	matrix(i, j++) = .7948;
	matrix(i, j++) = .2523;
	matrix(i, j++) = .2987;
	j = 0;
	i++;
	//Fifth Row
	matrix(i, j++) = .5417;
	matrix(i, j++) = .4966;
	matrix(i, j++) = .2897;
	matrix(i, j++) = .3704;
	matrix(i, j++) = .9568;
	matrix(i, j++) = .8757;
	matrix(i, j++) = .6614;
	j = 0;
	i++;
	//6th Row
	matrix(i, j++) = .1509;
	matrix(i, j++) = .8998;
	matrix(i, j++) = .3412;
	matrix(i, j++) = .7027;
	matrix(i, j++) = .5226;
	matrix(i, j++) = .7373;
	matrix(i, j++) = .2844;
	j = 0;
	i++;
	//7th Row
	matrix(i, j++) = .6979;
	matrix(i, j++) = .8216;
	matrix(i, j++) = .5341;
	matrix(i, j++) = .5466;
	matrix(i, j++) = .8801;
	matrix(i, j++) = .1365;
	matrix(i, j++) = .4692;

	return matrix;
}

ArrayXf InitVector(ArrayXf vector){
	int i = 0;
	vector(i++) = .5000;
	vector(i++) = -.3000;
	vector(i++) = -.2000;
	vector(i++) = .7000;
	vector(i++) = -.9000;
	vector(i++) = .3000;
	vector(i++) = .1000;
	return vector;
}


/* Last Revised Date: 13_DEC_2014 */
/* Lastest status: All ok    */

/*
  det_model_hh_post.hpp : implimentation of HH system (similar but not exact for the classical HH equations)
  !!!NOTE: all units are in SI system

  Copyright (C) 2015 Anup Gopalakrishna Pillai, Suhita Nadkarni Lab, IISER Pune <anupgpillai@gmail.com>

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

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

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

#ifndef DET_MODEL_HH_POST_HPP_INCLUDED
#define DET_MODEL_HH_POST_HPP_INCLUDED

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

//UNITS: all SI units - seconds, Volts, Ampere, Meters, Simenes, Farads

//DECLARATIONS FOR HH_POST_MODEL

class DET_MODEL_HH_POST
{
private:
  const double pi = boost::math::constants::pi<double>();
  double Cm = 1.0E-02;   // F/m^2 Specific capacitance
  //----Reversal potentials for Na, K, channels in mV
  double ENa = 50.0E-03;   // Reversal potential for Sodium currents in Volts
  double EK = -85.0E-03;    // Reversal potential for Potassium currents in Volts
  double ELeak = -81E-03; // Reversal potential for Leak currents in Volts

  double GNa = 130E01;   // Maximum conductance for Sodium channels S/m^2
  double GK = 36E01;    // Maximum conductance for Potassium channels S/m^2
  double GLeak = 0.3E01;  // Maximum conductance for Leak channels S/m^2

public:
  std::vector<double> X;

  double IExt = 0.0; //extern current in Amps
  double Area = 0.0;
  double Cap = 0.0;
  double V = 0.0;      // Membrane voltage V
  /* NEURON class explicit constructor */
  explicit DET_MODEL_HH_POST( std::vector<double> X_ = {0,0,0,0},double Area_ = 8.5E-12): Area(Area_), X(X_), Cap(Cm * Area) // Area in m^2
  {
  };
  //---Equations for bouton Na Channel (Ref: Dominique Engel & Peter Jonas, 2005, Neuron)
  double alpha_Na_act(const double V);
  double beta_Na_act(const double V);
  double alpha_Na_inact(const double V);
  double beta_Na_inact(const double V);
  //---Equations for bouton K Channel (Ref: Dominique Engel & Peter Jonas, 2005, Neuron)
  double alpha_K_act(const double V);
  double beta_K_act(const double V);
  //---Declaration of the ODE solver
  template <class State >
  void operator() ( const State &x, State &dxdt , const double  t );
};
//Function declarations
//------------- Na+ channels --- implimented from classical HH (1952)
double DET_MODEL_HH_POST::alpha_Na_act(const double V)
{
  double a = 0.1E03;
  double b = 45.33E-03;
  double c = 10E-03;
  double val = 0.0;
  val = (- a * ( (V + b) / (exp( -(V + b) / c ) - 1 ) ) );
  return (val);
}
double DET_MODEL_HH_POST::beta_Na_act(const double V)
{
  double a = 4;
  double b = 70.33E-03;
  double c = 18E-03;
  double val = 0.0;
  val = ( a * ( exp( - (V + b) / c )) );
  return (val);
}
double DET_MODEL_HH_POST::alpha_Na_inact(const double V)
{
  double val = 0.0;
  val = 0.07 * exp( -( V + 70.33E-03) /20E-03 );
  return (val);
}
double DET_MODEL_HH_POST::beta_Na_inact(const double V)
{
  double val = 0.0;
  val = ( 1 / ( exp( - (V + 40.33E-03) / 10E-03) + 1) );
  return (val);
}
//--------K+ channel alpha & beta
double DET_MODEL_HH_POST::alpha_K_act(const double V)
{
  double val = 0;
  val = -0.01E03 * (V + 60.33E-03) / ( exp( -(V +  60.33E-03) / 10E-03) - 1 );
  return (val);
};
double DET_MODEL_HH_POST::beta_K_act(const double V)
{
  double val = 0.0;
  val = 0.125 * exp (- (V + 70.33E-03) / 80E-03);
  return (val);
};
//-------HH_PRE class ODE Function

template <class State>
void DET_MODEL_HH_POST::operator() ( const State &x, State &dxdt , const double t )
{

  dxdt[0] = 1E03 * (( alpha_Na_act(x[3]) * (1 - x[0]) ) - (beta_Na_act(x[3]) * x[0]) ); // Na activation
  dxdt[1] = 1E03 * ( ( alpha_Na_inact(x[3]) * (1 - x[1]) ) - (beta_Na_inact(x[3]) * x[1])) ; // Na inactivation
  dxdt[2] = 1E03 * ( ( alpha_K_act(x[3]) * (1 - x[2]) ) - (beta_K_act(x[3]) * x[2])); // K activation
  double INa =   -(Area * GNa * pow(x[0],3) * x[1] * (x[3] - ENa) );
  double IK =   -(Area * GK * pow(x[2],4) * (x[3] - EK) );
  double ILeak = -(Area * GLeak *  (x[3] - ELeak) );
  dxdt[3] =  (1/Cap) * (INa + IK + ILeak  + (IExt * Area)  );
};
#endif // DET_MODEL_HH_POST_HPP_INCLUDED


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearfeelementmatrix.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>

namespace ElementMatrixComputation {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 4, 4> MyLinearFEElementMatrix::Eval(
    const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());
  // Matrix for returning element matrix
  Eigen::Matrix<double, 4, 4> elem_mat;

  //====================

  // Get element matrix for negative laplacian of given cell
  lf::uscalfe::LinearFELaplaceElementMatrix prov;
  Eigen::MatrixXd A = prov.Eval(cell);

  // First get the area
  double K = lf::geometry::Volume(*geo_ptr);

  // Initialize elem. stiffness matrix
  Eigen::Matrix<double, 4, 4> M;

  if (ref_el == lf::base::RefEl::kTria()) {
    M << 2.0, 1.0, 1.0, 0.0,
         1.0, 2.0, 1.0, 0.0,
         1.0, 1.0, 2.0, 0.0,
         0.0, 0.0, 0.0, 0.0;
    M *= K/12.0;
  } else if (ref_el == lf::base::RefEl::kQuad()) {
    M << 4.0, 2.0, 1.0, 2.0,
         2.0, 4.0, 2.0, 1.0,
         1.0, 2.0, 4.0, 2.0,
         2.0, 1.0, 2.0, 4.0;
    M *= K/36.0;
  }

  elem_mat = A + M;

  //====================

  return elem_mat;
}
/* SAM_LISTING_END_1 */
}  // namespace ElementMatrixComputation


/**
   \file conjugate_gradient.hpp
   \brief powerll optimization method
   \author Junhua Gu
 */

#ifndef CONJUGATE_GRADIENT
#define CONJUGATE_GRADIENT
#define OPT_HEADER
#include <core/optimizer.hpp>
//#include <blitz/array.h>
#include <limits>
#include <cassert>
#include <cmath>
#include "../linmin/linmin.hpp"
#include <math/num_diff.hpp>
#include <algorithm>
#include <iostream>

namespace opt_utilities
{
  /**
     \brief Impliment of an optimization method
     \tparam rT return type of the object function
     \tparam pT parameter type of the object function
   */
  template <typename rT,typename pT>
  class conjugate_gradient
    :public opt_method<rT,pT>
  {
  public:
    typedef pT array1d_type;
    typedef rT T;
  private:
    func_obj<rT,pT>* p_fo;
    optimizer<rT,pT>* p_optimizer;
    volatile bool bstop;
    //typedef blitz::Array<rT,2> array2d_type;

    const char* do_get_type_name()const
    {
      return "conjugate gradient method";
    }
  private:
    array1d_type start_point;
    array1d_type end_point;

  private:
    rT threshold;
    array1d_type g;
    array1d_type h;
    array1d_type xi;
  private:
    rT func(const pT& x)
    {
      assert(p_fo!=0);
      return p_fo->eval(x);
    }

  private:
    void clear_xi()
    {
    }

    void init_xi(int n)
    {
      clear_xi();
      g=array1d_type(n);
      h=array1d_type(n);
      xi=array1d_type(n);
    }

    void cg(array1d_type& p,const T ftol,
	   int& iter,T& fret)
    {
      const int ITMAX=200;
      const T EPS=std::numeric_limits<T>::epsilon();

      int j,its;
      int n=p.size();
      T gg,gam,fp,dgg;
      fp=func(p);
      xi=gradient(*p_fo,p);
      for(j=0;j<n;++j)
	{
	  g[j]=-xi[j];
	  xi[j]=h[j]=g[j];
	}
      for(its=1;its<=ITMAX;++its)
	{
	  iter=its;
	  linmin(p,xi,fret,*p_fo);
	  //std::cerr<<"######:"<<its<<"\t"<<abs(fret-fp)/(abs(fret)+fabs(fp)+EPS)<<std::endl;
	  if(2.0*abs(fret-fp)<=ftol*(abs(fret)+fabs(fp)+EPS))
	    {
	      return;
	    }
	    fp=func(p);
	    xi=gradient(*p_fo,p);
	    dgg=gg=0;
	    for(j=0;j<n;++j)
	      {
		gg+=g[j]*g[j];
		//dgg+=(xi[j]+g[j])*xi[j];
		dgg+=xi[j]*xi[j];
	      }
	    //std::cerr<<its<<"\t"<<gg<<std::endl;
	    if(gg==0.0)
	      {
		return;
	      }
	    gam=dgg/gg;
	    for(j=0;j<n;++j)
	      {
		g[j]=-xi[j];
		xi[j]=h[j]=g[j]+gam*h[j];
	      }
	}
      //std::cerr<<"Too many iterations in cg"<<std::endl;
    }

  public:

    conjugate_gradient()
      :threshold(1e-4),g(0),h(0),xi(0)
    {}

    virtual ~conjugate_gradient()
    {
      clear_xi();
    };

    conjugate_gradient(const conjugate_gradient<rT,pT>& rhs)
      :opt_method<rT,pT>(rhs),p_fo(rhs.p_fo),p_optimizer(rhs.p_optimizer),
       start_point(rhs.start_point),
       end_point(rhs.end_point),
       threshold(rhs.threshold),g(0),h(0),xi(0)
    {
    }

    conjugate_gradient<rT,pT>& operator=(const conjugate_gradient<rT,pT>& rhs)
    {
      threshold=rhs.threshold;
      p_fo=rhs.p_fo;
      p_optimizer=rhs.p_optimizer;
      start_point=rhs.start_point;
      end_point=rhs.end_point;
      threshold=rhs.threshold;
    }

    opt_method<rT,pT>* do_clone()const
    {
      return new conjugate_gradient<rT,pT>(*this);
    }

    void do_set_start_point(const array1d_type& p)
    {
      resize(start_point,get_size(p));
      opt_eq(start_point,p);
    }

    array1d_type do_get_start_point()const
    {
      return start_point;
    }

    void do_set_lower_limit(const array1d_type& p)
    {}

    void do_set_upper_limit(const array1d_type& p)
    {}

    void do_set_precision(rT t)
    {
      threshold=t;
    }

    rT do_get_precision()const
    {
      return threshold;
    }

    void do_set_optimizer(optimizer<rT,pT>& o)
    {
      p_optimizer=&o;
      p_fo=p_optimizer->ptr_func_obj();
    }

    pT do_optimize()
    {
      bstop=false;
      init_xi((int)get_size(start_point));

      int iter=100;
      opt_eq(end_point,start_point);
      rT fret;
#if 0
      for(int i=0;i<get_size(start_point);++i)
	{
	  array1d_type direction(start_point.size());
	  direction[i]=1;
	  linmin(end_point,direction,fret,(*p_fo));
	}
#endif
      cg(end_point,threshold,iter,fret);
      return end_point;
    }

    void do_stop()
    {
      bstop=true;
    }

  };

}

#endif
//EOF


/// \file
// --------------------------------------------------------------------------
// This file is part of the reference implementation for the paper
//    QFib: Fast and Efficient Brain Tractogram Compression
//    C. Mercier*, S. Rousseau*, P. Gori, I. Bloch and T. Boubekeur
//    NeuroInformatics 2020
//    DOI: 10.1007/s12021-020-09452-0
//
// All rights reserved. Use of this source code is governed by a
// MIT license that can be found in the LICENSE file.
// --------------------------------------------------------------------------
#pragma once

#include "saveload.hpp"
#include <fstream>
#include <iostream>
#include <limits>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Geometry>
#include <Eigen/Dense>

using namespace Eigen;

using namespace std;

namespace vc
{
int my_round(const unsigned type, const float value)
{
	switch (type) {
	case 0:
		return floor(value);
		break;
	case 1:
		return ceil(value);
		break;
	default:
		return round(value);
		break;
	}
}

///
/// \brief computeTrilinearInterpolatedValue Compute an interpolated value at the position, considering the grid
/// \param grid Grid to consider for the interpolated value
/// \param dim Dimension of the grid
/// \param gridPosition Position at which to compute the interpolated value
/// \return Return the computed value
///

float computeTrilinearInterpolatedValue(std::vector<float> const & grid, std::vector<unsigned> const & dim, Vector3f const & gridPosition)
{
	std::vector<float> value;
	for (unsigned pos2=0; pos2<2; pos2++)
		for (unsigned pos3=0; pos3<2; pos3++)
			for (unsigned pos1=0; pos1<2; pos1++)
				value.push_back(grid[(int)my_round(pos1,gridPosition(0))+dim[0]*(int)my_round(pos2,gridPosition(1))+dim[0]*dim[1]*(int)my_round(pos3,gridPosition(2))]);
	float xd = gridPosition(0) - floor(gridPosition(0));
	float yd = gridPosition(1) - floor(gridPosition(1));
	float zd = gridPosition(2) - floor(gridPosition(2));

	float c00 = value[0]*(1-xd)+value[1]*xd;
	float c01 = value[2]*(1-xd)+value[3]*xd;
	float c10 = value[4]*(1-xd)+value[5]*xd;
	float c11 = value[6]*(1-xd)+value[7]*xd;

	float c0 = c00*(1-yd)+c10*yd;
	float c1 = c01*(1-yd)+c11*yd;

	float val = c0*(1-zd)+c1*zd;

	return val;
}

//positive filter
int getSign(const float value)
{
	if (value>0)
		return 1;
	else
		return 0;
}

float k(const float x, const float dirX, const float dimV)
{
	return (getSign(dirX) + round(x/dimV)) * dimV;
}

float k2(const float x, const float dirX)
{
	return floor(x) + getSign(dirX);
}

///
/// \brief getValueFromSegment Compute the value along a segment
/// \param grid Grid to consider
/// \param dim Dimensions of the grid
/// \param p1 First point of the segment
/// \param p2 Second point of the segment
/// \return Return the value of the segment
///

float getValueFromSegment(std::vector<float> const & grid, std::vector<unsigned> const & dim, Vector3f const & p1, Vector3f const & p2)
{
	Vector3f direction = p2-p1;
	Vector3f directionN = direction.normalized();
	float totalDist = direction.norm();
	//Number of voxels changes along each axis
	unsigned voxelsX = static_cast<unsigned>(fabs(floor(p2.x())-floor(p1.x())));
	unsigned voxelsY = static_cast<unsigned>(fabs(floor(p2.y())-floor(p1.y())));
	unsigned voxelsZ = static_cast<unsigned>(fabs(floor(p2.z())-floor(p1.z())));
	unsigned totalVoxelsCrossed = 1 + voxelsX + voxelsY + voxelsZ;
	Vector3f pTemp = p1;
	float value = 0;
	float epsilon = 0.0001f;
	float gammaX, gammaY, gammaZ, gamma;

	for (unsigned i=0; i<totalVoxelsCrossed - 1; i++)
	{
		if (fabs(directionN.x())<epsilon)
			gammaX =  numeric_limits<float>::max();
		else
			gammaX = (k2(pTemp.x(), direction.x()) - pTemp.x()) / direction.x();
		if (fabs(directionN.y())<epsilon)
			gammaY =  numeric_limits<float>::max();
		else
			gammaY = (k2(pTemp.y(), direction.y()) - pTemp.y()) / direction.y();
		if (fabs(directionN.z())<epsilon)
			gammaZ =  numeric_limits<float>::max();
		else
			gammaZ = (k2(pTemp.z(), direction.z()) - pTemp.z()) / direction.z();
		gamma = min(min(gammaX, gammaY), gammaZ);
		pTemp += (gamma) * direction;
		value += (grid[(int)floor(pTemp.x())+dim[0]*(int)floor(pTemp.y())+(dim[0]*dim[1]*(int)floor(pTemp.z()))]);
	}
	value += (grid[(int)floor(p2.x())+dim[0]*(int)floor(p2.y())+(dim[0]*dim[1]*(int)floor(p2.z()))]);
	return (value / (float)(totalVoxelsCrossed));
}

///
/// \brief compareFA Function to compare the FA values and display the results
/// \param fibers1 First set of fibers
/// \param fibers2 Second set of fibers
/// \param faFilename FA file to use for FA computation
///

void compareFA(std::vector<std::vector<Vector3f>> const & fibers1, std::vector<std::vector<Vector3f>> const & fibers2, const string & faFilename)
{
	cout << faFilename << endl;
	std::vector<float> faValues, vox;
	std::vector<unsigned> dim;
	MatrixXf transform;
	fls::loadFAValues(faFilename, faValues, dim, transform, vox);

	transform.conservativeResize(4,4);
	transform.row(3) << 0,0,0,1;
	transform = transform.inverse();

	unsigned maxThreads = std::max(unsigned(1), std::thread::hardware_concurrency());
	std::vector<float> ecartMoyenTab(maxThreads, 0);
	std::vector<uint64_t> nbPointsTab(maxThreads, 0);
	std::vector<float> ecartMaxTab(maxThreads, 0);
	std::vector<float> averageFiber1Error(maxThreads, 0);
	std::vector<float> averageFiber2Error(maxThreads, 0);
#ifdef USE_OPENMP
#pragma omp parallel for
	for (unsigned i=0; i<fibers1.size(); i++)
	{
		if (fibers1[i].size() == fibers2[i].size())
		{
			nbPointsTab[omp_get_thread_num()] += fibers1[i].size();
			for (unsigned j=0; j<fibers1[i].size(); j++)
			{
				Vector3f p = fibers1[i][j];
				for (unsigned k=0; k<3; k++)
					p(k)/=vox[k];
				Vector3f gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
				float value1 = computeTrilinearInterpolatedValue(faValues, dim, gridPosition);
				Vector3f p2 = fibers2[i][j];
				for (unsigned k=0; k<3; k++)
					p2(k)/=vox[k];
				Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
				float value2=computeTrilinearInterpolatedValue(faValues, dim, gridPosition2);
				float ecart = fabs(value1-value2);
				ecartMaxTab[omp_get_thread_num()] = max(ecart, ecartMaxTab[omp_get_thread_num()]);
				ecartMoyenTab[omp_get_thread_num()]+=ecart;
			}
		}

		//Per fiber error
		float fiber1Value = 0.f;
		float fiber2Value = 0.f;
		Vector3f p = fibers1[i][0];
		for (unsigned k=0; k<3; k++)
		{
			p(k)/=vox[k];
		}
		Vector3f gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
		fiber1Value += faValues[(int)floor(gridPosition.x())+dim[0]*(int)floor(gridPosition.y())+(dim[0]*dim[1]*(int)floor(gridPosition.z()))];
		for (unsigned j=0; j<fibers1[i].size()-1; j++)
		{
			Vector3f p1 = fibers1[i][j];
			Vector3f p2 = fibers1[i][j+1];
			for (unsigned k=0; k<3; k++)
			{
				p1(k)/=vox[k];
				p2(k)/=vox[k];
			}
			Vector3f gridPosition1 = (p1*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			fiber1Value += getValueFromSegment(faValues, dim, gridPosition1, gridPosition2);
		}
		p = fibers2[i][0];
		for (unsigned k=0; k<3; k++)
		{
			p(k)/=vox[k];
		}
		gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
		fiber2Value += faValues[(int)floor(gridPosition.x())+dim[0]*(int)floor(gridPosition.y())+(dim[0]*dim[1]*(int)floor(gridPosition.z()))];
		for (unsigned j=0; j<fibers2[i].size()-1; j++)
		{
			Vector3f p1 = fibers2[i][j];
			Vector3f p2 = fibers2[i][j+1];
			for (unsigned k=0; k<3; k++)
			{
				p1(k)/=vox[k];
				p2(k)/=vox[k];
			}
			Vector3f gridPosition1 = (p1*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			fiber2Value += getValueFromSegment(faValues, dim, gridPosition1, gridPosition2);
		}
		fiber1Value /= float(fibers1[i].size());
		fiber2Value /= float(fibers2[i].size());
		averageFiber1Error[omp_get_thread_num()] += fiber1Value;
		averageFiber2Error[omp_get_thread_num()] += fiber2Value;
	}
#else
	parallel.For<unsigned>(0, fibers1.size(), 1, [&](unsigned i)
	{
		if (fibers1[i].size() == fibers2[i].size())
		{
			nbPointsTab[parallel.getID()] += fibers1[i].size();
			for (unsigned j=0; j<fibers1[i].size(); j++)
			{
				Vector3f p = fibers1[i][j];
				for (unsigned k=0; k<3; k++)
					p(k)/=vox[k];
				Vector3f gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
				float value1 = computeTrilinearInterpolatedValue(faValues, dim, gridPosition);
				Vector3f p2 = fibers2[i][j];
				for (unsigned k=0; k<3; k++)
					p2(k)/=vox[k];
				Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
				float value2=computeTrilinearInterpolatedValue(faValues, dim, gridPosition2);
				float ecart = fabs(value1-value2);
				ecartMaxTab[parallel.getID()] = max(ecart, ecartMaxTab[parallel.getID()]);
				ecartMoyenTab[parallel.getID()]+=ecart;
			}
		}

		//Per fiber error
		float fiber1Value = 0.f;
		float fiber2Value = 0.f;
		Vector3f p = fibers1[i][0];
		for (unsigned k=0; k<3; k++)
		{
			p(k)/=vox[k];
		}
		Vector3f gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
		fiber1Value += faValues[(int)floor(gridPosition.x())+dim[0]*(int)floor(gridPosition.y())+(dim[0]*dim[1]*(int)floor(gridPosition.z()))];
		for (unsigned j=0; j<fibers1[i].size()-1; j++)
		{
			Vector3f p1 = fibers1[i][j];
			Vector3f p2 = fibers1[i][j+1];
			for (unsigned k=0; k<3; k++)
			{
				p1(k)/=vox[k];
				p2(k)/=vox[k];
			}
			Vector3f gridPosition1 = (p1*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			fiber1Value += getValueFromSegment(faValues, dim, gridPosition1, gridPosition2);
		}
		p = fibers2[i][0];
		for (unsigned k=0; k<3; k++)
		{
			p(k)/=vox[k];
		}
		gridPosition = (p*transform.block(0,0,3,3)+transform.block(0,3,3,1));
		fiber2Value += faValues[(int)floor(gridPosition.x())+dim[0]*(int)floor(gridPosition.y())+(dim[0]*dim[1]*(int)floor(gridPosition.z()))];
		for (unsigned j=0; j<fibers2[i].size()-1; j++)
		{
			Vector3f p1 = fibers2[i][j];
			Vector3f p2 = fibers2[i][j+1];
			for (unsigned k=0; k<3; k++)
			{
				p1(k)/=vox[k];
				p2(k)/=vox[k];
			}
			Vector3f gridPosition1 = (p1*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			Vector3f gridPosition2 = (p2*transform.block(0,0,3,3)+transform.block(0,3,3,1));
			fiber2Value += getValueFromSegment(faValues, dim, gridPosition1, gridPosition2);
		}
		fiber1Value /= float(fibers1[i].size());
		fiber2Value /= float(fibers2[i].size());
		averageFiber1Error[parallel.getID()] += fiber1Value;
		averageFiber2Error[parallel.getID()] += fiber2Value;
	});
#endif

	float ecartMoyen = ecartMoyenTab[0], ecartMax = ecartMaxTab[0];
	float averageFiber1 = averageFiber1Error[0], averageFiber2 = averageFiber2Error[0];
	uint64_t nbPoints = nbPointsTab[0];
	for (unsigned i=1; i<maxThreads; i++)
	{
		nbPoints += nbPointsTab[i];
		ecartMoyen += ecartMoyenTab[i];
		ecartMax = max(ecartMax, ecartMaxTab[i]);
		averageFiber1 += averageFiber1Error[i];
		averageFiber2 += averageFiber2Error[i];
	}

	ecartMoyen /= nbPoints;
	averageFiber1 /= (float)maxThreads;
	averageFiber2 /= (float)maxThreads;
	cout << "fa per fiber mean error: " << fabs(averageFiber2 - averageFiber1) / averageFiber1 * 100 << "%" << endl;
	cout << "fa max error: " << ecartMax << endl;
	cout << "fa mean error: " << ecartMoyen << endl;
}
}


#ifndef CPPMATH_OPTIMIZATION_DOWNHILLSIMPLEXMETHODNM_HPP_
#define CPPMATH_OPTIMIZATION_DOWNHILLSIMPLEXMETHODNM_HPP_

#include <cstddef> // size_t

#include <Eigen/Dense>

namespace cppmath
{
    /**
     * Implementation of the original Downhill Simplex or Nelder-Mead method for nonlinear optimization from 1965.
     * J. Nelder, R. Mead, "A Simplex Method for Function Minimization," Computer Journal, 1965, 7, 308-313
     *
     * \author cpieloth
     * \copyright Copyright 2014 Christof Pieloth, Licensed under the Apache License, Version 2.0
     */
    template< size_t DIM >
    class DownhillSimplexMethodNM
    {
    public:
        typedef Eigen::Matrix< double, DIM, 1 > ParamsT; /**< Abbreviation for a vector of parameters. */

        /**
         * Enum to indicate how the optimization was converged.
         */
        enum Converged
        {
            CONVERGED_NO, /**< Optimization was not started. */
            CONVERGED_EPSILON, /**< Optimization is smaller than epsilon/threshold. */
            CONVERGED_ITERATIONS, /**< Maximum iterations was reached. */
            CONVERGED_YES /**< Optimization is converged, but not specified how. */
        };

        DownhillSimplexMethodNM();
        virtual ~DownhillSimplexMethodNM();

        /**
         * Implementation of the function to minimize.
         *
         * \param x  n-dimensional parameter vector.
         * \return function value for vector x.
         */
        virtual double func( const ParamsT& x ) const = 0;

        /**
         * Indicates how the optimization was converged.
         *
         * \return Enum::Converged
         */
        virtual Converged converged() const;

        double getReflectionCoeff() const;

        void setReflectionCoeff( double alpha );

        double getContractionCoeff() const;

        void setContractionCoeff( double beta );

        double getExpansionCoeff() const;

        void setExpansionCoeff( double gamma );

        size_t getMaximumIterations() const;

        void setMaximumIterations( size_t iterations );

        double getEpsilon() const;

        void setEpsilon( double eps );

        double getInitialFactor() const;

        void setInitialFactor( double factor );

        size_t getResultIterations() const;

        ParamsT getResultParams() const;

        double getResultError() const;

        /**
         * Starts the optimization.
         *
         * \param initial Initial start point.
         */
        void optimize( const ParamsT& initial );

    protected:
        /**
         * Orders the parameters x and f(x) from min to max.
         */
        virtual void order();

        /**
         * Creates the initial parameter set and their function values.
         *
         * \param initial Start parameter used to calculate initials.
         */
        virtual void createInitials( const ParamsT& initial );
        double m_initFactor; /**< Factor to create the initial parameter set. */

        ParamsT m_x[DIM + 1]; /**< Vector of all n+1 points. */
        double m_y[DIM + 1]; /**< Stores the function values to reduce re-calculation. */

        double m_epsilon; /**< Threshold or deviation for convergence. */
        size_t m_maxIterations; /**< Maximum iterations until the algorithm is canceled. */
        size_t m_iterations; /**< Iteration counter used for break condition. */

        const size_t DIMENSION; /**< Constant for dimension. */
        const size_t VALUES; /**< Constant for DIM+1. */

    private:
        enum Step
        {
            STEP_START, STEP_EXIT, STEP_REFLECTION, STEP_EXPANSION, STEP_CONTRACTION
        };

        double m_alpha; /**< Reflection coefficient. */
        double m_beta; /**< Contraction coefficient. */
        double m_gamma; /**< Expansion coefficient. */

        void centroid();
        Step reflection();
        Step expansion();
        Step contraction();

        ParamsT m_xo;
        ParamsT m_xr;
        double m_yr;
    };
} /* namespace cppmath */

// Load the implementation
#include "DownhillSimplexMethodNM-impl.hpp"

#endif  // CPPMATH_OPTIMIZATION_DOWNHILLSIMPLEXMETHODNM_HPP_


#pragma once
#include <algorithm>
#include <iomanip>
#include <iostream>

#include <armadillo>
#include <fmt/format.h>

#define SIGM 1
#define TAHN 2
#define RELU 3

namespace net{
    template < typename Scalar >
    class Perceptron {
        // friend class Layer;
        private:
            Scalar val;
            Scalar act;
            Scalar der;
            int id;
        public:
            Perceptron( Scalar val );
            Scalar& VAL();
            Scalar& ACT( int FUNC = SIGM );
            Scalar& DER( int FUNC = SIGM );
            int& ID() { return id; }
            void show();
    };

    template < typename Scalar >
    Perceptron< Scalar >::Perceptron( Scalar val ) {
        this->val = val;
        ACT();
        DER();
    }
    template < typename Scalar >
    Scalar& Perceptron< Scalar >::VAL() {
        return this->val;
    }
    template < typename Scalar >
    Scalar& Perceptron< Scalar >::ACT( int FUNC ) {
        switch( FUNC ) {
            case SIGM: this->act = 1.0 / ( 1.0 + exp( -this->val ) ); break;               // sigmoid activation
            case TAHN: this->act = std::tanh( this->val ); break;                          // tahn activation
            case RELU: this->act = this->val > 0 ? this->val : this->val * 0.01; break;    // LeakyReLU activation
        }
        return this->act;
    }
    template < typename Scalar >
    Scalar& Perceptron< Scalar >::DER( int FUNC ) {
        switch( FUNC ) {
            case SIGM: this->der = this->act * ( 1.0 - this->act ); break;             // sigmoid derivation
            case TAHN: this->der = 1.0 - std::exp( std::tanh( this->val ) ); break;    // tahn derivation
            case RELU: this->der = this->val >= 0 ? 1.0 : 0.01; break;                 // LeakyReLU derivation
        }
        return this->der;
    }
    template < typename Scalar >
    void Perceptron< Scalar >::show() {
        std::string s = fmt::format( "\t{0} \t {1} \t {2}\n", this->VAL(), this->ACT(), this->DER() );
        fmt::print( s );    // Python-like format string syntax
    }
}


#include <iostream>
#include <cmath>

#include <taylor.hpp>

#include <boost/numeric/odeint.hpp>

template <typename T> class pendulum {

private:
  /// Length of the pendulum
  double length_;
  /// Gravitational constant (little g)
  double g_;

public:
  pendulum(double l, double g = 9.81) : length_(l), g_(g) {}

  void operator()(const T &theta, T &dtheta_dt, const double /* t */) {
    dtheta_dt[0][0] = theta[0][1];
    dtheta_dt[0][1] = -(g_ / length_) * theta[0][0];
  }
};

class pendulum_exact {
private:
  /// Length of the pendulum
  double length_;
  /// Initial position (angle)
  double theta0_;
  /// Gravitational constant (little g)
  double g_;

public:
  pendulum_exact(double l, double theta0, double g = 9.81) : length_(l), theta0_(theta0), g_(g) {}
  double position(double t) {
    return theta0_ * std::cos(std::sqrt(g_/length_) * t);
  }
  double speed(double t) {
    return (-theta0_ * std::sqrt(g_/length_) * std::sin(std::sqrt(g_/length_) * t));
  }
};

int main() {
  using namespace std;
  using namespace boost::numeric::odeint;

  typedef std::vector< taylor<double, 1, 1> > state_type;

  //[ state_initialization
  state_type x(2);
  x[0][0] = 1.0; // start at x=1.0, p=0.0
  x[0][1] = 0.0; // start at x=1.0, p=0.0
  x[1][0] = 0.0;
  x[1][1] = 0.0;
  //]

  double length = 10.0;
  pendulum<state_type> approx(length);
  pendulum_exact exact(length, 1.0);

  runge_kutta4< state_type> stepper;

  const double dt = 0.1;
  const double tstart = 0.0;
  const double tend = 1.0;
  for( double t = tstart ; t < tend ; t += dt )
  {
    std::cout << t << "   " << std::setprecision(16) << exact.position(t) << "   " << std::setprecision(16) << exact.speed(t) << std::endl;
    std::cout << t << "   " << std::setprecision(16) << x[0]  << "   " << std::setprecision(16) << x[1] << std::endl;
    stepper.do_step(approx, x, t, dt);
  }

  return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Eigen>
#include <cmath>

namespace fluid {
namespace algorithm {

class SpectralShape
{

  using ArrayXd = Eigen::ArrayXd;

public:
  SpectralShape() {}

  void processFrame(Eigen::Ref<ArrayXd> in, double sampleRate, double minFreq,
                    double maxFreq, double rolloffTarget, bool logFreq,
                    bool usePower)
  {
    using namespace std;
    maxFreq = (maxFreq == -1) ? (sampleRate / 2) : min(maxFreq, sampleRate / 2);
    ArrayXd mag = in.max(epsilon);
    index   nBins = mag.size();
    double  binHz = sampleRate / ((nBins - 1) * 2.);
    index   minBin = ceil(minFreq / binHz);
    index   maxBin =
        min(static_cast<index>(floorl(maxFreq / binHz)), (nBins - 1));
    if (maxBin <= minBin)
    {
      mOutputBuffer.setZero();
      return;
    }

    if (logFreq && minBin == 0)
    {
      minBin = 1;
      mag(1) += mag(0);
    }
    ArrayXd amp = usePower ? mag.square() : mag;
    amp = amp.segment(minBin, maxBin - minBin);
    double  ampSum = amp.sum();
    ArrayXd freqs =
        ArrayXd::LinSpaced(maxBin - minBin, minBin * binHz, maxBin * binHz);
    if (logFreq)
    { freqs = 69 + (12 * (freqs / 440).log() * log2E); } // MIDI cents

    double centroid = (amp * freqs).sum() / ampSum;
    double spread = (amp * (freqs - centroid).square()).sum() / ampSum;
    double skewness = (amp * (freqs - centroid).pow(3)).sum() /
                      (spread * sqrt(spread) * ampSum);
    double kurtosis =
        (amp * (freqs - centroid).pow(4)).sum() / (spread * spread * ampSum);

    double flatness = exp(amp.log().mean()) / amp.mean();
    double rolloff = maxBin - 1;
    double cumSum = 0;
    double target = ampSum * rolloffTarget / 100.0;
    for (index i = 0; cumSum <= target && i < amp.size(); i++)
    {
      cumSum += amp(i);
      if (cumSum >= target)
      {
        rolloff = (i == 0) ? freqs(i)
                           : freqs(i) - (freqs(i) - freqs(i - 1)) *
                                            (cumSum - target) / amp(i);
        break;
      }
    }
    double crest = amp.maxCoeff() / amp.mean();

    mOutputBuffer(0) = centroid;
    mOutputBuffer(1) = sqrt(spread);
    mOutputBuffer(2) = skewness;
    mOutputBuffer(3) = kurtosis;
    mOutputBuffer(4) = rolloff;
    mOutputBuffer(5) = 20 * log10(max(flatness, epsilon));
    mOutputBuffer(6) = 20 * log10(max(crest, epsilon));
  }

  void processFrame(const RealVector& input, RealVectorView output,
                    double sampleRate, double minFreq = 0, double maxFreq = -1,
                    double rolloffTarget = 0.95, bool logFreq = false,
                    bool usePower = false)
  {
    assert(output.size() == 7);
    ArrayXd in = _impl::asEigen<Eigen::Array>(input);
    processFrame(in, sampleRate, minFreq, maxFreq, rolloffTarget, logFreq,
                 usePower);
    output = _impl::asFluid(mOutputBuffer);
  }

private:
  ArrayXd mOutputBuffer{7};
};

} // namespace algorithm
} // namespace fluid


#include "PolyMesh/IOManager.h"
#include <Eigen/Sparse>
#include <Eigen/SVD>

using namespace acamcad;
using namespace polymesh;
using namespace Eigen;

void MeshInterpolation(PolyMesh* source, PolyMesh* target, double delta_t)
{
	std::string command = "mkdir result ";
	system(command.c_str());

	int nv = source->numVertices();
	int nf = source->numPolygons();

	std::vector<Matrix2d> S(nf);
	std::vector<Vector3d> xx(nf);
	std::vector<Vector3d> yy(nf);
	std::vector<double> area(nf);
	std::vector<double> angle(nf);

	Matrix2d I = MatrixXd::Identity(2, 2);

	std::vector<std::vector<int>> v_id(nf);

	// Initial
	for (FaceIter f_it = source->polyfaces_begin(); f_it != source->polyfaces_end(); ++f_it)
	{
		int id = (*f_it)->index();
		MHalfedge* heh = (*f_it)->halfEdge();
		MVert* v0 = heh->fromVertex();
		MVert* v1 = heh->toVertex();
		MHalfedge* next_heh = heh->next();
		MVert* v2 = next_heh->toVertex();

		v_id[id].push_back(v0->index());
		v_id[id].push_back(v1->index());
		v_id[id].push_back(v2->index());

		area[id] = cross(v1->position() - v0->position(), v2->position()- v0->position()).norm() / 2.0;

		xx[id] = Vector3d(v2->position()[0] - v1->position()[0], v0->position()[0] - v2->position()[0], v1->position()[0] - v0->position()[0]);
		yy[id] = Vector3d(v1->position()[1] - v2->position()[1], v2->position()[1] - v0->position()[1], v0->position()[1] - v1->position()[1]);

		Vector3d u(target->vert(v0->index())->position()[0], target->vert(v1->index())->position()[0], target->vert(v2->index())->position()[0]);
		Vector3d v(target->vert(v0->index())->position()[1], target->vert(v1->index())->position()[1], target->vert(v2->index())->position()[1]);

		Matrix2d J;
		J << yy[id].dot(u) / (2 * area[id]), xx[id].dot(u) / (2 * area[id]), yy[id].dot(v) / (2 * area[id]), xx[id].dot(v) / (2 * area[id]);
		JacobiSVD<Matrix2d> svd(J, ComputeFullV | ComputeFullU);

		Matrix2d V = svd.matrixV();
		Matrix2d U = svd.matrixU();
		Matrix2d R = U * V.transpose();

		Matrix2d sigma;
		sigma(0, 1) = sigma(1, 0) = 0;
		sigma(0, 0) = svd.singularValues()[0];
		sigma(1, 1) = svd.singularValues()[1];
		S[id] = V * sigma * V.transpose();
		angle[id] = atan2(R(1, 0), R(1, 1));
	}

	// fix nv-1
	double u0 = source->vert(nv - 1)->position()[0];
	double v0 = source->vert(nv - 1)->position()[1];

	SparseMatrix<double> A(2 * nv - 2, 2 * nv - 2);
	std::vector<Triplet<double>>triplet;

	for (FaceIter f_it = source->polyfaces_begin(); f_it != source->polyfaces_end(); ++f_it)
	{
		int id = (*f_it)->index();
		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				if ((v_id[id][i] == nv - 1) || (v_id[id][j] == nv - 1))
				{
					continue;
				}
				else
				{
					triplet.push_back(Triplet<double>(2 * v_id[id][i], 2 * v_id[id][j], yy[id][i] * yy[id][j] / (area[id] * area[id] * 2)));
					triplet.push_back(Triplet<double>(2 * v_id[id][i], 2 * v_id[id][j], xx[id][i] * xx[id][j] / (area[id] * area[id] * 2)));
					triplet.push_back(Triplet<double>(2 * v_id[id][i] + 1, 2 * v_id[id][j] + 1, yy[id][i] * yy[id][j] / (area[id] * area[id] * 2)));
					triplet.push_back(Triplet<double>(2 * v_id[id][i] + 1, 2 * v_id[id][j] + 1, xx[id][i] * xx[id][j] / (area[id] * area[id] * 2)));
				}
			}
		}
	}
	A.setFromTriplets(triplet.begin(), triplet.end());
	SparseLU<SparseMatrix<double>> solver;
	solver.analyzePattern(A);
	solver.factorize(A);

	double t = 0;
	while (t <= 1)
	{
		std::cout << "iter:" << t << std::endl;

		VectorXd b(2 * nv - 2);
		b.setZero();

		for (FaceIter f_it = source->polyfaces_begin(); f_it != source->polyfaces_end(); ++f_it)
		{
			int id = (*f_it)->index();
			Matrix2d A, R;
			double theta_t = angle[id] * t;

			R << cos(theta_t), -sin(theta_t), sin(theta_t), cos(theta_t);
			A = R * ((1.0 - t)*I + t * S[id]);
			for (int i = 0; i < 3; i++)
			{
				for (int j = 0; j < 3; j++)
				{
					if ((v_id[id][i] == nv - 1) || (v_id[id][j] == nv - 1))
					{
						if ((v_id[id][i] != nv - 1) && (v_id[id][j] == nv - 1))
						{
							b[2 * v_id[id][i]] += -yy[id][i] * yy[id][j] * u0 / (area[id] * area[id] * 2);
							b[2 * v_id[id][i]] += -xx[id][i] * xx[id][j] * u0 / (area[id] * area[id] * 2);
							b[2 * v_id[id][i] + 1] += -yy[id][i] * yy[id][j] * v0 / (area[id] * area[id] * 2);
							b[2 * v_id[id][i] + 1] += -xx[id][i] * xx[id][j] * v0 / (area[id] * area[id] * 2);
						}
					}
				}
				if (v_id[id][i] != nv - 1)
				{
					b[2 * v_id[id][i]] += yy[id][i] * A(0, 0) / area[id];
					b[2 * v_id[id][i]] += xx[id][i] * A(0, 1) / area[id];
					b[2 * v_id[id][i] + 1] += yy[id][i] * A(1, 0) / area[id];
					b[2 * v_id[id][i] + 1] += xx[id][i] * A(1, 1) / area[id];
				}
			}
		}
		VectorXd result = solver.solve(b);
		for (VertexIter v_it = source->vertices_begin(); v_it != source->vertices_end(); ++v_it)
		{
			int id = (*v_it)->index();
			if (id != nv - 1)
			{
				(*v_it)->setPosition(result[2 * id], result[2 * id + 1], 0);
			}
		}

		std::string string_t = std::to_string(t);
		writeMesh("result\\" + string_t + ".obj", source);
		t += delta_t;
	}
}

int main(int argc, const char **argv)
{
	if (argc != 4)
	{
		std::cout << "========== Hw8 Usage  ==========\n";
		std::cout << std::endl;
		std::cout << "blablabla.exe	source.obj target.obj delta_t\n";
		std::cout << std::endl;
		std::cout << "=================================================\n";
        return 0;
	}
	std::string source_path = argv[1];
	std::string target_path = argv[2];
	std::string t_string = argv[3];

	PolyMesh* source_mesh = new PolyMesh();
	PolyMesh* target_mesh = new PolyMesh();
	loadMesh(source_path, source_mesh);
	loadMesh(target_path, target_mesh);
	double delta_t = atof(t_string.c_str());
	MeshInterpolation(source_mesh, target_mesh, delta_t);
    return 0;
}


//  (C) Copyright Thomas Becker 2015. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

#ifndef TMB_NORMAL_DISTRIBUTION_PIVOT_09_27_2015_HPP
#define TMB_NORMAL_DISTRIBUTION_PIVOT_09_27_2015_HPP

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

/*
 * Pivot calculator for normally distributed data.
 */

namespace median_project
{
namespace numerical_quick_median_detail
{

/**
 * Pivot calculator for normal distributions.
 */
class normal_distribution_pivot : public distribution_specific_pivot_base
{
  private:
    virtual double cdf(double x) const override
    {
        return .5 * (1.0 + boost::math::erf((x - m_total_sequence_mean) / (sqrt(2.0) * m_total_sequence_std_dev)));
    }

    virtual double quantile(double x) const override
    {
        return m_total_sequence_mean + m_total_sequence_std_dev * sqrt(2.0) * boost::math::erf_inv(2.0 * x - 1.0);
    }
};
} // end namespace numerical_quick_median_detail
} // end namespace median_project

#endif // TMB_NORMAL_DISTRIBUTION_PIVOT_09_27_2015_HPP


#include<iostream>
#include<thread>
#include<random>
#include<type_traits>
#include<vector>
#include <boost/multiprecision/cpp_int.hpp>
#include"printtable.h"
#include "calcSignature.hpp"

namespace ExactSignature {
  using std::vector;

  typedef boost::multiprecision::cpp_rational Number;

  class Signature {
  public:
    vector<vector<Number>> m_data;

    template<typename Numeric>
    void sigOfSegment(int d, int m, const Numeric* segment) {
      m_data.resize(m);
      auto& first = m_data[0];
      first.resize(d);
      for (int i = 0; i<d; ++i)
        first[i] = (Number)segment[i];
      for (int level = 2; level <= m; ++level) {
        const auto& last = m_data[level - 2];
        auto& s = m_data[level - 1];
        s.assign(calcSigLevelLength(d, level), 0);
        int i = 0;
        for (auto l : last)
          for (auto p = segment; p<segment + d; ++p)
            s[i++] = (Number)(*p * l * Number(1, level));
      }
    }

    void sigOfNothing(int d, int m) {
      m_data.resize(m);
      m_data[0].assign(d, 0);
      for (int level = 2; level <= m; ++level) {
        auto& s = m_data[level - 1];
        s.assign(calcSigLevelLength(d, level), 0);
      }
    }

    //if a is the signature of path A, b of B, then
    //a.concatenateWith(d,m,b) makes a be the signature of the concatenated path AB
    //This is also the (concatenation) product of the elements a and b in the tensor algebra.
    void concatenateWith(int d, int m, const Signature& other) {
      for (int level = m; level>0; --level) {
        for (int mylevel = level - 1; mylevel>0; --mylevel) {
          int otherlevel = level - mylevel;
          auto& oth = other.m_data[otherlevel - 1];
          for (auto dest = m_data[level - 1].begin(),
            my = m_data[mylevel - 1].begin(),
            myE = m_data[mylevel - 1].end(); my != myE; ++my) {
            for (const Number& d : oth) {
              *(dest++) += d * *my;
            }
          }
        }
        auto source = other.m_data[level - 1].begin();
        for (auto dest = m_data[level - 1].begin(),
          e = m_data[level - 1].end();
          dest != e;)
          *(dest++) += *(source++);

      }
    }
    void swap(Signature& other) {
      m_data.swap(other.m_data);
    }
    void multiplyByConstant(Number c) {
      for (auto& a : m_data)
        for (auto& b : a)
          b *= c;
    }
    template<typename Numeric>
    void writeOut(Numeric* dest) const {
      for (auto& a : m_data)
        for (auto& b : a)
          *(dest++) = (Numeric)b;
          //*(dest++) = (Numeric)denominator(b);
    }
  };

  //This also calculates the concatenation product in the tensor algebra,
  //but in the case where we assume 0 instead of 1 in the zeroth level.
  //It is not in-place
  Signature concatenateWith_zeroFirstLevel(int d, int m,
    const Signature& a,
    const Signature& b) {
    Signature out;
    out.sigOfNothing(d, m);
    for (int level = m; level>0; --level) {
      for (int alevel = level - 1; alevel>0; --alevel) {
        int blevel = level - alevel;
        auto& aa = a.m_data[alevel - 1];
        auto& bb = b.m_data[blevel - 1];
        auto dest = out.m_data[level - 1].begin();
        for (const Number& c : aa) {
          for (const Number& d : bb) {
            *(dest++) += d * c;
          }
        }
      }
    }
    return out;
  }

  void logTensorHorner(Signature& x) {
    const int m = (int)x.m_data.size();
    const int d = (int)x.m_data[0].size();
    if (m <= 1)
      return;
    Signature s, t;
    s.sigOfNothing(d, m - 1);
    t.sigOfNothing(d, m);
    for (int depth = m; depth > 0; --depth) {
      Number constant = (Number) 1.0 / depth;
      //make t be x*s up to level (1+m-depth). [this does nothing the first time round]
      for (int lev = 2; lev <= 1 + m - depth; ++lev) {
        //t.m_data[lev - 1] = x.m_data[lev - 1];
        auto& tt = t.m_data[lev - 1];
        std::fill(tt.begin(), tt.end(), (Number) 0.0);
        for (int leftLev = 1; leftLev < lev; ++leftLev) {
          int rightLev = lev - leftLev;
          auto& aa = x.m_data[leftLev - 1];
          auto& bb = s.m_data[rightLev - 1];
          auto dest = t.m_data[lev - 1].begin();
          for (const Number& c : aa)
            for (const Number& dd : bb)
              *(dest++) += dd * c;
        }
      }
      //make s be x*constant-t up to level (1+m-depth)
      if (depth>1)
        for (int lev = 1; lev <= 1 + m - depth; ++lev) {
          auto is = s.m_data[lev - 1].begin();
          auto ix = x.m_data[lev - 1].begin();
          auto es = s.m_data[lev - 1].end();
          auto it = t.m_data[lev - 1].begin();
          for (; is != es; ++is, ++it, ++ix)
            *is = constant * *ix - *it;
        }
    }
    //x isn't modified until this next bit.
    //make x be x-t
    for (int lev = 2; lev <= m; ++lev) {
      auto it = t.m_data[lev - 1].begin();
      auto ix = x.m_data[lev - 1].begin();
      auto ex = x.m_data[lev - 1].end();
      for (; ix != ex; ++ix, ++it)
        *ix -= *it;
    }
  }

}

std::mt19937 g_rnd;
using PathReal = double;
std::vector<PathReal> randomPath(int pathLength, int d){
  std::vector<PathReal> out(pathLength*d);
  std::uniform_real_distribution<PathReal> urd(0, 1);
  for (auto& d : out)
    d = urd(g_rnd);
  return out;
}

template<bool arbprec>
using SignatureType = std::conditional_t<arbprec,
  ExactSignature::Signature,
  CalcSignature::Signature>;

//the stroke ending at from ... the stroke ending before to
template<bool arbprec>
SignatureType<arbprec> sigPiece(const std::vector<PathReal>& path, int d, int m, size_t from, size_t to) {
  SignatureType<arbprec> s1, s2;
  std::vector<PathReal> displacement(d);
  for (size_t i = from; i < to; ++i) {
    for (int j = 0; j < d; ++j)
      displacement[j] = path[i*d + j] - path[(i - 1)*d + j];
    s1.sigOfSegment(d, m, displacement.data());
    if (i == from) {
      s2.swap(s1);
    }
    else {
      s2.concatenateWith(d, m, s1);
    }
  }
  return s2;
}

//Calculate the signature or log signature of path,
//using float or arbitrary precision
template<bool arbprec>
std::vector<double> sig(const std::vector<PathReal>& path, int d, int m, bool log) {
  size_t pathLength = path.size() / d;
  auto s2 = sigPiece<arbprec>(path, d, m, 1, pathLength);
  if (log)
    logTensorHorner(s2);
  std::vector<double> out(calcSigTotalLength(d, m));
  s2.writeOut(out.data());
  return out;
}

//like sig but split the work between processors
template<bool arbprec>
std::vector<double> sigParallel(const std::vector<PathReal>& path, int d, int m, bool log) {
  size_t pathLength = path.size() / d;
  size_t nThreads = 4;
  size_t each = (pathLength - 1) / nThreads;
  if (each < 1)
    return sig<arbprec>(path, d, m, log);
  size_t start = 1;
  std::vector<SignatureType<arbprec>> pieces(nThreads);
  std::vector<std::thread> threads;
  for (size_t thread = 0; thread < nThreads; ++thread) {
    size_t start = thread*each + 1;
    size_t end = (thread + 1 == nThreads) ? pathLength : (thread*each + each + 1);
    threads.emplace_back([&pieces,path,d,m,start, end,thread]() {
      pieces[thread] = sigPiece<arbprec>(path, d, m, start, end);
    });
  }
  for (auto& t : threads)
    t.join();
  auto s2 = pieces[0];
  for (size_t i = 1; i < nThreads; ++i)
    s2.concatenateWith(d, m, pieces[i]);
  if (log)
    logTensorHorner(s2);
  std::vector<double> out(calcSigTotalLength(d, m));
  s2.writeOut(out.data());
  return out;
}

void test() {
  using N = boost::multiprecision::cpp_rational;
  N a = 0.0625;
  N b = a - N(4, 5);
  std::cout << b << " "<<(double) b << "\n";
}

void go() {
  auto d = 2, m = 4, pathLength=4;
  auto path = randomPath(pathLength, d);
  bool log = true;
  auto s1 = sig<false>(path, d, m, log);
  //auto s2 = sig<true>(path, d, m, log);
  auto s2 = sigParallel<true>(path, d, m, log);

  //for (size_t i = 0; i < s1.size(); ++i) {
  //  std::cout << s1[i] << "  " << s2[i] << "\n";
  //}
  auto diffs = diffVectors(s1, s2);
  PrintTable::printTable(s1, s2, diffs);
  std::cout << "l2 norm " << l2norm(s1) << " "<< l2norm(s2) << "\n";
  std::cout << "l2 diff " << l2norm(diffs) << "\n";
  std::cout << "total diff "<<l1norm(diffs) << "\n";
  printMaxDiff(s1, s2);
  printMaxRelDiff(s1, s2);
  printGreatestWith0b(s1, s2);
}

int main() {
  //test();
  go();
}


#pragma once

#include "out.hh"
#include "types.hh"

#include "eigen.hh"
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <sophus/se2.hpp>
#include <sophus/so2.hpp>

namespace raytrace {
using Vec1 = Vec<1>;
using Vec2 = Eigen::Vector2d;
using Vec3 = Eigen::Vector3d;
using Mat2 = Eigen::Matrix2d;

double cost(const Vec3 &eta, const std::vector<Vec2> &x, const std::vector<Vec2> &z) {
  const se2 T = se2::exp(eta);

  double error_sq = 0.0;
  for (std::size_t k = 0; k < x.size(); ++k) {
    const Vec2 error = (T * x[k]) - z[k];
    error_sq += error.squaredNorm();
  }

  return error_sq;
}

Vec2 error(const Vec3 &eta, const std::vector<Vec2> &x, const std::vector<Vec2> &z) {
  const se2 T = se2::exp(eta);

  // double error_sq = 0.0;
  Vec2 error_sq = Vec2::Zero();
  for (std::size_t k = 0; k < x.size(); ++k) {
    const Vec2 error = (T * x[k]) - z[k];
    error_sq += error.array().square().matrix();
  }

  return error_sq;
}

struct dExpSe2 {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Eigen::Matrix2d Rprime;
  Eigen::Matrix2d V;
  Eigen::Matrix2d Vprime;
  Vec3            eta;

  Mat<2, 3> jacobian_from_y(const Vec2 &y) const {
    Mat<2, 3> m;
    m.setZero();
    m.block<2, 2>(0, 0) = V;
    m.block<2, 1>(0, 2) = (Rprime * y) + (Vprime * eta.head<2>());
    return m;
  }
};

void dexp_se2(const Vec3 &eta, Out<dExpSe2> jac) {
  // todo: Use taylor exp instead of explicit sincos for stability near zero
  const double c          = std::cos(eta(2));
  const double s          = std::sin(eta(2));
  const double inv_theta  = 1 / eta(2);
  const double inv_theta2 = inv_theta * inv_theta;

  const double cos_inv_theta  = c * inv_theta;
  const double sin_inv_theta  = s * inv_theta;
  const double sin_inv_theta2 = sin_inv_theta * inv_theta;

  const double cos_m1_invtheta2 = (c - 1) * inv_theta2;

  Eigen::Matrix2d v;
  v.row(0) << s, (c - 1);
  v.row(1) << 1 - c, s;
  v *= inv_theta;

  Eigen::Matrix2d dv_dtheta;
  dv_dtheta.row(0) << cos_inv_theta - (sin_inv_theta2), -sin_inv_theta - (cos_m1_invtheta2);
  dv_dtheta.row(1) << sin_inv_theta + cos_m1_invtheta2, cos_inv_theta - sin_inv_theta2;

  Eigen::Matrix2d dR_dtheta;
  dR_dtheta.row(0) << -s, -c;
  dR_dtheta.row(1) << c, -s;

  jac->Rprime = dR_dtheta;
  jac->V      = v;
  jac->Vprime = dv_dtheta;
  jac->eta    = eta;
}

Vec3 dcost(const Vec3 &eta, const std::vector<Vec2> &x, const std::vector<Vec2> &z) {
  const se2 T = se2::exp(eta);
  dExpSe2   preJ;
  dexp_se2(eta, out(preJ));

  Vec3 derror_sq_total = Vec3::Zero();
  for (std::size_t k = 0; k < x.size(); ++k) {
    const Vec2 error = (T * x[k]) - z[k];

    const Mat<2, 3> derror_deta = preJ.jacobian_from_y(x[k]);
    const Vec3 derror_sq = (2 * error.transpose()) * derror_deta;
    derror_sq_total += derror_sq;
  }

  return derror_sq_total;
}

template <int cols, typename Callable1, typename Callable2>
Eigen::Matrix<double, cols, 1> gauss_newton(const Eigen::Matrix<double, cols, 1> &x0,
                                            const Callable1 &fcn,
                                            const Callable2 &jac,
                                            const double     feps      = 1e-6,
                                            const int        max_iters = 15) {
  using VecX     = Eigen::Matrix<double, 1, cols>;
  using Gradient = Eigen::Matrix<double, cols, 1>;
  VecX x         = x0;

  double last_cost = fcn(x);
  for (int k = 0; k < max_iters; ++k) {
    // todo: llt
    const Gradient J = jac(x);
    const VecX     c = (J.transpose() * J).inverse() * J.transpose() * fcn(x);
    x                = x - c;
    double new_cost  = fcn(x);

    if (std::abs(new_cost - last_cost) < feps) {
      break;
    }

    last_cost = new_cost;
  }
  return x;
}
}


#ifndef CGWR_HPP
#define CGWR_HPP

#include <iostream>
#include <cmath>
#include <algorithm>

//#include <boost/rational.hpp>
//#include <boost/math/special_functions/factorials.hpp>
//#include <boost/math/special_functions/powm1.hpp>

/*********************************************************************
- CG:
< j1 j2 m1 m2 | J M >

- Wigner 3j or 6j:
(j1,j2,j3) or {j1,j2,j3}
(l1,l2,l3)    {m1,m2,m3}

- Wigner 9j:
{j1 j2 j3}
{j4 j5 j6}
{j7 j8 j9}

-Racah:
W(j1 j2 l2 l1 | j3 l3)

CG translated as array=[j1,j2,m1,m2,J,M] with _CGWR::C_CG token
or
Wigner 3j translated as array=[j1,j2,j3,m1,m2,m3] with _CGWR::C_W3j token
or
Wigner 6j translated as array=[j1,j2,j3,l1,l2,l3] with _CGWR::C_W6j token
or
and so on with usual notations

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

class _CGWR { // Clebsch-Gordan Wigner Racah
public:

    // Token
    /***********/
    static const unsigned int C_CG;
    static const unsigned int C_W3j;
    static const unsigned int C_W6j;
    static const unsigned int C_W9j;
    static const unsigned int C_RACAH;
    /***********/

    // Init the class with the coeff and the token
    /***********/
    _CGWR(double long [], unsigned int);
    /***********/

    _CGWR& operator= ( const _CGWR& other ); //TODO

    // Public methods
    /***********/
    double long CG(void);
    double long W3j(void);
    double long W6j(void);
    double long W9j(void); //TODO
    double long Racah(void);
    /***********/

private:
    double long j1, j2, j3, j;
    double long l1, l2, l3, l;
    double long m1, m2 , m3, m;

    unsigned int selected_Symb;

    // mid computations
    /***********/
    long double fact ( long double n ); // factorial
    double long Delta(double long , double long , double long ); // triangle relation
    double long delta_k(double long , double long ); // kronecker

    // variadic
    template<typename _T>
    bool is_integer(_T n); // return true if integer
    template<typename _T, typename ..._args>
    bool is_integer(_T, _args...);

    template<typename _T>
    bool is_halfinteger(_T); // return true if half integer
    template<typename _T, typename ..._args>
    bool is_halfinteger(_T, _args...);
    /***********/

};

#endif // CGWR_HPP


#include <Eigen/Core>
#include <Eigen/LU>

#include "args.hxx"

#include <fstream>
#include <iostream>

using ChartType = Eigen::Matrix<double, 24, 3>;
using ChartHmType = Eigen::Matrix<double, 24, 4>;
using CCMType = Eigen::Matrix<double, 4, 3>;

std::vector<std::string> StringSplit(const std::string &str, char sep) {
    std::vector<std::string> v;
    std::stringstream ss(str);
    std::string buffer;
    while( std::getline(ss, buffer, sep) ) {
        v.push_back(buffer);
    }
    return v;
}

bool LoadColorchartCsv(const std::string& filename, ChartType& dst_chart) {
    std::ifstream ifs(filename);
    if (!ifs) {
        std::cout << "Can't open " << filename << std::endl;
        return false;
    }

    std::string str;
    std::getline(ifs, str); // skip first line

    int i = 0;
    while (std::getline(ifs, str)) {
        std::vector<std::string> data_str = StringSplit(str, ',');
        dst_chart(i, 0) = std::stod(data_str[1]);
        dst_chart(i, 1) = std::stod(data_str[2]);
        dst_chart(i, 2) = std::stod(data_str[3]);
        i++;
        if (i == 24) break;
    }
    return true;
}

void conv_sRGB2XYZ(const ChartType &rgb, ChartType &xyz) {
    Eigen::Matrix<double, 3, 3> M;
    M << 0.412391, 0.357584, 0.180481,
         0.212639, 0.715169, 0.072192,
         0.019331, 0.119195, 0.950532;
    xyz = (M * rgb.transpose()).transpose();
}

void SolveCCM(const ChartType& source_xyz, const ChartType& reference_xyz,
              CCMType &ccm) {
    // source_xyz * ccm == reference_xyz
    // (24, 3 + 1) * (4, 3) = (24 * 3)
    ChartHmType source_xyz_hm;
    source_xyz_hm.col(0) = source_xyz.col(0);
    source_xyz_hm.col(1) = source_xyz.col(1);
    source_xyz_hm.col(2) = source_xyz.col(2);
    source_xyz_hm.col(3) = Eigen::VectorXd::Ones(24);
    auto source_xyz_hm_t = source_xyz_hm.transpose();
    auto pinv = (source_xyz_hm_t * source_xyz_hm).inverse() * source_xyz_hm_t;
    ccm = pinv * reference_xyz;
}

void WriteCCM(const std::string &filename, const CCMType& ccm) {
    std::ofstream csv(filename);
    for (int i = 0; i < 4; i++) {
        csv << ccm(i, 0) << ","
            << ccm(i, 1) << ","
            << ccm(i, 2) << std::endl;
    }
}

bool ParseArgs(const int argc, char** argv, std::string& ref_csv,
               std::string& src_csv, std::string& out_csv, double& gamma) {
    args::ArgumentParser parser("Compute CCM.");
    args::HelpFlag help(parser, "help", "Display this help menu",
                        {'h', "help"});
    args::Positional<std::string> ref_csv_arg(parser, "reference",
                                              "reference csv file");
    args::Positional<std::string> src_csv_arg(parser, "source",
                                              "source csv file");
    args::Positional<std::string> out_csv_arg(parser, "output",
                                              "output csv file", "ccm.csv");
    args::ValueFlag<double> gamma_arg(parser, "gamma",
                                     "Gamma value of reference and source data",
                                     {'g', "gamma"}, 1.0);
    try {
        parser.ParseCLI(argc, argv);
    } catch (args::Help) {
        std::cout << parser;
        return false;
    } catch (args::ParseError e) {
        std::cerr << e.what() << std::endl;
        std::cerr << parser;
        return false;
    } catch (args::ValidationError e) {
        std::cerr << e.what() << std::endl;
        std::cerr << parser;
        return false;
    }
    if (!ref_csv_arg) {
        std::cout << "Error: reference is not set" << std::endl;
        std::cout << parser;
        return false;
    }
    if (!src_csv_arg) {
        std::cout << "Error: src is not set" << std::endl;
        std::cout << parser;
        return false;
    }

    ref_csv = args::get(ref_csv_arg);
    src_csv = args::get(src_csv_arg);
    out_csv = args::get(out_csv_arg);
    gamma = args::get(gamma_arg);

    return true;
}

int main(int argc, char** argv) {
    // Parse arguments
    std::string ref_csv, src_csv, out_csv;
    double gamma;
    if (!ParseArgs(argc, argv, ref_csv, src_csv, out_csv, gamma)) {
        return 1;
    }

    // Load color charts
    ChartType reference_raw, source_raw;
    if (!LoadColorchartCsv(ref_csv, reference_raw)) {
        return 1;
    }
    if (!LoadColorchartCsv(src_csv, source_raw)) {
        return 1;
    }

    // Degamma
    ChartType reference_linear = reference_raw.array().pow(gamma);
    ChartType source_linear = source_raw.array().pow(gamma);

    // XYZ
    ChartType reference_xyz, source_xyz;
    conv_sRGB2XYZ(reference_linear, reference_xyz);
    conv_sRGB2XYZ(source_linear, source_xyz);

    // Solve
    CCMType ccm;
    SolveCCM(source_xyz, reference_xyz, ccm);
    std::cout << "CCM:" << std::endl;
    std::cout << ccm << std::endl;

    // Save
    WriteCCM(out_csv, ccm);

    return 0;
}


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

double* diff_ts(double *y, int len, double dt)
{
  double *dy;
  dy = new double[len];

  dy[0] = y[0];
  for (int i = 1; i<len; i++)
  {
    dy[i] = (y[i] - y[i-1])/dt;
  }
  return dy;
}

mat diffMat( int len, int DIM, double dt, mat y)
{
  mat dy(DIM, len) ;
  for (int d = 0; d<DIM; d++)
  {
    for (int i = 1; i<len; i++)
    {
      dy(d,i) = (y(d,i) - y(d,i-1))/dt;
    }
    dy(d,0) = dy(d,1);

  }
  return dy;

}


/* Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the MIT License
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the MIT License for further details.
 *
 * Contact information
 * ------------------------------------------
 * Thomas Jespersen, TKJ Electronics
 * Web      :  http://www.tkjelectronics.dk
 * e-mail   :  thomasj@tkjelectronics.dk
 * ------------------------------------------
 */

#include <signal.h>
#include <condition_variable>
#include <stdlib.h>
#include <stdio.h>

#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include <boost/format.hpp>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "omp.h"

#include <iostream>
#include <cstring>
#include <string>

#include <boost/math/quaternion.hpp> // see https://www.boost.org/doc/libs/1_66_0/libs/math/doc/html/quaternions.html

void QuaternionIntegration_ForwardEuler(
        double dt,
        double omeg_x, double omeg_y, double omeg_z,
        double& q0, double& q1, double& q2, double& q3);
void QuaternionIntegration_Exponential(
        double dt,
        double omeg_x, double omeg_y, double omeg_z,
        double& q0, double& q1, double& q2, double& q3);
void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3);

#define deg2rad(x) (M_PI*x/180.f)
#define rad2deg(x) (180.f*x/M_PI)

int main(int argc, char** argv ) {
    std::string argv_str(realpath(argv[0], 0));
    std::string base = argv_str.substr(0, argv_str.find_last_of("/"));

    double angularVelocity[3] = {0.3, 0.2, 0.1};
    double dt = 1.0/200;

    // Create initial quaternion for testing - using Boost Quaternions: https://www.boost.org/doc/libs/1_66_0/libs/math/doc/html/quaternions.html
    boost::math::quaternion<double> q0(0.9848,0.1736,0,0); // w,x,y,z
    q0 = q0 / sqrt(boost::math::norm(q0)); // normalize

    // Convert angular velocity vector into a quaternion
    boost::math::quaternion<double> q_omeg(0, angularVelocity[0], angularVelocity[1], angularVelocity[2]);   // use as body angular velocity

    std::cout << "Quaternion are printed in order: \t(w, x, y, z)" << std::endl << std::endl;

    /* Inertial vs Body angular velocity test */
    std::cout << ">>>>> Inertial vs Body angular velocity test <<<<<" << std::endl;
    boost::math::quaternion<double> q_body = q0 * boost::math::exp(0.5*dt*q_omeg); // compute the new quaternion using the quaternion exponential
    boost::math::quaternion<double> q_inertial = boost::math::exp(0.5*dt*q_omeg) * q0; // compute the new quaternion using the quaternion exponential

    // Print results for comparison
    std::cout << "q0 \t\t=\t" << q0 << std::endl;
    std::cout << "q_omeg \t\t=\t" << q_omeg << std::endl;
    std::cout << "q_body \t\t=\t" << q_body << std::endl;
    std::cout << "q_inertial \t=\t" << q_inertial << std::endl;

    std::cout << std::endl;

    /* Integration method test - methods are inspired from https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/  */
    std::cout << ">>>>> Integration method test <<<<<" << std::endl;
    // Method 1: Quaternion Exponential
    boost::math::quaternion<double> q1_exp = q0 * boost::math::exp(0.5*dt*q_omeg);

    // Method 2: Manual Quaternion Exponential
    boost::math::quaternion<double> q_exp_delta;
    if (norm(q_omeg) > 0)
        q_exp_delta = cos(0.5 * dt * sqrt(norm(q_omeg))) + q_omeg / sqrt(norm(q_omeg)) * sin(0.5 * dt * sqrt(norm(q_omeg)));
    else
        q_exp_delta = boost::math::quaternion<double>(1, 0, 0, 0); // unit quaternion since the angular velocity is zero (no movement)

    boost::math::quaternion<double> q2_exp_manual = q0 * q_exp_delta;

    // Method 3: Manual Quaternion Exponential
    double q_1[4] = {q0.R_component_1(), q0.R_component_2(), q0.R_component_3(), q0.R_component_4()};
    QuaternionIntegration_Exponential(dt, angularVelocity[0],angularVelocity[1],angularVelocity[2], q_1[0],q_1[1],q_1[2],q_1[3]);
    boost::math::quaternion<double> q3_exp_manual(q_1[0],q_1[1],q_1[2],q_1[3]);

    // Method 4: Delta Quaternion
    boost::math::quaternion<double> q_delta(1, 0.5*dt*angularVelocity[0], 0.5*dt*angularVelocity[1], 0.5*dt*angularVelocity[2]);
    boost::math::quaternion<double> q4_delta = q0 * q_delta; // compute the new quaternion using body angular velocity to construct the delta quaternion (hence right multiplication)

    // Method 5: Forward Euler
    boost::math::quaternion<double> dq = 0.5 * q0 * q_omeg; // quaternion derivative using body angular velocity
    //boost::math::quaternion<double> dq = 0.5 * q_omeg * q0; // quaternion derivative using inertial angular velocity
    boost::math::quaternion<double> q5_euler = q0 + dt*dq;
    q5_euler = q5_euler / sqrt(boost::math::norm(q5_euler)); // normalize

    // Method 6: Forward Euler 2
    double q_2[4] = {q0.R_component_1(), q0.R_component_2(), q0.R_component_3(), q0.R_component_4()};
    QuaternionIntegration_ForwardEuler(dt, angularVelocity[0],angularVelocity[1],angularVelocity[2], q_2[0],q_2[1],q_2[2],q_2[3]);
    boost::math::quaternion<double> q6_euler(q_2[0],q_2[1],q_2[2],q_2[3]);

    // Print results for comparison
    std::cout << "Angular Velocity \t=\t(" << angularVelocity[0] << ", " << angularVelocity[1] << ", " << angularVelocity[2] << ")" << std::endl;
    std::cout << "Boost Exponential \t=\t" << q1_exp << std::endl;
    std::cout << "Manual Exponential \t=\t" << q2_exp_manual << std::endl;
    std::cout << "Delta \t\t\t=\t" << q4_delta << std::endl;
    std::cout << "Forward Euler \t\t=\t" << q5_euler << std::endl;
    std::cout << "Manual Exponential my \t=\t" << q3_exp_manual << std::endl;
    std::cout << "Forward Euler my \t=\t" << q6_euler << std::endl;

    return 0;
}

void QuaternionIntegration_ForwardEuler(
        double dt,
        double omeg_x, double omeg_y, double omeg_z,
        double& q0, double& q1, double& q2, double& q3)
{
    // The angular velocity is given in body frame
    /* Forward Euler method
     * q_new = q + dt * dq
     * dq = 0.5 * q0 * q_omeg
     * q_omeg = [0,omeg_x,omeg_y,omeg_z]
     */

    // dq = 0.5*Phi(q0)*[0,omeg_x,omeg_y,omeg_z]
    double dq0 = 0.5 * (-q1*omeg_x -q2*omeg_y -q3*omeg_z);
    double dq1 = 0.5 * (+q0*omeg_x -q3*omeg_y +q2*omeg_z);
    double dq2 = 0.5 * (+q3*omeg_x +q0*omeg_y -q1*omeg_z);
    double dq3 = 0.5 * (-q2*omeg_x +q1*omeg_y +q0*omeg_z);

    q0 += dt*dq0;
    q1 += dt*dq1;
    q2 += dt*dq2;
    q3 += dt*dq3;

    normalizeQuaternion(q0, q1, q2, q3);
}

void QuaternionIntegration_Exponential(
        double dt,
        double omeg_x, double omeg_y, double omeg_z,
        double& q0, double& q1, double& q2, double& q3)
{
    // The angular velocity is given in body frame
    /* Quaternion Exponential method
     * q_new = q0 * exp(1/2*dt*q_omeg)
     * q_omeg = [0,omeg_x,omeg_y,omeg_z]
     */

    double omeg_norm = sqrt(omeg_x*omeg_x + omeg_y*omeg_y + omeg_z*omeg_z);

    double q_exp0, q_exp1, q_exp2, q_exp3;

    if (omeg_norm > 0) {
        double sinOmeg = sin(0.5 * dt * omeg_norm);
        q_exp0 = cos(0.5 * dt * omeg_norm);
        q_exp1 = sinOmeg * omeg_x / omeg_norm;
        q_exp2 = sinOmeg * omeg_y / omeg_norm;
        q_exp3 = sinOmeg * omeg_z / omeg_norm;
    } else {
        // unit quaternion since the angular velocity is zero (no movement)
        q_exp0 = 1.0;
        q_exp1 = 0.0;
        q_exp2 = 0.0;
        q_exp3 = 0.0;
    }

    // q_new = Phi(q0) * q_exp
    double q0_new = +q0*q_exp0 -q1*q_exp1 -q2*q_exp2 -q3*q_exp3;
    double q1_new = +q1*q_exp0 +q0*q_exp1 -q3*q_exp2 +q2*q_exp3;
    double q2_new = +q2*q_exp0 +q3*q_exp1 +q0*q_exp2 -q1*q_exp3;
    double q3_new = +q3*q_exp0 -q2*q_exp1 +q1*q_exp2 +q0*q_exp3;

    // Update quaternion
    q0 = q0_new;
    q1 = q1_new;
    q2 = q2_new;
    q3 = q3_new;
}

void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3)
{
    double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 /= norm;
    q1 /= norm;
    q2 /= norm;
    q3 /= norm;
}


/**
 * @file quasiinterpolation_main.cc
 * @brief NPDE exam TEMPLATE MAIN FILE
 * @author Oliver Rietmann
 * @date 15.07.2020
 * @copyright Developed at SAM, ETH Zurich
 */

#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

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

#include "iohelper.h"
#include "quasiinterpolation.h"

constexpr double PI = 3.14159265358979323846;
// A bump function
double u1(Eigen::Vector2d x) {
  double abs_x = x.norm();
  return abs_x < 0.5 ? std::cos(PI * abs_x) : 0.0;
};
// The gradient of the bump function
Eigen::Vector2d grad_u1(Eigen::Vector2d x) {
  double abs_x = x.norm();
  if (abs_x == 0.0 || abs_x >= 0.5)
    return Eigen::Vector2d::Zero();
  else
    return -PI * std::sin(PI * abs_x) / abs_x * x;
};

// A simplex auxiliary function
inline double square(double y) { return y * y; }

// A smoother bump function
double u2(Eigen::Vector2d x) { return square(u1(x)); };
// ... and its gradient
Eigen::Vector2d grad_u2(Eigen::Vector2d x) { return 2.0 * u1(x) * grad_u1(x); };

int main(int /* argc */, char** /*argv*/) {
  // Build mesh hierarchy
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory),
                            CURRENT_SOURCE_DIR "/../meshes/mesh.msh");
  std::shared_ptr<lf::mesh::Mesh> base_mesh_p{reader.mesh()};
  std::shared_ptr<lf::refinement::MeshHierarchy> mesh_hierarchy_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(base_mesh_p, 6);

  // Compute meshwidth for each refinement
  int L = mesh_hierarchy_p->NumLevels();
  Eigen::VectorXd meshwidth(L);
  for (int k = 0; k < L; ++k) {
    std::shared_ptr<const lf::mesh::Mesh> mesh_p = mesh_hierarchy_p->getMesh(k);
    auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
    meshwidth(k) = QuasiInterpolation::maxLength(mesh_p->Entities(1));
  }
  {
    // Errors for u1
    lf::mesh::utils::MeshFunctionGlobal u1_mf(u1);
    lf::mesh::utils::MeshFunctionGlobal grad_u1_mf(grad_u1);
    // Retrieve error norms
    auto [l2_error, h1_error] = QuasiInterpolation::interpolationError(
        mesh_hierarchy_p, u1_mf, grad_u1_mf);
    QuasiInterpolation::printError(meshwidth, l2_error, h1_error,
                                   "errors for u1");
    QuasiInterpolation::writeCSV(meshwidth, l2_error, h1_error,
                                 CURRENT_BINARY_DIR "/convergence_u1.csv");
    // Call a Python script to generate plots
    std::system("python3 " CURRENT_SOURCE_DIR
                "/plot_convergence.py " CURRENT_BINARY_DIR
                "/convergence_u1.csv " CURRENT_BINARY_DIR
                "/convergence_u1.eps 'Interpolation error for $u(x)=u_1(x)$'");
  }
  {
    // Errors for u2
    lf::mesh::utils::MeshFunctionGlobal u2_mf(u2);
    lf::mesh::utils::MeshFunctionGlobal grad_u2_mf(grad_u2);
    auto [l2_error, h1_error] = QuasiInterpolation::interpolationError(
        mesh_hierarchy_p, u2_mf, grad_u2_mf);
    QuasiInterpolation::printError(meshwidth, l2_error, h1_error,
                                   "errors for u2");
    QuasiInterpolation::writeCSV(meshwidth, l2_error, h1_error,
                                 CURRENT_BINARY_DIR "/convergence_u2.csv");
    std::system("python3 " CURRENT_SOURCE_DIR
                "/plot_convergence.py " CURRENT_BINARY_DIR
                "/convergence_u2.csv " CURRENT_BINARY_DIR
                "/convergence_u2.eps 'Interpolation error for $u(x)=u_2(x)$'");
  }
  return 0;
}


#pragma once

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

/* Creates a matrix that Rotate vector about the z axis
 * Currently needed for our robot relative coordinate system
 */
template <typename T>
boost::numeric::ublas::matrix<T> rotateZMatrix(T theta);

/* Creates a translation matrix
 */
template <typename T>
boost::numeric::ublas::matrix<T> translateMatrix(T x, T y, T z);

/* Creates a projection matrix
 */
template <typename T>
boost::numeric::ublas::matrix<T> projectionMatrix(T ex, T ey,
                                                  T ez);

/* Creates a DH matrix used in Kinematics
 */
template <typename T>
boost::numeric::ublas::matrix<T> createDHMatrix(T a, T alpha,
                                                T d, T theta);

/* Function to invert matrix
 */
template <typename T>
bool invertMatrix(const boost::numeric::ublas::matrix<T>& input,
                  boost::numeric::ublas::matrix<T>& inverse);

/* Creates a 4,1 vector */
template <typename T>
inline boost::numeric::ublas::matrix<T>
vec4(T a, T b, T c, T d) {
   boost::numeric::ublas::matrix<T> m(4, 1);
   m(0, 0) = a;
   m(1, 0) = b;
   m(2, 0) = c;
   m(3, 0) = d;
   return m;
}

/* Creates a 4,1 vector from a 4,1 array */
template <typename T>
inline boost::numeric::ublas::matrix<T>
vec4(const T a[]) {
   boost::numeric::ublas::matrix<T> m(4, 1);
   m(0, 0) = a[0];
   m(1, 0) = a[1];
   m(2, 0) = a[2];
   m(3, 0) = a[3];
   return m;
}

#include "utils/matrix_helpers.tcc"