#include <Core/Core.h>
#include <Eigen/Eigen.h>

using namespace Upp;

using namespace Eigen;

void NonLinearTests();
void FFTTests();

struct SerialTest {
	MatrixXd m;
	VectorXd v;
	SerialTest() : m(2, 2), v(3){}
	void Print() {
		Cout() << "\nHere is the matrix:\n" << m;
		Cout() << "\nHere is the vector:\n" << v;
	}
	void Serialize(Stream& stream) {
		::Serialize(stream, m);
		::Serialize(stream, v);
	}
	void Jsonize(JsonIO &jio) {
		jio("matrix", m)("vector", v);
	}
	void Xmlize(XmlIO &xml) {
		xml("matrix", m)("vector", v);
	}
};

CONSOLE_APP_MAIN
{
	StdLogSetup(LOG_COUT|LOG_FILE);

	UppLog() << "Eigen library demo";

	// https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html
	UppLog() << "\n\nTutorial page 1 - The Matrix class";

	UppLog() << "\n\nCoefficient accessors";
	{
		MatrixXd m(2,2);
		m(0,0) = 3;
		m(1,0) = 2.5;
		m(0,1) = -1;
		m(1,1) = m(1,0) + m(0,1);
		UppLog() << "\nHere is the matrix m:\n" << m;

		VectorXd v(2);
		v(0) = 4;
		v(1) = v(0) - 1;
		UppLog() << "\nHere is the vector v:\n" << v;
	}
	UppLog() << "\n\nResizing";
	{
		MatrixXd m(2,5);
		m.resize(4,3);
		UppLog() << "\nThe matrix m is of size " << m.rows() << "x" << m.cols();
		UppLog() << "\nIt has " << m.size() << " coefficients";

		VectorXd v(2);
		v.resize(5);
		UppLog() << "\nThe vector v is of size " << v.size();
		UppLog() << "\nAs a matrix, v is of size " << v.rows() << "x" << v.cols();
	}
	UppLog() << "\n\nAssignment and resizing";
	{
		double _dat[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17};// Assignment from C vector
		VectorXd dat = Map<VectorXd>(_dat, sizeof(_dat)/sizeof(double));
		UppLog() << "\nC array data is " << dat.transpose();

		const int dec = 5;
    	VectorXd decimated = Map<VectorXd, 0, InnerStride<dec>>(dat.data(), 1+((dat.size()-1)/dec));
    	UppLog() << "\nDecimated " << decimated.transpose();

		VectorXd even = Map<VectorXd, 0, InnerStride<2>>(dat.data(), (dat.size()+1)/2);
		UppLog() << "\nEven " << even.transpose();

		VectorXd odd = Map<VectorXd, 0, InnerStride<2>>(dat.data()+1, dat.size()/2);
		UppLog() << "\nOdd  " << odd.transpose();

		MatrixXf a(2,2);
		UppLog() << "\na is of size " << a.rows() << "x" << a.cols();
		MatrixXf b(3,3);
		a = b;
		UppLog() << "\na is now of size " << a.rows() << "x" << a.cols();
	}

	// https://eigen.tuxfamily.org/dox-devel/group__TutorialMatrixArithmetic.html
	UppLog() << "\n\nTutorial page 2 - Matrix and vector arithmetic";

	UppLog() << "\n\nAddition and subtraction";
	{
		Matrix2d a;
		a << 1, 2,
		     3, 4;
		MatrixXd b(2,2);
		b << 2, 3,
		     1, 4;

		UppLog() << "\na + b =\n" << a + b;
		UppLog() << "\na - b =\n" << a - b;
		UppLog() << "\nDoing a += b;";
		a += b;
		UppLog() << "\nNow a =\n" << a;

		Vector3d v(1,2,3);
		Vector3d w(1,0,0);
		UppLog() << "\n-v + w - v =\n" << -v + w - v;
	}
	UppLog() << "\n\nScalar multiplication and division";
	{
		Matrix2d a;
		a << 1, 2,
		     3, 4;
		Vector3d v(1,2,3);
		UppLog() << "\na * 2.5 =\n" << a * 2.5;
		UppLog() << "\n0.1 * v =\n" << 0.1 * v;
		UppLog() << "\nDoing v *= 2;";
		v *= 2;
		UppLog() << "\nNow v =\n" << v;
	}
	UppLog() << "\n\nTransposition and conjugation";
	{
		MatrixXcf a = MatrixXcf::Random(2,2);
		UppLog() << "\nHere is the matrix a\n" << a;
		UppLog() << "\nHere is the matrix a^T\n" << a.transpose();
		UppLog() << "\nHere is the conjugate of a\n" << a.conjugate();
		UppLog() << "\nHere is the matrix a^*\n" << a.adjoint();

		VectorXd v(5);
		v << 1, 2, 3, 4, 5;
		UppLog() << "\n\nInitial vector  " << v.transpose();
		UppLog() <<   "\nReversed vector " << v.reverse().transpose();
	}
	UppLog() << "\n\nMatrix-matrix and matrix-vector multiplication";
	{
		Matrix2d mat;
		mat << 1, 2,
		       3, 4;
		Vector2d u(-1,1), v(2,0);
		UppLog() << "\nHere is mat*mat:\n" << mat*mat;
		UppLog() << "\nHere is mat*u:\n" << mat*u;
		UppLog() << "\nHere is u^T*mat:\n" << u.transpose()*mat;
		UppLog() << "\nHere is u^T*v:\n" << u.transpose()*v;
		UppLog() << "\nHere is u*v^T:\n" << u*v.transpose();
		UppLog() << "\nLet's multiply mat by itself";
		mat *= mat;
		UppLog() << "\nNow mat is mat:\n" << mat;
	}
	UppLog() << "\n\nDot product and cross product";
	{
		Vector3d v(1,2,3);
	  	Vector3d w(0,1,2);

	  	UppLog() << "\nDot product: " << v.dot(w);
	  	double dp = v.adjoint()*w; // automatic conversion of the inner product to a scalar
	  	UppLog() << "\nDot product via a matrix product: " << dp;
	  	UppLog() << "\nCross product:\n" << v.cross(w);
	}
	UppLog() << "\n\nBasic arithmetic reduction operations";
	{
		Eigen::Matrix2d mat;
		mat << 1, 2,
		       3, 4;
		UppLog() << "\nHere is mat.sum():       " << mat.sum();
		UppLog() << "\nHere is mat.prod():      " << mat.prod();
		UppLog() << "\nHere is mat.mean():      " << mat.mean();
		UppLog() << "\nHere is mat.minCoeff():  " << mat.minCoeff();
		UppLog() << "\nHere is mat.maxCoeff():  " << mat.maxCoeff();
		UppLog() << "\nHere is mat.trace():     " << mat.trace();

		Matrix3f m = Matrix3f::Random();
		ptrdiff_t i, j;
		float minOfM = m.minCoeff(&i, &j);
		UppLog() << "\nHere is the matrix m:\n" << m;
		UppLog() << "\nIts minimum coefficient (" << minOfM
		       << ") is at position (" << i << "," << j << ")\n";

		RowVector4i v = RowVector4i::Random();
		ptrdiff_t maxOfV = v.maxCoeff(&i);
		UppLog() << "\nHere is the vector v: " << v;
		UppLog() << "\nIts maximum coefficient (" << maxOfV
			   << ") is at position " << i;
	}

	// https://eigen.tuxfamily.org/dox/group__TutorialArrayClass.html
	UppLog() << "\n\nTutorial page 3 - The Array class and coefficient-wise operations ";

	UppLog() << "\n\nAccessing values inside an Array";
	{
		ArrayXXf  m(2,2);

		// assign some values coefficient by coefficient
		m(0,0) = 1.0; m(0,1) = 2.0;
		m(1,0) = 3.0; m(1,1) = m(0,1) + m(1,0);

		// print values to standard output
		UppLog() << "\n" << m;

		// using the comma-initializer is also allowed
		m << 1.0,2.0,
		   	 3.0,4.0;

		// print values to standard output
		UppLog() << "\n" << m;
	}
	UppLog() << "\n\nAddition and subtraction";
	{
		ArrayXXf a(3,3);
		ArrayXXf b(3,3);
		a << 1,2,3,
		   	 4,5,6,
		   	 7,8,9;
		b << 1,2,3,
		   	 1,2,3,
		   	 1,2,3;

		// Adding two arrays
		UppLog() << "\na + b = " << "\n" << a + b;

		// Subtracting a scalar from an array
		UppLog() << "\na - 2 = " << "\n" << a - 2;
	}
	UppLog() << "\n\nArray multiplication";
	{
		ArrayXXf a(2,2);
		ArrayXXf b(2,2);
		a << 1,2,
		   	 3,4;
		b << 5,6,
		   	 7,8;
		UppLog() << "\na * b = " << "\n" << a * b;
	}
	UppLog() << "\n\nOther coefficient-wise operations";
	{
		ArrayXf a = ArrayXf::Random(5);
		a *= 2;
		UppLog() << "\na =" << "\n" << a;
		UppLog() << "\na.abs() =" << "\n" << a.abs();
		UppLog() << "\na.abs().sqrt() =" << "\n" << a.abs().sqrt();
		UppLog() << "\na.min(a.abs().sqrt()) =" << "\n" << a.min(a.abs().sqrt());
	}
	UppLog() << "\n\nConverting between array and matrix expressions";
	{
		MatrixXf m(2,2);
		MatrixXf n(2,2);
		m << 1,2,
		   	 3,4;
		n << 5,6,
		   	 7,8;

		UppLog() << "\n-- Matrix m*n: --" << "\n" << m * n;
		UppLog() << "\n-- Array m*n: --" << "\n" << m.array() * n.array();
		UppLog() << "\n-- With cwiseProduct: --" << "\n" << m.cwiseProduct(n);
		UppLog() << "\n-- Array m + 4: --" << "\n" << m.array() + 4;
	}
	{
		MatrixXf m(2,2);
		MatrixXf n(2,2);
		m << 1,2,
		     3,4;
		n << 5,6,
		     7,8;

		UppLog() << "\n-- Combination 1: --" << "\n" << (m.array() + 4).matrix() * m;
		UppLog() << "\n-- Combination 2: --" << "\n" << (m.array() * n.array()).matrix() * m;
	}

	// https://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html
	UppLog() << "\n\nTutorial page 4 - Block operations";

	UppLog() << "\n\nUsing block operations";
	{
		Eigen::MatrixXf m(4,4);
		m << 1, 2, 3, 4,
		     5, 6, 7, 8,
		     9,10,11,12,
		   	 13,14,15,16;
		UppLog() << "\nBlock in the middle\n";
		UppLog() << m.block<2,2>(1,1);
		for (ptrdiff_t i = 1; i <= 3; ++i) {
			UppLog() << "\nBlock of size " << i << "x" << i << "\n";
			UppLog() << m.block(0, 0, i, i);
		}
	}
	{
		Array22d m;
		m << 1,2,
		   	 3,4;
		Array44d a = Array44d::Constant(0.6);
		UppLog() << "\nHere is the array a:\n" << a;
		a.block<2,2>(1,1) = m;
		UppLog() << "\nHere is now a with m copied into its central 2x2 block:\n" << a;
		a.block(0,0,2,3) = a.block(2,1,2,3);
		UppLog() << "\nHere is now a with bottom-right 2x3 block copied into top-left 2x2 block:\n" << a;
	}
	UppLog() << "\n\nColumns and rows";
	{
		Eigen::MatrixXf m(3,3);
		m << 1,2,3,
			 4,5,6,
		   	 7,8,9;
		UppLog() << "\nHere is the matrix m:\n" << m;
		UppLog() << "\n2nd Row: " << m.row(1);
		m.col(2) += 3 * m.col(0);
		UppLog() << "\nAfter adding 3 times the first column into the third column, the matrix m is:\n";
		UppLog() << m;
	}
	UppLog() << "\n\nCorner-related operations";
	{
		Eigen::Matrix4f m;
		m << 1, 2, 3, 4,
		   	 5, 6, 7, 8,
		   	 9, 10,11,12,
		   	 13,14,15,16;
		UppLog() << "\nm.leftCols(2) =\n" << m.leftCols(2);
		UppLog() << "\nm.bottomRows<2>() =\n" << m.bottomRows<2>();
		m.topLeftCorner(1,3) = m.bottomRightCorner(3,1).transpose();
		UppLog() << "\nAfter assignment, m = \n" << m;
	}
	UppLog() << "\n\nBlock operations for vectors. Get a segment or fragment";
	{
		Eigen::ArrayXf v(6);
		v << 1, 2, 3, 4, 5, 6;
		UppLog() << "\nv.head(3) =\n" << v.head(3);
		UppLog() << "\nv.tail<3>() = \n" << v.tail<3>();
		v.segment(1,4) *= 2;
		UppLog() << "\nafter 'v.segment(1,4) *= 2', v =\n" << v;
	}

	// https://eigen.tuxfamily.org/dox/group__TutorialAdvancedInitialization.html
	UppLog() << "\n\nTutorial page 5 - Advanced initialization";

	UppLog() << "\n\nThe comma initializer";
	{
		RowVectorXd vec1(3);
		vec1 << 1, 2, 3;
		UppLog() << "\nvec1 = " << vec1;

		RowVectorXd vec2(4);
		vec2 << 1, 4, 9, 16;
		UppLog() << "\nvec2 = " << vec2;

		RowVectorXd joined(7);
		joined << vec1, vec2;
		UppLog() << "\njoined = " << joined;

		MatrixXf matA(2, 2);
		matA << 1, 2, 3, 4;
		MatrixXf matB(4, 4);
		matB << matA, matA/10, matA/10, matA;
		UppLog() << matB;

		Matrix3f m;
		m.row(0) << 1, 2, 3;
		m.block(1,0,2,2) << 4, 5, 7, 8;
		m.col(2).tail(2) << 6, 9;
		UppLog() << m;
	}
	UppLog() << "\n\nSpecial matrices and arrays";
	{
		UppLog() << "\nA fixed-size array:\n";
		Array33f a1 = Array33f::Zero();
		UppLog() << a1 << "\n\n";

		UppLog() << "\nA one-dimensional dynamic-size array:\n";
		ArrayXf a2 = ArrayXf::Zero(3);
		UppLog() << a2 << "\n\n";

		UppLog() << "\nA two-dimensional dynamic-size array:\n";
		ArrayXXf a3 = ArrayXXf::Zero(3, 4);
		UppLog() << a3 << "\n";

		UppLog() << "\nA two-dimensional dynamic-size array set to 1.23:\n";
		MatrixXd a4 = MatrixXd::Constant(3, 4, 1.23);
		UppLog() << a4 << "\n";

		ArrayXXd table(10, 4);
		table.col(0) = ArrayXd::LinSpaced(10, 0, 90);
		table.col(1) = M_PI / 180 * table.col(0);
		table.col(2) = table.col(1).sin();
		table.col(3) = table.col(1).cos();
		UppLog() << "\n  Degrees   Radians      Sine    Cosine\n";
		UppLog() << table;

		const ptrdiff_t size = 6;
		MatrixXd mat1(size, size);
		mat1.topLeftCorner(size/2, size/2)     = MatrixXd::Zero(size/2, size/2);
		mat1.topRightCorner(size/2, size/2)    = MatrixXd::Identity(size/2, size/2);
		mat1.bottomLeftCorner(size/2, size/2)  = MatrixXd::Identity(size/2, size/2);
		mat1.bottomRightCorner(size/2, size/2) = MatrixXd::Zero(size/2, size/2);
		UppLog() << "\n" << mat1;

		MatrixXd mat2(size, size);
		mat2.topLeftCorner(size/2, size/2).setZero();
		mat2.topRightCorner(size/2, size/2).setIdentity();
		mat2.bottomLeftCorner(size/2, size/2).setIdentity();
		mat2.bottomRightCorner(size/2, size/2).setZero();
		UppLog() << "\n" << mat2;

		MatrixXd mat3(size, size);
		mat3 << MatrixXd::Zero(size/2, size/2), MatrixXd::Identity(size/2, size/2),
		        MatrixXd::Identity(size/2, size/2), MatrixXd::Zero(size/2, size/2);
		UppLog() << "\n" << mat3;
	}
	UppLog() << "\n\nUsage as temporary objects";
	{
		MatrixXd m = MatrixXd::Random(3,3);
		m = (m + MatrixXd::Constant(3,3,1.2)) * 50;
		UppLog() << "\nm =\n" << m;
		VectorXd v(3);
		v << 1, 2, 3;
		UppLog() << "\nm * v =\n" << m * v;
	}
	{
		MatrixXf mat = MatrixXf::Random(2, 3);
		UppLog() << mat;
		mat = (MatrixXf(2,2) << 0, 1, 1, 0).finished() * mat;
		UppLog() << mat;
	}

	// https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
	UppLog() << "\n\nTutorial page 6 - Linear algebra and decompositions";

	UppLog() << "\n\nBasic linear solving	Ax = b";
	{
		Matrix3f A;
		Vector3f b;
		A << 1, 2, 3,
			 4, 5, 6,
			 7, 8,10;
		b << 3, 3, 4;
		UppLog() << "\nHere is the matrix A:\n" << A;
		UppLog() << "\nHere is the vector b:\n" << b;
		Vector3f x = A.colPivHouseholderQr().solve(b);
		UppLog() << "\nThe solution is:\n" << x;
	}
	{
		Matrix2f A, b;
		A << 2, -1, -1, 3;
		b << 1, 2, 3, 1;
		UppLog() << "\nHere is the matrix A:\n" << A;
		UppLog() << "\nHere is the right hand side b:\n" << b;
		Matrix2f x = A.ldlt().solve(b);
		UppLog() << "\nThe solution is:\n" << x;
	}
	UppLog() << "\n\nChecking if a solution really exists";
	{
		MatrixXd A = MatrixXd::Random(100,100);
		MatrixXd b = MatrixXd::Random(100,50);
		MatrixXd x = A.fullPivLu().solve(b);
		double relative_error = (A*x - b).norm() / b.norm(); // norm() is L2 norm
		UppLog() << "\nThe relative error is:\n" << relative_error;
	}
	UppLog() << "\n\nComputing eigenvalues and eigenvectors";
	{
		Matrix2f A;
		A << 1, 2, 2, 3;
		UppLog() << "\nHere is the matrix A:\n" << A;
		SelfAdjointEigenSolver<Matrix2f> eigensolver(A);
		UppLog() << "\nThe eigenvalues of A are:\n" << eigensolver.eigenvalues();
		UppLog() << "\nHere's a matrix whose columns are eigenvectors of A "
		    << "corresponding to these eigenvalues:\n"
		    << eigensolver.eigenvectors();
	}
	UppLog() << "\n\nComputing inverse and determinant";
	{
		Matrix3f A;
		A << 1, 2, 1,
		     2, 1, 0,
		    -1, 1, 2;
		UppLog() << "\nHere is the matrix A:\n" << A;
		UppLog() << "\nThe determinant of A is " << A.determinant();
		UppLog() << "\nThe inverse of A is:\n" << A.inverse();
	}
	UppLog() << "\n\nLeast squares solving";
	{
		MatrixXf A = MatrixXf::Random(5, 2);
		UppLog() << "\nHere is the matrix A:\n" << A;
		VectorXf b = VectorXf::Random(5);
		UppLog() << "\nHere is the right hand side b:\n" << b;
		UppLog() << "\nThe least-squares solution is:\n"
		       << A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
	}
	UppLog() << "\n\nSeparating the computation from the construction";
	{
		Matrix2f A, b;
		LLT<Matrix2f> llt;
		A << 2, -1, -1, 3;
		b << 1, 2, 3, 1;
		UppLog() << "\nHere is the matrix A:\n" << A;
		UppLog() << "\nHere is the right hand side b:\n" << b;
		UppLog() << "\nComputing LLT decomposition...";
		llt.compute(A);
		UppLog() << "\nThe solution is:\n" << llt.solve(b);
		A(1,1)++;
		UppLog() << "\nThe matrix A is now:\n" << A;
		UppLog() << "\nComputing LLT decomposition...";
		llt.compute(A);
		UppLog() << "\nThe solution is now:\n" << llt.solve(b);
	}
	UppLog() << "\n\nRank-revealing decompositions";
	{
		Matrix3f A;
		A << 1, 2, 5,
		     2, 1, 4,
		     3, 0, 3;
		UppLog() << "\nHere is the matrix A:\n" << A;
		FullPivLU<Matrix3f> lu_decomp(A);
		UppLog() << "\nThe rank of A is " << lu_decomp.rank();
		UppLog() << "\nHere is a matrix whose columns form a basis of the null-space of A:\n"
		    << lu_decomp.kernel();
		UppLog() << "\nHere is a matrix whose columns form a basis of the column-space of A:\n"
		    << lu_decomp.image(A); // yes, have to pass the original A
	}
	{
		Matrix2d A;
		A << 2, 1,
		     2, 0.9999999999;
		FullPivLU<Matrix2d> lu(A);
		UppLog() << "\nBy default, the rank of A is found to be " << lu.rank();
		lu.setThreshold(1e-5);
		UppLog() << "\nWith threshold 1e-5, the rank of A is found to be " << lu.rank();
	}

	// https://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html
	UppLog() << "\n\nTutorial page 7 - Reductions, visitors and broadcasting";

	UppLog() << "\n\nReductions";
	{
		Eigen::Matrix2d mat;
		mat << 1, 2,
			   3, 4;
		UppLog() << "\nHere is mat.sum():       " << mat.sum();
		UppLog() << "\nHere is mat.prod():      " << mat.prod();
		UppLog() << "\nHere is mat.mean():      " << mat.mean();
		UppLog() << "\nHere is mat.minCoeff():  " << mat.minCoeff();
		UppLog() << "\nHere is mat.maxCoeff():  " << mat.maxCoeff();
		UppLog() << "\nHere is mat.trace():     " << mat.trace();
	}
	UppLog() << "\n\nNorm computations";
	{
		VectorXf v(2);
		MatrixXf m(2,2), n(2,2);

		v << -1,
		   	  2;

		m << 1,-2,
			-3, 4;

		UppLog() << "\nv.squaredNorm() = " << v.squaredNorm();
		UppLog() << "\nv.norm() = " << v.norm();
		UppLog() << "\nv.lpNorm<1>() = " << v.lpNorm<1>();
		UppLog() << "\nv.lpNorm<Infinity>() = " << v.lpNorm<Infinity>();

		UppLog() << "\n";
		UppLog() << "\nm.squaredNorm() = " << m.squaredNorm();
		UppLog() << "\nm.norm() = " << m.norm();
		UppLog() << "\nm.lpNorm<1>() = " << m.lpNorm<1>();
		UppLog() << "\nm.lpNorm<Infinity>() = " << m.lpNorm<Infinity>();
	}
	UppLog() << "\n\nBoolean reductions";
	{
		ArrayXXf a(2,2);

		a << 1,2,
		   	 3,4;

		UppLog() << "\n(a > 0).all()   = " << (a > 0).all();
		UppLog() << "\n(a > 0).any()   = " << (a > 0).any();
		UppLog() << "\n(a > 0).count() = " << (a > 0).count();
		UppLog() << "\n";
		UppLog() << "\n(a > 2).all()   = " << (a > 2).all();
		UppLog() << "\n(a > 2).any()   = " << (a > 2).any();
		UppLog() << "\n(a > 2).count() = " << (a > 2).count();
	}
	UppLog() << "\n\nVisitors";
	{
		Eigen::MatrixXf m(2,2);

		m << 1, 2,
		   	 3, 4;

		//get location of maximum
		MatrixXf::Index maxRow, maxCol;
		float max = m.maxCoeff(&maxRow, &maxCol);

		//get location of minimum
		MatrixXf::Index minRow, minCol;
		float min = m.minCoeff(&minRow, &minCol);

		UppLog() << "\nMax: " << max <<  ", at: " << maxRow << "," << maxCol;
		UppLog() << "\nMin: " << min << ", at: " << minRow << "," << minCol;
	}
	UppLog() << "\n\nPartial reductions";
	{
		Eigen::MatrixXf mat(2,4);
		mat << 1, 2, 6, 9,
		       3, 1, 7, 2;

		UppLog() << "\nColumn's maximum: \n" << mat.colwise().maxCoeff();
	}
	{
		Eigen::MatrixXf mat(2,4);
		mat << 1, 2, 6, 9,
		       3, 1, 7, 2;

		UppLog() << "\nRow's maximum: \n" << mat.rowwise().maxCoeff();
	}
	UppLog() << "\n\nCombining partial reductions with other operations";
	{
		MatrixXf mat(2,4);
		mat << 1, 2, 6, 9,
		       3, 1, 7, 2;

		MatrixXf::Index   maxIndex;
		float maxNorm = mat.colwise().sum().maxCoeff(&maxIndex);

		UppLog() << "\nMaximum sum at position " << maxIndex;

		UppLog() << "\nThe corresponding vector is: ";
		UppLog() << "\n" << mat.col( maxIndex );
		UppLog() << "\nAnd its sum is is: " << maxNorm;
	}
	UppLog() << "\n\nBroadcasting";
	{
		Eigen::MatrixXf mat(2,4);
		Eigen::VectorXf v(2);

		mat << 1, 2, 6, 9,
		       3, 1, 7, 2;

		v << 0,
		     1;

		//add v to each column of m
		mat.colwise() += v;

		UppLog() << "\nBroadcasting result: ";
		UppLog() << "\n" << mat;
	}
	{
		Eigen::MatrixXf mat(2,4);
		Eigen::VectorXf v(4);

		mat << 1, 2, 6, 9,
		       3, 1, 7, 2;

		v << 0,1,2,3;

		//add v to each row of m
		mat.rowwise() += v.transpose();

		UppLog() << "\nBroadcasting result: ";
		UppLog() << "\n" << mat;
	}
	UppLog() << "\n\nCombining broadcasting with other operations";
	{
		Eigen::MatrixXf m(2,4);
		Eigen::VectorXf v(2);

		m << 1, 23, 6, 9,
		   	 3, 11, 7, 2;

		v << 2,
		     3;

		MatrixXf::Index index;
		// find nearest neighbour
		(m.colwise() - v).colwise().squaredNorm().minCoeff(&index);

		UppLog() << "\nNearest neighbour is column " << index << ":";
		UppLog() << "\n" << m.col(index);
	}

	UppLog() << "\n\nSerializing tests";
	{
		SerialTest serialTest, serialTest_j, serialTest_x, serialTest_s;
		serialTest.m << 1, 2,
		     			4, 8;
		serialTest.v << 1, 2, 4;

		StoreAsJsonFile(serialTest, GetExeDirFile("Json.txt"));
		LoadFromJsonFile(serialTest_j, GetExeDirFile("Json.txt"));
		UppLog() << "\nJSON demo";
		serialTest_j.Print();

		StoreAsXMLFile(serialTest, "XMLdata", GetExeDirFile("Xml.txt"));
		LoadFromXMLFile(serialTest_x, GetExeDirFile("Xml.txt"));
		UppLog() << "\nXML demo";
		serialTest_x.Print();

		StoreToFile(serialTest, GetExeDirFile("Serial.dat"));
		LoadFromFile(serialTest_s, GetExeDirFile("Serial.dat"));
		UppLog() << "\nSerialization demo";
		serialTest_s.Print();
	}

	NonLinearTests();

	FFTTests();

	#ifdef flagDEBUG
	Cout() << "\nPress enter key to end";
	ReadStdIn();
	#endif
}


