// solving A * X = B
// using driver function gesv()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/gesv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

typedef std::complex<double> cmpx_t;

typedef ublas::matrix<double, ublas::column_major> m_t;
typedef ublas::matrix<cmpx_t, ublas::column_major> cm_t;

int main() {

  cout << endl;
  cout << "real system:" << endl << endl;

  size_t n = 5;
  m_t a (n, n);   // system matrix

  size_t nrhs = 2;
  m_t x (n, nrhs), b (n, nrhs);  // b -- right-hand side matrix

  std::vector<int> ipiv (n);  // pivot vector

  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]

  m_t aa (a);  // copy of a, because a is `lost' after gesv()

  for (int i = 0; i < x.size1(); ++i) {
    x (i, 0) = 1.;
    x (i, 1) = 2.;
  }
  b = prod (a, x);

  print_m (a, "A");
  cout << endl;
  print_m (b, "B");
  cout << endl;

  lapack::gesv (a, ipiv, b);   // solving the system, b contains x
  print_m (b, "X");
  cout << endl;

  x = prod (aa, b);
  print_m (x, "B = A X");
  cout << endl;

  ////////////////////////////////////////////////////////

  cout << endl;
  cout << "complex system:" << endl << endl;
  cm_t ca (3, 3), cb (3, 1), cx (3, 1);
  std::vector<int> ipiv2 (3);

  ca (0, 0) = cmpx_t (3, 0);
  ca (0, 1) = cmpx_t (4, 2);
  ca (0, 2) = cmpx_t (-7, 5);
  ca (1, 0) = cmpx_t (4, -2);
  ca (1, 1) = cmpx_t (-5, 0);
  ca (1, 2) = cmpx_t (0, -3);
  ca (2, 0) = cmpx_t (-7, -5);
  ca (2, 1) = cmpx_t (0, 3);
  ca (2, 2) = cmpx_t (2, 0);
  print_m (ca, "CA");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmpx_t (1, -1);
  cb = prod (ca, cx);
  print_m (cb, "CB");
  cout << endl;

  int ierr = lapack::gesv (ca, ipiv2, cb);
  if (ierr == 0)
    print_m (cb, "CX");
  else
    cout << "matrix is singular" << endl;

  cout << endl;

}


#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/LU>
#include <cmath>
#include <iostream>
#include <mathtoolbox/log-determinant.hpp>

double CalcCovariance(const Eigen::VectorXd& x_1, const Eigen::VectorXd& x_2)
{
    return std::exp(-(x_1 - x_2).squaredNorm());
}

int main(int argc, char** argv)
{
    constexpr int num_points = 150;
    constexpr int num_dims   = 3;

    const Eigen::MatrixXd points = Eigen::MatrixXd::Random(num_dims, num_points);

    Eigen::MatrixXd covariance_matrix(num_points, num_points);
    for (int i = 0; i < num_points; ++i)
    {
        for (int j = i; j < num_points; ++j)
        {
            const double covariance = CalcCovariance(points.col(i), points.col(j));

            covariance_matrix(i, j) = covariance;
            covariance_matrix(j, i) = covariance;
        }
    }

    const double log_det       = mathtoolbox::CalcLogDetOfSymmetricPositiveDefiniteMatrix(covariance_matrix);
    const double log_det_naive = std::log(covariance_matrix.determinant());

    assert(!std::isnan(log_det));

    std::cout << "log(det(K)) = " << log_det << std::endl;
    std::cout << "log(det(K)) = " << log_det_naive
              << " (calculated by a naive approach, which may suffer from numerical instability)" << std::endl;

    return 0;
}


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

namespace gd {
using namespace Eigen;
//using namespace std;
typedef Matrix<double, 3, 1> Vector3;
typedef Matrix<double, 3, 3> Matrix3;

/*

 System3(System2(System1(..)))

*/
class CoordinateSystem {
public:
	CoordinateSystem() {
	}/*
	virtual Matrix3 matrix(double x, double y, double z) {
		return this->matrix_local(x, y, z) * parent->matrix(x, y, z);
	}*/
	virtual Vector3 ex(double x, double y, double z) = 0;
	virtual Vector3 ey(double x, double y, double z) = 0;
	virtual Vector3 ez(double x, double y, double z) = 0;
	//virtual Vector3 get_origin_cartesian() { return Vector3(0,0,0); }
	virtual Vector3 to_cartesian(Vector3d v) = 0;
	virtual Vector3 from_cartesian(Vector3d v) = 0;

	Matrix3 basis_matrix(double x, double y, double z) {
		Matrix3 m;
		m.block<3,1>(0,0) = ex(x, y, z);
		m.block<3,1>(0,1) = ey(x, y, z);
		m.block<3,1>(0,2) = ez(x, y, z);
		return m;
	}
	/*Matrix3 matrix_local(double x, double y, double z) {
		Matrix3 m;
		m.block<3,1>(0,0) = ex(x, y, z);
		m.block<3,1>(0,1) = ey(x, y, z);
		m.block<3,1>(0,2) = ez(x, y, z);
		return m;
	}
	Matrix3 matrix(Vector3 v) {
		return this->matrix(v(0),v(1),v(2));
	}

	Matrix3 transformation_matrix(double x, double y, double z, CoordinateSystem* prime_system) {
		Matrix3 m1 = this->matrix(x, y, z);
		Matrix3 m2 = prime_system->matrix(x, y, z);
		return m1*m2.transpose();
	}*/
};

class Cartesian : public CoordinateSystem {
public:
	Vector3 ex(double x, double y, double z) {
		return Vector3(1, 0, 0);
	}
	Vector3 ey(double x, double y, double z) {
		return Vector3(0, 1, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
	Vector3 to_cartesian(Vector3d v) {
		return v;
	}
	Vector3 from_cartesian(Vector3d v) {
		return v;
	}
};

class Cylindrical : public CoordinateSystem {
public:
	Vector3 ex(double x, double y, double z) {
		double rho = sqrt(x*x+y*y);
		return Vector3(x/rho, y/rho, 0);
	}
	Vector3 ey(double x, double y, double z) {
		double rho = sqrt(x*x+y*y);
		return Vector3(y/rho, -x/rho, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
	Vector3 to_cartesian(Vector3d v) {
		double rho = v(0);
		double theta = v(1);
		double z = v(2);
		double x = rho * cos(theta);
		double y = rho * sin(theta);
		Vector3 cartesian(x,y,z);
		return cartesian;
	}
	Vector3 from_cartesian(Vector3d v) {
		double x = v(0);
		double y = v(1);
		double z = v(2);
		double rho = sqrt(x*x+y*y);
		double theta = atan2(y,x);
		Vector3 cylindrical(rho,theta,z);
		return cylindrical;
	}
};

class SphericalGalactic : public CoordinateSystem {
public:
	SphericalGalactic() {
		//rotation = AngleAxisd(M_PI, Vector3::UnitZ());
		//std::cout << "rot[" << rotation << "]" << std::endl;
	}
	Vector3 ex(double x, double y, double z) {
		double r = sqrt(x*x+y*y+z*z);
		return Vector3(x/r, y/r, z/r);
	}
	Vector3 ey(double x, double y, double z) {
		double rho = sqrt(x*x+y*y);
		return Vector3(-y/rho, x/rho, 0);
	}
	Vector3 ez(double x, double y, double z) {
		double r = sqrt(x*x+y*y+z*z);
		double rho = sqrt(x*x+y*y);
		Vector3 v = Vector3(z*x/rho/r, z*y/rho/r, -rho/r);
		return v;
	}
	Vector3 to_cartesian(Vector3d v) {
		double r = v(0);
		double phi = v(1);
		double theta = v(2);
		double x = r * sin(theta) * cos(phi);
		double y = r * sin(theta) * sin(phi);
		double z = r * cos(theta);
		Vector3 cartesian(x,y,z);
		return cartesian;
	}
	Vector3 from_cartesian(Vector3d cartesian) {
		double x = cartesian(0);
		double y = cartesian(1);
		double z = cartesian(2);
		double r = sqrt(x*x+y*y+z*z);
		double phi = atan2(y,x);
		double theta = acos(z/r);
		Vector3 spherical(r,phi,theta);
		return spherical;
	}
};

class Coordinate {
public:
	CoordinateSystem* coordinate_system;
	Vector3 x;
	Coordinate(double x1, double x2, double x3, CoordinateSystem* coordinate_system) : coordinate_system(coordinate_system), x(x1, x2, x3) {
	}
	/*virtual Matrix3 matrix(double x, double y, double z) {
		return this->matrix_local(x, y, z) * parent->matrix(x, y, z);
	}
	virtual Vector3 ex(double x, double y, double z) = 0;
	virtual Vector3 ey(double x, double y, double z) = 0;
	virtual Vector3 ez(double x, double y, double z) = 0;
	//virtual Vector3 get_origin_cartesian() { return Vector3(0,0,0); }
	virtual Vector3 cartesian_transformation(Vector3 v) { return v;}*/
	virtual Vector3 to_cartesian() {
		return coordinate_system->to_cartesian(x);
	}
	//virtual Vector3 to_cartesian_local(Vector3d v) = 0;
	//virtual Vector3 from_cartesian_local(Vector3d v) = 0;
	/*Matrix3 matrix_local(double x, double y, double z) {
		Matrix3 m;
		m.block<3,1>(0,0) = ex(x, y, z);
		m.block<3,1>(0,1) = ey(x, y, z);
		m.block<3,1>(0,2) = ez(x, y, z);
		return m;
	}
	Matrix3 matrix(Vector3 v) {
		return this->matrix(v(0),v(1),v(2));
	}

	Matrix3 transformation_matrix(double x, double y, double z, CoordinateSystem* prime_system) {
		Matrix3 m1 = this->matrix(x, y, z);
		Matrix3 m2 = prime_system->matrix(x, y, z);
		return m1*m2.transpose();
	}*/
};

class VelocityCoordinate {
public:
	CoordinateSystem* coordinate_system;
	Vector3 v;
	VelocityCoordinate(double v1, double v2, double v3, CoordinateSystem* coordinate_system) : coordinate_system(coordinate_system), v(v1, v2, v3) {
	}
	/*virtual Matrix3 matrix(double x, double y, double z) {
		return this->matrix_local(x, y, z) * parent->matrix(x, y, z);
	}
	virtual Vector3 ex(double x, double y, double z) = 0;
	virtual Vector3 ey(double x, double y, double z) = 0;
	virtual Vector3 ez(double x, double y, double z) = 0;
	//virtual Vector3 get_origin_cartesian() { return Vector3(0,0,0); }
	virtual Vector3 cartesian_transformation(Vector3 v) { return v;}*/
	virtual Vector3 to_cartesian(double x, double y, double z) {
		return \
			v(0) * coordinate_system->ex(x, y, z) +\
			v(1) * coordinate_system->ey(x, y, z) +\
			v(2) * coordinate_system->ez(x, y, z);
	}
	virtual Vector3 to_cartesian_vec(Vector3 p) {
		return to_cartesian(p(0), p(1), p(2));
	}

};

class ReferenceFrameBase {
public:
	Matrix3 basis_matrix(double x, double y, double z) {
		Matrix3 m;
		m.block<3,1>(0,0) = ex(x, y, z);
		m.block<3,1>(0,1) = ey(x, y, z);
		m.block<3,1>(0,2) = ez(x, y, z);
		return m;
	}
	virtual Vector3 ex(double x, double y, double z) = 0;
	virtual Vector3 ey(double x, double y, double z) = 0;
	virtual Vector3 ez(double x, double y, double z) = 0;
	virtual Vector3 to_global(Vector3 local) = 0;
	virtual Vector3 to_local(Vector3 global) = 0;
	virtual Vector3 to_global_velocity(Vector3 v_local) { return v_local; }
	virtual Vector3 to_local_velocity(Vector3 v_global) { return v_global; }
};

class Position {
public:
	Coordinate* c;
	ReferenceFrameBase* f;
	Position(Coordinate* c, ReferenceFrameBase* f) : c(c), f(f) {
	}
	Vector3 to(ReferenceFrameBase* target_frame) {
		return target_frame->to_local( f->to_global(c->to_cartesian()) );;
	}
	Vector3 to_global() {
		return f->to_global(c->to_cartesian());
	}
	Vector3 to_coordinate_system(ReferenceFrameBase* target_frame, CoordinateSystem* target_coordinate_system) {
		Vector3 v = to(target_frame);
		return target_coordinate_system->from_cartesian(v);
	}
};

class Velocity {
public:
	VelocityCoordinate* vc;
	ReferenceFrameBase* f;
	Position* p;
	ReferenceFrameBase* pos_frame;
	Velocity(VelocityCoordinate* vc, ReferenceFrameBase* f, Position* p, ReferenceFrameBase* pos_frame) : vc(vc), f(f), p(p), pos_frame(pos_frame) {
	}
	Vector3 to_coordinate_system(ReferenceFrameBase* target_frame, CoordinateSystem* target_coordinate_system) {
		Vector3 v = to(target_frame);
		Vector3 local_coordinate = p->to(target_frame); //f->to_global(Vector3(0, 0, 0));
		double x = local_coordinate(0);
		double y = local_coordinate(1);
		double z = local_coordinate(2);
		//std::cout << "[x,y,z = ( " << x << ", " << y << ", " << z <<")]" << std::endl;
		Matrix3 m = target_coordinate_system->basis_matrix(x, y, z);
		return m.inverse() * v;
	}
	Vector3 to(ReferenceFrameBase* target_frame) {
		Vector3 global_velocity = to_global();
		return target_frame->to_local_velocity(global_velocity);
	}
	Vector3 to_global() {
		Vector3 local_coordinate = p->to(pos_frame); //f->to_global(Vector3(0, 0, 0));
		Vector3 local_velocity = vc->to_cartesian_vec(local_coordinate);
		Vector3 global_velocity = f->to_global_velocity(local_velocity);
		return global_velocity;
	}
};

class ZeroReferenceFrame : public ReferenceFrameBase {
public:
	ZeroReferenceFrame()  {
	}
	Vector3 ex(double x, double y, double z) {
		return Vector3(1, 0, 0);
	}
	Vector3 ey(double x, double y, double z) {
		return Vector3(0, 1, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
	virtual Vector3 to_global(Vector3 local) { return local; }
	virtual Vector3 to_local(Vector3 global) { return global; }
};

class ReferenceFrame : public ReferenceFrameBase {
public:
	Vector3 x0;
	ReferenceFrameBase* parent;
	ReferenceFrame(Position* origin, ReferenceFrameBase* parent) : x0(origin->to(parent)), parent(parent) {
	}
	virtual Vector3 to_global(Vector3 local) { return parent->to_global(local + x0); }
	virtual Vector3 to_local(Vector3 global) { return parent->to_local(global) - x0; }
	Vector3 ex(double x, double y, double z) {
		return Vector3(1, 0, 0);
	}
	Vector3 ey(double x, double y, double z) {
		return Vector3(0, 1, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
};

class RotatedReferenceFrame : public ReferenceFrameBase {
public:
	Matrix3 rotation;
	Matrix3 rotation_inverse;
	ReferenceFrameBase *parent;
	RotatedReferenceFrame(double angle, ReferenceFrameBase* parent) : parent(parent) {
		rotation = AngleAxisd(angle, Vector3::UnitZ());
		rotation_inverse = rotation.inverse();
	}
	virtual Vector3 to_global_velocity(Vector3 v_local) { return parent->to_global_velocity(rotation*v_local); }
	virtual Vector3 to_local_velocity(Vector3 v_global) { return rotation_inverse*parent->to_local_velocity(v_global); }
	virtual Vector3 to_global(Vector3 local) { return parent->to_global(rotation*local); }
	virtual Vector3 to_local(Vector3 global) { return rotation_inverse*parent->to_local(global); }
	//Vector3 get_origin_cartesian() { return origin->to_cartesian(); }
	Vector3 ex(double x, double y, double z) {
		return rotation * Vector3::UnitX();
	}
	Vector3 ey(double x, double y, double z) {
		return rotation * Vector3::UnitY();
	}
	Vector3 ez(double x, double y, double z) {
		return rotation * Vector3::UnitZ();
	}
};

class EqReferenceFrame : public ReferenceFrameBase {
public:
	Matrix3 rotation;
	Matrix3 rotation_inverse;
	ReferenceFrameBase *parent;
	EqReferenceFrame(double theta0, double a_NGP, double d_NGP, ReferenceFrameBase* parent) : parent(parent) {
		Matrix3 a,b,c;
		c << 	cos(a_NGP),  sin(a_NGP), 0,
				sin(a_NGP), -cos(a_NGP), 0,
				0, 0, 1;
		b << 	-sin(d_NGP), 0, cos(d_NGP),
				0, -1, 0,
				cos(d_NGP), 0, sin(d_NGP);
		a << 	cos(theta0),  sin(theta0), 0,
				sin(theta0), -cos(theta0), 0,
				0, 0, 1;
		rotation = a*b*c;
		rotation_inverse = rotation.inverse();
		//std::cout << "[" << rotation << "]" << std::endl;
		//std::cout << rotation_inverse << std::endl;
	}
	virtual Vector3 to_global_velocity(Vector3 v_local) { return parent->to_global_velocity(rotation*v_local); }
	virtual Vector3 to_local_velocity(Vector3 v_global) { return rotation_inverse*parent->to_local_velocity(v_global); }
	virtual Vector3 to_global(Vector3 local) { return parent->to_global(rotation*local); }
	virtual Vector3 to_local(Vector3 global) { return rotation_inverse*parent->to_local(global); }
	//Vector3 get_origin_cartesian() { return origin->to_cartesian(); }
	Vector3 ex(double x, double y, double z) {
		return rotation * Vector3::UnitX();
	}
	Vector3 ey(double x, double y, double z) {
		return rotation * Vector3::UnitY();
	}
	Vector3 ez(double x, double y, double z) {
		return rotation * Vector3::UnitZ();
	}
};

class MovingReferenceFrame : public ReferenceFrameBase {
public:
	ReferenceFrameBase* parent;
	Vector3 velocity_frame;
	MovingReferenceFrame(Velocity* velocity , ReferenceFrameBase* parent) : parent(parent) {
		velocity_frame = velocity->to(parent);
	}
	virtual Vector3 to_global_velocity(Vector3 v_local) { return parent->to_global_velocity(v_local+velocity_frame); }
	virtual Vector3 to_local_velocity(Vector3 v_global) { return parent->to_local_velocity(v_global)-velocity_frame; }
	virtual Vector3 to_global(Vector3 local) { return parent->to_global(local); }
	virtual Vector3 to_local(Vector3 global) { return parent->to_local(global); }
	Vector3 ex(double x, double y, double z) {
		return Vector3::UnitX();
	}
	Vector3 ey(double x, double y, double z) {
		return Vector3::UnitY();
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3::UnitZ();
	}
};

/*
class TranslatedReferenceFrame : public CoordinateSystem {
public:
	Vector3 translation;
	TranslatedReferenceFrame(double x, double y, double z, CoordinateSystem* parent) : CoordinateSystem(parent), translation(x, y, z) {
	}
	Vector3 ex(double x, double y, double z) {
		return Vector3(1, 0, 0);
	}
	Vector3 ey(double x, double y, double z) {
		return Vector3(0, 1, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
	Vector3 to_cartesian_local(Vector3d v) {
		return v+translation;
	}
	Vector3 from_cartesian_local(Vector3d cartesian) {
		return cartesian-translation;
	}
};*/
/*
class Cylindrical : public CoordinateSystem {
public:
	Position* origin;
	Cylindrical(Position* origin) : origin(origin) {
	}
	Vector3 get_origin_cartesian() { return origin->to_cartesian(); }
	Vector3 ex(double x, double y, double z) {
		double rho = sqrt(x*x+y*y);
		return Vector3(x/rho, y/rho, 0);
	}
	Vector3 ey(double x, double y, double z) {
		double rho = sqrt(x*x+y*y);
		return Vector3(y/rho, -x/rho, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
	Vector3 to_cartesian_local(Vector3d v) {
		double rho = v(0);
		double theta = v(1);
		double z = v(2);
		double x = rho * cos(theta);
		double y = rho * sin(theta);
		Vector3 cartesian(x,y,z);
		return cartesian;
	}
	Vector3 from_cartesian_local(Vector3d v) {
		double x = v(0);
		double y = v(1);
		double z = v(2);
		double rho = sqrt(x*x+y*y);
		double theta = atan2(y,x);
		Vector3 cylindrical(rho,theta,z);
		return cylindrical;
	}
};
*/
/*
class RotatedCoordinateSystem : public CoordinateSystem {
public:
	Matrix3 rotation;
	Matrix3 rotation_inverse;
	RotatedCoordinateSystem(CoordinateSystem* parent) : CoordinateSystem(parent) {
		rotation = AngleAxisd(M_PI, Vector3::UnitZ());
		rotation_inverse = rotation.inverse();
	}
	//Vector3 get_origin_cartesian() { return origin->to_cartesian(); }
	Vector3 ex(double x, double y, double z) {
		return rotation * parent->ex(x, y, z);
	}
	Vector3 ey(double x, double y, double z) {
		return rotation * parent->ey(x, y, z);
	}
	Vector3 ez(double x, double y, double z) {
		return rotation * parent->ez(x, y, z);
	}
	Vector3 to_cartesian_local(Vector3d v) {
		//return rotation*parent->to_cartesian(v);
		return rotation*v;
	}
	Vector3 from_cartesian_local(Vector3d cartesian) {
		//return parent->from_cartesian(rotation_inverse * cartesian);
		return rotation_inverse*cartesian;
	}
};

*/

/*
class Spherical {

	Vector3 ex(double x, double y, double z) {

	}
	Vector3 ey(double x, double y, double z) {
	}
	Vector3 ez(double x, double y, double z) {
	}
};*/

}


#include <kortex/eigen_conversion.h>

#include <Eigen/Eigenvalues>

#include <iostream>

namespace kortex {

    int matrix_invert_g_lu( const double* A, int ar, double* iA ) {
        passert_pointer( A && iA );
        assert_pointer_size( ar );
        Eigen::MatrixXd m;
        convert_mat( A, ar, ar, ar, m );
        int asz = ar*ar;
        convert_mat( m.inverse(), iA, asz );
        return !is_valid_array(iA,asz);
    }

    //
    // Least Square Solvers
    //
    void lsq_solver_svd( const double* A, int ar, int ac, int lda,
                         const double* B, int bc, int ldb,
                         double* x, int xsz ) {
        Eigen::MatrixXd eA; convert_mat( A, ar, ac, lda, eA );
        Eigen::MatrixXd eB; convert_mat( B, ar, bc, ldb, eB );
        Eigen::MatrixXd eX = eA.jacobiSvd( Eigen::ComputeThinU | Eigen::ComputeThinV ).solve(eB);
        convert_mat( eX, x, xsz );
        assert_array( "EIGEN - SVD LSQ", x, xsz );
    }

    void lsq_solver_cholesky( const double* A, int ar, int lda,
                              const double* B, int bc, int ldb,
                              double* x, int xsz ) {
        assert_statement( is_symmetric( A, ar, ar ), "A is not symmetric - call lsq_solver_svd" );
        Eigen::MatrixXd eA; convert_mat( A, ar, ar, lda, eA );
        Eigen::MatrixXd eB; convert_mat( B, ar, bc, ldb, eB );
        Eigen::MatrixXd eX = eA.ldlt().solve(eB);
        convert_mat( eX, x, xsz );
        assert_array( "EIGEN - CHOLESKY LSQ", x, xsz );
    }

    void lsq_solver_svd( const KMatrix& A, const KMatrix& B, KMatrix& x ) {
        x.resize( A.w(), B.w() );
        lsq_solver_svd( A(), A.h(), A.w(), A.w(), B(), B.w(), B.w(), x.get_pointer(), x.size() );
    }

    void lsq_solver_cholesky( const KMatrix& A, const KMatrix& B, KMatrix& x ) {
        x.resize( A.w(), B.w() );
        lsq_solver_cholesky( A(), A.h(), A.w(), B(), B.w(), B.w(), x.get_pointer(), x.size() );
    }

    //
    //
    //
    bool find_eigenvalues( const KMatrix& m, vector<double>& eig_real, vector<double>& eig_imag ) {
        eig_real.clear();
        eig_imag.clear();

        passert_statement( m.is_square(), "matrix needs to be square" );
        Eigen::MatrixXd em;
        convert_mat( m, em );
        Eigen::EigenSolver<Eigen::MatrixXd> esolver(em);

        auto eigs = esolver.eigenvalues();

        int n_eigs = eigs.rows();
        for( int i=0; i<n_eigs; i++ ) {
            std::complex<double> r = eigs.col(0)[i];
            eig_real.push_back( r.real() );
            eig_imag.push_back( r.imag() );
        }

        return true;
    }

}


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

int main() {
    Eigen::MatrixXd A(2, 2);
    A << 3.0, -1.5,
         2.5,   0.5;
    std::cout << "A =" << std::endl << A << std::endl;
    Eigen::VectorXd b(2);
    b << 3.5, -2.5;
    std::cout << "b =" << std::endl << b << std::endl;
    auto decomposition = A.partialPivLu();
    Eigen::VectorXd x = decomposition.solve(b);
    std::cout << "x =" << std::endl << x << std::endl;
    std::cout << "A*x - b =" << std::endl << A*x - b << std::endl;
    return 0;
}


#ifndef KALMAN_FILTER_HPP_

#define KALMAN_FILTER_HPP_
#include <Eigen/Dense>

namespace mineral_deposit_tracking
{

template<int StateSize>
class KalmanFilter
{
public:
	using VectorType = Eigen::Matrix<double, StateSize, 1>;
	using MatrixType = Eigen::Matrix<double, StateSize, StateSize>;

	KalmanFilter(
		const MatrixType & transition_matrix,
		const MatrixType & process_covariance,
		const MatrixType & observation_matrix)
	:	transition_matrix_(transition_matrix),
		process_covariance_(process_covariance),
		observation_matrix_(observation_matrix),
		estimate_(VectorType::Zero()),
		estimate_covariance_(MatrixType::Identity() * 500)
	{
	}

	void Reset(const VectorType & initial_state, const MatrixType & initial_covariance)
	{
		estimate_ = initial_state;
		estimate_covariance_ = initial_covariance;
	}

	void TimeUpdate()
	{
		estimate_ = transition_matrix_ * estimate_;
		estimate_covariance_ = (transition_matrix_ * estimate_covariance_ * transition_matrix_.transpose()) + process_covariance_;
	}

	void MeasurementUpdate(const VectorType & measurement, const MatrixType & measurement_covariance)
	{
		const MatrixType innovation_covariance = (observation_matrix_ * estimate_covariance_ * observation_matrix_.transpose()) + measurement_covariance;

		const MatrixType kalman_gain = estimate_covariance_ * observation_matrix_.transpose() * innovation_covariance.inverse();

		estimate_ = estimate_ + (kalman_gain * (measurement - (observation_matrix_ * estimate_)) );

		const MatrixType tmp = MatrixType::Identity() - (kalman_gain * observation_matrix_);

		estimate_covariance_ = ( tmp * estimate_covariance_ * tmp.transpose() ) + ( kalman_gain * measurement_covariance * kalman_gain.transpose() );

	}

	const VectorType & GetEstimate() const
	{
		return estimate_;
	}

	const MatrixType & GetEstimateCovariance() const
	{
		return estimate_covariance_;
	}

private:
	const MatrixType transition_matrix_;
	const MatrixType process_covariance_;
	const MatrixType observation_matrix_;
	VectorType estimate_;
	MatrixType estimate_covariance_;
};

}

#endif


/**
 *  @file    Mesh.hpp
 *  @brief   Definition of the finite difference mesh.
 *  @author  Francois Roy
 *  @date    12/04/2019
 */
#ifndef MESH_H
#define MESH_H

#include <stdexcept>
#include <vector>
#include <unordered_set>
#include <Eigen/Core>
#include "spdlog/spdlog.h"
#include "utils/Utils.hpp"

namespace numerical {

namespace fdm {

/*!
* Holds data structures for a uniform mesh on a hypercube in
* space, plus a uniform mesh in time.
*
*  \anchor fig_mesh
*  \image html bloc.png <"Figure 1: A typical mesh for a bloc. The scene shows
*  the bottom (orange), left (magenta) and front (blue) boundaries.">
*  \image latex bloc.eps "bloc" width=10cm
*
* @param lengths List of 2-lists of min and max coordinates in each spatial
* direction.
* @param t0 The initial time in time mesh.
* @param tend The final time in time mesh.
* @param nt Number of cells in time mesh.
* @param n List of number of cells in the spatial directions.
*/
template <typename T>
class Mesh {
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
private:
	const std::vector<std::vector<T>> m_lengths;
	const T m_t0, m_tend;
	T m_dt;
	const std::vector<int> m_nx;
	std::vector<T> m_dx;
	const int m_nt;
	std::vector<Vec> m_x;
	Vec m_t;
	std::size_t m_dim;
public:
	Mesh(const std::vector<std::vector<T>>& lengths, T t0, T tend,
		const std::vector<int>& nx, int nt)
	  : m_lengths(lengths),
	  m_t0(t0),
      m_tend(tend),
      m_nx(nx),
      m_nt(nt)
	{
        m_dim = m_lengths.size();
        if (m_dim < 1){
        	throw std::invalid_argument(
        		"At least one 2-lists of min and max coords must be given."
        		);
        }
        if (m_nx.size() != m_lengths.size()){
        	throw std::invalid_argument("n and lengths have different size.");
        }
        for(int i=0; i<m_dim; ++i){
        	if(m_lengths[i].size() != 2){
        		throw std::invalid_argument("length item is not a 2-list.");
        	}
        	m_dx.push_back(T (m_lengths[i][1] - m_lengths[i][0])/m_nx[i]);
        }
        m_dt = m_tend / T(nt);
        for(int i=0; i<m_dim; ++i){
        	Vec temp(m_nx[i] + 1);
        	for(int j=0; j<m_nx[i] + 1; ++j){
        		temp[j] = j * m_dx[i];
        	}
            m_x.push_back(temp);
        }
        m_t = Vec(m_nt + 1);
        for(int i=0; i<m_nt; ++i){
        	m_t[i] = i * m_nt;
        }
	}

    /*!
    * If 1D, \c ny\c and \c nz\c = 0, if 2D \c nz\c = 0, else all are > 1. See
    * \ref fig_mesh
    *
    * @return The number of divisions per axis.
    */
    std::vector<int> division_per_axis(){
        int n_x = m_nx[0];
        int n_y = 0;
        int n_z = 0;
        if (m_dim >= 2){
            n_y = m_nx[1];
        }
        if (m_dim == 3) {
            n_z = m_nx[2];
        }
        return {n_x, n_y, n_z};
    }

    /*!
    *  The indices of the nodes on the left boundary,
    *  i.e. at x = lengths[0][0].
    */
    std::vector<int> left(){
        std::vector<int> nx = division_per_axis();
        std::unordered_set<int> temp1;
        std::vector<int> temp;
        if (m_dim == 1){
            temp = {0};
        }
        else if (m_dim == 2){
            for (int j=0; j<nx[1] + 1; ++j){
                temp.push_back(j * (nx[0] + 1));
            }
        }
        else {  // m_dim = 3
            for (int j=0; j<nx[1] + 1; ++j){
                for (int k=0; k<nx[2] + 1; ++k){
                    temp.push_back(j * (nx[0] + 1) +
                        k * ((nx[0] + 1)*(nx[1] + 1)));
                }
            }
        }
        return temp;
    }

    /*!
    *  The indices of the nodes on the right boundary, i.e. at
    *  x = lengths[0][1]. It is just left + n_x.
    */
    std::vector<int> right(){
        std::vector<int> nx = division_per_axis();
        std::vector<int> out = left();
        for(int& i : out){
            i += nx[0];
        }
    	return out;
    }

    /*!
    *  The indices of the nodes on the bottom boundary, i.e. at
    *  y = lengths[1][0]. Only for 2D and 3D models.
    */
    std::vector<int> bottom(){
        std::vector<int> nx = division_per_axis();
        std::vector<int> temp;
        if (m_dim == 2){
            for (int i=0; i<nx[0] + 1; ++i){
                temp.push_back(i);
            }
        }
        else {  // m_dim = 3
            for (int i=0; i<nx[0] + 1; ++i){
                for (int k=0; k<nx[2] + 1; ++k){
                    temp.push_back(i + k * (nx[0] + 1) * (nx[1] + 1));
                }
            }
        }
        return temp;
    }

    /*!
    *  The indices of the nodes on the top boundary i.e. at
    *  y = lengths[1][1]. Only for 2D and 3D models.
    */
    std::vector<int> top(){
    	std::vector<int> nx = division_per_axis();
        std::vector<int> out = bottom();
        if (m_dim == 2){
            for(int& i : out){
                i += nx[1] * (nx[0] + 1);
            }
        }
        else { // 3D
            for(int& i : out){
                i += (nx[0] + 1) * (nx[0] + 1);
            }
        }
        return out;
    }

    /*!
    *  The indices of the nodes on the front, i.e. at
    *  z = length[2][0]. Only for 3D models.
    */
    std::vector<int> front(){
        std::vector<int> nx = division_per_axis();
        std::vector<int> out = back();
        for(int& i : out){
            i += nx[2] * (nx[0] + 1) * (nx[1] + 1);
        }
        return out;
    }

    /*!
    *  The indices of the nodes on the back boundary, i.e. at
    *  z = length[2][1]. Only for 3D models.
    */
    std::vector<int> back(){
        std::vector<int> nx = division_per_axis();
        std::vector<int> temp;
        for (int i=0; i<nx[0] + 1; ++i){
                for (int j=0; j<nx[1] + 1; ++j){
                    temp.push_back(i + j * (nx[0] + 1));
                }
            }
        return temp;
    }

    /*!
    * @return The indices of the external boundaries.
    */
    std::vector<int> boundaries(){
        // points in 1D
        // lines in 2D
        // surfaces in 3D
        std::vector<int> nx = division_per_axis();
        std::vector<int> temp;
        if (m_dim == 1){
            temp = {0, nx[0]};
        }
        else if (m_dim == 2){
            for (int j=0; j<nx[1] + 1; ++j){
                temp.push_back(j * (nx[0] + 1)); // left
                temp.push_back(j * (nx[0] + 1) + nx[0]); // right
            }
            for (int i=0; i<nx[0] + 1; ++i){
                temp.push_back(i);  // bottom
                temp.push_back(i + nx[1] * (nx[0] + 1));  // top
            }
            // make unique
            std::sort(temp.begin(), temp.end());
            auto last = std::unique(temp.begin(), temp.end());
            temp.erase(last, temp.end());
        }
        else {  // m_dim = 3
            for (int j=0; j<nx[1] + 1; ++j){
                for (int k=0; k<nx[2] + 1; ++k){
                    temp.push_back(j * (nx[0] + 1) +
                        k * ((nx[0] + 1)*(nx[1] + 1)));  // left
                    temp.push_back(j * (nx[0] + 1) +
                        k * ((nx[0] + 1)*(nx[1] + 1)) + nx[0]); // right
                }
            }
            for (int i=0; i<nx[0] + 1; ++i){
                for (int k=0; k<nx[2] + 1; ++k){
                    temp.push_back(i + k * (nx[0] + 1) *
                        (nx[1] + 1));  // bottom
                    temp.push_back(i + k * (nx[0] + 1) *
                        (nx[1] + 1) + (nx[0] + 1) * (nx[0] + 1));  // top
                }
            }
            for (int i=0; i<nx[0] + 1; ++i){
                for (int j=0; j<nx[1] + 1; ++j){
                    temp.push_back(i + j * (nx[0] + 1));  // back
                    temp.push_back(i + j * (nx[0] + 1) +
                        nx[2] * (nx[0] + 1) * (nx[1] + 1));   // front
                }
            }
            // make unique
            std::sort(temp.begin(), temp.end());
            auto last = std::unique(temp.begin(), temp.end());
            temp.erase(last, temp.end());
        }
        return temp;
    }

    /*!
    *
    */
    std::vector<int> domain(){
        std::vector<int> nx = division_per_axis();
        std::vector<int> all;
        if (m_dim == 1){
            all == utils::linear_spaced<int>(0, nx[0], nx[0] + 1);
        }
        else if (m_dim ==2){
            all == utils::linear_spaced<int>(0, (nx[0] + 1) * (nx[1] + 1) - 1,
                (nx[0] + 1) * (nx[1] + 1));
        } else {
            all == utils::linear_spaced<int>(0,
                (nx[0] + 1) * (nx[1] + 1) * (nx[2] + 1) - 1,
                (nx[0] + 1) * (nx[1] + 1) * (nx[2] + 1));
        }
        return all;
    }

    /*!
    *  @return The interior nodes.
    */
    std::vector<int> interior(){
        return {0};
    }

    /*!
    *  Returns the boundary nodes in 3D.
    *  @return The surfaces.
    */
    std::vector<int> surfaces(){
        std::vector<int> out;
        if (m_dim == 3){
            out = boundaries();
        }
        return out;
    }

    /*!
    *  2D, 3D
    */
    std::vector<int> edges(){
        // TODO define in 3D
        // boundaries in 2D
        return {0};
    }

    /*!
    *  1D, 2D, 3D
    */
    std::vector<int> corners(){
        // TODO define in 3D
        // 2D: 0, nx, ny * (nx+1), (ny+1) * (nx+1) -1
        // boundaries in 1D
        return {0};
    }

	/*!
	* @return the space dimension.
	*/
	std::size_t dim(){
		return m_dim;
	}
	/*!
	* @return the space increment for each dimension.
	*/
	std::vector<T> dx() {
		return m_dx;
	}

    /*!
    * Defines a mapping \f$m(i, j, k)\f$ from a mesh point with indices
    * \f$(i, j, k)\f$ to the corresponding unknown index \f$p\f$ in the
    * equation system:
    *
    * \f[
    *  p = m(i, j, k) = i+ j(n_x + 1) + k((n_x + 1)(n_y+1)))
    * \f]
    *
    * where \f$n_x\f$  and \f$n_y\f$ are the number of division along the
    * \f$x\f$- and \f$y\f$-directions.
    *
    * We number the points along the x axis, starting with y = y_0, and
    * z = z_0. We then progress one mesh line at a time
    * (from y = y_0 to y = y_end) until the slice
    * located at z = z_0 has been processed. We then continue on the next
    * slice until the entire meshed cube has been mapped.
    *
    * The coordinates are returned in a 3D vector.
    * @return The mesh node coordinates.
    */
    Eigen::Matrix<Eigen::Matrix<T, 3, 1>, Eigen::Dynamic, 1> coordinates() {
        // get the number of divisions per axis
        int n_x = m_nx[0];
        int n_y = 0;
        int n_z = 0;
        Vec y = Vec::Zero(n_x + 1);
        Vec z = Vec::Zero(n_x + 1);
        if (m_dim >= 2){
            n_y = m_nx[1];
            y = m_x[1];
            z = Vec::Zero((n_x + 1)*(n_y + 1));
        }
        if (m_dim == 3) {
            n_z = m_nx[2];
            z = m_x[2];
        }
        // generate the vector of coordinates
        Eigen::Matrix<T, 3, 1> node;
        Eigen::Matrix<Eigen::Matrix<T, 3, 1>, Eigen::Dynamic, 1> coords;
        for (int k=0; k<n_z + 1; ++k){
            for (int j=0; j<n_y + 1; ++j) {
                for (int i=0; i<n_x + 1; ++i){
                    // spdlog::debug("{}",i + j*(n_x + 1) + k*((n_x + 1) * (n_y+1)));
                    node = {m_x[0][i], y[j], z[k]};
                    // spdlog::debug("x: {} y: {} z: {}", m_x[0][i], y[j], z[k]);
                }
            }
        }
        return coords;
    }

	/*!
	* @return the time increment.
	*/
	T dt() {
		return m_dt;
	}
	/*!
	* @return the list of coordinates in each dimensions.
	*/
	std::vector<Vec> x() {
		return m_x;
	}
	/*!
	* @return the time list.
	*/
	Vec t() {
		return m_t;
	}
};

}  // namespace fdm

} // namespace numerical

#endif  // MESH_H


#include <Eigen/Dense>

/*
   A collection of metrics to use for determining the
   error between the predicted and actual Y values.
*/

double mse(Eigen::MatrixXd X, Eigen::VectorXd W, Eigen::VectorXd Y) {
	/*
	  Computes the mean squared error for a XW and Y.
	*/
	return 0.5 * (Y - X * W).transpose() * (Y - X * W);
}


// Test101.cpp
//
// First program test the boost statistics library.
//
// 2008-6-27 DD initial code
// 2008-10-10 DD Bernoulli distribution
// 2011-11-8 DD for QN students
//
// (C) Datasim Education BV 2009-2011
//

#include <boost/math/distributions/uniform.hpp>
#include <boost/math/distributions/bernoulli.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions.hpp> // For non-member functions of distributions

#include <iostream>
using namespace std;

int main()
{
	// Don't forget to tell compiler which namespace
	using namespace boost::math;

	uniform_distribution<> myUniform(0.0, 1.0); // Default type ie 'double'
	cout << "Lower value: " << myUniform.lower() << ", upper value: " << myUniform.upper() << endl;

	// Choose another data type
	uniform_distribution<float> myUniform2(0.0, 1.0);
	cout << "Lower value: " << myUniform2.lower() << ", upper value: " << myUniform2.upper() << endl;

	// Distributional properties
	double x = 0.25;

	cout << "pdf of Uniform: " << pdf(myUniform, x) << endl;
	cout << "cdf of Uniform: " << cdf(myUniform, x) << endl;

	// Bernoulli distributions
	bernoulli_distribution<> myBernoulli(0.4);
	cout << "Probability of success: " << myBernoulli.success_fraction() << endl;

	int k = 0;
	cout << "pdf of Bernoulli: " << pdf(myBernoulli, k) << endl;
	cout << "cdf of Bernoulli : " << cdf(myBernoulli, k) << endl << endl;

	// Choose precision
	cout.precision(8); // Number of values behind the comma

	// Other properties
	cout << "\n***Uniform distribution: \n";
	cout << "mean: " << mean(myUniform) << endl;

	// ... more

	cout << "hazard: " << hazard(myUniform, x) << endl;

	// STUDENT
	students_t_distribution<float> myStudent(30);

	// Quantiles and conversions between significance levels (fractions, 0.05)
	// and cofidence levels (in percentages, e.g. 95%)
	double Alpha = 0.25;
	cout << "Confidence 1: " << quantile(myStudent, Alpha / 2) << endl;
	cout << "Confidence, in %: " << quantile(complement(myStudent, Alpha / 2)) << endl;

	// Required sample sizes for Students t-distribution
	double M = 1.2;		// True mean
	double Sm = 1.8;	// Sample mean
	double Sd = 2.4;	// Sample standard deviation

	try
	{
		double df = students_t::find_degrees_of_freedom (fabs(M-Sm), Alpha, Alpha, Sd);
		int dof = ceil(df) + 1; // ceil(x) == smallest integer not less than x
		cout << "One-sided degrees of freedom: " << dof << endl;
	}
	catch(const std::exception& e)
	{
      cout << e.what() << endl;
	}

	return 0;
}

Level_8/V. An Introduction to the Boost C++ Libraries/05a - Statistical Functions/TestStatistics101.cpp


#include "Kinematics.h"
#include <Eigen/SVD>

using namespace MotionControl;

Matrix<double,3,3> Kinematics::Rodrigues(Matrix<double,3,1> a, double q)
{
	return AngleAxisd(q,a).toRotationMatrix();
}

Matrix<double,3,1> Kinematics::rot2omega(Matrix<double,3,3> R)
{
	double alpha = (R(0,0)+R(1,1)+R(2,2)-1)/2;
	double th;
	Matrix<double,3,1> vector_R(Matrix<double,3,1>::Zero());

	if(fabs(alpha-1) < eps)
		return Matrix<double,3,1>::Zero();

	th = acos(alpha);
	vector_R << R(2,1)-R(1,2), R(0,2)-R(2,0), R(1,0)-R(0,1);
	return 0.5*th/sin(th)*vector_R;
}

vector<int> Kinematics::FindRoute(int to)
{
	vector<int> idx;
	int link_num = to;

	while(link_num != 0)
	{
		idx.push_back(link_num);
		link_num = ulink[link_num].parent;
	}
	reverse(idx.begin(), idx.end());
	return idx;
}

Matrix<double,6,1> Kinematics::calcVWerr(Link Cref, Link Cnow)
{
	Matrix<double,3,1> perr = Cref.p - Cnow.p;
	Matrix<double,3,3> Rerr = Cref.R - Cnow.R;
	Matrix<double,3,1> werr = Cnow.R * rot2omega(Rerr);
	Matrix<double,6,1> err;

	err << perr,werr;
	return err;
}

void Kinematics::calcForwardKinematics(int rootlink)
{
	if(rootlink == -1)
		return ;
	if(rootlink != 0)
	{
		int parent = ulink[rootlink].parent;
		ulink[rootlink].p = ulink[parent].R * ulink[rootlink].b + ulink[parent].p;
		ulink[rootlink].R = ulink[parent].R * Rodrigues(ulink[rootlink].a, ulink[rootlink].q);
	}
	calcForwardKinematics(ulink[rootlink].sister);
	calcForwardKinematics(ulink[rootlink].child);
}

MatrixXd Kinematics::calcJacobian(vector<int> idx)
{
	size_t jsize = idx.size();
	Matrix<double,3,1> target = ulink[idx.back()].p;
	Matrix<double,6,11> J = MatrixXd::Zero(6,11);

	for(size_t i=0;i<jsize;i++)
	{
		int j = idx[i];
		Matrix<double,3,1> a = ulink[j].R * ulink[j].a;
		Matrix<double,3,1> b = a.cross(target - ulink[j].p);
		J(0,i) = b(0); J(1,i) = b(1); J(2,i) = b(2);
		J(3,i) = a(0); J(4,i) = a(1); J(5,i) = a(2);
	}

	return J;
}

template <typename t_matrix>
t_matrix Kinematics::PseudoInverse(const t_matrix& m, const double &tolerance)
{
	typedef JacobiSVD<t_matrix> TSVD;
	unsigned int svd_opt(ComputeThinU | ComputeThinV);
	if(m.RowsAtCompileTime!=Dynamic || m.ColsAtCompileTime!=Dynamic)
		svd_opt= ComputeFullU | ComputeFullV;
	TSVD svd(m, svd_opt);
	const typename TSVD::SingularValuesType &sigma(svd.singularValues());
	typename TSVD::SingularValuesType sigma_inv(sigma.size());
	for(long i=0; i<sigma.size(); ++i)
	{
		if(sigma(i) > tolerance)
			sigma_inv(i)= 1.0/sigma(i);
		else
			sigma_inv(i)= 0.0;
	}
	return svd.matrixV()*sigma_inv.asDiagonal()*svd.matrixU().transpose();
}

bool Kinematics::calcInverseKinematics(int to, Link target)
{
	MatrixXd J, dq;
	Matrix<double,6,1> err;

	ColPivHouseholderQR<MatrixXd> QR; //QR分解?
	const double dampingConstantSqr = 1.0e-12;
	const double lambda = 0.5;
	const int iteration = 100;

	calcForwardKinematics(WAIST);

	vector<int> idx = FindRoute(to);
	const int jsize = idx.size();

	J.resize(6,jsize); dq.resize(jsize,1);

	for(int n=0;n<iteration;n++){
		J = calcJacobian(idx);
		err = calcVWerr(target, ulink[to]);
		if(err.norm() < eps) return true;

		MatrixXd JJ = J*J.transpose()+dampingConstantSqr*MatrixXd::Identity(J.rows(),J.rows());
		dq = J.transpose() * QR.compute(JJ).solve(err) * lambda;

		for(size_t nn=0;nn<jsize;nn++){
			int j = idx[nn];
			ulink[j].q += dq(nn);
		}
		calcForwardKinematics(WAIST);
	}
	return false;
}


#include "tools.hpp"

#include <Eigen/Geometry>

using namespace std ;
using namespace Eigen ;
using namespace vsim ;

static float polygon_area(const vector<Vector2f> &contour)
{
    uint n = contour.size();

    float A = 0.0f;

    for( uint p=n-1, q=0; q<n; p=q++ )
    {
        A+= contour[p].x()*contour[q].y() - contour[q].x()*contour[p].y();
    }

    return A*0.5f;
}

/*
     InsideTriangle decides if a point P is Inside of the triangle
     defined by A, B, C.
   */
bool inside_triangle(float Ax, float Ay,
                                 float Bx, float By,
                                 float Cx, float Cy,
                                 float Px, float Py)

{
    float ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
    float cCROSSap, bCROSScp, aCROSSbp;

    ax = Cx - Bx;  ay = Cy - By;
    bx = Ax - Cx;  by = Ay - Cy;
    cx = Bx - Ax;  cy = By - Ay;
    apx= Px - Ax;  apy= Py - Ay;
    bpx= Px - Bx;  bpy= Py - By;
    cpx= Px - Cx;  cpy= Py - Cy;

    aCROSSbp = ax*bpy - ay*bpx;
    cCROSSap = cx*apy - cy*apx;
    bCROSScp = bx*cpy - by*cpx;

    return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
}

bool snip(const vector<Vector2f> &contour, int u,int v,int w,int n, vector<uint> &V)
{
    static const float EPSILON = 1.0e-10f;

    uint p;
    float Ax, Ay, Bx, By, Cx, Cy, Px, Py;

    Ax = contour[V[u]].x();
    Ay = contour[V[u]].y();

    Bx = contour[V[v]].x();
    By = contour[V[v]].y();

    Cx = contour[V[w]].x();
    Cy = contour[V[w]].y();

    if ( EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))) ) return false;

    for ( p=0 ; p<n ; p++ )
    {
        if ( (p == u) || (p == v) || (p == w) ) continue;
        Px = contour[V[p]].x();
        Py = contour[V[p]].y();
        if ( inside_triangle(Ax,Ay,Bx,By,Cx,Cy,Px,Py) ) return false;
    }

    return true;
}

static float triangle_area(const Vector3f &v1, const Vector3f &v2, const Vector3f &v3) {
    Vector3f n1, n2 ;

    n1 = v1 - v2 ;
    n2 = v1 - v3 ;
    return  n1.cross(n2).norm() ;
}

Eigen::Hyperplane<float, 3> fitPlaneToPoints(const vector<Vector3f> &pts) {
    uint n_pts = pts.size() ;
    assert(n_pts >= 3) ;

    if ( n_pts == 3 )
        return Eigen::Hyperplane<float, 3>::Through(pts[0], pts[1], pts[2]) ;
    else {
        Eigen::Map<Matrix<float,Dynamic,3,RowMajor> > mat((float *)pts.data(), pts.size(), 3);
        VectorXf centroid = mat.colwise().mean();
        MatrixXf centered = mat.rowwise() - centroid.adjoint() ;
        MatrixXf cov = (centered.adjoint() * centered) / double(mat.rows() - 1);
        JacobiSVD<Matrix3f> svd(cov, ComputeFullU);
        Vector3f normal = svd.matrixU().col(2);
        return Eigen::Hyperplane<float, 3>(normal, centroid) ;
    }
}

bool triangulate(const vector<Vector3f> &pts, vector<uint32_t> &result)
{
    if ( pts.size() < 3 ) return false ;

    if ( pts.size() == 3 ) {

        result.push_back(0) ; result.push_back(1) ; result.push_back(2) ;
        return true ;
    }

    // fit plane to points

    Eigen::Hyperplane<float, 3> plane = fitPlaneToPoints(pts) ;

    Vector3f na, nb, nz = plane.normal() ;
    double q = sqrt(nz.x() * nz.x() + nz.y() * nz.y()) ;
    if ( q < 1.0e-4 )
    {
        na = Vector3f(1, 0, 0) ;
        nb = nz.cross(na) ;
    }
    else {
        na = Vector3f(nz.y()/q, -nz.x()/q, 0) ;
        nb = Vector3f(nz.x() * nz.z()/q, nz.y() * nz.z()/q, -q) ;
    }

    // project points to plane

    vector<Vector2f> contour ;
    for(uint i=0 ; i<pts.size() ; i++ ) {
        const Vector3f &pt = pts[i] ;
        contour.push_back(Vector2f(na.dot(pt), nb.dot(pt))) ;
    }

    /* allocate and initialize list of Vertices in polygon */

    uint n = contour.size();

    vector<uint> V(n) ;

    /* we want a counter-clockwise polygon in V */

    if (  polygon_area(contour) > 0 )
        for ( uint v=0; v<n; v++) V[v] = v;
    else
        for(int v=0; v<n; v++) V[v] = (n-1)-v;

    uint nv = n;

    /*  remove nv-2 Vertices, creating 1 triangle every time */
    uint count = 2*nv;   /* error detection */

    for(int m=0, v=nv-1; nv>2; )
    {
        /* if we loop, it is probably a non-simple polygon */
        if (0 >= (count--))
        {
            //** Triangulate: ERROR - probable bad polygon!
            return false;
        }

        /* three consecutive vertices in current polygon, <u,v,w> */
        uint u = v  ; if (nv <= u) u = 0;     /* previous */
        v = u+1; if (nv <= v) v = 0;     /* new v    */
        uint w = v+1; if (nv <= w) w = 0;     /* next     */

        if ( snip(contour,u,v,w,nv,V) )
        {
            uint a,b,c,s,t;

            /* true names of the vertices */
            a = V[u]; b = V[v]; c = V[w];

            /* output Triangle */
            result.push_back( a );
            result.push_back( b );
            result.push_back( c );

            m++;

            /* remove v from remaining polygon */
            for(s=v,t=v+1;t<nv;s++,t++) V[s] = V[t]; nv--;

            /* resest error detection counter */
            count = 2*nv;
        }
    }

    return true;
}

void flatten_mesh(const Mesh &mesh, std::vector<Vector3f> &vertices, std::vector<Vector3f> &normals, std::vector<Vector3f> &colors,
                  std::vector<Vector2f> tex_coords[])
{
    vector<Vector3f> cnormals ;

    if ( mesh.normals_.empty() && mesh.ptype_ == Mesh::Triangles )
        compute_normals(mesh.vertices_, mesh.vertex_indices_, cnormals );

    for( uint v=0 ; v<mesh.vertex_indices_.size() ; v++) {

        uint32_t vidx = mesh.vertex_indices_[v] ;
        const Vector3f &pos = mesh.vertices_[vidx] ;
        vertices.push_back(pos) ;

        if ( !cnormals.empty() ) {
            const Vector3f &normal = cnormals[vidx] ;
            normals.push_back(normal) ;
        }
        else if ( !mesh.normal_indices_.empty() ) {
            uint32_t nidx = mesh.normal_indices_[v] ;
            const Vector3f &normal = mesh.normals_[nidx] ;
            normals.push_back(normal) ;
        } else if ( !mesh.normals_.empty() ){
            const Vector3f &norm = mesh.normals_[vidx] ;
            normals.push_back(norm) ;
        }

        if ( !mesh.colors_.empty() ) {
            if ( !mesh.color_indices_.empty() ) {
                uint32_t cidx = mesh.color_indices_[v] ;
                const Vector3f &color = mesh.colors_[cidx] ;
                colors.push_back(color) ;
            }
            else {
                const Vector3f &color = mesh.colors_[vidx] ;
                colors.push_back(color) ;
            }
        }

        for( uint t=0 ; t<MAX_MESH_TEXTURES ; t++ ) {
            if ( !mesh.tex_coords_[t].empty() ) {
                if ( !mesh.tex_coord_indices_[t].empty() ) {
                    uint32_t tidx = mesh.tex_coord_indices_[t][v] ;
                    const Vector2f &uv = mesh.tex_coords_[t][tidx] ;
                    tex_coords[t].push_back(uv) ;
                }
                else {
                    const Vector2f &uv = mesh.tex_coords_[t][vidx] ;
                    tex_coords[t].push_back(uv) ;
                }
            }
        }
    }
}

static Vector3f normal_triangle(const Vector3f &v1, const Vector3f &v2, const Vector3f &v3)
{
    Vector3f n1, n2 ;

    n1 = v1 - v2 ;
    n2 = v1 - v3 ;
    return  n1.cross(n2).normalized() ;

}
void compute_normals(const vector<Vector3f> &vertices, const vector<uint> &indices, vector<Vector3f> &vtx_normals)
{
    vtx_normals.resize(vertices.size()) ;
    for( int i=0 ; i<vertices.size() ; i++ ) vtx_normals[i] = Vector3f::Zero() ;

    for( int i=0 ; i<indices.size() ; i+=3 )
    {
        uint idx0 = indices[i] ;
        uint idx1 = indices[i+1] ;
        uint idx2 = indices[i+2] ;
        Vector3f n = normal_triangle(vertices[idx0], vertices[idx1], vertices[idx2]) ;

        vtx_normals[idx0] += n ;
        vtx_normals[idx1] += n ;
        vtx_normals[idx2] += n ;
    }

    for( int i=0 ; i<vertices.size() ; i++ ) vtx_normals[i].normalize() ;

}


/**
 * \file inc/dcs/math/function/bell.hpp
 *
 * \brief Compute the n-th Bell number.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 *

 *
 * Copyright 2014 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_FUNCTION_BELL_HPP
#define DCS_MATH_FUNCTION_BELL_HPP

#include <dcs/debug.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <vector>

namespace dcs { namespace math {

namespace detail { namespace /*<unnamed>*/ {

/**
 * \brief Compute the n-th Bell's number with the recursive formula.
 *
 * \tparam RealT The real type of the return value
 * \param n The order of the Bell's number
 * \return the n-th Bell's number
 *
 * The n-th Bell number \f$B_n\f$ can be computed as:
 * \f{equation}
 *  B_n=sum_(k=0)^(n-1){\binom{n-1}{k} B_k}
 * \f}
 *
 * \note This is a naive implementation that should not be used in practice.
 */
template <typename RealT>
RealT bell_rec(unsigned int n)
{
	if (n <= 1)
	{
		return 1;
	}

	RealT Bn = 0;
	for (unsigned int k = 0; k < n; ++k)
	{
		Bn += ::boost::math::binomial_coefficient<RealT>(n-1, k)*bell_rec<RealT>(k);
	}

	return Bn;
}

/**
 * \brief Compute the n-th Bell's number with the triangle method.
 *
 * \tparam RealT The real type of the return value
 * \param n The order of the Bell's number
 * \return the n-th Bell's number
 *
 * For details about the triangle method see [1].
 *
 * References:
 * -# J. Shallit
 *    "A triangle for the Bell numbers",
 *    In: A collection of manuscripts related to the Fibonacci sequence (eds. V.E. Hoggatt, Jr. and M. Bicknell-Johnson), pp. 69-71, Fibonacci Association, 1980
 *    [http://www.fq.math.ca/Books/Collection/shallit.pdf]
 * .
 */
template <typename RealT>
RealT bell_triangle(unsigned int n)
{
	if (n <= 1)
	{
		return 1;
	}

	::std::vector<RealT> Tup(n); // The upper row of Bell triangle
	::std::vector<RealT> Tlo(n); // The lower row of Bell triangle

    Tup[0] = 1;
	for (unsigned int i = 1; i < n; ++i)
	{
		Tlo[0] = Tup[i-1];
		for (unsigned int k = 1; k <= i; ++k)
		{
			Tlo[k] = Tlo[k-1]+Tup[k-1];
			Tup[k-1] = Tlo[k-1];
		}
		Tup[i] = Tlo[i];
	}

DCS_DEBUG_TRACE("B("<<n<<")="<<Tlo.back());
	return Tlo.back();
}

}} // Namespace detail::<unnamed>

#if 0 // Naive implementation, only used for testing/debugging purpose
/**
 * \brief Compute the n-th Bell number
 *
 * \tparam RealT The real type of the return value
 * \param n The order of the Bell's number
 * \return the n-th Bell's number
 *
 * The n-th Bell number \f$B_n\f$ is the number of ways a set of n elements can
 * be partitioned into nonempty subsets.
 * \f$B_n\f$ can be computed as:
 * \f{equation}
 *  B_n=sum_(k=0)^(n-1){\binom{n-1}{k} B_k}
 * \f}
 */
template <typename RealT>
RealT bell_naive(unsigned int n)
{
	return detail::bell_rec<RealT>(n);
}
#endif // if 0

/**
 * \brief Compute the n-th Bell number
 *
 * \tparam RealT The real type of the return value
 * \param n The order of the Bell's number
 * \return the n-th Bell's number
 *
 * The n-th Bell number \f$B_n\f$ (also called the exponential
 * number) is the number of ways a set of n elements can be partitioned
 * into nonempty subsets.
 * \f$B_n\f$ can be computed as:
 * \f{equation}
 *  B_n=sum_(k=0)^(n-1){\binom{n-1}{k} B_k}
 * \f}
 */
template <typename RealT>
RealT bell(unsigned int n)
{
	return detail::bell_triangle<RealT>(n);
}

}} // Namespace dcs::math

#endif // DCS_MATH_FUNCTION_BELL_HPP


#ifndef INVERT_MATRIX_GJ_HPP
#define INVERT_MATRIX_GJ_HPP

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

#define abs(x) ((x) < 0 ? -x : x)

namespace ublas = boost::numeric::ublas;

template<class T>
bool invert_matrix_gj(const ublas::matrix<T>& input, ublas::matrix<T>& inverse) {
    typedef ublas::matrix<T> tmatrix;
    typedef ublas::vector<T> tvector;
    typedef ublas::identity_matrix<T> imatrix;

    T eps = 1e-10;

    int h = input.size1();
    int w = input.size2();

    // get a working copy of the input and augment it by the identity matrix
    tmatrix A(h, w + h);
    imatrix I(h, h);
    for (int i = 0; i < A.size1(); i++) {
	for (int j = 0; j < A.size2(); j++) {
	    if (j < input.size2()) {
		A(i, j) = input(i, j);
	    } else {
		A(i, j) = I(i, j - input.size2());
	    }
	}
    }

    // do gauss-jordan
    for (int y = 0; y < h; y++) {
	// find max pivot
	int maxrow = y;
	for (int y2 = y+1; y2 < h; y2++) {
	    if (abs(A(y2, y)) > abs(A(maxrow,y))) {
		maxrow = y2;
	    }
	}
	// swap row y with maxrow
	ublas::matrix_row<tmatrix> current(A, y);
	ublas::matrix_row<tmatrix> toswap (A, maxrow);
	current.swap(toswap);
	// check for singularity
	if (abs(A(y, y)) <= eps) {
	    return false;
	}
	// eliminate row y
	for (int y2 = y+1; y2 < h; y2++) {
	    T c = A(y2, y) / A(y, y);
	    for (int x = y; x < w+h; x++) {
		A(y2, x) -= A(y, x) * c;
	    }
	}
    }
    // backsubstitution
    for (int y = h - 1; y >= 0; y--) {
	T c = A(y, y);
	for (int y2 = 0; y2 < y; y2++) {
	    for (int x = w+h - 1; x >= 0; x--) {
		A(y2, x) -= A(y, x) * A(y2, y) / c;
	    }
	}
	A(y, y) /= c;
	for (int x = h; x < w+h; x++) {
	    A(y, x) /= c;
	}
    }
    // get out the inverted part
    ublas::matrix_range<tmatrix> orig_range(A, ublas::range(0, h), ublas::range(h, w+h));
    inverse = orig_range;
    return true;
}

#endif/*INVERT_MATRIX_GJ_HPP*/


// vim: set fileencoding=utf-8
// File:     problem7.cpp
// Author:   Yusuke Sasaki <y_sasaki@nuem.nagoya-u.ac.jp>
// URL:      http://y-sasaki-nuem.github.io/
// License:  MIT License
// Created:  2015-03-13T01:40:14

// output:
// nth_prime(6) = 13	(in 0 secs)
// nth_prime(10001) = 104743	(in 1.048 secs)

#include <iostream>
#include <algorithm>
#include <stdexcept>
#include <boost/timer.hpp>
using namespace std;

template <size_t N>
void next_prime(long long (&primes)[N], int& i)
{
    for (int v = primes[i] + 1; v <= numeric_limits<long long>::max(); ++v)
    {
        if (all_of(primes, primes+i+1, [=](long long p){ return v%p != 0; })) {
            primes[++i] = v;
            return;
        }
    }
    throw std::logic_error("next prime is not found.");
}

template <size_t N>
long long nth_prime()
{
    boost::timer t;

    long long primes[N] = {0};
    primes[0] = 2L;

    for (int i = 0; i < N - 1; next_prime(primes, i));

    cout << "nth_prime("<<N<<") = "<<primes[N-1]<<"\t(in "<<t.elapsed()<<" secs)\n";
}

int main(int argc, char** argv)
{
    nth_prime<6>();
    nth_prime<10001>();
}


/*
 * Copyright (c) 2007-2010 Tao Wang <dancefire@gmail.org>
 * See the file "LICENSE.txt" for usage and redistribution license requirements
 *
 *	$Id$
 */

#pragma once
#ifndef _OPENCLAS_UNIT_TEST_VITERBI_HPP_
#define _OPENCLAS_UNIT_TEST_VITERBI_HPP_

#include <openclas/viterbi.hpp>
#include <boost/test/floating_point_comparison.hpp>

BOOST_AUTO_TEST_SUITE( viterbi )

using namespace openclas;

enum State { Rainy, Sunny };
enum Observation { Walk, Shop, Clean };
const char* StateName[] = { "Rainy", "Sunny" };
const char* ObservationName[] = { "walk", "shop", "clean" };

const int number_of_states = 2;
const int number_of_observations = 3;	//not for sequence, it's number of total obs

const int observation_sequence[] = { Walk, Shop, Clean };
const int number_of_observation_sequence = 3;

double start_probability[] = {/*Rainy*/ 0.6, /*Sunny*/ 0.4};
double transition_probability[] = {
	/*Rainy : */ /*Rainy*/ 0.7, /*Sunny*/ 0.3,
	/*Sunny : */ /*Rainy*/ 0.4, /*Sunny*/ 0.6
};
double emission_probability[] = {
	/*Rainy : */ /*walk*/ 0.1, /*shop*/ 0.4, /*clean*/ 0.5,
	/*Sunny : */ /*walk*/ 0.6, /*shop*/ 0.3, /*clean*/ 0.1
};

BOOST_AUTO_TEST_CASE( test_viterbi_array )
{

	std::vector<size_t> obs(observation_sequence, observation_sequence + number_of_observation_sequence);
	viterbi_info<double> result;
	forward_viterbi(number_of_states,
		number_of_observations,
		obs,
		start_probability,
		transition_probability,
		emission_probability,
		result);

	BOOST_CHECK_CLOSE( result.prob, 0.033612, 0.00001 );
	BOOST_CHECK_CLOSE( result.v_prob, 0.009408, 0.00001 );
	BOOST_CHECK_EQUAL( result.v_path.size(), 4 );
	BOOST_CHECK_EQUAL( result.v_path[0], Sunny );
	BOOST_CHECK_EQUAL( result.v_path[1], Rainy );
	BOOST_CHECK_EQUAL( result.v_path[2], Rainy );
	BOOST_CHECK_EQUAL( result.v_path[3], Rainy );

	//print(result, StateName);
	/*
	Example output:

	Total probability is 0.033612
	The Viterbi path is ['Sunny', 'Rainy', 'Rainy', 'Rainy']
	with probability 0.009408
	*/
}

BOOST_AUTO_TEST_CASE( test_viterbi_vector )
{

	std::vector<size_t> obs(observation_sequence, observation_sequence + number_of_observation_sequence);
	viterbi_info<double> result;
	//	Example of using STL container for start_p, tran_p and emit_p
	std::vector<double> start_p(start_probability, start_probability + number_of_states);
	std::vector<double> tran_p(transition_probability, transition_probability + (number_of_states * number_of_states));
	std::vector<double> emit_p(emission_probability, emission_probability + (number_of_states * number_of_observations));
	forward_viterbi(number_of_states,
		number_of_observations,
		obs,
		start_p, tran_p, emit_p, result);
	BOOST_CHECK_CLOSE( result.prob, 0.033612, 0.00001 );
	BOOST_CHECK_CLOSE( result.v_prob, 0.009408, 0.00001 );
	BOOST_CHECK_EQUAL( result.v_path.size(), 4 );
	BOOST_CHECK_EQUAL( result.v_path[0], Sunny );
	BOOST_CHECK_EQUAL( result.v_path[1], Rainy );
	BOOST_CHECK_EQUAL( result.v_path[2], Rainy );
	BOOST_CHECK_EQUAL( result.v_path[3], Rainy );

	//print(result, StateName);
	/*
	Example output:

	Total probability is 0.033612
	The Viterbi path is ['Sunny', 'Rainy', 'Rainy', 'Rainy']
	with probability 0.009408
	*/
}

BOOST_AUTO_TEST_SUITE_END()

//	_OPENCLAS_UNIT_TEST_VITERBI_HPP_
#endif


#include <algorithm>
#include <cmath>
#include <stdexcept>

#include <Eigen/QR>
#include <Euclid/Geometry/Spectral.h>

namespace Euclid
{

template<typename Mesh>
void WKS<Mesh>::build(const Mesh& mesh, unsigned k)
{
    _mesh = &mesh;
    auto n = spectrum(mesh, k, _loglambda, _phi2);
    // abs fix numerical error
    _lambda_max = std::abs(_loglambda(n - 1));
    _lambda_min = std::abs(_loglambda(1));
    _loglambda = _loglambda.array().abs().log().matrix().eval();
    _phi2 = _phi2.array().square().matrix().eval();
}

template<typename Mesh>
void WKS<Mesh>::build(const Mesh& mesh,
                      const Vec* eigenvalues,
                      const Mat* eigenfunctions)
{
    _mesh = &mesh;
    // abs fix numerical error
    _lambda_max = std::abs(eigenvalues->coeff(eigenvalues->size() - 1));
    _lambda_min = std::abs(eigenvalues->coeff(1));
    _loglambda = (*eigenvalues).array().abs().log().matrix().eval();
    _phi2 = (*eigenfunctions).array().square().matrix().eval();
}

template<typename Mesh>
template<typename Derived>
void WKS<Mesh>::compute(Eigen::ArrayBase<Derived>& wks,
                        unsigned escales,
                        float emin,
                        float emax,
                        float sigma)
{
    if (emin >= emax || sigma <= 0) {
        // the parameters described in paper form a linear system
        Eigen::Matrix3f A;
        Eigen::Vector3f B;
        A << 1.0f, 0.0f, -2.0f, 0.0f, 1.0f, 2.0f, 7.0f, -7.0f, escales + 0.0f;
        B << std::log(_lambda_min), std::log(_lambda_max), 0.0f;
        Eigen::Vector3f x = A.colPivHouseholderQr().solve(B);
        emin = x(0);
        emax = x(1);
        sigma = x(2);
        EASSERT(emin < 0);
        EASSERT(emax > emin);
        EASSERT(sigma > 0);
    }
    auto estep = (emax - emin) / escales;
    auto edenom = 0.5f / (sigma * sigma);
    auto vimap = get(boost::vertex_index, *_mesh);
    auto nv = num_vertices(*_mesh);
    wks.derived().resize(escales, nv);

    for (auto v : vertices(*_mesh)) {
        auto idx = get(vimap, v);
        for (size_t i = 0; i < escales; ++i) {
            auto e = emin + estep * i;
            auto wks_e = static_cast<FT>(0);
            auto ce = static_cast<FT>(0);
            for (int j = 1; j < _loglambda.size(); ++j) {
                auto exp = std::exp(-std::pow(e - _loglambda(j), 2) * edenom);
                ce += exp;
                wks_e += exp * _phi2(idx, j);
            }
            wks(i, idx) = wks_e / ce;
        }
    }
}

} // namespace Euclid


/*******************************************************************
Author: David Ge (dge893@gmail.com, aka Wei Ge)
Last modified: 11/02/2020
Allrights reserved by David Ge

Modifications
Date            Author      Description
---------------------------------------------

********************************************************************/
#include <malloc.h>

#include <math.h>
#define _USE_MATH_DEFINES // for C++
#include <cmath>

#include "HiMatrix.h"

#include <boost/multiprecision/cpp_dec_float.hpp>
using namespace boost::multiprecision;
HiMatrixTools::HiMatrixTools()
{

}

// Function to get cofactor of A[p][q] in temp[][]. n is current
// dimension of A[][]
void HiMatrixTools::HIgetCofactor(void *inA, void *intemp, int p, int q, int n)
{
	int i = 0, j = 0;
	int rowx = 0, ix = 0;
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *temp = (cpp_dec_float_100*)intemp;
	// Looping for each element of the matrix
	for (int row = 0; row < n; row++)
	{
		if (row != p)
		{
			for (int col = 0; col < n; col++)
			{
				//  Copying into temporary matrix only those element
				//  which are not in given row and column
				if (col != q)
				{
					temp[ix + j] = A[rowx + col];//A[row][col];

					j++;
					// Row is filled, so increase row index and
					// reset col index
					if (j == n - 1)
					{
						j = 0;
						i++;
						ix += (n - 1);
					}
				}
			}
		}
		rowx += n;
	}
}

/* Recursive function for finding determinant of matrix.
n is current dimension of A[][]. */
void HiMatrixTools::HIdeterminant(void *inA,void *outret, int n)
{
	cpp_dec_float_100 D = 0; // Initialize result
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *ret = (cpp_dec_float_100 *)outret;
	//  Base case : if matrix contains single element
	if (n == 1)
	{
		ret[0] = A[0];
		return;
	}
	cpp_dec_float_100 det;
	cpp_dec_float_100 *temp = (cpp_dec_float_100 *)malloc((n - 1)*(n - 1)*sizeof(cpp_dec_float_100));// [N][N]; // To store cofactors
	if (temp == NULL)
	{
		throw;
	}
	int sign = 1;  // To store sign multiplier

	// Iterate for each element of first row
	for (int f = 0; f < n; f++)
	{
		// Getting Cofactor of A[0][f]
		HIgetCofactor(A, temp, 0, f, n);
		//D += sign * A[0][f] * determinant(temp, n - 1);
		if (sign > 0)
		{
			HIdeterminant(temp, &det, n - 1);
			D += A[f] * det;
		}
		else
		{
			HIdeterminant(temp, &det, n - 1);
			D -= A[f] * det;
		}
		// terms are to be added with alternate sign
		sign = -sign;
	}
	free(temp);
	ret[0] = D;
	return;
}

// Function to get adjoint of A[N][N] in adj[N][N].
void HiMatrixTools::HIadjoint(void *inA, void *inadj, int N)
{
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *adj = (cpp_dec_float_100 *)inadj;
	if (N == 1)
	{
		adj[0] = 1;
		return;
	}

	// temp is used to store cofactors of A[][]
	int sign = 1;
	int ix;
	cpp_dec_float_100 det;
	cpp_dec_float_100 *temp = (cpp_dec_float_100 *)malloc((N - 1)*(N - 1)*sizeof(cpp_dec_float_100));

	for (int i = 0; i<N; i++)
	{
		ix = 0;
		for (int j = 0; j<N; j++)
		{
			// Get cofactor of A[i][j]
			HIgetCofactor(A, temp, i, j, N);

			// sign of adj[j][i] positive if sum of row
			// and column indexes is even.
			sign = ((i + j) % 2 == 0) ? 1 : -1;

			// Interchanging rows and columns to get the
			// transpose of the cofactor matrix
			//adj[j+ix] = (sign)*(determinant(temp, N-1));
			if (sign > 0)
			{
				HIdeterminant(temp, &det, N - 1);
				adj[i + ix] = det;
			}
			else
			{
				HIdeterminant(temp, &det, N - 1);
				adj[i + ix] = -det;
			}
			ix += N;
		}

	}
	free(temp);
}

// Function to calculate and store inverse, returns false if
// matrix is singular
bool HiMatrixTools::HIinverse(void *inA, void *inInverse, int N)
{
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *Inverse = (cpp_dec_float_100 *)inInverse;
	// Find determinant of A[][]
	cpp_dec_float_100 det;
	double ddet;
	HIdeterminant(A, &det, N);
	ddet = det.convert_to<double>();
	if (abs(ddet) < 1.1e-10)
	{
		//cout << "Singular matrix, can't find its inverse";
		return false;
	}

	// Find adjoint
	cpp_dec_float_100 *adj = (cpp_dec_float_100 *)malloc(N*N*sizeof(cpp_dec_float_100));
	if (adj == NULL) throw;
	HIadjoint(A, adj, N);

	// Find Inverse using formula "inverse(A) = adj(A)/det(A)"
	int ix = 0;
	for (int i = 0; i<N; i++)
	{
		for (int j = 0; j<N; j++)
		{
			Inverse[ix + j] = adj[ix + j] / det;
		}
		ix += N;
	}
	free(adj);
	return true;
}

void HiMatrixTools::HIMatrixMultiply(void *inA, void *inB, void *inC, int N)
{
	int i, j, ix, k, kx;
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *B = (cpp_dec_float_100 *)inB;
	cpp_dec_float_100 *C = (cpp_dec_float_100 *)inC;
	ix = 0;
	for (i = 0; i<N; i++)
	{
		for (j = 0; j<N; j++)
		{
			C[ix + j] = 0.0;
			kx = 0;
			for (k = 0; k<N; k++)
			{
				C[ix + j] += A[ix + k] * B[kx + j];
				kx += N;
			}
		}
		ix += N;
	}
}
double HiMatrixTools::HIinverseError(void *inA, void *inInverse, int N, double *errMax)
{
	double err = 0.0;
	size_t k;
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *B = (cpp_dec_float_100 *)inInverse;
	cpp_dec_float_100 *C = (cpp_dec_float_100 *)malloc(N*N*sizeof(cpp_dec_float_100));
	if (C == NULL)
		throw;
	cpp_dec_float_100 e0,em = 0.0;
	HIMatrixMultiply(A, B, C, N);
	k = 0;
	for ( int i = 0; i < N; i++)
	{
		for ( int j = 0; j < N; j++)
		{
			if (i == j)
			{
				e0 = 1.0 - C[k];
			}
			else
			{
				e0 = C[k];
			}
			err += abs(e0.convert_to<double>());
			if (e0 > em)
			{
				em = e0;
			}
			k++;
		}
	}
	free(C);
	*errMax = em.convert_to<double>();
	return err;
}


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

#include "utils.hpp"

using namespace std;
using namespace Eigen;

int main(void)
{
    // Make skew symmetric matrix
    Vector3d v;
    v << 8, 5, 6;

    cout << "== The vector ==" << endl;
    cout << v << endl;

    Matrix3d v_hat;
    Vector::wedge<double>(v_hat,v);
    cout << "== The skew symmetric matrix ==" << endl;
    cout << v_hat << endl;

    // Example Vector
    Vector3d a;
    a << 2, 5, 7;

    cout << "== Example vector ==" << endl;
    cout << a << endl;

    // Make Cross
    cout << "== v_hat * a ==" << endl;
    cout << v_hat * a << endl;
    cout << "== v x a ==" << endl;
    cout << v.cross(a) << endl;
    cout << "== -v_hat * a ==" << endl;
    cout << -v_hat * a << endl;
    cout << "== a x v ==" << endl;
    cout << a.cross(v) << endl;

    return 0;
}


#include <ql/option.hpp>
#include <ql/handle.hpp>
#include <ql/quote.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/termstructures/yield/flatforward.hpp>

#include <ql/termstructures/volatility/equityfx/blackconstantvol.hpp>
#include <ql/termstructures/volatility/equityfx/blackvoltermstructure.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <boost/make_shared.hpp>
#include <iostream>
#include <ql/math/distributions/normaldistribution.hpp>

using std::cout;
using std::endl;
using std::vector;
using boost::shared_ptr;
using boost::make_shared;
using namespace QuantLib;

struct blackscholes {
    double value;
    double delta;
    double gamma;
    double theta;

};

blackscholes black_scholes_formula(double S, double K, double T,
                                   double r, double sigma, bool call);

using std::sqrt;
using std::exp;
using std::log;

namespace {
    QuantLib::CumulativeNormalDistribution N;
    QuantLib::NormalDistribution n;
}

blackscholes black_scholes_formula(double S, double K, double T,
                                   double r, double sigma, bool call) {
    double d1 = (1/(sigma*sqrt(T))) * (log(S/K) + (r+sigma*sigma/2)*T);
    double d2 = d1 - sigma*sqrt(T);

    blackscholes results;
    if (call) {
        cout << "Call is called: ";
        results.value = N(d1)*S - N(d2)*K*exp(-r*T);
        results.delta = N(d1);
        results.theta = -(S*n(d1)*sigma)/(2*sqrt(T)) - r*K*exp(-r*T)*N(d2);
    } else {
        cout << "Put is called: ";
        results.value = N(-d2)*K*exp(-r*T) -N(-d1)*S;
        results.delta = -N(-d1);
        results.theta = -(S*n(d1)*sigma)/(2*sqrt(T)) + r*K*exp(-r*T)*N(-d2);
    }
    results.gamma = n(d1)/(S*sigma*sqrt(T));

    return results;
}

class EuropeanOption : public Instrument {