#include "Euclidean.h"

#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;

Euclidean::Euclidean()
{};

Euclidean::~Euclidean()
{};

int256_t Euclidean::extendedEuclidean(int256_t a, int256_t b, int256_t* x, int256_t* s)
{
  if ((a == 0) || (b == 0))
  {
    return 0;
  }

  int256_t q = 0;
  int256_t r = 0;

  q = a / b;

  //remainder
  r = a % b;

  int256_t x_tmp = *x;
  int256_t s_tmp = *s;
  if (r != 0)
  {
    extendedEuclidean(b, r, &x_tmp, &s_tmp);
  }

  if (r == 0)
  {
    *x = 0;
    *s = 1;

    return b;
  }

  *x = s_tmp;
  *s = x_tmp - (q * s_tmp);

  return b;
}

int256_t Euclidean::euclidean(int256_t a, int256_t b)
{

  if ((a == 0) || (b == 0))
  {
    return 0;
  }

  int256_t q;
  int256_t r;

  do
  {
    q = a / b;
    r = a % b;

    a = b;
    b = r;
  } while (r != 0);

  return a;
}


#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include <iostream>
#include <random>

class MultiVariateNormal
{
public:
  MultiVariateNormal(const Eigen::VectorXd mean, const Eigen::MatrixXd covariance)
  : mean_(mean), covariance_(covariance)
  {
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigen_solver(covariance_);
    transform_ = eigen_solver.eigenvectors() * eigen_solver.eigenvalues().cwiseSqrt().asDiagonal();
  }
  double pdf(const Eigen::VectorXd & x) const
  {
    double n = x.rows();
    double a = (x - mean_).transpose() * covariance_.inverse() * (x - mean_);
    double b = std::pow(std::sqrt(2 * M_PI), n) * std::sqrt(covariance_.determinant());
    return std::exp(-0.5 * a) / b;
  }