  public:
    EuropeanOption(Real strike, Option::Type, const Date& exerciseDate,
                   const Handle<Quote>& u,
                   const Handle<YieldTermStructure>& r,
                   const Handle<BlackVolTermStructure>& sigma);
    bool isExpired() const;
    void performCalculations() const;
    Real delta() const;
    Real gamma() const;
    Real theta() const;
  private:
    Real strike_;
    Option::Type type_;
    const Date& exerciseDate_;
    const Handle<Quote>& u_;
    const Handle<YieldTermStructure>& r_;
    const Handle<BlackVolTermStructure>& sigma_;
    mutable Real delta_;
    mutable Real gamma_;
    mutable Real theta_;

};

EuropeanOption ::EuropeanOption(Real strike, Option::Type type, const Date& exerciseDate, const Handle<Quote>& u,
                                const Handle<YieldTermStructure>& r, const Handle<BlackVolTermStructure>& sigma)
: strike_(strike), type_(type), exerciseDate_(exerciseDate), u_(u), r_(r), sigma_(sigma){
    cout << "Type is " <<type_<<endl;
    registerWith(u_);
    registerWith(r_);
    registerWith(sigma_);
}

bool EuropeanOption::isExpired() const {
    Date today = Settings::instance().evaluationDate();
    return today >= exerciseDate_;
}

void EuropeanOption::performCalculations() const {
    DayCounter dayCounter = r_->dayCounter();
    Date today = Settings::instance().evaluationDate();
    Time T = dayCounter.yearFraction(today, exerciseDate_);
    Rate r = r_->zeroRate(T, Continuous);
    Volatility sigma = sigma_->blackVol(T, strike_);
    blackscholes results = black_scholes_formula(u_->value(), strike_, T, r, sigma,
                                             type_ == Option::Call);

    NPV_ = results.value;
    delta_ = results.delta;
    gamma_ = results.gamma;
    theta_ = results.theta;
}

Real EuropeanOption::delta() const {

    // before returning delta_, we ensure that it is calculated: checking calculated flag (whether instrument is
    // upto date) or it's going to trigger the calculation in case its not.
    calculate();
    return delta_;
}

Real EuropeanOption::gamma() const {

    // before returning gamma_, we ensure that it is calculated: checking calculated flag (whether instrument is
    // upto date) or it's going to trigger the calculation in case its not.
    calculate();
    return gamma_;
}

Real EuropeanOption::theta() const {

    // before returning theta_, we ensure that it is calculated: checking calculated flag (whether instrument is
    // upto date) or it's going to trigger the calculation in case its not.
    calculate();
    return theta_;
}

int main() {
    Date today(1, May, 2022);
    Settings::instance().evaluationDate() = today;

    Real strike = 100.0;
    Option::Type type = Option::Call;
    Date exerciseDate = today + 360*3;

    shared_ptr<SimpleQuote> u = make_shared<SimpleQuote>(120.0);
    Handle<Quote> U(u);

    shared_ptr<YieldTermStructure> r =
        make_shared<FlatForward>(today, 0.01, Actual360());
    RelinkableHandle<YieldTermStructure> R(r);

    shared_ptr<BlackVolTermStructure> sigma =
        make_shared<BlackConstantVol>(today, TARGET(), 0.20, Actual360());
    Handle<BlackVolTermStructure> Sigma(sigma);

    EuropeanOption option(strike, type, exerciseDate, U, R, Sigma);

    cout << "option value: " << option.NPV() << endl;
    cout << "delta: " << option.delta() << endl;
    cout << "gamma: " << option.gamma() << endl;
    cout << "theta: " << option.theta() << endl;

    u->setValue(115.0);
    cout << "option value after u decreases: " << option.NPV() << endl;

    /*
     * OUTPUT:
     *
        Type is Call
        option value: Call is called: 29.0795
        delta: 0.784103
        gamma: 0.00704601
        theta: -2.67938
        option value after u decreases: Call is called: 25.2511
     */
    return 0;
}


// TestFactorial.cpp
//
// Factorials and Binomial Coefficients.
//
// Copyright Datasim Education BV 2009-2010
// Copyright John Maddock and Paul A. Bristow 2010

// 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)

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

#include <iostream>
using namespace std;

int main()
{
  using namespace boost::math;

  // Factorials
  unsigned int n = 3;

  try
  {
     cout << "Factorial: " << factorial<double>(n) << endl;

     // Caution: You must provide a return type template value, so this will not compile
     // unsigned int nfac = factorial(n); // could not deduce template argument for 'T'
     // You must provide an explicit floating-point (not integer) return type.
     // If you do provide an integer type, like this:
     // unsigned int uintfac = factorial<unsigned int>(n);
     // you will also get a compile error, for MSVC C2338.
     // If you really want an integer type, you can convert from double:
     unsigned int intfac = static_cast<unsigned int>(factorial<double>(n));
     // this will be exact, until the result of the factorial overflows the integer type.

     cout << "Unchecked factorial: " << boost::math::unchecked_factorial<float>(n) << endl;
     // Note:
     // unsigned int unfac = boost::math::unchecked_factorial<unsigned int>(n);
     // also fails to compile for the same reasons.
  }
  catch(exception& e)
  {
    cout << e.what() << endl;
  }

  // Double factorial n!!
  try
  {
    //cout << "Double factorial: " << boost::math::double_factorial<unsigned>(n);
  }
  catch(exception& e)
  {
    cout << e.what() << endl;
  }

  // Rising and falling factorials
  try
  {
    int i = 2; double x = 8;
    cout << "Rising factorial: " << rising_factorial(x,i) << endl;
    cout << "Falling factorial: " << falling_factorial(x,i) << endl;
  }
  catch(exception& e)
  {
    cout << e.what() << endl;
  }

  // Binomial coefficients
  try
  {
    unsigned n = 10; unsigned k = 2;
    // cout << "Binomial coefficient: " << boost::math::binomial_coefficient<unsigned>(n,k) << endl;
  }
  catch(exception& e)
  {
    cout << e.what() << endl;
  }
  return 0;
}

/*

Output:

  factorial_example.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\factorial_example.exe
  Factorial: 6
  Unchecked factorial: 6
  Rising factorial: 72
  Falling factorial: 56

*/


#include "cor_algorithm/sources/utilities.h"
#include "cor_system/sources/logger.h"
#include "cor_type/sources/math/vector3_tmpl_impl.h"
#include "cor_type/sources/math/vector4_tmpl_impl.h"

#define BOOST_TEST_NO_LIB
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(vector)

BOOST_AUTO_TEST_CASE(vector_3)
{
    cor::type::Vector3F a(1.0f, 0.0f, 0.0f);
    cor::type::Vector3F b(1.0f, 1.0f, 0.0f);
    cor::type::Vector3F c = a.cross(b);
    cor::RFloat d = a.dot(c);

    BOOST_CHECK_EQUAL(d, 0);

    cor::type::Vector3F ab = a + b;
    cor::type::Vector3F e = a + b - b;

    BOOST_CHECK_CLOSE((e - a).get_magnitude(), 0.0, 0.00001);

}

BOOST_AUTO_TEST_CASE(vector_4)
{
    cor::type::Vector4F a(1.0f, 0.0f, 0.0f, 2.0f);
    cor::type::Vector4F b(1.0f, 1.0f, 3.0f, 1.0f);
    cor::RFloat d = a.dot(b);

    BOOST_CHECK_CLOSE(d, 3.0, 0.00001);

    cor::type::Vector4F ab = a + b;
    cor::type::Vector4F e = a + b - b;

    BOOST_CHECK_CLOSE((e - a).get_magnitude(), 0.0, 0.00001);

}

BOOST_AUTO_TEST_SUITE_END()


// Copyright John Maddock 2006, 2007
// 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)

#include <iostream>
using std::cout; using std::endl;
using std::left; using std::fixed; using std::right; using std::scientific;
#include <iomanip>
using std::setw;
using std::setprecision;
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/special_functions/erf.hpp>

double upper_confidence_limit_on_std_deviation(
        double Sd,    // Sample Standard Deviation
        unsigned N,   // Sample size
        double pds)   // Probability
{
  // Calculate confidence intervals for the standard deviation.
  // For example if we set the confidence limit to
  // 0.95, we know that if we repeat the sampling
  // 100 times, then we expect that the true standard deviation
  // will be between out limits on 95 occations.
  // Note: this is not the same as saying a 95%
  // confidence interval means that there is a 95%
  // probability that the interval contains the true standard deviation.
  // The interval computed from a given sample either
  // contains the true standard deviation or it does not.
  // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda358.htm

  // using namespace boost::math;
  using boost::math::chi_squared;
  using boost::math::quantile;
  using boost::math::complement;

  // Start by declaring the distribution we'll need:
  chi_squared dist(N - 1);

  // Calculate limits:
  double lower_limit = ((N - 1) * Sd * Sd / quantile(complement(dist, 1 - pds))); // Needs to be checked if its pds or pds/2
  double upper_limit = ((N - 1) * Sd * Sd / quantile(dist, 1 - pds));
  std::cout << "Lower Limit = " << lower_limit << "\n";
  std::cout << "Upper Limit = " << upper_limit << "\n";

  return upper_limit;
}

double upper_confidence_limit_gaussian(
        double Sd,    // Sample Standard Deviation
        double pdv)   // Probability
{
  // Calculate confidence intervals for the standard deviation.
  // For example if we set the confidence limit to
  // 0.95, we know that if we repeat the sampling
  // 100 times, then we expect that the true standard deviation
  // will be between out limits on 95 occations.
  // Note: this is not the same as saying a 95%
  // confidence interval means that there is a 95%
  // probability that the interval contains the true standard deviation.
  // The interval computed from a given sample either
  // contains the true standard deviation or it does not.
  // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda358.htm

  // using namespace boost::math;
  using boost::math::normal;
  using boost::math::quantile;
  using boost::math::complement;

  // Start by declaring the distribution we'll need:
  normal normdist(0, Sd);

  // Calculate limits:
  double upper_limit = quantile(normdist, pdv);

  return upper_limit;
}

double get_inverse_error_func(double probability)
{
  // Calculate the inverse gaussian error function for a given probability
  // Check if probability value is in a valid range (absolute value)
  if(probability > 1 && probability < -1)
    return 0;

  return boost::math::erf_inv(probability);
}


/**
 * @file
 * @brief We integrate a function over a square using quadrature rules of a
 * fixed degree and refine the mesh to achieve convergence.
 * @author Raffael Casagrande
 * @date   2018-09-02 05:17:45
 * @copyright MIT License
 */

#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/quad/quad.h>

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

#include "lf/refinement/test/refinement_test_utils.h"

template <class F>
double integrate(const lf::mesh::Mesh& mesh, lf::quad::quadDegree_t degree,
                 F f) {
  double result = 0.;
  auto qr_tria = lf::quad::make_QuadRule(lf::base::RefEl::kTria(), degree);
  auto qr_quad = lf::quad::make_QuadRule(lf::base::RefEl::kQuad(), degree);

  Eigen::MatrixXd points(1, 1);
  Eigen::VectorXd weights(1);

  for (const auto* e : mesh.Entities(0)) {
    if (e->RefEl() == lf::base::RefEl::kTria()) {
      points = qr_tria.Points();
      weights = qr_tria.Weights();
    } else {
      points = qr_quad.Points();
      weights = qr_quad.Weights();
    }
    auto mapped_points = e->Geometry()->Global(points);
    auto integration_elements = e->Geometry()->IntegrationElement(points);
    for (Eigen::Index j = 0; j < points.cols(); ++j) {
      result += f(mapped_points.col(j)) * weights(j) * integration_elements(j);
    }
  }
  return result;
}

int main(int argc, char** argv) {
  // define allowed command line arguments:
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  // clang-format off
  desc.add_options()
  ("help", "produce this help message")
  ("quad_degree", po::value<int>()->default_value(3), "The degree of the local quadrature rule.")
  ("max_level", po::value<int>()->default_value(5), "The number of refinement levels")
  ;
  // clang-format on
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);
  if (vm.count("help") != 0U) {
    std::cout << desc << std::endl;
    return 1;
  }