  Eigen::VectorXd operator()() const
  {
    static std::random_device seed;
    static std::mt19937 engine(seed());
    static std::normal_distribution<> dist;

    return mean_ + transform_ * Eigen::VectorXd{mean_.size()}.unaryExpr([&](auto x) { return dist(engine); });
  }
  void setMean(const Eigen::VectorXd mean) { mean_ = mean; }
  void setCovariance(const Eigen::MatrixXd covariance) { covariance_ = covariance; }

private:
  Eigen::VectorXd mean_;
  Eigen::MatrixXd covariance_;
  Eigen::MatrixXd transform_;
};

int main(int argc, char ** argv)
{
  Eigen::VectorXd mean(2);
  Eigen::MatrixXd covariance(2, 2);

  mean << 0.0, 0.0;
  covariance << 1.0, 0.1, 0.1, 1.0;

  MultiVariateNormal multi_variate_normal(mean, covariance);

  Eigen::VectorXd x(2);
  x << 0.2, 0.3;
  std::cout << multi_variate_normal.pdf(x) << std::endl;

  return 0;
}


// Tester.cpp : Defines the entry point for the console application.
//

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

int DemoConsoleOutput();
int MatrixMultiplication();
int TransposeProperties();
int MultiplyingTransposedVectors();
int MatrixTransposedVectorMultiplication();
int InnerProduct();

int main()
{
	std::cout << "Linear Algebra: Concepts and Methods with Eigen" << std::endl;
	//MatrixMultiplication();
	//TransposeProperties();
	//MultiplyingTransposedVectors();
	//MatrixTransposedVectorMultiplication();
	//InnerProduct();
	Chapter3 chapter3;
	chapter3.IntroduceSelf();
	return 0;
}

int MatrixMultiplication()
{
	std::cout << "1.10: Matrix Multiplication" << std::endl;
	Eigen::MatrixXd matrix1(4, 3);
	matrix1(0, 0) = 1;
	matrix1(1, 0) = 2;
	matrix1(2, 0) = 1;
	matrix1(3, 0) = 2;

	matrix1(0, 1) = 1;
	matrix1(1, 1) = 0;
	matrix1(2, 1) = 2;
	matrix1(3, 1) = 2;

	matrix1(0, 2) = 1;
	matrix1(1, 2) = 1;
	matrix1(2, 2) = 4;
	matrix1(3, 2) = -1;

	std::cout << "Matrix 1:" << std::endl;
	std::cout << matrix1 << std::endl;

	Eigen::MatrixXd matrix2(3, 2);
	matrix2(0, 0) = 3;
	matrix2(1, 0) = 1;
	matrix2(2, 0) = -1;

	matrix2(0, 1) = 0;
	matrix2(1, 1) = 1;
	matrix2(2, 1) = 3;

	std::cout << "Matrix 2:" << std::endl;
	std::cout << matrix2 << std::endl;

	std::cout << "Matrix 1 * Matrix 2:" << std::endl;
	std::cout << matrix1 * matrix2 << std::endl;

	return 0;
}

int TransposeProperties()
{
	std::cout << "1.29: Transpose Properties" << std::endl;

	Eigen::Matrix2f matrix1;
	matrix1 << 1, 2, 6, 8;
	Eigen::Matrix2f matrix2;
	matrix2 << 3, 4, 10, 12;
	std::cout << "matrix1*matrix2 transpose" << std::endl;
	std::cout << (matrix1 * matrix2).transpose() << std::endl;
	std::cout << "should equal matrix2 transpose *matrix1 transpose" << std::endl;
	std::cout << matrix2.transpose() * matrix1.transpose() << std::endl;
	std::cout << "and should not equal matrix1 transpose * matrix2 transpose" << std::endl;
	std::cout << matrix1.transpose() * matrix2.transpose() << std::endl;

	return 0;
}

int MultiplyingTransposedVectors()
{
	std::cout << "1.33: Calculate aTb and abT" << std::endl;

	Eigen::Vector3d vectorA(1,2,3);
	Eigen::Vector3d vectorB(4, -2, 1);
	std::cout << "a transpose" << std::endl;
	std::cout << vectorA.transpose() << std::endl;
	std::cout << "times b" << std::endl;
	std::cout << vectorA.transpose() * vectorB<< std::endl;
	std::cout << "and b transpose * a" << std::endl;
	std::cout << vectorA * vectorB.transpose() << std::endl;

	return 0;
}

int MatrixTransposedVectorMultiplication()
{
	std::cout << "1.38: Vectors and matrices" << std::endl;

	Eigen::Vector3d vectorA(1, 2, 3);
	Eigen::Matrix3d matrixA;
	matrixA << 1, 1, 1,
		2, 2, 2,
		3, 3, 3;
	std::cout << "a transpose" << std::endl;
	std::cout << vectorA.transpose() << std::endl;
	std::cout << "times matrixA" << std::endl;
	std::cout << matrixA << std::endl;
	std::cout << "equals" << std::endl;
	std::cout << vectorA.transpose() * matrixA << std::endl;

	return 0;
}

int InnerProduct()
{
	std::cout << "1.92: Inner Product" << std::endl;

	Eigen::Vector2d a(1, 2);
	Eigen::Vector2d b(3, 1);
	auto t = a.transpose().dot(b.transpose());
	auto l_of_a = sqrt(a.transpose().dot(a.transpose()));
	auto l_of_b = sqrt(b.transpose().dot(b.transpose()));
	auto lp = l_of_a * l_of_b;
	auto ct = t / lp;

	std::cout << "law of cosines states the inner product off the transposes of (1,2) and (3,1) " << std::endl;
	std::cout << t << std::endl;
	std::cout << "divided by the product of the lengths of said matrices" << std::endl;
	std::cout << lp << std::endl;
	std::cout << ct << std::endl;
	std::cout << "is equal to the product of the square root of the inner product of each of said matrixes with themselves times cosine theta" << std::endl;
	std::cout << lp * ct << std::endl;

	return 0;
}

int DemoConsoleOutput()
{
	Eigen::MatrixXd matrix1(2, 2);
	matrix1(0, 0) = 3;
	matrix1(1, 0) = 2.5;
	matrix1(0, 1) = -1;
	matrix1(1, 1) = matrix1(1, 0) + matrix1(0, 1);
	std::cout << matrix1 << std::endl;

	std::cout << "This is a simple string literal" << std::endl;

	std::cout << "Pi when approximated is 22 / 7 = " << 22 / 7 << std::endl;

	std::cout << "Pi actually is 22 / 7 = " << 22.0 / 7 << std::endl;

	return 0;
}


#pragma once

// Include Eigen
#include <Eigen/Core>
#include <Eigen/LU>
#ifdef USE_SPARSE_EIGEN
#include <Eigen/Sparse>
typedef Eigen::SparseMatrix<float> MatrixType;
#else
typedef Eigen::MatrixXf MatrixType;
#endif

// Include STL
#include <cmath>
#include <vector>

// Local include
#include "AxialMoments.hpp"

/* _Zonal Weigths_
 *
 * Compute the matrix of weights w_{l,i} that converts dot powers (wd, w)^i
 * to a Zonal Hamonics. More precisely:
 *
 *     y_{l, 0}(w · wd) = ∑_i w_{l,i} (w · wd)^i, V l in [0 .. order]
 *
 */
inline Eigen::MatrixXf ZonalWeights(int order) {

   // W is the matrix of the weights. It is build recursively using the Legendre
   // polynomials. The trick here is that Legendre polynomials are simply the
   // shift in order of a previous pol. summed to another previous pol.
   Eigen::MatrixXf W = Eigen::MatrixXf::Zero(order, order);
   W(0,0) = 1.0f;
   W(1,1) = 1.0f;
   for(int n=2; n<order; ++n) {
      const int subsize = n;
      W.row(n).segment(1, subsize) = (2*n-1) * W.row(n-1).segment(0, subsize);
      W.row(n) -= (n-1) * W.row(n-2);
      W.row(n) /= n;
   }

   // Scale each columns by the corresponding √(2l+1 / 4π) factor.
   const float factor = 1.0f/sqrt(4.0f*M_PI);
   for(int n=0; n<W.rows(); ++n) {
      W.row(n) = W.row(n) * sqrt(2*n+1) * factor;
   }

   return W;
}

/* _Vector Group Zonal Weigths_
 *
 * TODO This matrix should do the mixing between the ordered power of cosines
 * vector and the shuffled vector of ZH coefficients.
 */
template<class Vector>
inline Eigen::MatrixXf ZonalWeights(const std::vector<Vector>& directions) {

   const int dsize = directions.size();
   const int order = (dsize-1) / 2 + 1;
   const int mrows = order*order;

   Eigen::MatrixXf result = Eigen::MatrixXf::Zero(mrows, dsize*order);

   const auto ZW = ZonalWeights(order);

   // Each vector fills a given set of column entries with decreasing
   // number to do the swapping. For example, the 0th vector will fill
   // entries from order 0 to max_order but the 1srt vector will fill
   // entries from 1 to max_order.
   for(int i=0; i<dsize; ++i) {
      for(int j=0; j<order; ++j) {
         if(i >= 2*j+1) {
            continue;
         }

         const int shift_rows = j*j + i;
         const int shift_cols = order*i;

         result.block(shift_rows, shift_cols, 1, order) = ZW.row(j);
      }
   }
   return result;
}

/* _Zonal Normalization_
 *
 * This little code compute the zonal normalization for spherical harmonics
 * basis using the √(2l+1 / 4π) factor. It can later by applied to a zonal
 * vector using Eigen fast product: a.cwiseProduct(b)
 */
inline Eigen::VectorXf ZonalNormalization(int order) {
   Eigen::VectorXf res = Eigen::VectorXf(order);
   const float f = 1.0f/sqrt(4.0f*M_PI);
   for(int i=0; i<order; ++i) {
      res[i] = sqrt(2*i+1)*f;
   }
   return res;
}

/* _Rotated ZH_
 *
 * Evaluate the rotated ZH, with respect to principal direction `d` in
 * direction `w` for order in [0..order].
 *
 *   z_i = P_i( d · w ),  where P_i is Legendre polynomial
 */
template<class Vector>
inline Eigen::VectorXf RotatedZH(const Vector& d, const Vector& w, int order) {

   Eigen::VectorXf res = Eigen::VectorXf::Zero(order);
   const float z = Vector::Dot(d, w);

   res[0] = 1.0f;
   if(order == 1) {
      return res /sqrt(4.0f*M_PI);
   }

   res[1] = z;
   if(order == 2) {
      res[1] *= sqrt(3.0f);
      return res / sqrt(4.0f*M_PI);
   }

   // Using Bonnet recurrence formula
   for(int i=2; i<order; ++i) {
      const int i2 = i-1;
      res[i]  = (2*(i2)+1)*z*res[i2] - (i2)*res[i-2];
      res[i] /= i;
   }

   // Apply zonal weights
   const auto f = ZonalNormalization(order);
   res = res.cwiseProduct(f);
   return res;
}

/* _ZH Fast Evaluation_
 *
 * Evaluate the Zonal Harmonics vector for all `directions` while respecting
 * a lobe sharing order. This means that the output vector will be composed as:
 *
 *     v = [ z_{0,0}, z_{1,0}, z_{1,1}, z_{1,2}, ...]
 *
 * where z_{i,j} = Ylm_{i, 0}(w_j).
 */
template<class Vector>
inline Eigen::VectorXf ZHEvalFast(const std::vector<Vector>& dirs, const Vector& w) {

   // Get the number of elements to compute
   const int dsize = dirs.size();
   const int order = (dsize-1) / 2 + 1;
   const int vsize = order*order;

   Eigen::VectorXf v(vsize);

   // The loop is first over the order term then over the set of directions
   // starting from the 0 index that is reused for all bands.
   float ylm = 0.0;
   for(int i=0; i<dsize; ++i) {

      const Vector& n   = dirs[i];
      const auto    ylm = RotatedZH<Vector>(n, w, order);

      for(int l=0; l<order; ++l) {
         if(i >= 2*l+1) {
            continue;
         }
         v[l*l + i] = ylm[l];
      }
   }

   return v;
}

/* _Zonal Expansion_
 *
 * Expands a Spherical Harmonics vector into a Zonal Harmonics matrix transform.
 * Each band of spherical harmonics is decomposed into a set of Zonal Hamonics
 * with specific directions `directions`.
 *
 * The conversion matrix A is computed as the inverse of the evaluation of the
 * SH basis a the particular directions.
 *
 * The vector of directions must be of size `2*order+1`, where `order` is the
 * max order of the SH expansion.
 *
 * The template class SH must permit to access its basis elements, the y_{l,m}
 * as the static function `SH::FastBasis(const Vector& w, int order)`.
 */
template<class SH, class Vector>
inline MatrixType ZonalExpansion(const std::vector<Vector>& directions) {

   // Get the current band. Here I use a shifted order number. The integer
   // order is actually `order+1` to compute the number of rows and iterate
   // over it. Later in the code I evaluate the correct order to get the
   // ylm from SH::FastBasis.
   const int dsize = directions.size();
   const int order = (dsize-1) / 2 + 1;
   const int mrows = order*order;
   assert(order >= 0);

   const auto zhNorm = ZonalNormalization(order);

#ifdef USE_SPARSE_EIGEN
   MatrixType Y(mrows, mrows);
   std::vector<Eigen::Triplet<float>> triplets;
#else
   MatrixType Y = Eigen::MatrixXf::Zero(mrows, mrows);
#endif
   for(int i=0; i<dsize; ++i) {

      // Get the vector associated to the current row
      const Vector& w = directions[i];

      // Evaluate all the Y_{l,m} for the current vector
      auto ylm = SH::FastBasis(w, order-1);

      // Complete all the submatrices starting from column `i` using order `j`
      // size for the ZH element.
      for(int j=0; j<order; ++j) {
         if(i >= 2*j+1) {
            continue;
         }

         const int  shift = j*j;
         const int  vsize = 2*j+1;
#ifdef USE_SPARSE_EIGEN
         for(int k=0; k<vsize; ++k) {
            const float& v = ylm[shift+k];
            triplets.push_back(Eigen::Triplet<float>(shift+i, shift+k, v));
         }
         Y.setFromTriplets(triplets.begin(), triplets.end());
#else
         Y.block(shift+i, shift, 1, vsize) = ylm.segment(shift, vsize).transpose() / zhNorm[j];
#endif
      }
   }

   return Y;
}

/* _Compute the Inverse of Matrix Y_
 *
 * Since the matrix resulting from ZonalExpansion is block diagonal, its
 * inverse is trivial to compute. We must simply take the inverse of each
 * block, in place.
 *
 * TODO: Make the sparse version.
 */
inline MatrixType computeInverse(const MatrixType& Y) {
   const int nrows = Y.rows();
   const int order = sqrt(nrows);

#ifdef USE_SPARSE_EIGEN
   MatrixType A(mrows, nrows);
   std::vector<Eigen::Triplet<float>> triplets;
#else
   MatrixType A = Eigen::MatrixXf::Zero(nrows, nrows);
#endif

   for(int j=0; j<order; ++j) {
      const int shift = j*j;
      const int size  = 2*j+1;

#ifdef USE_SPARSE_EIGEN
      assert(false);
#else
      const auto& block = Y.block(shift, shift, size, size);

      // Check if the block is Zero or not in debug mode
      if(block.isZero()) {
         std::cout << "Block of order " << j << " is zero" << std::endl;
         std::cout << block << std::endl << std::endl;
      }
      //assert(!block.isZero());

      A.block(shift, shift, size, size) = block.inverse();
#endif
   }
   return A;
}

/* _Compute the SH Integral over a Spherial Triangle_
 *
 * This function is provided as an example of how to use the different
 * components of this package. It is probably much faster to precompute the
 * product `Prod` of the ZonalWeights and the Zonal to SH conversion matrix.
 */
template<class Triangle, class Vector, class SH>
inline float computeSHIntegral(const Eigen::VectorXf& clm,
                               const std::vector<Vector>& basis,
                               const Triangle& triangle) {

   // Get the Zonal weights matrix and the Zlm -> Ylm conversion matrix
   // and compute the product of the two: `Prod = A x Zw`.
   const auto ZW = ZonalWeights<Vector>(basis);
   const auto Y  = ZonalExpansion<SH, Vector>(basis);
   const auto A  = computeInverse(Y);

   const auto Prod = A*ZW;

   // Analytical evaluation of the integral of power of cosines for
   // the different basis elements up to the order defined by the
   // number of elements in the basis
   const auto moments = AxialMoments<Triangle, Vector>(triangle, basis);

   return clm.dot(Prod * moments);
}

#ifdef LATER
/* _Compute the SH Integral over a Spherial Triangle_
 *
 * This function is provided as an example of how to use the different
 * components of this package. It is probably much faster to precompute the
 * product `Prod` of the ZonalWeights and the Zonal to SH conversion matrix.
 */
template<class Triangle, class Vector, class SH>
inline float computeSHIntegral(const Eigen::VectorXf& clm,
                               const Triangle& triangle) {

   // Get the order of the provided SH vector and compute the directional
   // sampling of the sphere for rotated ZH/cosines.
   const auto order = sqrt(clm.size())-1;
   const auto basis = SamplingFibonacci<Vector>(2*order+1);

   // Get the Zonal weights matrix and the Zlm -> Ylm conversion matrix
   // and compute the product of the two: `Prod = A x Zw`.
   const auto ZW = ZonalWeights<Vector>(basis);
   const auto Y  = ZonalExpansion<SH, Vector>(basis);
   const auto A  = computeInverse(Y);

   const auto Prod = A*ZW;

   // Analytical evaluation of the integral of power of cosines for
   // the different basis elements up to the order defined by the
   // number of elements in the basis
   const auto moments = AxialMoments<Triangle, Vector>(triangle, basis);

   return clm.dot(Prod * moments);
}
#endif


//
// Created by James Koh on 03/02/2021.
//

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

namespace DM {
    using namespace Eigen;

    MatrixXd Direction(VectorXd jacobian, MatrixXd hessian_inverse) {
        return - hessian_inverse * jacobian;
    }

    double LambdaSquared(VectorXd jacobian, MatrixXd hessian_inverse) {
        return jacobian.transpose() * hessian_inverse * jacobian;
    }

    VectorXd LineSearchUpdate(VectorXd x, VectorXd delta_x, double alpha, double beta,
                              double F (VectorXd), VectorXd jacobian) {
        double t = 1.0;

        while (F(x + t * delta_x) > (F(x) + jacobian.dot(delta_x) * (alpha * t))) {
            t = beta * t;
        }

        return x + t * delta_x;
    }

    VectorXd Newton(VectorXd x0, double epsilon, double alpha, double beta,
                    double F (VectorXd), VectorXd J (VectorXd), MatrixXd H (VectorXd)) {
        VectorXd x = x0;
        int i = 1;

        while (true) {
            std::cout << "Iteration " << i << ":" << std::endl;

            VectorXd jacobian = J(x);
            MatrixXd hessian = H(x);
            MatrixXd hessian_inverse = H(x).inverse();

            VectorXd delta_x = Direction(jacobian, hessian_inverse);
            double lambda_squared = LambdaSquared(jacobian, hessian_inverse);

            std::cout << "Lambda^2: " << lambda_squared << std::endl;

            if (lambda_squared / 2 < epsilon || i > 100)
                break;

            x = LineSearchUpdate(x, delta_x, alpha, beta, F, jacobian);
            std::cout << "Solution :" << F(x) << std::endl;

            i++;
        }

        return x;
    }
}


#include "writer.hpp"
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <cmath>
#include <iostream>
#include <stdexcept>

//! Sparse Matrix type. Makes using this type easier.
typedef Eigen::SparseMatrix<double> SparseMatrix;

//! Used for filling the sparse matrix.
typedef Eigen::Triplet<double> Triplet;

//! Vector type
typedef Eigen::VectorXd Vector;

//! Our function pointer, typedef'd to make it easier to use
typedef double (*FunctionPointer)(double);

//! Create the Stiffness matrix for 1D with boundary terms
//! @param[out] A will be the Galerkin matrix (as in the exercise)
//! @param[in] N as in the exercise
//! @param[in] dx the cell length
void createStiffnessMatrixWithBoundary(SparseMatrix &A, int N, double dx) {
	std::vector<Triplet> triplets;
	A.resize(N + 2, N + 2);

	// Reserve the space we will allocate
	triplets.reserve((N + 2) + 2 * N + 2);

	// (write your solution here)
	A.setZero();
	for (int i = 0; i < N + 2; i++) {
		triplets.push_back(Triplet(i, i, 2 / dx));
		if (i >= 1) {
			triplets.push_back(Triplet(i, i - 1, -1 / dx));
		}
		if (i <= N + 1) {
			triplets.push_back(Triplet(i, i + 1, -1 / dx));
		}
	}
	A.setFromTriplets(triplets.begin(), triplets.end());
}

//! Creates the right hand side for the finite element method in the exericse
//! @param[out] rhs the resulting right hand side (or phi in the exercise)
//! @param[in] f the function pointer to f
//! @param[in] dx the cell length
//! @param[in] x the x points
void createRHS(Vector &rhs, FunctionPointer f, int N, double dx, const Vector &x) {
	rhs.resize(N + 2);

	// (write your solution here)
	rhs(0) = dx / 2 * f(x(0));
	for (int i = 1; i < N + 1; i++) {
		rhs(i) = dx * f(x(i));
	}
	rhs(N + 1) = dx / 2 * f(x(N + 1));
}

//! Solve the equation
//!
//!    $-u''(x) = f(x)$
//!
//! using FEM on the interval $[a,b]$.
//!
//! @param[out] u at the end, will have all the values of u
//! @param[out] x will be the x points
//! @param[in] f should be a pointer to the function f
//! @param[in] N number of inner cells to use
//! @param[in] a endpoint on left side
//! @param[in] b endpoint on right side
//! @param[in] ua boundary value at a
//! @param[in] ub boundary value at b
void femSolve(Vector &u, Vector &x, FunctionPointer f, int N, double a, double b, double ua = 0.0, double ub = 0.0) {
	double dx = (b - a) / (N + 1);

	// Fill x vector
	x.setLinSpaced(N + 2, a, b);

	SparseMatrix A;
	createStiffnessMatrixWithBoundary(A, N, dx);

	Vector phi;
	createRHS(phi, f, N, dx, x);

	// (write your solution here)

	u.resize((N + 2));
	u.setZero();

	// Do boundary conditions
	// (write your solution here)
	u(0)     = ua;
	u(N + 1) = ub;
	phi -= A * u;

	//solve inner system
	// (write your solution here)
	SparseMatrix innerA = A.block(1, 1, N, N);

	Eigen::SparseLU<SparseMatrix> solver;
	solver.compute(innerA);
	if (solver.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose the matrix.");
	}
	u.segment(1, N) = solver.solve(phi.segment(1, N));
}

void computeWithoutBoundaryValues() {
	Vector u;
	Vector x;
	femSolve(u, x, std::sin, 100, -M_PI, M_PI);

	writeToFile("u_wo_bc_fem.txt", u);
	writeToFile("x_wo_bc_fem.txt", x);
}

void computeWithBoundaryValues() {
	Vector u;
	Vector x;
	femSolve(u, x, std::cos, 100, -M_PI, M_PI, -1.0, 1.0 / 2.0);

	writeToFile("u_w_bc_fem.txt", u);
	writeToFile("x_w_bc_fem.txt", x);
}

int main(int, char **) {
	computeWithoutBoundaryValues();
	computeWithBoundaryValues();
}


#include <iostream>
#include <chrono>

#include <Eigen/Core>  // Eigen 核心部分
#include <Eigen/Dense>  // 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Geometry>  // Eigen/Geometry 模块提供了各种旋转和平移的表示

#include <opencv2/opencv.hpp>

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

    double a_e = 2.0, b_e = -1.0, c_e = 5.0;  // 待拟合参数 及估计初值
    double a = 1.0, b = 2.0, c = 1.0;  // 待拟合参数 实际值
    cv::RNG rng;  // OpenCV 随机数产生器
    double w_sigma = 1.0;
    double inv_sigma = 1.0 / w_sigma;

    // 生成数据
    int num_data = 100;
    std::vector<double> t_data, y_data;

    for(int i = 0; i < num_data; i++){
        double t = i / 100.0;
        double y = exp(a*t*t + b*t + c) + rng.gaussian(w_sigma * w_sigma);
        t_data.push_back(t);
        y_data.push_back(y);
    }

    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    // 迭代-优化
    Eigen::Vector3d x(a_e, b_e, c_e);
    int num_iters = 20;
    double cost = 0, lastCost = 0;

    for(int iter = 0; iter < num_iters; iter++){
        Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
        Eigen::Vector3d g = Eigen::Vector3d::Zero();
        cost = 0;
        for (int i = 0; i < num_data; i++){
            double t = t_data[i];
            double y = y_data[i];
            double y_e = exp(x[0]*t*t + x[1]*t + x[2]);

            double error = y_e - y;
            Eigen::Matrix<double, 1, 3> J;
            J[0] = t * t * exp(x[0]*t*t + x[1]*t + x[2]);
            J[1] = t * exp(x[0]*t*t + x[1]*t + x[2]);
            J[2] = exp(x[0]*t*t + x[1]*t + x[2]);

            H += inv_sigma * inv_sigma * J.transpose() * J;
            g += - inv_sigma * inv_sigma * J * error;
            cost += error * error;
        }

        if (iter >= 1 && cost >= lastCost){
            std::cout << "cost >= lastCost, break." << std::endl;
            break;
        }
        lastCost = cost;

        Eigen::Vector3d dx = H.ldlt().solve(g);
        if (dx.norm() < 1e-6){
            std::cout << "||dx|| < 1e-6, break." << std::endl;
            break;
        }
        x += dx;

        std::cout << "iter = " << iter << " , estimated parameters = " << x.transpose()
                  << " , cost = " << cost << std::endl;
    }

    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast <std::chrono::duration<double>> (t2 - t1);

    std::cout << "final , estimated parameters = " << x.transpose()
                  << " , cost = " << cost << std::endl;
    std::cout << "time_cost = " << time_used.count() << "second" << std::endl;

    return 0;
}


/**
 * Reference: The code is copied from:
 * https://eigen.tuXdamily.org/dox/classEigen_1_1JacobiSVD.html
 */

#include <Eigen/Core>
#include <Eigen/Geometry>
// #include <Eigen/Dense> // For initializing MatrixXd
#include <iostream>

using namespace std;