  int max_level = vm["max_level"].as<int>();
  int quad_degree = vm["quad_degree"].as<int>();

  // Create a three element mesh that contains two triangles and one
  // quadrilateral
  lf::mesh::hybrid2d::MeshFactory mesh_factory(2);
  mesh_factory.AddPoint(Eigen::Vector2d{0, 0});
  mesh_factory.AddPoint(Eigen::Vector2d{0.5, 0});
  mesh_factory.AddPoint(Eigen::Vector2d{1, 0});
  mesh_factory.AddPoint(Eigen::Vector2d{1, 1});
  mesh_factory.AddPoint(Eigen::Vector2d{0.5, 1});
  mesh_factory.AddPoint(Eigen::Vector2d{0, 1});
  Eigen::MatrixXd node_coords(2, 3);
  node_coords << 0, 0.5, 0.5, 0, 0, 1;
  mesh_factory.AddEntity(lf::base::RefEl::kTria(),
                         std::vector<lf::base::size_type>{0, 1, 4},
                         std::make_unique<lf::geometry::TriaO1>(node_coords));
  node_coords << 0, 0.5, 0, 0, 1, 1;
  mesh_factory.AddEntity(lf::base::RefEl::kTria(),
                         std::vector<lf::base::size_type>{0, 4, 5},
                         std::make_unique<lf::geometry::TriaO1>(node_coords));
  node_coords = Eigen::MatrixXd(2, 4);
  node_coords << 0.5, 1, 1, 0.5, 0, 0, 1, 1;
  mesh_factory.AddEntity(lf::base::RefEl::kQuad(),
                         std::vector<lf::base::size_type>{1, 2, 3, 4},
                         std::make_unique<lf::geometry::QuadO1>(node_coords));