int main(int argc, char const *argv[])
{
     // Eigen::MatrixXd mat = Eigen::MatrixXd::Random(3, 2);
     // Eigen::MatrixXd mat = Eigen::MatrixXd::Zero(3, 2);
     // Eigen::Matrix3d mat = Eigen::Matrix3d::Zero();
     Eigen::MatrixXd mat(2, 2);
     mat(0, 0) = 1.0;
     mat(1, 0) = 1.0;
     mat(0, 1) = 5.0;
     mat(1, 1) = 5.0;

     cout << "Here is the matrix mat:" << endl
          << mat << endl;

     Eigen::JacobiSVD<Eigen::MatrixXd> svd(mat, Eigen::ComputeThinU | Eigen::ComputeThinV);
     Eigen::Vector2d s = svd.singularValues();
     Eigen::Matrix<double, 2, 2> u = svd.matrixU();
     Eigen::Matrix<double, 2, 2> v = svd.matrixV();
     cout << "Its singular values are:" << endl
          << svd.singularValues() << endl;
     cout << "Its left singular vectors are the columns of the thin U matrix:" << endl
          << svd.matrixU() << endl;
     cout << "Its right singular vectors are the columns of the thin V matrix:" << endl
          << svd.matrixV() << endl;

     // Eigen::Vector3d rhs(1, 0, 0);
     Eigen::Vector2d rhs(1, 0);
     cout << "Now consider this rhs vector:" << endl
          << rhs << endl;
     cout << "A least-squares solution of mat*x = rhs is:" << endl
          << svd.solve(rhs) << endl;

     Eigen::Vector2d mean = mat.colwise().mean();     // 1, 5
     Eigen::Vector2d mean_row = mat.rowwise().mean(); // 3, 3
     typedef Eigen::Matrix<double, 1, 2> Mat12d;
     Mat12d mean_col = mat.colwise().mean(); // 1, 5

     cout << "Column wise mean: " << mean << endl;
     cout << "Row wise mean: " << mean_row << endl;
     cout << "Subtraction by colwise(): " << mat.colwise() - mean << endl;
     cout << "Subtraction by rowwise(): " << mat.rowwise() - mean.transpose() << endl;

     Eigen::Vector2d first_column = mat.col(0);
     cout << "\n first_column: " << first_column << endl;
     double norm_of_first_column = first_column.dot(first_column);
     cout << "\n norm_of_first_column: " << norm_of_first_column << endl;

     return 0;
}


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <vector>
#include <iomanip>

#include "errors.hpp"

using namespace Eigen;

int main() {

    // Construct data for RK order 4
    MatrixXd A = MatrixXd::Zero(4,4);
    A(1,0) = .5;
    A(2,1) = .5;
    A(3,2) = 1;
    VectorXd b(4);
    b << 1./6, 1./3, 1./3, 1./6;

    // Construct data for the IVP
    double T = 1;
    int n = 5;
    VectorXd y0(2*n);
    for(int i = 0; i < n; ++i) {
        y0(i)=(i+1.)/n;
        y0(i+n)=-1;
    }

    auto f = [n] (VectorXd y) {
        VectorXd fy(2*n);

        VectorXd g(n);
        g(0) = y(0)*(y(1)+y(0));
        g(n-1) = y(n-1)*(y(n-1)+y(n-2));
        for(int i = 1; i < n-1; ++i) {
            g(i) = y(i)*(y(i-1)+y(i+1));
        }

        Eigen::SparseMatrix<double> C(n,n);
        C.reserve(3);
        for(int i = 0; i < n; ++i) {
            C.insert(i,i) = 2;
            if(i < n-1) C.insert(i,i+1) = -1;
            if(i >= 1)  C.insert(i,i-1) = -1;
        }
        C.makeCompressed();
        fy.head(n) = y.head(n);

        Eigen::SparseLU< Eigen::SparseMatrix<double> >  solver;
        solver.analyzePattern(C);
        solver.compute(C);
        fy.tail(n) = solver.solve(g);
        return fy;
    };

    errors(f, T, y0, A, b);
}

Nummerical Methods for CSE/PS12/solutions_ps12/system.cpp


#include <Eigen/Core>
#include <Eigen/QR>
#include <iostream>
using namespace Eigen;

void func1(void) {
  MatrixXd A = MatrixXd::Random(5, 8);
  VectorXd b = VectorXd::Random(5);
  VectorXd b0 = b; //save b because we are going to write over during computations  // You should have already done something like this.
  ColPivHouseholderQR<MatrixXd> qr(A.transpose());
  MatrixXd Q = qr.matrixQ();  // This computes A^+ * b
  auto R = qr.matrixQR().topRows(5).transpose().template triangularView<Lower>();
  b = qr.colsPermutation().transpose()* b;
  R.solveInPlace(b);
  VectorXd x = Q.leftCols(5) * b;  // just checking the result
  std::cout << "x = " << x.transpose() << std::endl;
  std::cout << "||A*x-b0|| = " << (A * x - b0).norm() << std::endl;  // for comparison, the solve of CompleteOrthogonalDecomposition does a pseudo-inverse
  MatrixXd pinvA = A.completeOrthogonalDecomposition().pseudoInverse();
  VectorXd x0 = pinvA * b0;  std::cout << "x0 = " << x0.transpose() << std::endl;
  std::cout << "||A*x0-b0|| = " << (A * x0 - b0).norm() << std::endl;
  std::cout << "||x - x0|| = " << (x0 - x).norm() << std::endl;
}

void func2(void) {
  using namespace std;
  MatrixXd A = MatrixXd::Random(5, 8);
  VectorXd b = VectorXd::Random(5);
  VectorXd b0 = b; //save b because we are going to write over during computations  // You should have already done something like this.
  ColPivHouseholderQR<MatrixXd> qr(A.transpose());
  MatrixXd Q = qr.matrixQ();  // This computes A^+ * b
  MatrixXd R = qr.matrixR();
  MatrixXd P = qr.colsPermutation();
  double rank = qr.rank();

  MatrixXd R_upper = R.topRows(rank);
  auto RT = R_upper.transpose().template triangularView<Lower>();
  MatrixXd PT = P.transpose();
  RT.solveInPlace(PT);
  cout << "A * (Q*R^(-T)*P^T)" << endl;
  cout << A*(Q.leftCols(rank)*PT) << endl;

  MatrixXd pinvA = A.completeOrthogonalDecomposition().pseudoInverse();
  VectorXd x0 = pinvA * b0;
  cout << "Psuedo Inverse A : " << endl;
  cout << pinvA << endl;
  cout << "(Q*R^(-T)*P^T) : " << endl;
  cout << Q.leftCols(rank)*PT << endl;
}

int main()
{
  func1();
  func2();
}


// { Driver Code Starts
//Initial template for C++

#include <boost/multiprecision/cpp_int.hpp>
using boost::multiprecision::cpp_int;  // https://www.geeksforgeeks.org/factorial-large-number-using-boost-multiprecision-library/
using namespace std;

 // } Driver Code Ends
//User function template for C++

class Solution
{
    public:

    // Catalan series:  1, 1, 2, 5, 14, 42, 132, 429, 1430, 4862, …
    // Formulae to get the series: C0 = 0, C(N+1) = sum0ToN(Ci*C(N-i))

    //Function to find the nth catalan number.

    cpp_int nthCatalan(int n){
       vector<cpp_int> dp(n+1,0);

       for(int i=0;i<n+1;i++){
           if(i==0 or i==1) dp[i] = 1;
           else{
               for(int j=0;j<i;j++)
                   dp[i] += dp[j]*dp[i-j-1];
           }
       }
       return dp[n];
    }

    cpp_int findCatalan(int n)
    {
        //code here
        cpp_int ans = nthCatalan(n);
        return ans;
    }
};

// { Driver Code Starts.

int main()
{
    //taking count of testcases
	int t;
	cin>>t;
	while(t--) {

	    //taking nth number
	    int n;
	    cin>>n;
	    Solution obj;
	    //calling function findCatalan function
	    cout<< obj.findCatalan(n) <<"\n";
	}
	return 0;
}  // } Driver Code Ends


#include <iostream>
#include <ctime>
using namespace std;

//#include <Eigen/Core>
#include <Eigen/SVD>
//#include <Eigen/Dense>
// 稠密矩阵的代数运算 逆矩阵，特征值
#include "/usr/include/eigen3/Eigen/Dense"
#include "/usr/include/eigen3/Eigen/Core"

#define MATRIX_SIZE (50)

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

	Eigen::Matrix<float,2,3> matrix_23;
	/* Vertor3D is double 3,1, 三维坐标就相当于矩阵中1行3列 */
	Eigen::Vector3d v_3d;
	/* matrxi3d is doule 3,3 */
//	Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
	/* 特殊类型 */
	Eigen::MatrixXd matrix_x;

	matrix_23 << 1,1,2,2,5,5;
	cout << matrix_23 <<endl;
	cout << matrix_23.cols() << endl;

	for (int i = 0; i < matrix_23.rows(); i++)
		for(int j = 0;j < matrix_23.cols(); j++)
				cout << matrix_23(i,j) << " ";
	v_3d << 1, 2, 3;
	cout << endl <<  "-- " << v_3d.transpose() << endl;

//	Eigen::Matrix<double,2,3> wrong_type = matrix_23;
//	Eigen::Matrix<double,2,3> cast_type = matrix_23.cast<double>();
	Eigen::Matrix<double,2,1> cast_type = matrix_23.cast<double>() * v_3d;
	cout << cast_type << endl;

	/* 四则运算 */
	Eigen::Matrix3d matrix_3d = Eigen::Matrix3d::Random();
	matrix_3d << 1,2,0,0,2,3,1,0,3;
	cout << "---------------" << endl << matrix_3d << endl;

	cout << "-------transpose--------" << endl << matrix_3d.transpose() << endl;
	cout << "-------sum--------" << endl << matrix_3d.sum() << endl;
	cout << "-------trace--------" << endl << matrix_3d.trace() << endl;
	cout << "-------multiple--------" << endl << matrix_3d*96 << endl;
	cout << "-------inverse--------" << endl << matrix_3d.inverse() << endl;
	cout << "-------[ 1 ]--------" << endl << matrix_3d.inverse()*matrix_3d << endl;
	cout << "-------determinant------" << endl << matrix_3d.determinant() << endl;

	matrix_3d = Eigen::Matrix3d::Random();
	cout << "-------eigen random:" << endl << matrix_3d << endl;
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(matrix_3d.transpose()*matrix_3d);
	cout << "-------eigen tra*mat:" << endl << matrix_3d.transpose()*matrix_3d << endl;
	cout << "-------eigen values:" << endl << solver.eigenvalues() << endl;
	cout << "-------eigen vector" << endl << solver.eigenvectors() << endl;
//	Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> Matrix_MM = Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE>::Random(); //eigen default size is 50
	//Eigen::Matrix<double, 50,50> Matrix_MM = Eigen::Matrix<double,50,50>::Random(); //eigen default size is 50
	//Eigen::Matrix<double, MATRIX_SIZE, 1> v_M = Eigen::Matrix<double,MATRIX_SIZE,1>::Random();
	//cout << "-------Matrix_MM:" << endl << Matrix_MM << endl;
	/* 求逆 */
	clock_t time_st = clock();
//	cout << "-------Result:" << endl << Matrix_MM.inverse()*v_M << endl;
//	cout << "-------Timest:" << endl << 1000*(clock()-time_st)/(double)CLOCKS_PER_SEC <<endl;
//
//	time_st = clock();
//	cout << "-------Result:" << endl << Matrix_MM.colPivHouseholderQr().solve(v_M) << endl;
//	cout << "-------Result:" << endl << Matrix_MM.fullPivHouseholderQr().solve(v_M) << endl;
//	cout << "-------Timest:" << endl << 1000*(clock()-time_st)/(double)CLOCKS_PER_SEC <<endl;
//	/* 速度没有变快，反而变慢，不懂为何。8ms , 12.1ms*/

	/* 100*100的随机矩阵 */
	Eigen::Matrix<double, 100, 1> v_D = Eigen::Matrix<double,100,1>::Random();
	//Eigen::Matrix< double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic;
	Eigen::Matrix< double, 100, 100> matrix_dynamic;
	matrix_dynamic = Eigen::Matrix<double,100,100>::Random();
	time_st = clock();
	cout << "-------Result:" << endl << matrix_dynamic.colPivHouseholderQr().solve(v_D).transpose() << endl;
	cout << "-------Timest:" << endl << 1000*(clock()-time_st)/(double)CLOCKS_PER_SEC <<endl;

	time_st = clock();
	cout << "-------Result:" << endl << matrix_dynamic.ldlt().solve(v_D).transpose() << endl;
	cout << "-------Timest:" << endl << 1000*(clock()-time_st)/(double)CLOCKS_PER_SEC <<endl;

	return 0;
}


// finite_difference.cpp example program to demonstrate finite difference calculations
//
// Copyright (C) 2017-2019 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.
#include "common.hpp"
#include <boost/multiprecision/cpp_int.hpp>
// silence any posit arithmetic exceptions: this basically enables a silent signalling NaR
#define POSIT_THROW_ARITHMETIC_EXCEPTION 0
#include <universal/posit/posit>

/* concepts
   The complete definition of a template function or class should contain the list of required
   concepts as is done for functions in the Standard template Library; see http://www.sgi.com/tech/stl/

   */
template <typename F, typename T>
class forward_difference {
public:
	forward_difference(const F& f, const T& h) : f(f), h(h) {}
	T operator()(const T& x) const { return (f(x+h) - f(x)) / h; }
private:
	const F& f;
	T        h;
};

template<typename F, typename T>
class backward_difference {
public:
	backward_difference(const F& f, const T& h) : f(f), h(h) {}
	T operator()(const T& x) const { return (f(x) - f(x - h)) / h; }
private:
	const F& f;
	T        h;
};

template<typename F, typename T>
class central_difference {
public:
	central_difference(const F& f, const T& h) : f(f), h(h) {}
	T operator()(const T& x) const { return (f(x + T(0.5)*h) - f(x - T(0.5)*h)) / h; }
private:
	const F& f;
	T        h;
};

// Use recursion to define arbitrary n-th order derivatives

template<unsigned N, typename F, typename T>
class nth_derivative {
	using n_minus_1_derivative = nth_derivative<N - 1, F, T>;
public:
	nth_derivative(const F& f, const T& h) : h(h), fprev(f, h) {}
	T operator()(const T& x) const { return (fprev(x + h) - fprev(x)) / h; }
private:
	n_minus_1_derivative fprev;
	T                    h;
};

// termination condition
template<typename F, typename T>
class nth_derivative<1, F, T> : public forward_difference<F, T> {
	using forward_difference<F, T>::forward_difference;
};

/*
   Define sine/cosine/tangent functors to use.
   Depending on the provided type, the sin(x)/cos(x)/tan(x) functions
   are ADL matched to functions in different namespaces.
   For regular float/double/long double these trigonometry functions
   will be provided by the std namespace.
   For the sw::unum posit/valid types, these functions will be provided
   by the sw::unum namespace.
 */

// trigonometry sine functor
template<typename T>
struct sine_f {
	T operator() (const T& x) const { return sin(x); }
};

// a functor with a scale parameter
template<typename T>
class scaled_sine_f {
public:
	scaled_sine_f(const T& scale) : scale(scale) {}
	T operator() (const T& x) const { return scale * sin(x); }
private:
	T scale;
};

// trigonometry cosine functor
template<typename T>
struct cosine_f {
	T operator() (const T& x) const { return cos(x); }
};

// trigonometry tangent functor
template<typename T>
struct tangent_f {
	T operator() (const T& x) const { return tan(x); }
};

template<typename Real>
void EnumerateFirstDerivativeError(const Real& x) {
	using namespace std;
	using namespace sw::unum; // for m_pi_4

	cout << "sin at " << double(x) << " : " << sine_f<Real>()(x) << endl;
	cout << "cos at " << double(x) << " : " << cosine_f<Real>()(x) << endl;
//	cout << "tan at " << double(x) << " : " << tan_f<Real>()(x) << endl;

	using forward_sin_f = forward_difference<sine_f<Real>, Real>;
	using backward_sin_f = backward_difference<sine_f<Real>, Real>;
	using central_sin_f = central_difference<sine_f<Real>, Real>;

	constexpr int WIDTH = 16;

	cout << "Finite Difference approximation of the first derivative of the function sine(x)\n";
	cout << "Using " << typeid(Real).name() << endl;
	Real h = Real(0.05);
	sine_f<Real> sin_o;
	cout << setw(WIDTH) << "step (h)" << setw(WIDTH) << "backward" << setw(WIDTH) << "error" << setw(WIDTH) << "forward" << setw(WIDTH) << "error" << setw(WIDTH) << "central" << setw(WIDTH) << "error\n";
	for (unsigned i = 0; i < 10; ++i) {
		// backward = left
		backward_sin_f ld_sin_o(sin_o, h);
		Real ld_sin = ld_sin_o(x);
		Real lerror = ld_sin - cos(x);

		// forward = right
		forward_sin_f rd_sin_o(sin_o, h);
		Real rd_sin = rd_sin_o(x);
		Real rerror = rd_sin - cos(x);

		// central = middle
		central_sin_f md_sin_o(sin_o, h);
		Real md_sin = md_sin_o(x);
		Real merror = md_sin - cos(x);

		cout << setw(WIDTH) << h << setw(WIDTH) << ld_sin << setw(WIDTH) << lerror << setw(WIDTH) << rd_sin << setw(WIDTH) << rerror << setw(WIDTH) << md_sin << setw(WIDTH) << merror << endl;

		h *= 0.5;
	}
}

template<typename Real>
void EnumerateSecondDerivativeError(const Real& x) {
	using namespace std;
	using namespace sw::unum; // for m_pi_4

	cout << "sin at " << double(x) << " : " << sine_f<Real>()(x) << endl;
	cout << "cos at " << double(x) << " : " << cosine_f<Real>()(x) << endl;
	//	cout << "tan at " << double(x) << " : " << tan_f<Real>()(x) << endl;

	using forward_2nd_sin_f = nth_derivative<2, sine_f<Real>, Real>;
	//using backward_2nd_sin_f = backward_difference<sine_f<Real>, Real>;
	//using central_2nd_sin_f = central_difference<sine_f<Real>, Real>;

	constexpr int WIDTH = 16;

	cout << "Finite Difference approximation of the second derivative of the function sine(x)\n";
	cout << "Using " << typeid(Real).name() << endl;
	Real h = Real(0.05);
	sine_f<Real> sin_o;
	cout << setw(WIDTH) << "step (h)" << setw(WIDTH) << "backward" << setw(WIDTH) << "error" << setw(WIDTH) << "forward" << setw(WIDTH) << "error" << setw(WIDTH) << "central" << setw(WIDTH) << "error\n";
	for (unsigned i = 0; i < 10; ++i) {
		// backward = left
//		backward_sin_f ld_sin_o(sin_o, h);
		Real ld2_sin = NAN; //  ld2_sin_o(x);
		Real lerror = ld2_sin + sin(x);

		// forward = right
		forward_2nd_sin_f rd2_sin_o(sin_o, h);
		Real rd2_sin = rd2_sin_o(x);
		Real rerror = rd2_sin + sin(x);

		// central = middle
//		central_sin_f md_sin_o(sin_o, h);
		Real md2_sin = NAN; //  md2_sin_o(x);
		Real merror = md2_sin + sin(x);

		cout << setw(WIDTH) << h << setw(WIDTH) << ld2_sin << setw(WIDTH) << lerror << setw(WIDTH) << rd2_sin << setw(WIDTH) << rerror << setw(WIDTH) << md2_sin << setw(WIDTH) << merror << endl;

		h *= 0.5;
	}
}

template<typename Real>
void EnumerateThirdDerivativeError(const Real& x) {
	using namespace std;
	using namespace sw::unum; // for m_pi_4

	cout << "sin at " << double(x) << " : " << sine_f<Real>()(x) << endl;
	cout << "cos at " << double(x) << " : " << cosine_f<Real>()(x) << endl;
	//	cout << "tan at " << double(x) << " : " << tan_f<Real>()(x) << endl;

	using forward_3rd_sin_f = nth_derivative<3, sine_f<Real>, Real>;
	//using backward_2nd_sin_f = backward_difference<sine_f<Real>, Real>;
	//using central_2nd_sin_f = central_difference<sine_f<Real>, Real>;

	constexpr int WIDTH = 16;

	cout << "Finite Difference approximation of the 3rd derivative of the function sine(x)\n";
	cout << "Using " << typeid(Real).name() << endl;
	Real h = Real(0.05);
	sine_f<Real> sin_o;
	cout << setw(WIDTH) << "step (h)" << setw(WIDTH) << "backward" << setw(WIDTH) << "error" << setw(WIDTH) << "forward" << setw(WIDTH) << "error" << setw(WIDTH) << "central" << setw(WIDTH) << "error\n";
	for (unsigned i = 0; i < 10; ++i) {
		// backward = left
//		backward_sin_f ld_sin_o(sin_o, h);
		Real ld3_sin = NAN; //  ld3_sin_o(x);
		Real lerror = ld3_sin + cos(x);

		// forward = right
		forward_3rd_sin_f rd3_sin_o(sin_o, h);
		Real rd3_sin = rd3_sin_o(x);
		Real rerror = rd3_sin + cos(x);

		// central = middle
//		central_sin_f md_sin_o(sin_o, h);
		Real md3_sin = NAN; //  md2_sin_o(x);
		Real merror = md3_sin + cos(x);

		cout << setw(WIDTH) << h << setw(WIDTH) << ld3_sin << setw(WIDTH) << lerror << setw(WIDTH) << rd3_sin << setw(WIDTH) << rerror << setw(WIDTH) << md3_sin << setw(WIDTH) << merror << endl;

		h *= 0.5;
	}
}

// compare accuracy of first derivative using different Real types
void CompareTypeAccuracyOnFirstDerivative() {
	using namespace sw::unum;

	constexpr unsigned NR_SAMPLES = 3;
	long double samples[3];
	samples[0] = m_pi_4;
	samples[1] = m_1_pi / 3.0l;
	samples[2] = m_pi_2;

	for (unsigned i = 0; i < NR_SAMPLES; ++i) {
		{
			using Real = float;
			EnumerateFirstDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = double;
			EnumerateFirstDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<32, 2>;
			EnumerateFirstDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<64, 3>;
			EnumerateFirstDerivativeError<Real>(Real(samples[i]));
		}
	}
}

// compare accuracy of second derivative using different Real types
void CompareTypeAccuracyOnSecondDerivative() {
	using namespace sw::unum;

	constexpr unsigned NR_SAMPLES = 3;
	long double samples[3];
	samples[0] = m_pi_4;
	samples[1] = m_1_pi / 3.0l;
	samples[2] = m_pi_2;

	for (unsigned i = 0; i < NR_SAMPLES; ++i) {
		{
			using Real = float;
			EnumerateSecondDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = double;
			EnumerateSecondDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<32, 2>;
			EnumerateSecondDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<64, 3>;
			EnumerateSecondDerivativeError<Real>(Real(samples[i]));
		}
	}
}

// compare accuracy of third derivative using different Real types
void CompareTypeAccuracyOnThirdDerivative() {
	using namespace sw::unum;

	constexpr unsigned NR_SAMPLES = 3;
	long double samples[3];
	samples[0] = m_pi_4;
	samples[1] = m_1_pi / 3.0l;
	samples[2] = m_pi_2;

	for (unsigned i = 0; i < NR_SAMPLES; ++i) {
		{
			using Real = float;
			EnumerateThirdDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = double;
			EnumerateThirdDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<32, 2>;
			EnumerateThirdDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<64, 3>;
			EnumerateThirdDerivativeError<Real>(Real(samples[i]));
		}
	}
}