  auto base_mesh = mesh_factory.Build();

  // parameters:
  auto pi = boost::math::constants::pi<double>();
  auto f = [&](const Eigen::Vector2d& x) {
    return std::sin(pi * x(0)) * std::pow(std::cos(pi * x(1)), 2);
  };
  auto exact_integral = 1. / pi;

  auto mesh = base_mesh;
  auto errors = Eigen::VectorXd(max_level + 1);
  for (int level = 0; level <= max_level; ++level) {
    lf::io::VtkWriter vtk_writer(mesh,
                                 "level" + std::to_string(level) + ".vtk");

    auto approx = integrate(*mesh, quad_degree, f);
    errors(level) = std::abs(approx - exact_integral);

    lf::refinement::MeshHierarchy mh(
        mesh, std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2));
    mh.RefineRegular();
    mesh = mh.getMesh(1);
  }

  std::cout << "measured errors for each level: " << std::endl;
  std::cout << errors << std::endl << std::endl;

  // estimate the rate of convergence:
  Eigen::MatrixXd A(max_level + 1, 2);
  A.col(0).setOnes();
  A.col(1) = (-Eigen::ArrayXd::LinSpaced(max_level + 1, 1, max_level + 1) *
              std::log(2))
                 .matrix();

  Eigen::VectorXd b = errors.transpose().array().log().matrix();