int main()
try {
	using namespace std;
//	using namespace boost::multiprecision;
	using namespace sw::unum;

	CompareTypeAccuracyOnFirstDerivative();
	CompareTypeAccuracyOnSecondDerivative();
	CompareTypeAccuracyOnThirdDerivative();

	{
		using Real = float;
		EnumerateSecondDerivativeError<Real>(Real(m_pi_4));
	}

	{
		using Real = float;
		EnumerateThirdDerivativeError<Real>(Real(m_pi_4));
	}

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


#include "problemes.h"
#include "arithmetique.h"

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

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;
typedef std::pair<nombre, nombre> paire;

namespace {
    typedef std::tuple<long double, long double, long double> triplet;

    long double aire(long double k) {
        long double rayon = std::abs(1.0L / k);
        return rayon * rayon * M_PIl;
    }

    long double iteration(std::map<triplet, nombre> &apollonios) {
        long double A = 0.0L;
        std::map<triplet, nombre> resultat;
        for (auto p: apollonios) {
            // k4 = k1 + k2 + k3 +/- 2*sqrt(k1.k2 + k2.k3 + k3.k1)
            auto[k1, k2, k3] = p.first;
            long double k4 = k1 + k2 + k3 + 2.0L * std::sqrt(k1 * k2 + k2 * k3 + k1 * k3);
            A += p.second * aire(k4);
            resultat[std::make_tuple(k4, k1, k2)] += p.second;
            resultat[std::make_tuple(k4, k1, k3)] += p.second;
            resultat[std::make_tuple(k4, k2, k3)] += p.second;
        }

        std::swap(apollonios, resultat);
        return A;
    }
}

ENREGISTRER_PROBLEME(199, "Iterative Circle Packing") {
    // Three circles of equal radius are placed inside a larger circle such that each pair of circles is tangent to one
    // another and the inner circles do not overlap. There are four uncovered "gaps" which are to be filled iteratively
    // with more tangent circles.
    //
    // At each iteration, a maximally sized circle is placed in each gap, which creates more gaps for the next iteration.
    // After 3 iterations (pictured), there are 108 gaps and the fraction of the area which is not covered by circles is
    // 0.06790342, rounded to eight decimal places.
    //
    // What fraction of the area is not covered by circles after 10 iterations?
    // Give your answer rounded to eight decimal places using the format x.xxxxxxxx.
    std::map<triplet, nombre> apollonios;

    // Théorème de Descartes : (k1 + k2 + k3 + k4)² = sqrt(k1² + k2² + k3² + k4²)
    // k4 = k1 + k2 + k3 +/- 2*sqrt(k1.k2 + k2.k3 + k3.k1)
    long double k0 = -1.0L;
    long double k1 = k0 / (3.0L - 2 * std::sqrt(3.0L)); // k1 = k2 = k3

    apollonios[std::make_tuple(k1, k1, k1)] = 1;
    apollonios[std::make_tuple(k1, k1, k0)] = 3;

    long double a0 = aire(k0);
    long double A = 3.0L * aire(k1);

    std::cout << std::fixed << std::setprecision(8);
    std::cout << "Iteration 0: " << 1.0L - A / a0 << std::endl;

    for (nombre n = 1; n <= 10; ++n) {
        A += iteration(apollonios);
        std::cout << "Iteration " << n << ": " << 1.0L - A / a0 << std::endl;
    }

    long double resultat = 1.0L - A / a0;
    return std::to_fixed(resultat, 8);
}


/******************************************************************************
Prácticas de cálculo matricial de estructuras
https://github.com/ingmec-ual/practicas-calculo-matricial-estructuras

Copyright 2017 - Jose Luis Blanco Claraco <jlblanco@ual.es>
University of Almeria
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
1. Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
Complete BSD-3-clause License: https://opensource.org/licenses/BSD-3-Clause
******************************************************************************/

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

int main()
{
	try
	{
		const double E = 200*1e9; // Modulo de Young (Pa)
		const double A = 2*1e-4;  // Area (m^2)

		// Matrices de rigidez de las barras individuales en
		// coordenadas LOCALES, y convertir a globales usando:
		// T*K*T' =
		//  [Ri*Kii*Rit, Ri*Kij*Rjt]
		//  [Rj*Kji*Rit, Rj*Kjj*Rjt]
		// -----------------------------------------------
		// barra a:
		Eigen::Matrix2d Ka_11_loc, Ka_12_loc, Ka_22_loc, Ka_11, Ka_12, Ka_22;
		calcmat::matriz_barra_art_art(0.5 /*L*/, E, A, Ka_11_loc /*Kii*/, Ka_22_loc /*Kjj*/, Ka_12_loc /*Kij*/);
		const double ang_a = 0.0 * M_PI / 180;

		{
			const auto Ra_i = calcmat::matriz_rotacion2(ang_a), Ra_j = Ra_i;  // En este caso Ri=Rj al ser ambas de 2x2
			Ka_11 = Ra_i*Ka_11_loc*Ra_i.transpose();
			Ka_22 = Ra_j*Ka_22_loc*Ra_j.transpose();
			Ka_12 = Ra_i*Ka_12_loc*Ra_j.transpose();
		}

		// barra b:
		Eigen::Matrix2d Kb_11_loc, Kb_13_loc, Kb_33_loc, Kb_11, Kb_13, Kb_33;
		calcmat::matriz_barra_art_art(1.0 /*L*/, E, A, Kb_11_loc /*Kii*/, Kb_33_loc /*Kjj*/, Kb_13_loc /*Kij*/);
		const double ang_b = 90.0 * M_PI / 180;

		{
			const auto Rb_i = calcmat::matriz_rotacion2(ang_b), Rb_j = Rb_i;  // En este caso Ri=Rj al ser ambas de 2x2
			Kb_11 = Rb_i*Kb_11_loc*Rb_i.transpose();
			Kb_33 = Rb_j*Kb_33_loc*Rb_j.transpose();
			Kb_13 = Rb_i*Kb_13_loc*Rb_j.transpose();
		}

		// barra c:
		Eigen::Matrix2d Kc_22_loc, Kc_23_loc, Kc_33_loc, Kc_22, Kc_23, Kc_33;
		calcmat::matriz_barra_art_art( hypot(0.5,1.0) /*L*/, E, A, Kc_22_loc /*Kii*/, Kc_33_loc /*Kjj*/, Kc_23_loc /*Kij*/);
		const double ang_c = atan2(1.0 /*y*/, -0.5 /*x*/);

		{
			const auto R23i = calcmat::matriz_rotacion2(ang_c), R23j = R23i;  // En este caso Ri=Rj al ser ambas de 2x2
			Kc_22 = R23i*Kc_22_loc*R23i.transpose();
			Kc_33 = R23j*Kc_33_loc*R23j.transpose();
			Kc_23 = R23i*Kc_23_loc*R23j.transpose();
		}

		// Ensamblar la matriz de rigidez de la estructura:
		// -----------------------------------------------
		Eigen::Matrix<double, 6, 6> K;
		K.setZero(); // Initializa a todo ceros

		K.block<2, 2>(0, 0) = Ka_11 + Kb_11;
		K.block<2, 2>(2, 2) = Ka_22 + Kc_22;
		K.block<2, 2>(4, 4) = Kb_33 + Kc_33;

		K.block<2, 2>(0, 2) = Ka_12;
		K.block<2, 2>(2, 0) = Ka_12.transpose();

		K.block<2, 2>(0, 4) = Kb_13;
		K.block<2, 2>(4, 0) = Kb_13.transpose();

		K.block<2, 2>(2, 4) = Kc_23;
		K.block<2, 2>(4, 2) = Kc_23.transpose();

		std::cout << "K:\n" << K << std::endl << std::endl;

		// Resolver:
		//  UL = KLL^{-1} * ( FL - KLR * UR )
		//  FR = KRR * UR + KRL * UL
		// -----------------------------------------------
		const std::array<int, 3> idxs_R = {0,1,3}; // gdl "restringidos"
		const std::array<int, 3> idxs_L = {2,4,5}; // gdl "libres"

		const auto KLL = indexing(K, idxs_L, idxs_L);
		const auto KRR = indexing(K, idxs_R, idxs_R);
		const auto KLR = indexing(K, idxs_L, idxs_R);
		const auto KRL = KLR.transpose();

		Eigen::Matrix<double, 3, 1> FL; // Cargas en gdl "libres"
		FL << 0, 1000, 0;

		Eigen::Matrix<double, 3, 1> UR; // Desplazamientos en gdl "restringidos" (apoyos)
		UR << 0,0,0;

		// UL = KLL^{-1} * ( FL - KLR * UR )
		const Eigen::Matrix<double, 3, 1> UL = KLL.colPivHouseholderQr().solve( FL - KLR*UR );

		// FR = KRR * UR + KRL * UL
		const Eigen::Matrix<double, 3, 1>  FR = KRR * UR + KRL * UL;

		// Ensamblar vector U completo:
		Eigen::Matrix<double, 6, 1>  U;
		indexing(U, idxs_L) = UL;
		indexing(U, idxs_R) = UR;

		std::cout << "UL: " << UL.transpose() << std::endl;
		std::cout << "FR: " << FR.transpose() << std::endl;
		//std::cout << "U: " << U.transpose() << std::endl;

		// Esfuerzos en cada barra
		// f^a_loc = K_^a_loc * T^a^t * u^a   (formula 3.6.4 del libro)
		// ----------------------------------------------------------------

		// Barra a:
		Eigen::Matrix<double, 4, 4> Ka_loc, Ta;
		Ka_loc <<
			Ka_11_loc, Ka_12_loc,
			Ka_12_loc.transpose(), Ka_22_loc;
		Ta <<
			calcmat::matriz_rotacion2(ang_a), Eigen::Matrix2d::Zero(),
			Eigen::Matrix2d::Zero(), calcmat::matriz_rotacion2(ang_a);
		const Eigen::Vector4d ua = indexing(U, std::array<int, 4>{0, 1, 2, 3});
		const Eigen::Vector4d fa = Ka_loc * Ta.transpose() * ua;

		// Barra b:
		Eigen::Matrix<double, 4, 4> Kb_loc, Tb;
		Kb_loc <<
			Kb_11_loc, Kb_13_loc,
			Kb_13_loc.transpose(), Kb_33_loc;
		Tb <<
			calcmat::matriz_rotacion2(ang_b), Eigen::Matrix2d::Zero(),
			Eigen::Matrix2d::Zero(), calcmat::matriz_rotacion2(ang_b);
		const Eigen::Vector4d ub = indexing(U, std::array<int, 4>{0, 1, 4, 5});
		const Eigen::Vector4d fb = Kb_loc * Tb.transpose() * ub;

		// Barra c:
		Eigen::Matrix<double, 4, 4> Kc_loc, Tc;
		Kc_loc <<
			Kc_22_loc, Kc_23_loc,
			Kc_23_loc.transpose(), Kc_33_loc;
		Tc <<
			calcmat::matriz_rotacion2(ang_c), Eigen::Matrix2d::Zero(),
			Eigen::Matrix2d::Zero(), calcmat::matriz_rotacion2(ang_c);
		const Eigen::Vector4d uc = indexing(U, std::array<int, 4>{2, 3, 4, 5});
		const Eigen::Vector4d fc = Kc_loc * Tc.transpose() * uc;

		std::cout << std::endl << "Esfuerzos en barras (coordenadas locales):" << std::endl;
		std::cout << "fa: " << fa.transpose() << std::endl;
		std::cout << "fb: " << fb.transpose() << std::endl;
		std::cout << "fc: " << fc.transpose() << std::endl;

		return 0; // el programa finaliza sin errores
	}
	catch (std::exception &e)
	{
		std::cerr << e.what() << std::endl;
		return -1;
	}
}


// (C) Copyright Renaud Detry   2007-2015.
// Distributed under the GNU General Public License and under the
// BSD 3-Clause License (See accompanying file LICENSE.txt).

/** @file */

#include <boost/algorithm/string.hpp>

#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>

#include <nuklei/LinearAlgebra.h>

namespace nuklei
{

  namespace la
  {
    void eigenDecomposition(Matrix3 &eVectors, Vector3& eValues, const Matrix3& sym)
    {
       coord_t data[9];
       for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j)
        data[i*3+j] = sym(i,j);

       gsl_matrix_view m
         = gsl_matrix_view_array (data, 3, 3);

       gsl_vector *eval = gsl_vector_alloc (3);
       gsl_matrix *evec = gsl_matrix_alloc (3, 3);

       gsl_eigen_symmv_workspace * w =
         gsl_eigen_symmv_alloc (3);

       gsl_eigen_symmv (&m.matrix, eval, evec, w);

       gsl_eigen_symmv_free (w);

       gsl_eigen_symmv_sort (eval, evec,
                             GSL_EIGEN_SORT_ABS_DESC);

       for (int i = 0; i < 3; i++)
       {
         eValues[i]
            = gsl_vector_get (eval, i);
         //NUKLEI_ASSERT(eValues[i] >= 0);
         gsl_vector_view evec_i
            = gsl_matrix_column (evec, i);
         for (int j = 0; j < 3; ++j)
           eVectors(j, i) = gsl_vector_get(&evec_i.vector, j);
       }

       // There should be a more efficient way to do this...
       if (Vector3(eVectors.GetColumn(0)).Cross(eVectors.GetColumn(1)).Dot(eVectors.GetColumn(2)) < 0)
         eVectors.SetColumn(2, -Vector3(eVectors.GetColumn(2)));

       gsl_vector_free (eval);
       gsl_matrix_free (evec);
    }

    // The following function
    //     Vector3 project(const Plane3& plane, const Vector3& point)
    // is currently defined in KernelCollectionPCA.cpp

    double determinant(const GMatrix& m)
    {
      NUKLEI_FAST_ASSERT(m.GetRows() == m.GetColumns());
      const int dim = m.GetRows();

      gsl_matrix *lu;
      gsl_permutation *perm;
      int signum = 0;

      lu = gsl_matrix_alloc(dim, dim);
      perm = gsl_permutation_alloc(dim);

      for (int i = 0; i < dim; ++i)
        for (int j = 0; j < dim; ++j)
        {
          gsl_matrix_set(lu, i, j, m(i,j));
        }

      gsl_linalg_LU_decomp(lu, perm, &signum);
      double det = gsl_linalg_LU_det(lu, signum);

      gsl_matrix_free(lu);
      gsl_permutation_free(perm);

      return det;
    }

    GMatrix inverse(const GMatrix& m)
    {
      NUKLEI_FAST_ASSERT(m.GetRows() == m.GetColumns());
      const int dim = m.GetRows();

      gsl_matrix *lu, *inverse;
      gsl_permutation *perm;
      int signum = 0;

      lu = gsl_matrix_alloc(dim, dim);
      inverse = gsl_matrix_alloc(dim, dim);
      perm = gsl_permutation_alloc(dim);

      for (int i = 0; i < dim; ++i)
        for (int j = 0; j < dim; ++j)
        {
          gsl_matrix_set(lu, i, j, m(i,j));
        }

      gsl_linalg_LU_decomp(lu, perm, &signum);
      gsl_linalg_LU_invert(lu, perm, inverse);

      GMatrix inv(dim, dim);
      for (int i = 0; i < dim; ++i)
        for (int j = 0; j < dim; ++j)
        {
          inv(i,j) = gsl_matrix_get(inverse, i, j);
        }

      gsl_matrix_free(lu);
      gsl_matrix_free(inverse);
      gsl_permutation_free(perm);

      return inv;
    }

  }

  std::ostream& operator<<(std::ostream &out, const GMatrix &m)
  {
    for (int i = 0; i < m.GetRows(); ++i)
    {
      for (int j = 0; j < m.GetColumns(); ++j)
        NUKLEI_ASSERT(out << m(i,j) << " ");
      NUKLEI_ASSERT(out << "\n");
    }
    out << std::flush;
    return out;
  }

  std::istream& operator>>(std::istream &in, GMatrix &m)
  {
    std::vector< std::vector<double> > rows;
    std::string line;
    int ntok = -1;
    while (std::getline(in, line))
    {
      cleanLine(line);
      std::vector<std::string> tokens;
      boost::split(tokens, line, boost::is_any_of(" "), boost::token_compress_on);
      if (tokens.front() == "")
        tokens.erase(tokens.begin());
      if (tokens.back() == "")
        tokens.pop_back();

      if (ntok == -1)
        ntok = tokens.size();
      else
        NUKLEI_ASSERT(ntok == tokens.size());

      if (tokens.size() == 0)
        break;

      rows.push_back(std::vector<double>());
      for (std::vector<std::string>::const_iterator i = tokens.begin();
           i != tokens.end(); ++i)
        rows.back().push_back(numify<double>(*i));
    }

    m.SetSize(rows.size(), ntok);
    for (int i = 0; i < m.GetRows(); ++i)
      for (int j = 0; j < m.GetColumns(); ++j)
        m(i,j) = rows.at(i).at(j);

    return in;
  }

}


#include <iostream>
#include <Eigen/Dense>
#include <fstream>
#include <sstream>
#include <cmath>
using Eigen::MatrixXd;
using Eigen::VectorXd;
using namespace std;

VectorXd calculateError(VectorXd state,double gt_x,double gt_y,double gt_vx,double gt_vy){
  VectorXd gt(4);
  gt << gt_x,gt_y,gt_vx,gt_vy;
  VectorXd error = state-gt;
  return error;
}

MatrixXd calculateJacobian(VectorXd state){
   double px = state(0);
   double py = state(1);
   double vx = state(2);
   double vy = state(3);
   MatrixXd jacobian(3,4);
   double sqrt_power = sqrt((pow(px,2)+pow(py,2)));
   double constant = (px*px + py*py)*sqrt_power;
   jacobian << (px/sqrt_power),(py/sqrt_power),0,0,
               (-1*py/(pow(px,2)+pow(py,2))),(px/(pow(px,2)+pow(py,2))),0,0,
               py*(vx*py - vy*px)/constant, px*(px*vy - py*vx)/constant, px/sqrt_power, py/sqrt_power;
   return jacobian;
}