  // consider only the three smallest meshes:
  A = A.bottomRows(3);
  b = b.bottomRows(3);

  Eigen::VectorXd x =
      A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
  std::cout << "estimated order of convergence = " << x(1) << std::endl;
}


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

using namespace std;
using namespace Eigen;

int main(){
    const int n = 100;
    MatrixXd a = MatrixXd::Random(n,n);
    MatrixXd A;
    A.noalias() = a * a.transpose();
    VectorXd b = VectorXd::Random(n);
    VectorXd x;

    cout<<"直接求逆"<<endl;
    auto t0 = chrono::steady_clock::now();
    x = A.inverse()*b;
    auto t1 = chrono::steady_clock::now();
    cout<<"Cost time: "<< chrono::duration<double,std::milli>(t1-t0).count()<<"ms"<<endl;
    cout<<"x= "<<x.transpose()<<endl;

    cout<<"QR 分解"<<endl;
    auto t2 = chrono::steady_clock::now();
    x = A.colPivHouseholderQr().solve(b);
    auto t3 = chrono::steady_clock::now();
    cout<<"Cost time: "<< chrono::duration<double,std::milli>(t3-t2).count()<<"ms"<<endl;
    cout<<"x= "<<x.transpose()<<endl;

    cout<<"Cholesky 分解"<<endl;
    auto t4 = chrono::steady_clock::now();
    x = A.ldlt().solve(b);
    auto t5 = chrono::steady_clock::now();
    cout<<"Cost time: "<< chrono::duration<double,std::milli>(t5-t4).count()<<"ms"<<endl;
    cout<<"x= "<<x.transpose()<<endl;

    return 0;
}


#include <bits/stdc++.h>
#include <boost/random.hpp>
#include "util.h"

using namespace std;

void Util::generate_divisors(const vector< pair<int,int> > &prime_factorization, vector<int> &ret, int at, int sofar) {
  if (at == prime_factorization.size()) {
    ret.push_back(sofar);
  } else {
    generate_divisors(prime_factorization, ret, at+1, sofar);
    for (int i = 0; i < prime_factorization[at].second; ++i) {
      sofar *= prime_factorization[at].first;
      generate_divisors(prime_factorization, ret, at+1, sofar);
    }
  }
}

// calculate generator for multiplicative group of F_q, q prime
int Util::primitive_root(int q, boost::random::mt19937 &rng) {
  int val = q-1;
  vector< pair<int, int> > prime_factorization; // factorize q-1;
  for (int d = 2; d*d <= q; ++d)
    if (val % d == 0) {
      int cnt = 0;
      while (val % d == 0) {
	++cnt;
	val /= d;
      }
      prime_factorization.push_back(make_pair(d, cnt));
    }
  if (val != 1)
    prime_factorization.push_back(make_pair(val, 1));
  vector<int> divisors;
  generate_divisors(prime_factorization, divisors, 0, 1);
  boost::uniform_int<> unif_int = boost::uniform_int<>(1, q - 1);
  while (true) {
    // a random element of F_p^* has probability 1-o(1) of being a generator
    int z = unif_int(rng);
    bool bad = false;
    for (int div : divisors) {
      if ((div == 1) || (div == q-1))
	continue;
      if (fast_mod_pow(z, div, q) == 1) {
	bad = true;
	break;
      }
    }
    if (!bad)
      return z;
  }
}