VectorXd radar2State(VectorXd m_radar){
   double r_m_rho = m_radar(0);
   double r_m_pi = m_radar(1);
   double r_m_rdot = m_radar(2);
   double r_m_px = r_m_rho*cos(r_m_pi);
        if ( r_m_px < 0.0001 ) {
        r_m_px = 0.0001;
      }
        double r_m_py = r_m_rho*sin(r_m_pi);
        if(r_m_py< 0.0001){
          r_m_py = 0.0001;
        }
        double r_m_vx = r_m_rdot*cos(r_m_pi);
        double r_m_vy = r_m_rdot*sin(r_m_pi);
  VectorXd r_x(4) ;
  r_x << r_m_px,r_m_py,r_m_vx,r_m_vy;
  return r_x;
}

int main()
{
  ifstream reader("C:\\DEV\\website\\data\\obj_pose-laser-radar-synthetic-input.txt");
  int tick = -10;
  //Matrix for laser
   MatrixXd R_laser_ = MatrixXd(2, 2);//measurement noise
    //measurement covariance matrix - laser
   R_laser_ << 0.0225, 0,
        0, 0.0225;

    MatrixXd l_H(2,4);// used to convert predicted state to measurement state, shape wise
     l_H << 1,0,0,0,
          0,1,0,0;

    VectorXd l_m_x(2);//measured state

  //Matrix for Radar
   MatrixXd R_radar_ = MatrixXd(3, 3);// measurement noise
    //measurement covariance matrix - radar
   R_radar_ << 0.09, 0, 0,
        0, 0.0009, 0,
        0, 0, 0.09;

   VectorXd R_m_x(3);
   MatrixXd Hj(3,4);

  string line;
  string part;

  //Global Variables , same for laser and radar
  MatrixXd P(4,4);//process covariance matrix
  MatrixXd p_P(4,4);//predicted covariance matrix
  MatrixXd F(4,4);//prediction matrix
  F << 1,0,0,0,
         0,1,0,0,
         0,0,1,0,
         0,0,0,1;
  MatrixXd Q(4,4);//process covariance matrix
  VectorXd y;// difference between predicted and measured
  MatrixXd I = MatrixXd::Identity(4,4);
  VectorXd x(4);//state
  VectorXd p_x(4);//predicted state
  VectorXd noise(4);//noise due to model simplicity
  MatrixXd S;
  MatrixXd K;
  VectorXd p_R_x(3);//state measurement in radar space

  char type;
  int count = 0;//for initializing
  double gt_x;//ground truth
  double gt_y;//ground truth
  double gt_vy;//ground truth
  double gt_vx;//ground truth

  double noise_ax = 9.0;//noise due to model simplicity
  double noise_ay = 9.0;//noise due to model simplicity

  long long c_timestamp;
  long long p_timestamp;
  double dt;

  float m_px,m_py,m_vx,m_vy;
  float m_rho,m_pi,m_rdot;

  while(getline(reader,line)){
    istringstream my_stream(line);
    my_stream>>type;
    if(type=='L'){
      my_stream>>m_px;
      my_stream>>m_py;
      l_m_x<< m_px,m_py;
      my_stream>>c_timestamp;
      //for intializing state and covariance matrix
      if(count==0){
        x <<m_px,m_py,0,0;
        P << 1, 0, 0, 0,
              0, 1, 0, 0,
              0, 0, 1000, 0,
              0, 0, 0, 1000;
        p_timestamp = c_timestamp;
        count++;
        continue;
      }
      dt = (c_timestamp - p_timestamp)/1000000.0;
       p_timestamp = c_timestamp;
      F(0,2) = dt;
      F(1,3) = dt;
      noise<< (noise_ax*dt*dt/2),(noise_ay*dt*dt/2),(noise_ax*dt),(noise_ay*dt);
      p_x = F*x + noise;

      Q << (dt*dt*dt*dt*noise_ax*noise_ax/4),0,(dt*dt*dt*noise_ax*noise_ax/2),0,
            0,(dt*dt*dt*dt*noise_ay*noise_ay/4),0,(dt*dt*dt*noise_ay*noise_ay/2),
            (dt*dt*dt*noise_ax*noise_ax/2),0,(dt*dt*noise_ax*noise_ax),0,
            0,(dt*dt*dt*noise_ay*noise_ay/2),0,(dt*dt*noise_ay*noise_ay);
      p_P = F*P*F.transpose() + Q;
      y = l_m_x-(l_H*p_x);
      S = l_H*p_P*l_H.transpose() + R_laser_;
      K = p_P*l_H.transpose()*S.inverse();
      x = p_x + K*y;
      P = (I-K*l_H)*p_P;
      my_stream>>gt_x;
      my_stream>>gt_y;
      my_stream>>gt_vx;
      my_stream>>gt_vy;
      cout<<"Error"<<calculateError(x,gt_x,gt_y,gt_vx,gt_vy)<<endl;
       VectorXd gt(4);
       gt << gt_x,gt_y,gt_vx,gt_vy;

      cout<<"predicted state = "<<x<<endl;
      cout<<"ground truth"<<gt<<endl;
    }
    if(type=='R'){
     my_stream>>m_rho;
      my_stream>>m_pi;
      my_stream>>m_rdot;
      R_m_x << m_rho,m_pi,m_rdot;
      my_stream>>c_timestamp;
       if(count==0){
        x  = radar2State(R_m_x);
        P << 1, 0, 0, 0,
              0, 1, 0, 0,
              0, 0, 1000, 0,
              0, 0, 0, 1000;
        p_timestamp = c_timestamp;
        count++;
        continue;
      }
      dt = (c_timestamp - p_timestamp)/1000000.0;
      p_timestamp = c_timestamp;
      F(0,2) = dt;
      F(1,3) = dt;
      noise<< (noise_ax*dt*dt/2),(noise_ay*dt*dt/2),(noise_ax*dt),(noise_ay*dt);
      p_x = F*x + noise;
      Q << (dt*dt*dt*dt*noise_ax*noise_ax/4),0,(dt*dt*dt*noise_ax*noise_ax/2),0,
            0,(dt*dt*dt*dt*noise_ay*noise_ay/4),0,(dt*dt*dt*noise_ay*noise_ay/2),
            (dt*dt*dt*noise_ax*noise_ax/2),0,(dt*dt*noise_ax*noise_ax),0,
            0,(dt*dt*dt*noise_ay*noise_ay/2),0,(dt*dt*noise_ay*noise_ay);
      p_P = F*P*F.transpose() + Q;
      double px = p_x(0);
      double py = p_x(1);
      double vx = p_x(2);
      double vy = p_x(3);
      double rho = sqrt(px*px + py*py);
      double theta = atan2(py, px);
      double rho_dot = (px*vx + py*vy) / rho;
      p_R_x << rho,theta,rho_dot;
      y = R_m_x - p_R_x;
      while ( y(1) > M_PI || y(1) < -M_PI ) {
    if ( y(1) > M_PI ) {
      y(1) -= M_PI;
    } else {
      y(1) += M_PI;
    }
  }
      Hj = calculateJacobian(p_x);
      S = Hj*p_P*Hj.transpose() + R_radar_;
      K = p_P*Hj.transpose()*S.inverse();
      x = p_x + K*y;
      P = (I-K*Hj)*p_P;
      my_stream>>gt_x;
      my_stream>>gt_y;
      my_stream>>gt_vx;
      my_stream>>gt_vy;
       VectorXd gt(4);
       gt << gt_x,gt_y,gt_vx,gt_vy;
      cout<<"Error"<<calculateError(x,gt_x,gt_y,gt_vx,gt_vy)<<endl;
      cout<<"predicted state = "<<x<<endl;
      cout<<"ground truth"<<gt<<endl;
    }
  }
  return 0;
}


#include <iostream>
#include <math.h>
#include <vector>
#include <Eigen/Dense>

using namespace std;

//Evaluate the Chebyshev polynomials up to order n in x.
vector<double> chebpolmult(const int &n,const double &x)
{
    vector<double> V={1,x};
    for (int k=1; k<n; k++)
        V.push_back(2*x*V[k]-V[k-1]);
    return V;
}

//Check orthogonality of Chebyshev polynomials.
int main(){
    int n=10;
    vector<double> V;
    Eigen::MatrixXd scal(n+1,n+1);
    for (int j=0; j<n+1; j++) {
        V=chebpolmult(n,cos(M_PI*(2*j+1)/2/(n+1)));
        for (int k=0; k<n+1; k++) scal(j,k)=V[k];
    }
    cout<<"Scalar products: "<<endl;
    for (int k=0; k<n+1; k++)
        for (int l=k+1; l<n+1; l++)
            cout<<scal.col(k).dot(scal.col(l))<<endl;
}

Nummerical Methods for CSE/PS9/solutions_ps9/ChebOrthog.cpp


// find_location.cpp

// Copyright Paul A. Bristow 2007.

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

// Example of finding location (mean)
// for normal (Gaussian) & Cauchy distribution.

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[find_location1
/*`
First we need some includes to access the normal distribution,
the algorithms to find location (and some std output of course).
*/

#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.
#include <boost/math/distributions/cauchy.hpp> // for cauchy_distribution
  using boost::math::cauchy; // typedef provides default type is double.
#include <boost/math/distributions/find_location.hpp>
  using boost::math::find_location;
  using boost::math::complement; // Needed if you want to use the complement version.
  using boost::math::policies::policy;

#include <iostream>
  using std::cout; using std::endl;
#include <iomanip>
  using std::setw; using std::setprecision;
#include <limits>
  using std::numeric_limits;
//] [/find_location1]

int main()
{
  cout << "Example: Find location (mean)." << endl;
  try
  {
//[find_location2
/*`
For this example, we will use the standard normal distribution,
with mean (location) zero and standard deviation (scale) unity.
This is also the default for this implementation.
*/
  normal N01;  // Default 'standard' normal distribution with zero mean and
  double sd = 1.; // normal default standard deviation is 1.
/*`Suppose we want to find a different normal distribution whose mean is shifted
so that only fraction p (here 0.001 or 0.1%) are below a certain chosen limit
(here -2, two standard deviations).
*/
  double z = -2.; // z to give prob p
  double p = 0.001; // only 0.1% below z

  cout << "Normal distribution with mean = " << N01.location()
    << ", standard deviation " << N01.scale()
    << ", has " << "fraction <= " << z
    << ", p = "  << cdf(N01, z) << endl;
  cout << "Normal distribution with mean = " << N01.location()
    << ", standard deviation " << N01.scale()
    << ", has " << "fraction > " << z
    << ", p = "  << cdf(complement(N01, z)) << endl; // Note: uses complement.
/*`
[pre
Normal distribution with mean = 0, standard deviation 1, has fraction <= -2, p = 0.0227501
Normal distribution with mean = 0, standard deviation 1, has fraction > -2, p = 0.97725
]
We can now use ''find_location'' to give a new offset mean.
*/
   double l = find_location<normal>(z, p, sd);
   cout << "offset location (mean) = " << l << endl;
/*`
that outputs:
[pre
offset location (mean) = 1.09023
]
showing that we need to shift the mean just over one standard deviation from its previous value of zero.

Then we can check that we have achieved our objective
by constructing a new distribution
with the offset mean (but same standard deviation):
*/
  normal np001pc(l, sd); // Same standard_deviation (scale) but with mean (location) shifted.
/*`
And re-calculating the fraction below our chosen limit.
*/
cout << "Normal distribution with mean = " << l
    << " has " << "fraction <= " << z
    << ", p = "  << cdf(np001pc, z) << endl;
  cout << "Normal distribution with mean = " << l
    << " has " << "fraction > " << z
    << ", p = "  << cdf(complement(np001pc, z)) << endl;
/*`
[pre
Normal distribution with mean = 1.09023 has fraction <= -2, p = 0.001
Normal distribution with mean = 1.09023 has fraction > -2, p = 0.999
]

[h4 Controlling Error Handling from find_location]
We can also control the policy for handling various errors.
For example, we can define a new (possibly unwise)
policy to ignore domain errors ('bad' arguments).

Unless we are using the boost::math namespace, we will need:
*/
  using boost::math::policies::policy;
  using boost::math::policies::domain_error;
  using boost::math::policies::ignore_error;

/*`
Using a typedef is often convenient, especially if it is re-used,
although it is not required, as the various examples below show.
*/
  typedef policy<domain_error<ignore_error> > ignore_domain_policy;
  // find_location with new policy, using typedef.
  l = find_location<normal>(z, p, sd, ignore_domain_policy());
  // Default policy policy<>, needs "using boost::math::policies::policy;"
  l = find_location<normal>(z, p, sd, policy<>());
  // Default policy, fully specified.
  l = find_location<normal>(z, p, sd, boost::math::policies::policy<>());
  // A new policy, ignoring domain errors, without using a typedef.
  l = find_location<normal>(z, p, sd, policy<domain_error<ignore_error> >());
/*`
If we want to use a probability that is the
[link complements complement of our probability],
we should not even think of writing `find_location<normal>(z, 1 - p, sd)`,
but, [link why_complements to avoid loss of accuracy], use the complement version.
*/
  z = 2.;
  double q = 0.95; // = 1 - p; // complement.
  l = find_location<normal>(complement(z, q, sd));

  normal np95pc(l, sd); // Same standard_deviation (scale) but with mean(location) shifted
  cout << "Normal distribution with mean = " << l << " has "
    << "fraction <= " << z << " = "  << cdf(np95pc, z) << endl;
  cout << "Normal distribution with mean = " << l << " has "
    << "fraction > " << z << " = "  << cdf(complement(np95pc, z)) << endl;
  //] [/find_location2]
  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}  // int main()

//[find_location_example_output
/*`
[pre
Example: Find location (mean).
Normal distribution with mean = 0, standard deviation 1, has fraction <= -2, p = 0.0227501
Normal distribution with mean = 0, standard deviation 1, has fraction > -2, p = 0.97725
offset location (mean) = 1.09023
Normal distribution with mean = 1.09023 has fraction <= -2, p = 0.001
Normal distribution with mean = 1.09023 has fraction > -2, p = 0.999
Normal distribution with mean = 0.355146 has fraction <= 2 = 0.95
Normal distribution with mean = 0.355146 has fraction > 2 = 0.05
]
*/
//] [/find_location_example_output]