vector<int> Util::hadamard_transform(vector<int> x) {
  int n = x.size();
  if (n == 1)
    return x;
  else {
    assert((n&1) == 0);
    vector<int> ret(n);
    vector<int> xl(n/2);
    vector<int> xh(n/2);
    for (int i = 0; i < n/2; ++i)
      xl[i] = x[i];
    for (int i = n/2; i < n; ++i)
      xh[i-n/2] = x[i];
    xl = hadamard_transform(xl);
    xh = hadamard_transform(xh);
    for (int i = 0; i < n/2; ++i) {
      ret[i] = xl[i] + xh[i];
      ret[n/2+i] = xl[i] - xh[i];
    }
    return ret;
  }
}

int Util::fast_mod_pow(int x, int n, int q) {
  if (n==0)
    return 1;
  else {
    int z = fast_mod_pow(x, n/2, q);
    z = ((int64_t)z * z) % q;
    if (n&1 == 1)
      z = ((int64_t)z * x) % q;
    return z;
  }
}

vector<int> Util::compute_inverse_table(int q, boost::random::mt19937 &rng) {
  // primitive root of finite field F_q, allows us to calculate table of all
  // compute table of all F_q multiplicative inverses in O(q) time; naively
  // would be O(q log q), which is O(k log k) for t=2 since q=Theta(k) for t=2
  assert(is_prime(q));
  int z = primitive_root(q, rng);
  vector<int> zpows(q), ret(q);
  zpows[0] = 1;
  for (int i = 1; i < q; ++i)
    zpows[i] = ((int64_t)zpows[i-1] * z) % q;
  for (int i = 0; i < q-1; ++i)
    ret[zpows[i]] = zpows[q-i-1];
  return ret;
}

int Util::first_non_zero(const vector<int> &v) {
  int i = 0;
  while ((i<v.size()) && !v[i])
    ++i;
  if (i == v.size())
    return -1;
  else
    return i;
}

// normal a nonzero vector so that it's canonical (first non-entry is a 1)
void Util::canonicalize(vector<int> &v, int q, const vector<int> &qinv) {
  int i = first_non_zero(v);
  assert(i != -1);
  int g = qinv[v[i]];
  for (int j = i; j < v.size(); ++j)
    v[j] = ((int64_t)v[j] * g) % q;
}

vector<int> Util::canonicalized_index_to_vec(int j, int l, int q) {
  // given an index in {0,...,K-1}, return the corresponding canonical vector
  // recall a canonical vector is a vector in {0,...,q-1}^t which is nonzero,
  // and its first nonzero entry (going from smallest vector index to largest)
  // has the value 1
  int tot = 0;
  int qpow = 1;
  int num_zeroes = l - 1;
  while (j >= tot + qpow) {
    tot += qpow;
    qpow *= q;
    num_zeroes -= 1;
  }
  vector<int> ret(l);
  ret[num_zeroes] = 1;
  int r = j - tot;
  int at = l - 1;
  while (r > 0) {
    ret[at] = r % q;
    r /= q;
    at -= 1;
  }
  return ret;
}

int Util::canonicalized_vec_to_index(const vector<int> &v, int q, const vector<int> &qpows) {
  // given a canonical vector in {0,...,q-1}^t, return its index in
  // {0,...,K-1}
  int leftmost_one = Util::first_non_zero(v);
  assert((leftmost_one!=-1) && (v[leftmost_one]==1));
  int ret = (qpows[v.size() - 1 - leftmost_one] - 1) / (q - 1);
  int c = 0;
  for (int i = leftmost_one + 1; i < v.size(); ++i)
    c = (c*q) + v[i];
  return ret + c;
}

// vindex is index (1-based indexing) to a canonical vector v in F_q^l
// returns a 3dim-vector (v[0], g, vsuff_index), where g is the value of the
// first non-zero entry of vsuffix (vector v with v[0] removed) or 1 if
// vsuffix is the 0 vector. vsuff_index is the index of vsuffix amongst all
// canonical vectors in F_q^{l-1}
vector<int> Util::decompose_canonical_vector(int vindex, int l, int q,
					     const vector<int> &qpows, const vector<int> &qinv) {
  assert(l > 0);
  vector<int> v(l), vsuffix(l - 1); // vindex and its suffix as vectors
  if (vindex) {
    v = Util::canonicalized_index_to_vec(vindex - 1, l, q);
    assert(Util::canonicalized_vec_to_index(v, q, qpows) == vindex - 1);
    for (int i = 1; i < v.size(); ++i)
      vsuffix[i - 1] = v[i];
  }
  int g = 1; // value of first nonzero entry in vbsuffix, else 1 if it's all zeroes
  bool vsuffix_all_zeroes = true;
  for (int i = 0; i < vsuffix.size(); ++i)
    if (vsuffix[i]) {
      g = vsuffix[i];
      vsuffix_all_zeroes = false;
      break;
    }
  if (!vsuffix_all_zeroes)
    Util::canonicalize(vsuffix, q, qinv); // multiplies vsuffix through by g^{-1} entry-wise (mod q)
  int vsuff_index = 0; // a number in the range [0, (q^{l-1}-1)/(q-1) + 1]
  if (!vsuffix_all_zeroes) {
    vsuff_index = Util::canonicalized_vec_to_index(vsuffix, q, qpows) + 1;
    assert(Util::canonicalized_index_to_vec(vsuff_index - 1, l - 1, q) == vsuffix);
  }
  assert(vsuff_index <= (qpows[l - 1] - 1) / (q - 1) + 1);
  return vector<int>( { v[0], g, vsuff_index } );
}

int Util::mod_dot_product(const vector<int> &u, const vector<int> &v, int q) {
  assert(u.size() == v.size());
  int ret = 0;
  for (int i = 0; i < u.size(); ++i)
    (ret += ((int64_t)u[i]*v[i]) % q) %= q;
  return ret;
}

bool Util::is_prime(int q) {
  if (q < 2) return false;
  else if (q == 2) return true;
  else if (q % 2 == 0) return false;
  else {
    for (int d = 3; d < q; d += 2) {
      if ((int64_t)d*d > q) break;
      if (q % d == 0) return false;
    }
    return true;
  }
}

// pick a random nonzero vector u in F_q^t amongst those with <u,v> = s
// we assume here that v is not zero (else this isn't possible for all s)
vector<int> Util::randvec_with_specified_dot_product(const vector<int>& v, int s, int q,
						     const vector<int> &qinv, boost::random::mt19937 &rng) {
  boost::uniform_int<> unif_int = boost::uniform_int<>(0, q - 1);
  vector<int> ret(v.size());
  // pick ret to be a random element of (F_q)^t \ {0} s.t. <ret,v> = s mod q
  while (true) {
    for (int i = 0; i < v.size(); ++i)
      ret[i] = unif_int(rng);
    if (accumulate(ret.begin(), ret.end(), 0) == 0)
      continue;

    int i = first_non_zero(v);
    assert(i != -1);

    // compute the dot product of ret with v, ignoring index i
    int z = 0;
    for (int j = 0; j < v.size(); ++j)
      if (j != i)
	z = (z + (int64_t)v[j]*ret[j]) % q;
    // now set ret[i] to make the dot product s
    // we need z + ret[i]*v[i] = s, so ret[i] = (s - z)*v[i]^{-1}
    ret[i] = ((int64_t)(q + s - z) * qinv[v[i]]) % q;
    // be careful; ret might now be the 0 vector. try again if it is.
    if (accumulate(ret.begin(), ret.end(), 0))
      break;
  }
#ifdef DEBUG
  // make sure the dot product is actually what we said it would be
  int calc = 0;
  for (int i = 0; i < v.size(); ++i)
    (calc += (int64_t)v[i]*ret[i]) %= q;
  assert(calc == s);
#endif
  return ret;
}


/*=============================================================================

  PHAS0100ASSIGNMENT2: PHAS0100 Assignment 2 Gravitational N-body Simulation

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "catch.hpp"
#include "nbsimCatchMain.h"
#include "nbsimBasicTypes.h"
#include "nbsimMyFunctions.h"
#include <iostream>
#include <Eigen/Dense>
#include <cmath>

/* precision for isApprox() */
#define PREC 0.01

TEST_CASE( "1: isApprox Test", "[PartA]" ) {
	Eigen::Vector3d v1(1,0,0), v2(0,1,0), exp(0,1.0001,0.0001);
	REQUIRE(v2.isApprox(exp, PREC));
}

TEST_CASE( "2: particle move test", "[PartA]" ) {
	Eigen::Vector3d v1(1,0,0), v2(0,1,0), exp(0,1,0), zero(0,0,0), v1_1(1.00001, 0.00001, 0);
	nbsim::Particle parA(v1, v2), parB(v1, v2), parC(v1, v2);
	/* Check if correctly initialized. */
	REQUIRE(parA.getPosition() == v1);
	REQUIRE(parA.getVelocity() == v2);
	double ts = 0.0001;
	/* a. no acceleration. */
	for (double t=0; t<2*M_PI; t+=ts) {
		parA.integrateTimestep(zero, ts);
	}
	/* xt should be x + v * t . */
	std::cout << parA.getPosition() << std::endl;
	REQUIRE(parA.getPosition().isApprox(v1 + v2 * 2*M_PI, PREC));

	/* b. constant acc. */
	for (double t=0; t<2*M_PI; t+=ts) {
		parB.integrateTimestep(v2, ts);
	}
	/* xt should be x + v*t + 0.5*a*t^2 */
	std::cout << parB.getPosition() << std::endl;
	std::cout << v1 + v2*2*M_PI + 0.5*v2*2*M_PI*2*M_PI << std::endl;
	REQUIRE(parB.getPosition().isApprox(v1 + v2*2*M_PI + 0.5*v2*2*M_PI*2*M_PI, PREC));

	/* c.  centripetal a=-x. */
	for (double t=0; t<2*M_PI; t+=ts) {
		Eigen::Vector3d acc = -parC.getPosition();
		parC.integrateTimestep(acc, ts);
	}
	std::cout << parC.getPosition() << std::endl;
	REQUIRE(parC.getPosition().isApprox(v1, PREC));

}

TEST_CASE("MassiveParticle test", "[PartA]") {
	Eigen::Vector3d x1(1,0,0), v1(0,0.5,0), x2(-1,0,0), v2(0,-0.5,0);
	std::shared_ptr<nbsim::MassiveParticle> mp0(new nbsim::MassiveParticle(x1, v1, 10)),
											mp1(new nbsim::MassiveParticle(x1, v1, 1/GRAV)),
											mp2(new nbsim::MassiveParticle(x2, v2, 1/GRAV));
	double ts = 0.0001, dur = 2*M_PI;
	/* check init */
	REQUIRE(mp0->getPosition().isApprox(x1));
	REQUIRE(mp0->getVelocity().isApprox(v1));
	/* a. no attractor test. */
	for (double t=0; t<dur; t+=ts) {
		mp0->integrateTimestep(ts);
	}
	/* xt should be x + v * t . */
	std::cout << mp0->getPosition() << std::endl;
	std::cout << x1 + v1 * dur << std::endl;
	REQUIRE(mp0->getPosition().isApprox(x1 + v1 * dur, PREC));

	/* b. gravitationally attract */
	mp1->addAttractor(mp2);
	mp2->addAttractor(mp1);
	std::cout << "mu" << mp1->getMu() << std::endl;
	for (double t=0; t<dur; t+=ts) {
		mp1->calculateAcceleration();
		mp1->integrateTimestep(ts);
		mp2->calculateAcceleration();
		mp2->integrateTimestep(ts);
	}
	/* distance should be the same */
	Eigen::Vector3d ro = (x1 - x2);
	Eigen::Vector3d r = (mp1->getPosition() - mp2->getPosition());
	std::cout << ro.norm() << std::endl;
	std::cout << r.norm() << std::endl;
	REQUIRE(abs(ro.norm() - r.norm()) < PREC);

}