#include "registration.h"
#include <Eigen/Cholesky>
#include <vector>

struct SPoint2D
{
	double x[2];
};

double computeAngle(const Eigen::Vector2d& e_im, const Eigen::Vector2d& e_i)
{
	//compute theta
	//when e_im.dot(e_i) = 1.0, theta = pi;
	const double r_sinT = e_im(0)*e_i(1) - e_im(1)*e_i(0);
	const double r_cosT = -e_im.dot(e_i);
	return atan2(r_sinT, r_cosT);
}

double sgn(const double& x)
{
	return (x >= 0) ? 1.0 : -1.0;
}

double computeOmegaFromAngle(const double& theta)
{
	const double sinT = sin(theta);
	const double cosT = cos(theta);
	const double tan_phi_over_2 = sgn(sinT) * sqrt((1.0+cosT)/(max(0.0, 1.0-cosT) + 1.0e-10));
	return 2.0 * tan_phi_over_2;
}

double computeOmega(const Eigen::Vector2d& e_im, const Eigen::Vector2d& e_i)
{
	//compute 2.0 * tan(phi / 2.0)
	//phi = pi - theta
	return computeOmegaFromAngle(computeAngle(e_im, e_i));
}

double sampleDistanceField(const SImage<double, 1>& in_DF, const SRegion& in_Region, const Eigen::Vector2d& x)
{
	assert(in_DF.width == in_DF.height);
	assert(in_Region.right-in_Region.left == in_Region.top-in_Region.bottom);

	Eigen::Vector2d proj_x = x;
	proj_x(0) = std::max(in_Region.left, std::min(in_Region.right, x(0)));
	proj_x(1) = std::max(in_Region.bottom, std::min(in_Region.top, x(1)));

	const double dist_scale = (in_Region.right-in_Region.left) / in_DF.width;

	double fi = (proj_x(0) - in_Region.left) * in_DF.width / (in_Region.right - in_Region.left);
	double fj = (proj_x(1) - in_Region.bottom) * in_DF.height / (in_Region.top - in_Region.bottom);
	double _i = floor(fi);
	double _j = floor(fj);

	int i = std::max(0, std::min(in_DF.width-1, int(_i)));
	int j = std::max(0, std::min(in_DF.height-1, int(_j)));
	int ip = std::min(in_DF.width-1, i+1);
	int jp = std::min(in_DF.height-1, j+1);

	double s = fi - _i;
	double t = fj - _j;

	double dist_proj = (1.0 - t) * (s * in_DF.ptr[j*in_DF.width+ip] + (1.0-s) * in_DF.ptr[j*in_DF.width+i])
		+ t * (s * in_DF.ptr[jp*in_DF.width+ip] + (1.0-s) * in_DF.ptr[jp*in_DF.width+i]);

	return dist_proj * dist_scale + (proj_x - x).norm();
}

double integrateDF2OverSegment(const SParameters* in_Params, const Eigen::Vector2d& x1, const Eigen::Vector2d& x2, const double restL)
{
	double tot = 0.0;
	const double len = restL / in_Params->substep_fit;

	//printf("integ_DF: ");
	for(int i=0; i<in_Params->substep_fit; i++)
	{
		const Eigen::Vector2d& p1 = x1 + (x2-x1) * double(i)/double(in_Params->substep_fit);
		const Eigen::Vector2d& p2 = x1 + (x2-x1) * double(i+1)/double(in_Params->substep_fit);

		const double d1 = sampleDistanceField(in_Params->df, in_Params->region, p1);
		const double d2 = sampleDistanceField(in_Params->df, in_Params->region, p2);

		//printf("[%f, %f, %f], ", d1, d2, len);

		const double e1 = 0.5 * (exp(d1) + exp(-d1)) - 1.0;
		const double e2 = 0.5 * (exp(d2) + exp(-d2)) - 1.0;

		//tot += 0.5 * (d1*d1+d2*d2) * len;
		tot += 0.5 * (e1*e1+e2*e2) * len;
	}
	//printf("\n");

	return tot;
}

inline double computeSegmentElasticEnergy(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, const double& YA, const double& restLength)
{
	const Eigen::Vector2d diff = p1 - p2;
	return 0.5 * YA * restLength
		* (diff.norm() / restLength - 1)
		* (diff.norm() / restLength - 1);
}

inline double computeSegmentBendingEnergy(const Eigen::Vector2d& pp, const Eigen::Vector2d& pc, const Eigen::Vector2d& pn,
	const double& alpha, const double& restLengthP, const double& restLengthN, const double& theta)
{
	const Eigen::Vector2d e_im = pc - pp;
	const Eigen::Vector2d e_i = pn - pc;

	/*
	double omega = 2.0 * fabs(e_im(0)*e_i(1)-e_im(1)*e_i(0)) / (e_im.norm()*e_i.norm() + e_im.dot(e_i));
	double omega_bar = 2.0 * fabs(tan(phi*0.5));
	//*/
	double omega = computeOmega(e_im, e_i);
	double omega_bar = computeOmegaFromAngle(theta);
	return alpha * (omega - omega_bar) * (omega - omega_bar) / (restLengthP + restLengthN);
}

double computeEnergy_elastic(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	double energy = 0.0;

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_Curve->nVertices;
		energy += computeSegmentElasticEnergy(in_Position.col(ip), in_Position.col(i), in_Params->YA, in_Curve->restLengths(i));
	}

	return energy;
}

double computeEnergy_bending(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position)
{
	int nAngles = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 2;

	double energy = 0.0;

	for(int i=0; i<nAngles; i++)
	{
		int iim = in_Curve->closed ?
			(i + in_Curve->nVertices - 1) % in_Curve->nVertices : i;
		int ii = in_Curve->closed ? i : i + 1;
		int iip = in_Curve->closed ? (i + 1) % in_Curve->nVertices : i + 2;

		double restL_m = in_Curve->closed ? in_Curve->restLengths((i + in_Curve->nVertices - 1) % in_Curve->nVertices) :
			in_Curve->restLengths(i);
		double restL = in_Curve->closed ? in_Curve->restLengths(i) :
			in_Curve->restLengths(i+1);

		energy += computeSegmentBendingEnergy(in_Position.col(iim), in_Position.col(ii), in_Position.col(iip),
			in_Params->alpha, restL_m, restL, in_Curve->restAngles(i));
	}

	return energy;
}

double computeEnergy_fit(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	double energy = 0.0;

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_Curve->nVertices;
		double int_df_seg = integrateDF2OverSegment(in_Params, in_Position.col(i), in_Position.col(ip), in_Curve->restLengths(i));
		energy += 0.5 * in_Params->fit * int_df_seg;
	}

	return energy;
}

double computeEnergy(const SParameters* in_Params, const SCurve* in_Curve, const SVar& in_Vars)
{
	const double E_elastic = computeEnergy_elastic(in_Params, in_Curve, in_Vars.pos);
	const double E_bending = computeEnergy_bending(in_Params, in_Curve, in_Vars.pos);
	const double E_fit = computeEnergy_fit(in_Params, in_Curve, in_Vars.pos);
	return E_elastic + E_bending + E_fit;
}

void compute_f_elastic(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, Eigen::VectorXd& io_f, int offset_row)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	assert(io_f.rows() >= nSegs + offset_row);
	assert(in_Curve->nVertices == in_Position.cols());

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_Curve->nVertices;
		const Eigen::Vector2d diff = in_Position.col(ip) - in_Position.col(i);
		io_f(i+offset_row) = sqrt(0.5 * in_Params->YA * in_Curve->restLengths(i)) * (diff.norm() / in_Curve->restLengths(i) - 1);
	}
}

void compute_f_bending(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, Eigen::VectorXd& io_f, int offset_row)
{
	int nAngles = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 2;

	assert(io_f.rows() >= nAngles + offset_row);
	assert(in_Curve->nVertices == in_Position.cols());

	for(int i=0; i<nAngles; i++)
	{
		int iim = in_Curve->closed ?
			(i + in_Curve->nVertices - 1) % in_Curve->nVertices : i;
		int ii = in_Curve->closed ? i : i + 1;
		int iip = in_Curve->closed ? (i + 1) % in_Curve->nVertices : i + 2;

		double restL_m = in_Curve->closed ? in_Curve->restLengths((i + in_Curve->nVertices - 1) % in_Curve->nVertices) :
			in_Curve->restLengths(i);
		double restL = in_Curve->closed ? in_Curve->restLengths(i) :
			in_Curve->restLengths(i+1);

		const Eigen::Vector2d e_im = in_Position.col(ii) - in_Position.col(iim);
		const Eigen::Vector2d e_i = in_Position.col(iip) - in_Position.col(ii);

		/*
		double omega = 2.0 * fabs(e_im(0)*e_i(1)-e_im(1)*e_i(0)) / (e_im.norm()*e_i.norm() + e_im.dot(e_i));
		double omega_bar = 2.0 * fabs(tan(in_Curve->restAngles(i)*0.5));
		//*/
		double omega = computeOmega(e_im, e_i);
		double omega_bar = computeOmegaFromAngle(in_Curve->restAngles(i));
		io_f(i+offset_row) = sqrt(in_Params->alpha/(restL_m + restL)) * (omega - omega_bar);
	}
}

void compute_f_fit(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, Eigen::VectorXd& io_f, int offset_row)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	assert(io_f.rows() >= nSegs + offset_row);
	assert(in_Curve->nVertices == in_Position.cols());

	//printf("int_seg: ");
	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_Curve->nVertices;
		double int_df_seg = integrateDF2OverSegment(in_Params, in_Position.col(i), in_Position.col(ip), in_Curve->restLengths(i));
		//printf("%f, ", int_df_seg);
		io_f(i+offset_row) = sqrt(in_Params->fit * 0.5) * sqrt(int_df_seg);
	}
	//printf("\n");
}

void compute_f(const SParameters* in_Params, const SCurve* in_Curve, const SVar& in_Vars, Eigen::VectorXd& io_f)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;
	int nAngles = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 2;

	int nElems = nSegs + nAngles + nSegs;

	assert(io_f.rows() == nElems);
	assert(in_Curve->nVertices == in_Vars.pos.cols());
	assert(in_Curve->nVertices == in_Vars.pos.cols());

	io_f.setZero();

	compute_f_elastic(in_Params, in_Curve, in_Vars.pos, io_f, 0);
	compute_f_bending(in_Params, in_Curve, in_Vars.pos, io_f, nSegs);
	compute_f_fit(in_Params, in_Curve, in_Vars.pos, io_f, nSegs+nAngles);

	/*
	printf("f: [");
	for(int i=0; i<nElems; i++)
	{
		if(i<nElems-1) printf("%f, ", io_f(i));
		else printf("%f]\n", io_f(i));
	}
	//*/
}

void computeNumericalDerivative_elastic(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, double epsilon, Eigen::MatrixXd& io_Jacobian, int offset_row)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	assert(io_Jacobian.rows() >= nSegs + offset_row);
	assert(io_Jacobian.cols() == in_Curve->nVertices * 2);

	const Eigen::Vector2d dx(epsilon, 0.0);
	const Eigen::Vector2d dy(0.0, epsilon);

	//dJ/dxi, dJ/dyi
	for(int i=0; i<nSegs; i++)
	{
		//l_i is a function of x_i, x_{i+1}, y_i and y_{i+1}
		int ip = (i+1) % in_Curve->nVertices;

		const Eigen::Vector2d xi = in_Position.col(i);
		const Eigen::Vector2d xip = in_Position.col(ip);

		const Eigen::Vector2d diff0 = xip - xi;
		const double fi = sqrt(in_Params->YA * in_Curve->restLengths(i) * 0.5) * (diff0.norm() / in_Curve->restLengths(i) - 1);

		//dJ/dxi_i
		const Eigen::Vector2d xi_dx = xi + dx;
		const Eigen::Vector2d diff1 = xip - xi_dx;
		const double fi_i_dx = sqrt(in_Params->YA * in_Curve->restLengths(i) * 0.5) * (diff1.norm() / in_Curve->restLengths(i) - 1);
		io_Jacobian(i+offset_row, i) = (fi_i_dx - fi) / epsilon;

		//dJ/dyi_i
		const Eigen::Vector2d xi_dy = xi + dy;
		const Eigen::Vector2d diff2 = xip - xi_dy;
		const double fi_i_dy = sqrt(in_Params->YA * in_Curve->restLengths(i) * 0.5) * (diff2.norm() / in_Curve->restLengths(i) - 1);
		io_Jacobian(i+offset_row, i+in_Curve->nVertices) = (fi_i_dy - fi) / epsilon;

		//dJ/dxi_ip
		const Eigen::Vector2d xip_dx = xip + dx;
		const Eigen::Vector2d diff3 = xip_dx - xi;
		const double fi_ip_dx = sqrt(in_Params->YA * in_Curve->restLengths(i) * 0.5) * (diff3.norm() / in_Curve->restLengths(i) - 1);
		io_Jacobian(i+offset_row, ip) = (fi_ip_dx - fi) / epsilon;

		//dJ/dyi_i
		const Eigen::Vector2d xip_dy = xip + dy;
		const Eigen::Vector2d diff4 = xip_dy - xi;
		const double fi_ip_dy = sqrt(in_Params->YA * in_Curve->restLengths(i) * 0.5) * (diff4.norm() / in_Curve->restLengths(i) - 1);
		io_Jacobian(i+offset_row, ip+in_Curve->nVertices) = (fi_ip_dy - fi) / epsilon;
	}
}

void computeNumericalDerivative_bending(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, double epsilon, Eigen::MatrixXd& io_Jacobian, int offset_row)
{
	int nAngles = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 2;

	assert(io_Jacobian.rows() >= nAngles + offset_row);
	assert(io_Jacobian.cols() == in_Curve->nVertices * 2);

	const Eigen::Vector2d dx(epsilon, 0.0);
	const Eigen::Vector2d dy(0.0, epsilon);

	//dB/dxi, dB/dyi
	for(int i=0; i<nAngles; i++)
	{
		int iim = in_Curve->closed ?
			(i + in_Curve->nVertices - 1) % in_Curve->nVertices : i;
		int ii = in_Curve->closed ? i : i + 1;
		int iip = in_Curve->closed ? (i + 1) % in_Curve->nVertices : i + 2;

		double restL_m = in_Curve->restLengths(iim);
		double restL = in_Curve->restLengths(ii);

		const Eigen::Vector2d xim = in_Position.col(iim);
		const Eigen::Vector2d xi = in_Position.col(ii);
		const Eigen::Vector2d xip = in_Position.col(iip);

		const Eigen::Vector2d xim_dx = xim + dx; const Eigen::Vector2d xim_dy = xim + dy;
		const Eigen::Vector2d xi_dx = xi + dx; const Eigen::Vector2d xi_dy = xi + dy;
		const Eigen::Vector2d xip_dx = xip + dx; const Eigen::Vector2d xip_dy = xip + dy;

		const Eigen::Vector2d e_prev = xi - xim;
		const Eigen::Vector2d e_next = xip - xi;

		const Eigen::Vector2d e_prev_im_dx = xi - xim_dx; const Eigen::Vector2d e_prev_im_dy = xi - xim_dy;
		const Eigen::Vector2d e_prev_i_dx = xi_dx - xim; const Eigen::Vector2d e_prev_i_dy = xi_dy - xim;

		const Eigen::Vector2d e_next_i_dx = xip - xi_dx; const Eigen::Vector2d e_next_i_dy = xip - xi_dy;
		const Eigen::Vector2d e_next_ip_dx = xip_dx - xi; const Eigen::Vector2d e_next_ip_dy = xip_dy - xi;

		/*
		const double omega = 2.0 * fabs(e_prev(0)*e_next(1)-e_prev(1)*e_next(0)) / (e_prev.norm()*e_next.norm() + e_prev.dot(e_next));

		const double omega_im_dx = 2.0 * fabs(e_prev_im_dx(0)*e_next(1)-e_prev_im_dx(1)*e_next(0)) / (e_prev_im_dx.norm()*e_next.norm() + e_prev_im_dx.dot(e_next));
		const double omega_im_dy = 2.0 * fabs(e_prev_im_dy(0)*e_next(1)-e_prev_im_dy(1)*e_next(0)) / (e_prev_im_dy.norm()*e_next.norm() + e_prev_im_dy.dot(e_next));
		const double omega_i_dx = 2.0 * fabs(e_prev_i_dx(0)*e_next_i_dx(1)-e_prev_i_dx(1)*e_next_i_dx(0)) / (e_prev_i_dx.norm()*e_next_i_dx.norm() + e_prev_i_dx.dot(e_next_i_dx));
		const double omega_i_dy = 2.0 * fabs(e_prev_i_dy(0)*e_next_i_dy(1)-e_prev_i_dy(1)*e_next_i_dy(0)) / (e_prev_i_dy.norm()*e_next_i_dy.norm() + e_prev_i_dy.dot(e_next_i_dy));
		const double omega_ip_dx = 2.0 * fabs(e_prev(0)*e_next_ip_dx(1)-e_prev(1)*e_next_ip_dx(0)) / (e_prev.norm()*e_next_ip_dx.norm() + e_prev.dot(e_next_ip_dx));
		const double omega_ip_dy = 2.0 * fabs(e_prev(0)*e_next_ip_dy(1)-e_prev(1)*e_next_ip_dy(0)) / (e_prev.norm()*e_next_ip_dy.norm() + e_prev.dot(e_next_ip_dy));
		//*/

		const double omega = computeOmega(e_prev, e_next);

		const double omega_im_dx = computeOmega(e_prev_im_dx, e_next);
		const double omega_im_dy = computeOmega(e_prev_im_dy, e_next);
		const double omega_i_dx = computeOmega(e_prev_i_dx, e_next_i_dx);
		const double omega_i_dy = computeOmega(e_prev_i_dy, e_next_i_dy);
		const double omega_ip_dx = computeOmega(e_prev, e_next_ip_dx);
		const double omega_ip_dy = computeOmega(e_prev, e_next_ip_dy);

		const double fi = sqrt(in_Params->alpha/(restL_m + restL)) * omega; //omega_bar will be cancelled out, so omit it
		const double fi_im_dx = sqrt(in_Params->alpha/(restL_m + restL)) * omega_im_dx;
		const double fi_im_dy = sqrt(in_Params->alpha/(restL_m + restL)) * omega_im_dy;
		const double fi_i_dx = sqrt(in_Params->alpha/(restL_m + restL)) * omega_i_dx;
		const double fi_i_dy = sqrt(in_Params->alpha/(restL_m + restL)) * omega_i_dy;
		const double fi_ip_dx = sqrt(in_Params->alpha/(restL_m + restL)) * omega_ip_dx;
		const double fi_ip_dy = sqrt(in_Params->alpha/(restL_m + restL)) * omega_ip_dy;

		io_Jacobian(i+offset_row, iim) = (fi_im_dx - fi) / epsilon;
		io_Jacobian(i+offset_row, iim+in_Curve->nVertices) = (fi_im_dy - fi) / epsilon;

		io_Jacobian(i+offset_row, ii) = (fi_i_dx - fi) / epsilon;
		io_Jacobian(i+offset_row, ii+in_Curve->nVertices) = (fi_i_dy - fi) / epsilon;

		io_Jacobian(i+offset_row, iip) = (fi_ip_dx - fi) / epsilon;
		io_Jacobian(i+offset_row, iip+in_Curve->nVertices) = (fi_ip_dy - fi) / epsilon;
	}
}

void computeNumericalDerivative_fit(const SParameters* in_Params, const SCurve* in_Curve, const Eigen::Matrix2Xd& in_Position, double epsilon, Eigen::MatrixXd& io_Jacobian, int offset_row)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;

	assert(io_Jacobian.rows() >= nSegs + offset_row);
	assert(io_Jacobian.cols() == in_Curve->nVertices * 2);

	const Eigen::Vector2d dx(epsilon, 0.0);
	const Eigen::Vector2d dy(0.0, epsilon);

	//dfit/dxi, dfit/dyi
	for(int i=0; i<nSegs; i++)
	{
		//l_i is a function of x_i, x_{i+1}, y_i and y_{i+1}
		int ip = (i+1) % in_Curve->nVertices;

		const Eigen::Vector2d xi = in_Position.col(i);
		const Eigen::Vector2d xip = in_Position.col(ip);
		const double restL = in_Curve->restLengths(i);

		const double fi = sqrt(in_Params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_Params, xi, xip, restL));

		//dfit/dxi_i
		const Eigen::Vector2d xi_dx = xi + dx;
		const double fi_i_dx = sqrt(in_Params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_Params, xi_dx, xip, restL));
		io_Jacobian(i+offset_row, i) = (fi_i_dx - fi) / epsilon;

		//dfit/dyi_i
		const Eigen::Vector2d xi_dy = xi + dy;
		const double fi_i_dy = sqrt(in_Params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_Params, xi_dy, xip, restL));
		io_Jacobian(i+offset_row, i+in_Curve->nVertices) = (fi_i_dy - fi) / epsilon;

		//dfit/dxi_ip
		const Eigen::Vector2d xip_dx = xip + dx;
		const double fi_ip_dx = sqrt(in_Params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_Params, xi, xip_dx, restL));
		io_Jacobian(i+offset_row, ip) = (fi_ip_dx - fi) / epsilon;

		//dfit/dyi_i
		const Eigen::Vector2d xip_dy = xip + dy;
		const double fi_ip_dy = sqrt(in_Params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_Params, xi, xip_dy, restL));
		io_Jacobian(i+offset_row, ip+in_Curve->nVertices) = (fi_ip_dy - fi) / epsilon;
	}
}

void computeNumericalDerivative(const SParameters* in_Params, const SCurve* in_Curve, const SVar& in_Vars, double epsilon, Eigen::MatrixXd& io_Jacobian)
{
	int nSegs = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 1;
	int nAngles = in_Curve->closed ? in_Curve->nVertices : in_Curve->nVertices - 2;

	//assert(io_Jacobian.rows() == nSegs + nAngles + nSegs);
	//assert(io_Jacobian.cols() == in_Curve->nVertices * 2);
	//assert(in_Vars.pos.cols() == in_Curve->nVertices);
	//assert(in_Vars.conf.cols() == in_Curve->nVertices);

	io_Jacobian.setZero();

	computeNumericalDerivative_elastic(in_Params, in_Curve, in_Vars.pos, epsilon, io_Jacobian, 0);
	computeNumericalDerivative_bending(in_Params, in_Curve, in_Vars.pos, epsilon, io_Jacobian, nSegs);
	computeNumericalDerivative_fit(in_Params, in_Curve, in_Vars.pos, epsilon, io_Jacobian, nSegs+nAngles);

	/*
	printf("B: [\n");
	for(int j=0; j<io_Jacobian.rows(); j++)
	{
		printf("[");
		for(int i=0; i<io_Jacobian.cols(); i++)
		{
			if(i<io_Jacobian.cols()-1) printf("%f, ", io_Jacobian(j, i));
			else printf("%f],\n", io_Jacobian(j, i));
		}
	}
	printf("]\n");
	//*/
}

void updateRestLength(const Eigen::Matrix2Xd& in_Position, SCurve* io_Curve)
{
	int nSegs = io_Curve->closed ? io_Curve->nVertices : io_Curve->nVertices - 1;
	assert(io_Curve->nVertices == in_Position.cols());

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % io_Curve->nVertices;
		const Eigen::Vector2d diff = in_Position.col(ip) - in_Position.col(i);
		io_Curve->restLengths(i) = diff.norm();
	}
}

void updateCurveSubdivision(const SParameters* in_Params, SVar& io_InitialVars, SVar& io_Vars, SCurve* io_Curve, SSolverVars& io_SolverVars)
{
	int nSegs = io_Curve->closed ? io_Curve->nVertices : io_Curve->nVertices - 1;
	//assert(io_Curve->nVertices == in_Position.cols());

	std::vector<SPoint2D> pos;
	std::vector<double> angles;
	std::vector<int> vids;

	bool subdivided = false;

	for(int i=0; i<nSegs; i++)
	{
		SPoint2D p; p.x[0] = io_Vars.pos.col(i)(0); p.x[1] = io_Vars.pos.col(i)(1);
		pos.push_back(p);
		vids.push_back(io_Curve->vertexIDs(i));

		if(i!=0 || io_Curve->closed) angles.push_back(io_Curve->restAngles(i));

		int ip = (i+1) % io_Curve->nVertices;
		const Eigen::Vector2d diff = io_Vars.pos.col(ip) - io_Vars.pos.col(i);
		if(diff.norm() > in_Params->refLength)
		{
			SPoint2D p;
			p.x[0] = (io_Vars.pos.col(ip)(0) + io_Vars.pos.col(i)(0)) * 0.5;
			p.x[1] = (io_Vars.pos.col(ip)(1) + io_Vars.pos.col(i)(1)) * 0.5;
			pos.push_back(p);
			//angles.push_back((io_Curve->restAngles(i) + io_Curve->restAngles(ip)) * 0.5);
			angles.push_back(PI);
			vids.push_back(-1);
			subdivided = true;
		}
	}

	if(!io_Curve->closed)
	{
		int ilast = io_Curve->nVertices-1;
		SPoint2D p; p.x[0] = io_Vars.pos.col(ilast)(0); p.x[1] = io_Vars.pos.col(ilast)(1);
		pos.push_back(p);
		vids.push_back(io_Curve->vertexIDs(ilast));
	}

	io_Curve->nVertices = pos.size();
	nSegs = io_Curve->closed ? io_Curve->nVertices : io_Curve->nVertices - 1;
	io_Curve->restLengths.resize(nSegs);
	int nAngles = io_Curve->closed ? io_Curve->nVertices : io_Curve->nVertices - 2;
	io_Curve->restAngles.resize(nAngles);
	io_Curve->vertexIDs.resize(io_Curve->nVertices);

	resize(io_Vars, io_Curve->nVertices);
	resize(io_InitialVars, io_Curve->nVertices);

	for(int i=0; i<io_Curve->nVertices; i++)
	{
		io_Vars.pos.col(i)(0) = pos[i].x[0];
		io_Vars.pos.col(i)(1) = pos[i].x[1];
		io_Vars.conf(i) = 0.0;
		io_Curve->vertexIDs(i) = vids[i];
	}

	nSegs = io_Curve->closed ? io_Curve->nVertices : io_Curve->nVertices - 1;
	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % io_Curve->nVertices;
		const Eigen::Vector2d diff = io_Vars.pos.col(ip) - io_Vars.pos.col(i);
		io_Curve->restLengths(i) = diff.norm();
	}

	for(int i=0; i<nAngles; i++)
		io_Curve->restAngles(i) = angles[i];

	io_InitialVars = io_Vars;

	initSolverVars(in_Params, io_Curve, io_InitialVars, io_SolverVars, true);
}

bool secantLMMethodSingleUpdate(const SParameters* in_Params, const SCurve* in_Curve, const SVar& in_InitialVars, SSolverVars& io_SolverVars, SVar& solution)
{
	if(io_SolverVars.found || io_SolverVars.k > in_Params->kmax)
		return true;

	io_SolverVars.k++;
	io_SolverVars.A_muI = io_SolverVars.B.transpose() * io_SolverVars.B + io_SolverVars.mu * io_SolverVars.I;
	io_SolverVars.h = io_SolverVars.A_muI.ldlt().solve(-io_SolverVars.g);

	if(io_SolverVars.h.norm() <= in_Params->epsilon_2 * (io_SolverVars.x.pos.norm() + in_Params->epsilon_2))
		io_SolverVars.found = true;
	else
	{
		for(int q=0; q<io_SolverVars.nV; q++)
		{
			io_SolverVars.xnew.pos(0, q) = io_SolverVars.x.pos(0, q) + io_SolverVars.h(q);
			io_SolverVars.xnew.pos(1, q) = io_SolverVars.x.pos(1, q) + io_SolverVars.h(q + io_SolverVars.nV);
		}
	}

	double Fnew = 0.5 * computeEnergy(in_Params, in_Curve, io_SolverVars.xnew);
	double F = 0.5 * computeEnergy(in_Params, in_Curve, io_SolverVars.x);

	double gain_denom = - io_SolverVars.h.dot(io_SolverVars.B.transpose() * io_SolverVars.f)
		- 0.5 * io_SolverVars.h.dot(io_SolverVars.B.transpose() * io_SolverVars.B * io_SolverVars.h);
	double gain = (F - Fnew) / gain_denom;

	if(gain > 0)
	{
		io_SolverVars.x = io_SolverVars.xnew;
		compute_f(in_Params, in_Curve, io_SolverVars.x, io_SolverVars.f);
		computeNumericalDerivative(in_Params, in_Curve, io_SolverVars.x, io_SolverVars.epsilon, io_SolverVars.B);
		io_SolverVars.g = io_SolverVars.B.transpose() * io_SolverVars.f;
		io_SolverVars.found = (io_SolverVars.g.lpNorm<Eigen::Infinity>() <= in_Params->epsilon_1);
		printf("k: %d, gain: %f, |g|_inf: %f\n", io_SolverVars.k, gain, io_SolverVars.g.lpNorm<Eigen::Infinity>());
		io_SolverVars.mu = io_SolverVars.mu * std::max(1.0/3.0, 1.0 - (2.0 * gain - 1.0) * (2.0 * gain - 1.0) * (2.0 * gain - 1.0));
		io_SolverVars.nu = 2.0;
	}
	else
	{
		io_SolverVars.mu = io_SolverVars.mu * io_SolverVars.nu;
		io_SolverVars.nu = io_SolverVars.nu * 2.0;
	}

	if(io_SolverVars.found)
		printf("found in %d steps\n", io_SolverVars.k);

	solution = io_SolverVars.x;

	return io_SolverVars.found || io_SolverVars.k > in_Params->kmax;
}

void showFeaturePoints(const SCurve* in_Curve, const SVar& solution)
{
	printf("Feature points:\n");
	for(int i=0; i<in_Curve->nVertices; i++)
	{
		if(in_Curve->vertexIDs(i) >= 0)
		{
			printf("%d: %f, %f\n", in_Curve->vertexIDs(i), solution.pos.col(i).x(), solution.pos.col(i).y());
		}
	}
}

void secantLMMethod(const SParameters* in_Params, SCurve* in_Curve, SVar& in_InitialVars, SSolverVars& io_SolverVars, SVar& solution)
{
	initSolverVars(in_Params, in_Curve, in_InitialVars, io_SolverVars);

	while(1)
	{
		if(secantLMMethodSingleUpdate(in_Params, in_Curve, in_InitialVars, io_SolverVars, solution))
			break;
		updateCurveSubdivision(in_Params, in_InitialVars, solution, in_Curve, io_SolverVars);
	}

	printf("found in %d steps\n", io_SolverVars.k);
	solution = io_SolverVars.x;
	showFeaturePoints(in_Curve, solution);
}


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

#include <iostream>
#include <ctime>
#include <cassert>
#include <vector>
#include <algorithm>
#include <math.h>

double condRankP(unsigned int r, unsigned int c, double p, unsigned int q);
double nCkF(unsigned int n, unsigned int k);
double rhoF(unsigned int l, unsigned int e, unsigned int q, double p);
double piF(unsigned int l, unsigned int e, unsigned int q, double p);


/* boost random/linear_congruential.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: linear_congruential.hpp 12208 2002-01-03 22:21:34Z jmaurer $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_LINEAR_CONGRUENTIAL_HPP
#define BOOST_RANDOM_LINEAR_CONGRUENTIAL_HPP

#include <iostream>
#include <cassert>
#include <boost/config.hpp>
#include <boost/random/detail/const_mod.hpp>

namespace boost {
namespace random {

// compile-time configurable linear congruential generator
template<class IntType, IntType a, IntType c, IntType m, IntType val>
class linear_congruential
{
public:
  typedef IntType result_type;
#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
  static const bool has_fixed_range = true;
  static const result_type min_value = ( c == 0 ? 1 : 0 );
  static const result_type max_value = m-1;
#else
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
#endif
  BOOST_STATIC_CONSTANT(IntType, multiplier = a);
  BOOST_STATIC_CONSTANT(IntType, increment = c);
  BOOST_STATIC_CONSTANT(IntType, modulus = m);

  result_type min() const { return c == 0 ? 1 : 0; }
  result_type max() const { return m-1; }
  explicit linear_congruential(IntType x0 = 1)
    : _x(x0)
  {
    assert(c || x0); /* if c == 0 and x(0) == 0 then x(n) = 0 for all n */
    // overflow check
    // disabled because it gives spurious "divide by zero" gcc warnings
    // assert(m == 0 || (a*(m-1)+c) % m == (c < a ? c-a+m : c-a));
  }
  // compiler-generated copy constructor and assignment operator are fine
  void seed(IntType x0) { assert(c || x0); _x = x0; }
  IntType operator()()
  {
    _x = const_mod<IntType, m>::mult_add(a, _x, c);
    return _x;
  }
  bool validation(IntType x) const { return val == x; }

#ifndef  BOOST_NO_OPERATORS_IN_NAMESPACE
  friend std::ostream& operator<<(std::ostream& os,
                                  const linear_congruential& lcg)
  { os << lcg._x; return os; }
  friend std::istream& operator>>(std::istream& is, linear_congruential& lcg)
  { is >> lcg._x; return is; }
  friend bool operator==(const linear_congruential& x,
                         const linear_congruential& y)
  { return x._x == y._x; }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const linear_congruential& rhs) const
  { return _x == rhs._x; }
#endif
private:
  IntType _x;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class IntType, IntType a, IntType c, IntType m, IntType val>
const bool linear_congruential<IntType, a, c, m, val>::has_fixed_range;
template<class IntType, IntType a, IntType c, IntType m, IntType val>
const typename linear_congruential<IntType, a, c, m, val>::result_type linear_congruential<IntType, a, c, m, val>::min_value;
template<class IntType, IntType a, IntType c, IntType m, IntType val>
const typename linear_congruential<IntType, a, c, m, val>::result_type linear_congruential<IntType, a, c, m, val>::max_value;
#endif

} // namespace random

// validation values from the publications
typedef random::linear_congruential<int32_t, 16807, 0, 2147483647,
  1043618065> minstd_rand0;
typedef random::linear_congruential<int32_t, 48271, 0, 2147483647,
  399268537> minstd_rand;

#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)
// emulate the lrand48() C library function; requires support for uint64_t
class rand48
{
public:
  typedef int32_t result_type;
#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
  static const bool has_fixed_range = true;
  static const int32_t min_value = 0;
  static const int32_t max_value = integer_traits<int32_t>::const_max;
#else
  enum { has_fixed_range = false };
#endif
  int32_t min() const { return 0; }
  int32_t max() const { return std::numeric_limits<int32_t>::max(); }

  explicit rand48(int32_t x0 = 1) : lcf(cnv(x0)) { }
  explicit rand48(uint64_t x0) : lcf(x0) { }
  // compiler-generated copy ctor and assignment operator are fine
  void seed(int32_t x0) { lcf.seed(cnv(x0)); }
  void seed(uint64_t x0) { lcf.seed(x0); }
  int32_t operator()() { return lcf() >> 17; }
  // by experiment from lrand48()
  bool validation(int32_t x) const { return x == 1993516219; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend std::ostream& operator<<(std::ostream& os, const rand48& r)
  { os << r.lcf; return os; }
  friend std::istream& operator>>(std::istream& is, rand48& r)
  { is >> r.lcf; return is; }
  friend bool operator==(const rand48& x, const rand48& y)
  { return x.lcf == y.lcf; }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const rand48& rhs) const
  { return lcf == rhs.lcf; }
#endif
private:
  random::linear_congruential<uint64_t,
    uint64_t(0xDEECE66DUL) | (uint64_t(0x5) << 32), // xxxxULL is not portable
    0xB, uint64_t(1)<<48, /* unknown */ 0> lcf;
  static uint64_t cnv(int32_t x)
  { return (static_cast<uint64_t>(x) << 16) | 0x330e;  }
};
#endif /* !BOOST_NO_INT64_T && !BOOST_NO_INTEGRAL_INT64_T */

} // namespace boost

#endif // BOOST_RANDOM_LINEAR_CONGRUENTIAL_HPP


#define BOOST_TEST_MODULE geometry test
#include <boost/test/included/unit_test.hpp>

#include <cmath>

#include <pastel/geometry/vector.hpp>
#include <pastel/geometry/vector_io.hpp>
#include <pastel/geometry/point.hpp>
#include <pastel/geometry/point_io.hpp>
#include <pastel/geometry/get.hpp>
#include <pastel/geometry/dot_product.hpp>
#include <pastel/geometry/cross_product.hpp>
#include <pastel/geometry/squared_norm.hpp>
#include <pastel/geometry/norm.hpp>
#include <pastel/geometry/meta/dimension_of.hpp>

BOOST_AUTO_TEST_CASE(geometry_1dim_test, * boost::unit_test::tolerance(0.000001))
{
  using vector_type = pastel::geometry::vector<1u, double>;
  using point_type = pastel::geometry::point<1u, double>;
  BOOST_TEST(pastel::geometry::meta::dimension_of<vector_type>::value == 1u);
  BOOST_TEST(pastel::geometry::meta::dimension_of<point_type>::value == 1u);

  auto const vec1 = vector_type{1.0};
  auto const vec2 = vector_type{2.0};
  BOOST_TEST(pastel::geometry::get<0>(vec1) == 1.0);
  BOOST_TEST(vec1 + vec1 == vec2);
  BOOST_TEST(vec1 - vec2 == -vec1);
  BOOST_TEST(2.0 * vec1 == vec2);
  BOOST_TEST(vec1 * 2.0 == vec2);
  BOOST_TEST(vec2 / 2.0 == vec1);
  BOOST_TEST(pastel::geometry::dot_product(vec1, vec2) == 2.0);
  BOOST_TEST(pastel::geometry::squared_norm(vec1) == 1.0);
  using std::sqrt;
  BOOST_TEST(pastel::geometry::norm(vec1) == sqrt(1.0));
  BOOST_TEST(pastel::geometry::cross_product(vec1, vec2) == 0.0);

  auto const pnt1 = point_type{1.0};
  auto const pnt2 = point_type{3.0};
  BOOST_TEST(pastel::geometry::get<0>(pnt1) == 1.0);
  BOOST_TEST(pnt1 + vec2 == pnt2);
  BOOST_TEST(vec2 + pnt1 == pnt2);
  BOOST_TEST(pnt2 - vec2 == pnt1);
  BOOST_TEST(pnt2 - pnt1 == vec2);

  auto vec3 = vector_type{0.0};
  vec3.fill(1.0);
  BOOST_TEST((vec3 == vector_type{1.0}));
  auto vec4 = vec2;
  using std::swap;
  swap(vec3, vec4);
  BOOST_TEST(vec3 == vec2);

  for (auto& component: vec3)
    component = 1.0;
  BOOST_TEST((vec3 == vector_type{1.0}));
  for (auto iter = vec3.rbegin(), last = vec3.rend(); iter != last; ++iter)
    *iter = 2.0;
  BOOST_TEST((vec3 == vector_type{2.0}));
  BOOST_TEST(vec1.size() == pastel::geometry::meta::dimension_of<vector_type>::value);
  BOOST_TEST(vec1.max_size() == pastel::geometry::meta::dimension_of<vector_type>::value);
  BOOST_TEST(!vec1.empty());
  BOOST_TEST(vec1.at(0u) == vec1.front());
  BOOST_TEST(vec1.at(vec1.size()-1u) == vec1.back());

  auto pnt3 = point_type{0.0};
  pnt3.fill(1.0);
  BOOST_TEST((pnt3 == point_type{1.0}));
  auto pnt4 = pnt2;
  using std::swap;
  swap(pnt3, pnt4);
  BOOST_TEST(pnt3 == pnt2);

  for (auto& component: pnt3)
    component = 1.0;
  BOOST_TEST((pnt3 == point_type{1.0}));
  for (auto iter = pnt3.rbegin(), last = pnt3.rend(); iter != last; ++iter)
    *iter = 2.0;
  BOOST_TEST((pnt3 == point_type{2.0}));
  BOOST_TEST(pnt1.size() == pastel::geometry::meta::dimension_of<point_type>::value);
  BOOST_TEST(pnt1.max_size() == pastel::geometry::meta::dimension_of<point_type>::value);
  BOOST_TEST(!pnt1.empty());
  BOOST_TEST(pnt1.at(0u) == pnt1.front());
  BOOST_TEST(pnt1.at(pnt1.size()-1u) == pnt1.back());
}

BOOST_AUTO_TEST_CASE(geometry_2dim_test, * boost::unit_test::tolerance(0.000001))
{
  using vector_type = pastel::geometry::vector<2u, double>;
  using point_type = pastel::geometry::point<2u, double>;
  BOOST_TEST(pastel::geometry::meta::dimension_of<vector_type>::value == 2u);
  BOOST_TEST(pastel::geometry::meta::dimension_of<point_type>::value == 2u);

  auto const vec1 = vector_type{1.0, 2.0};
  auto const vec2 = vector_type{2.0, 4.0};
  BOOST_TEST(pastel::geometry::get<0>(vec1) == 1.0);
  BOOST_TEST(vec1 + vec1 == vec2);
  BOOST_TEST(vec1 - vec2 == -vec1);
  BOOST_TEST(2.0 * vec1 == vec2);
  BOOST_TEST(vec1 * 2.0 == vec2);
  BOOST_TEST(vec2 / 2.0 == vec1);
  BOOST_TEST(pastel::geometry::dot_product(vec1, vec2) == 10.0);
  BOOST_TEST(pastel::geometry::squared_norm(vec1) == 5.0);
  using std::sqrt;
  BOOST_TEST(pastel::geometry::norm(vec1) == sqrt(5.0));
  BOOST_TEST(pastel::geometry::cross_product(vec1, vec2) == 0.0);

  auto const pnt1 = point_type{1.0, 2.0};
  auto const pnt2 = point_type{3.0, 6.0};
  BOOST_TEST(pastel::geometry::get<0>(pnt1) == 1.0);
  BOOST_TEST(pnt1 + vec2 == pnt2);
  BOOST_TEST(vec2 + pnt1 == pnt2);
  BOOST_TEST(pnt2 - vec2 == pnt1);
  BOOST_TEST(pnt2 - pnt1 == vec2);

  auto vec3 = vector_type{0.0, 0.0};
  vec3.fill(1.0);
  BOOST_TEST((vec3 == vector_type{1.0, 1.0}));
  auto vec4 = vec2;
  using std::swap;
  swap(vec3, vec4);
  BOOST_TEST(vec3 == vec2);

  for (auto& component: vec3)
    component = 1.0;
  BOOST_TEST((vec3 == vector_type{1.0, 1.0}));
  for (auto iter = vec3.rbegin(), last = vec3.rend(); iter != last; ++iter)
    *iter = 2.0;
  BOOST_TEST((vec3 == vector_type{2.0, 2.0}));
  BOOST_TEST(vec1.size() == pastel::geometry::meta::dimension_of<vector_type>::value);
  BOOST_TEST(vec1.max_size() == pastel::geometry::meta::dimension_of<vector_type>::value);
  BOOST_TEST(!vec1.empty());
  BOOST_TEST(vec1.at(vec1.size()-1u) == vec1.back());

  auto pnt3 = point_type{0.0, 0.0};
  pnt3.fill(1.0);
  BOOST_TEST((pnt3 == point_type{1.0, 1.0}));
  auto pnt4 = pnt2;
  using std::swap;
  swap(pnt3, pnt4);
  BOOST_TEST(pnt3 == pnt2);

  for (auto& component: pnt3)
    component = 1.0;
  BOOST_TEST((pnt3 == point_type{1.0, 1.0}));
  for (auto iter = pnt3.rbegin(), last = pnt3.rend(); iter != last; ++iter)
    *iter = 2.0;
  BOOST_TEST((pnt3 == point_type{2.0, 2.0}));
  BOOST_TEST(pnt1.size() == pastel::geometry::meta::dimension_of<point_type>::value);
  BOOST_TEST(pnt1.max_size() == pastel::geometry::meta::dimension_of<point_type>::value);
  BOOST_TEST(!pnt1.empty());
  BOOST_TEST(pnt1.at(0u) == pnt1.front());
  BOOST_TEST(pnt1.at(pnt1.size()-1u) == pnt1.back());
}

BOOST_AUTO_TEST_CASE(geometry_3dim_test, * boost::unit_test::tolerance(0.000001))
{
  using vector_type = pastel::geometry::vector<3u, double>;
  using point_type = pastel::geometry::point<3u, double>;
  BOOST_TEST(pastel::geometry::meta::dimension_of<vector_type>::value == 3u);
  BOOST_TEST(pastel::geometry::meta::dimension_of<point_type>::value == 3u);

  auto const vec1 = vector_type{0.0, 1.0, 2.0};
  auto const vec2 = vector_type{0.0, 2.0, 4.0};
  BOOST_TEST(pastel::geometry::get<1>(vec1) == 1.0);
  BOOST_TEST(vec1 + vec1 == vec2);
  BOOST_TEST(vec1 - vec2 == -vec1);
  BOOST_TEST(2.0 * vec1 == vec2);
  BOOST_TEST(vec1 * 2.0 == vec2);
  BOOST_TEST(vec2 / 2.0 == vec1);
  BOOST_TEST(pastel::geometry::dot_product(vec1, vec2) == 10.0);
  BOOST_TEST(pastel::geometry::squared_norm(vec1) == 5.0);
  using std::sqrt;
  BOOST_TEST(pastel::geometry::norm(vec1) == sqrt(5.0));
  auto const vec0 = vector_type{0.0, 0.0, 0.0};
  BOOST_TEST(pastel::geometry::cross_product(vec1, vec2) == vec0);

  auto const pnt1 = point_type{0.0, 1.0, 2.0};
  auto const pnt2 = point_type{0.0, 3.0, 6.0};
  BOOST_TEST(pastel::geometry::get<1>(pnt1) == 1.0);
  BOOST_TEST(pnt1 + vec2 == pnt2);
  BOOST_TEST(vec2 + pnt1 == pnt2);
  BOOST_TEST(pnt2 - vec2 == pnt1);
  BOOST_TEST(pnt2 - pnt1 == vec2);

  auto vec3 = vector_type{0.0, 0.0, 0.0};
  vec3.fill(1.0);
  BOOST_TEST((vec3 == vector_type{1.0, 1.0, 1.0}));
  auto vec4 = vec2;
  using std::swap;
  swap(vec3, vec4);
  BOOST_TEST(vec3 == vec2);

  for (auto& component: vec3)
    component = 1.0;
  BOOST_TEST((vec3 == vector_type{1.0, 1.0, 1.0}));
  for (auto iter = vec3.rbegin(), last = vec3.rend(); iter != last; ++iter)
    *iter = 2.0;
  BOOST_TEST((vec3 == vector_type{2.0, 2.0, 2.0}));
  BOOST_TEST(vec1.size() == pastel::geometry::meta::dimension_of<vector_type>::value);
  BOOST_TEST(vec1.max_size() == pastel::geometry::meta::dimension_of<vector_type>::value);
  BOOST_TEST(!vec1.empty());
  BOOST_TEST(vec1.at(vec1.size()-1u) == vec1.back());

  auto pnt3 = point_type{0.0, 0.0, 0.0};
  pnt3.fill(1.0);
  BOOST_TEST((pnt3 == point_type{1.0, 1.0, 1.0}));
  auto pnt4 = pnt2;
  using std::swap;
  swap(pnt3, pnt4);
  BOOST_TEST(pnt3 == pnt2);

  for (auto& component: pnt3)
    component = 1.0;
  BOOST_TEST((pnt3 == point_type{1.0, 1.0, 1.0}));
  for (auto iter = pnt3.rbegin(), last = pnt3.rend(); iter != last; ++iter)
    *iter = 2.0;
  BOOST_TEST((pnt3 == point_type{2.0, 2.0, 2.0}));
  BOOST_TEST(pnt1.size() == pastel::geometry::meta::dimension_of<point_type>::value);
  BOOST_TEST(pnt1.max_size() == pastel::geometry::meta::dimension_of<point_type>::value);
  BOOST_TEST(!pnt1.empty());
  BOOST_TEST(pnt1.at(0u) == pnt1.front());
  BOOST_TEST(pnt1.at(pnt1.size()-1u) == pnt1.back());
}


#include <iostream>
#include <rmagine/math/math.h>
#include <rmagine/math/types.h>
#include <rmagine/util/StopWatch.hpp>

#include <Eigen/Dense>

using namespace rmagine;

void print(Matrix3x3 M)
{
    for(size_t i=0; i<3; i++)
    {
        for(size_t j=0; j<3; j++)
        {
            std::cout << M(i,j) << " ";
        }
        std::cout << std::endl;
    }
}

void print(Matrix4x4 M)
{
    for(size_t i=0; i<4; i++)
    {
        for(size_t j=0; j<4; j++)
        {
            std::cout << M(i,j) << " ";
        }
        std::cout << std::endl;
    }
}

void print(Eigen::Matrix4f M)
{
    for(size_t i=0; i<4; i++)
    {
        for(size_t j=0; j<4; j++)
        {
            std::cout << M(i,j) << " ";
        }
        std::cout << std::endl;
    }
}

void print(Vector v)
{
    std::cout << v.x << " " << v.y << " " << v.z << std::endl;
}

void print(Quaternion q)
{
    std::cout << q.x << " " << q.y << " " << q.z << " " << q.w << std::endl;
}

void print(Transform T)
{
    print(T.R);
    print(T.t);
}

bool rotationConversionTest()
{
    std::cout << std::endl;
    std::cout << "--------------------------------" << std::endl;
    std::cout << "---- rotationConversionTest ----" << std::endl;
    std::cout << "--------------------------------" << std::endl;
    std::cout << std::endl;

    EulerAngles e0;
    e0.roll = -0.1;
    e0.pitch = 0.1;
    e0.yaw = M_PI / 2.0;

    EulerAngles e;
    Quaternion q;
    Matrix3x3 R;

    Vector x1{1.0, 0.0, 0.0};
    Vector x2{0.0, 1.0, 0.0};
    Vector x3{0.0, 0.0, 1.0};

    std::cout << "Euler -> Quat" << std::endl;
    q = e0;
    print(q * x1);
    print(q * x2);
    print(q * x3);
    std::cout << std::endl;

    std::cout << "Quat -> Euler" << std::endl;
    e = q;
    std::cout << e.roll << " " << e.pitch << " " << e.yaw << std::endl;
    std::cout << std::endl;

    if(    fabs(e.roll - e0.roll) > 0.0001
        || fabs(e.pitch - e0.pitch) > 0.0001
        || fabs(e.yaw - e0.yaw) > 0.0001)
    {
        std::cout << "Euler -> Quat -> Euler error." << std::endl;
        return false;
    }

    std::cout << "Euler -> Matrix" << std::endl;
    R = e0;
    print(R * x1);
    print(R * x2);
    print(R * x3);
    std::cout << std::endl;

    std::cout << "Matrix -> Euler" << std::endl;
    e = R;
    std::cout << e.roll << " " << e.pitch << " " << e.yaw << std::endl;
    std::cout << std::endl;

    std::cout << "Quat -> Matrix" << std::endl;
    R = q;
    print(R * x1);
    print(R * x2);
    print(R * x3);
    std::cout << std::endl;

    std::cout << "Matrix -> Quat" << std::endl;
    q = R;
    print(q * x1);
    print(q * x2);
    print(q * x3);
    std::cout << std::endl;

    return true;
}

Eigen::Vector3f& eigenView(Vector3& v)
{
    return *reinterpret_cast<Eigen::Vector3f*>( &v );
}

Eigen::Matrix3f& eigenView(Matrix3x3& M)
{
    return *reinterpret_cast<Eigen::Matrix3f*>( &M );
}

Eigen::Matrix4f& eigenView(Matrix4x4& M)
{
    return *reinterpret_cast<Eigen::Matrix4f*>( &M );
}

bool checkMatrix3x3()
{
    std::cout << "---------- checkMatrix3x3" << std::endl;
    EulerAngles e{-0.1, 0.1, M_PI / 2.0};

    Matrix3x3 M;
    M = e;
    M(0,1) = 10.0;

    // shallow copy.
    Eigen::Matrix3f& Meig_shallow = eigenView(M);
    std::cout << Meig_shallow << std::endl;

    // deep copy
    Eigen::Matrix3f Meig(&M(0,0));

    // Eigen::Matrix3f Meig_inv = Meig.inverse();
    Matrix3x3 M_inv = ~M;
    Eigen::Matrix3f Meig_inv = Meig.inverse();

    // std::cout << Meig_inv << std::endl;
    // print(M_inv);
    Matrix3x3 I = M_inv * M;
    Eigen::Matrix3f Ieig = Meig_inv * Meig;

    std::cout << "M = " << std::endl;
    print(M);

    std::cout << "M_inv = " << std::endl;
    print(M_inv);

    std::cout << "M_inv * M = " << std::endl;
    print(I);

    std::cout << "Meig = " << std::endl;
    std::cout << Meig << std::endl;

    std::cout << "Meig_inv = " << std::endl;
    std::cout << Meig_inv << std::endl;

    std::cout << "Meig_inv * Meig =" << std::endl;
    std::cout << Meig_inv * Meig <<  std::endl;

    std::cout << "Eigen::Matrix3f stats: " << std::endl;
    std::cout << "- det: " << Meig.determinant() << std::endl;
    std::cout << "- trace: " << Meig.trace() << std::endl;

    std::cout << "Matrix3x3 stats:" << std::endl;
    std::cout << "- det: " << M.det() << std::endl;
    std::cout << "- trace: " << M.trace() << std::endl;

    return true;
}

bool checkMatrix4x4()
{
    std::cout << "------- checkMatrix4x4" << std::endl;
    EulerAngles e{-0.1, 0.1, M_PI / 2.0};

    Matrix4x4 M;
    M.setIdentity();
    M.setRotation(e);

    Eigen::Matrix4f& Meig_shallow = eigenView(M);

    // M(0,1) = 10.0;

    Vector trans{0.0, 0.0, 1.0};
    M.setTranslation(trans);

    std::cout << Meig_shallow << std::endl;

    Matrix4x4 M_inv = M.inv();
    Matrix4x4 I = M_inv * M;

    std::cout << "M = " << std::endl;
    print(M);
    std::cout << "M_inv = " << std::endl;
    print(M_inv);
    std::cout << "(invRigid)=" << std::endl;
    print(M.invRigid());

    std::cout << "M_inv * M = " << std::endl;
    print(I);

    Eigen::Matrix4f Meig(&M(0,0));
    Eigen::Matrix4f Meig_inv = Meig.inverse();

    std::cout << "Meig = " << std::endl;
    std::cout << Meig << std::endl;

    std::cout << "Meig_inv = " << std::endl;
    std::cout << Meig_inv << std::endl;

    std::cout << "Meig_inv * Meig =" << std::endl;
    std::cout << Meig_inv * Meig <<  std::endl;

    std::cout << "Eigen::Matrix4f stats: " << std::endl;
    std::cout << "- det: " << Meig.determinant() << std::endl;
    std::cout << "- trace: " << Meig.trace() << std::endl;

    std::cout << "Matrix4x4 stats:" << std::endl;
    std::cout << "- det: " << M.det() << std::endl;
    std::cout << "- trace: " << M.trace() << std::endl;

    return true;
}

int main(int argc, char** argv)
{
    std::cout << "Rmagine Test: Basic Math" << std::endl;
    // rotationConversionTest();

    // checkMatrix3x3();
    // checkMatrix4x4();

    Vector ab{1.0, 2.0, 3.0};
    Vector ac{4.0, 5.0, 6.0};
    Vector n{7.0, 8.0, 9.0};

    Eigen::Matrix3f R;
    R.col(0) = Eigen::Vector3f(ab.x, ab.y, ab.z);
    R.col(1) = Eigen::Vector3f(ac.x, ac.y, ac.z);
    R.col(2) = Eigen::Vector3f(n.x, n.y, n.z);

    std::cout << R << std::endl;

    R(0,0) = ab.x;
    R(1,0) = ab.y;
    R(2,0) = ab.z;

    R(0,1) = ac.x;
    R(1,1) = ac.y;
    R(2,1) = ac.z;

    R(0,2) = n.x;
    R(1,2) = n.y;
    R(2,2) = n.z;

    std::cout << R << std::endl;

    return 0;
}


/* Boost numeric test of the rosenbrock4 stepper test file

 Copyright 2012 Karsten Ahnert
 Copyright 2012 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
*/

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE numeric_rosenbrock

#include <iostream>
#include <cmath>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

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

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef double value_type;
typedef boost::numeric::ublas::vector< value_type > state_type;
typedef boost::numeric::ublas::matrix< value_type > matrix_type;

// harmonic oscillator, analytic solution x[0] = sin( t )
struct sys
{
    void operator()( const state_type &x , state_type &dxdt , const value_type &t ) const
    {
        dxdt( 0 ) = x( 1 );
        dxdt( 1 ) = -x( 0 );
    }
};

struct jacobi
{
    void operator()( const state_type &x , matrix_type &jacobi , const value_type &t , state_type &dfdt ) const
    {
        jacobi( 0 , 0 ) = 0;
        jacobi( 0 , 1 ) = 1;
        jacobi( 1 , 0 ) = -1;
        jacobi( 1 , 1 ) = 0;
        dfdt( 0 ) = 0.0;
        dfdt( 1 ) = 0.0;
    }
};

BOOST_AUTO_TEST_SUITE( numeric_rosenbrock4 )

BOOST_AUTO_TEST_CASE( rosenbrock4_numeric_test )
{
    typedef rosenbrock4< value_type > stepper_type;
    stepper_type stepper;

    const int o = stepper.order()+1;

    state_type x0( 2 ) , x1( 2 );
    x0(0) = 0.0; x0(1) = 1.0;

    double dt = 0.5;

    stepper.do_step( std::make_pair( sys() , jacobi() ) , x0 , 0.0 , x1 , dt );
    const double f = 2.0 * std::abs( sin(dt) - x1(0) ) / std::pow( dt , o );

    std::cout << o << " , " << f << std::endl;

    while( f*std::pow( dt , o ) > 1E-16 )
    {
        stepper.do_step( std::make_pair( sys() , jacobi() ) , x0 , 0.0 , x1 , dt );
        std::cout << "Testing dt=" << dt << std::endl;
        BOOST_CHECK_SMALL( std::abs( sin(dt) - x1(0) ) , f*std::pow( dt , o ) );
        dt *= 0.5;
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef GEO_UTILS_HPP
#define GEO_UTILS_HPP

#include "quickhull.hpp"
#include "sdlp.hpp"

#include <Eigen/Eigen>

#include <cfloat>
#include <cstdint>
#include <set>
#include <chrono>

namespace geo_utils
{

    // Each row of hPoly is defined by h0, h1, h2, h3 as
    // h0*x + h1*y + h2*z + h3 <= 0
    inline bool findInterior(const Eigen::MatrixX4d &hPoly,
                             Eigen::Vector3d &interior)
    {
        const int m = hPoly.rows();

        Eigen::MatrixX4d A(m, 4);
        Eigen::VectorXd b(m);
        Eigen::Vector4d c, x;
        const Eigen::ArrayXd hNorm = hPoly.leftCols<3>().rowwise().norm();
        A.leftCols<3>() = hPoly.leftCols<3>().array().colwise() / hNorm;
        A.rightCols<1>().setConstant(1.0);
        b = -hPoly.rightCols<1>().array() / hNorm;
        c.setZero();
        c(3) = -1.0;

        const double minmaxsd = sdlp::linprog<4>(c, A, b, x);
        interior = x.head<3>();

        return minmaxsd < 0.0 && !std::isinf(minmaxsd);
    }

    inline bool overlap(const Eigen::MatrixX4d &hPoly0,
                        const Eigen::MatrixX4d &hPoly1,
                        const double eps = 1.0e-6)

    {
        const int m = hPoly0.rows();
        const int n = hPoly1.rows();
        Eigen::MatrixX4d A(m + n, 4);
        Eigen::Vector4d c, x;
        Eigen::VectorXd b(m + n);
        A.leftCols<3>().topRows(m) = hPoly0.leftCols<3>();
        A.leftCols<3>().bottomRows(n) = hPoly1.leftCols<3>();
        A.rightCols<1>().setConstant(1.0);
        b.topRows(m) = -hPoly0.rightCols<1>();
        b.bottomRows(n) = -hPoly1.rightCols<1>();
        c.setZero();
        c(3) = -1.0;

        const double minmaxsd = sdlp::linprog<4>(c, A, b, x);

        return minmaxsd < -eps && !std::isinf(minmaxsd);
    }

    struct filterLess
    {
        inline bool operator()(const Eigen::Vector3d &l,
                               const Eigen::Vector3d &r)
        {
            return l(0) < r(0) ||
                   (l(0) == r(0) &&
                    (l(1) < r(1) ||
                     (l(1) == r(1) &&
                      l(2) < r(2))));
        }
    };

    inline void filterVs(const Eigen::Matrix3Xd &rV,
                         const double &epsilon,
                         Eigen::Matrix3Xd &fV)
    {
        const double mag = std::max(fabs(rV.maxCoeff()), fabs(rV.minCoeff()));
        const double res = mag * std::max(fabs(epsilon) / mag, DBL_EPSILON);
        std::set<Eigen::Vector3d, filterLess> filter;
        fV = rV;
        int offset = 0;
        Eigen::Vector3d quanti;
        for (int i = 0; i < rV.cols(); i++)
        {
            quanti = (rV.col(i) / res).array().round();
            if (filter.find(quanti) == filter.end())
            {
                filter.insert(quanti);
                fV.col(offset) = rV.col(i);
                offset++;
            }
        }
        fV = fV.leftCols(offset).eval();
        return;
    }

    // Each row of hPoly is defined by h0, h1, h2, h3 as
    // h0*x + h1*y + h2*z + h3 <= 0
    // proposed epsilon is 1.0e-6
    inline void enumerateVs(const Eigen::MatrixX4d &hPoly,
                            const Eigen::Vector3d &inner,
                            Eigen::Matrix3Xd &vPoly,
                            const double epsilon = 1.0e-6)
    {
        const Eigen::VectorXd b = -hPoly.rightCols<1>() - hPoly.leftCols<3>() * inner;
        const Eigen::Matrix<double, 3, -1, Eigen::ColMajor> A =
            (hPoly.leftCols<3>().array().colwise() / b.array()).transpose();

        quickhull::QuickHull<double> qh;
        const double qhullEps = std::min(epsilon, quickhull::defaultEps<double>());
        // CCW is false because the normal in quickhull towards interior
        const auto cvxHull = qh.getConvexHull(A.data(), A.cols(), false, true, qhullEps);
        const auto &idBuffer = cvxHull.getIndexBuffer();
        const int hNum = idBuffer.size() / 3;
        Eigen::Matrix3Xd rV(3, hNum);
        Eigen::Vector3d normal, point, edge0, edge1;
        for (int i = 0; i < hNum; i++)
        {
            point = A.col(idBuffer[3 * i + 1]);
            edge0 = point - A.col(idBuffer[3 * i]);
            edge1 = A.col(idBuffer[3 * i + 2]) - point;
            normal = edge0.cross(edge1); //cross in CW gives an outter normal
            rV.col(i) = normal / normal.dot(point);
        }
        filterVs(rV, epsilon, vPoly);
        vPoly = (vPoly.array().colwise() + inner.array()).eval();
        return;
    }

    // Each row of hPoly is defined by h0, h1, h2, h3 as
    // h0*x + h1*y + h2*z + h3 <= 0
    // proposed epsilon is 1.0e-6
    inline bool enumerateVs(const Eigen::MatrixX4d &hPoly,
                            Eigen::Matrix3Xd &vPoly,
                            const double epsilon = 1.0e-6)
    {
        Eigen::Vector3d inner;
        if (findInterior(hPoly, inner))
        {
            enumerateVs(hPoly, inner, vPoly, epsilon);
            return true;
        }
        else
        {
            return false;
        }
    }

} // namespace geo_utils

#endif


/* Copyright (c) 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <ensemble_management.hh>

#include <deal.II/base/mpi.h>

#include <execution>
#include <numeric>

#define BOOST_TEST_MODULE EnsembleManagement

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

#include "main.cc"

BOOST_AUTO_TEST_CASE(fill_and_sync_random_vector)
{
  // Fairly loose tolerance because this is a statistical check
  double tolerance = 10.0;

  // Create the random vector
  double mean = -1.2;
  double stddev = 0.25;
  unsigned int ensemble_size = 5000;
  std::vector<double> vec =
      adamantine::fill_and_sync_random_vector(ensemble_size, mean, stddev);

  // Check vector size
  BOOST_CHECK(vec.size() == ensemble_size);

  // Check vector mean
  double mean_check =
      std::reduce(std::execution::par, vec.cbegin(), vec.cend()) /
      ensemble_size;

  BOOST_CHECK_CLOSE(mean, mean_check, tolerance);

  // Check vector variance
  boost::accumulators::accumulator_set<
      double, boost::accumulators::features<boost::accumulators::tag::variance>>
      acc;
  for (unsigned int member = 0; member < ensemble_size; ++member)
  {
    acc(vec[member]);
  }
  BOOST_CHECK_CLOSE(stddev * stddev, boost::accumulators::variance(acc),
                    tolerance);
}


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

//! Used for filling the sparse matrix.
typedef Eigen::Triplet<double> Triplet;
#include "igl/edge_lengths.h"
#include "igl/readMESH.h"
#include "stiffness_matrix.hpp"
#include "time_evolution_explicit.hpp"
#include "time_evolution_implicit.hpp"
#include "writer.hpp"
#include <sstream>

int main(int, char **) {
	try {
		Eigen::MatrixXd vertices;
		Eigen::MatrixXi triangles;
		Eigen::MatrixXi tetrahedra;

		igl::readMESH(NPDE_DATA_PATH "square_3.mesh", vertices, tetrahedra, triangles);
		writeMatrixToFile("vertices.txt", vertices);
		writeMatrixToFile("triangles.txt", triangles);

		Eigen::MatrixXd edgeLengths;
		igl::edge_lengths(vertices, triangles, edgeLengths);
		double dx = edgeLengths.minCoeff();

		double gamma = 1;

		// We want dt to scale roughly as dx to make sure
		// the error of the time discretization scales in the same
		// manner as the spatial discretization.
		int m = std::ceil(1 / dx);

		Eigen::VectorXd u0(vertices.rows());
		u0.setOnes();
		auto uImplicit = radiativeTimeEvolutionImplicit(vertices, triangles, u0, gamma, m);

		writeToFile("energy_implicit.txt", uImplicit.second);
		writeToFile("u_implicit.txt", uImplicit.first);

		auto uExplicitNoCfl = radiativeTimeEvolutionExplicit(vertices, triangles, u0, gamma, m);

		writeToFile("energy_explicit_no_cfl.txt", uExplicitNoCfl.second);
		writeToFile("u_explicit_no_cfl.txt", uExplicitNoCfl.first);

		auto uExplicitCfl = radiativeTimeEvolutionExplicit(vertices, triangles, u0, gamma, std::ceil(std::pow(4.0 / dx, 2)));

		writeToFile("energy_explicit_cfl.txt", uExplicitCfl.second);
		writeToFile("u_explicit_cfl.txt", uExplicitCfl.first);

	} catch (std::runtime_error &e) {
		std::cerr << "An error occurred. Error message: " << std::endl;
		std::cerr << "    \"" << e.what() << "\"" << std::endl;
		return EXIT_FAILURE;
	} catch (...) {
		std::cerr << "An unknown error occurred." << std::endl;
		throw;
	}

	return EXIT_SUCCESS;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.

// This file was modified by Oracle on 2016-2020.
// Modifications copyright (c) 2016-2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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)

#ifndef BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP
#define BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP

#include <cstddef>
#include <type_traits>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/make.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/static_assert.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <std::size_t Dimension>
struct cross_product
{
    // We define cross product only for 2d (see Wolfram) and 3d.
    // In Math, it is also well-defined for 7-dimension.
    // Generalisation of cross product to n-dimension is defined as
    // wedge product but it is not direct analogue to binary cross product.
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not implemented for this Dimension.",
        std::integral_constant<std::size_t, Dimension>);
};

template <>
struct cross_product<2>
{
    template <typename P1, typename P2, typename ResultP>
    static void apply(P1 const& p1, P2 const& p2, ResultP& result)
    {
        assert_dimension<P1, 2>();
        assert_dimension<P2, 2>();
        assert_dimension<ResultP, 2>();

        // For 2-dimensions, analog of the cross product U(x,y) and V(x,y) is
        // Ux * Vy - Uy * Vx
        // which is returned as 0-component (or X) of 2d vector, 1-component is undefined.
        set<0>(result, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
    }
};

template <>
struct cross_product<3>
{
    template <typename P1, typename P2, typename ResultP>
    static void apply(P1 const& p1, P2 const& p2, ResultP& result)
    {
        assert_dimension<P1, 3>();
        assert_dimension<P2, 3>();
        assert_dimension<ResultP, 3>();

        set<0>(result, get<1>(p1) * get<2>(p2) - get<2>(p1) * get<1>(p2));
        set<1>(result, get<2>(p1) * get<0>(p2) - get<0>(p1) * get<2>(p2));
        set<2>(result, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
    }

    template <typename ResultP, typename P1, typename P2>
    static constexpr ResultP apply(P1 const& p1, P2 const& p2)
    {
        assert_dimension<P1, 3>();
        assert_dimension<P2, 3>();
        assert_dimension<ResultP, 3>();

        return traits::make<ResultP>::apply(
                get<1>(p1) * get<2>(p2) - get<2>(p1) * get<1>(p2),
                get<2>(p1) * get<0>(p2) - get<0>(p1) * get<2>(p2),
                get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Computes the cross product of two vectors.
\details All vectors should have the same dimension, 3 or 2.
\ingroup arithmetic
\param p1 first vector
\param p2 second vector
\return the cross product vector

*/

template
<
    typename ResultP, typename P1, typename P2,
    std::enable_if_t
        <
            dimension<ResultP>::value != 3
         || ! traits::make<ResultP>::is_specialized,
            int
        > = 0
>
inline ResultP cross_product(P1 const& p1, P2 const& p2)
{
    BOOST_CONCEPT_ASSERT( (concepts::Point<ResultP>) );
    BOOST_CONCEPT_ASSERT( (concepts::ConstPoint<P1>) );
    BOOST_CONCEPT_ASSERT( (concepts::ConstPoint<P2>) );

    ResultP result;
    detail::cross_product<dimension<ResultP>::value>::apply(p1, p2, result);
    return result;
}

template
<
    typename ResultP, typename P1, typename P2,
    std::enable_if_t
        <
            dimension<ResultP>::value == 3
         && traits::make<ResultP>::is_specialized,
            int
        > = 0
>
// workaround for VS2015
#if !defined(_MSC_VER) || (_MSC_VER >= 1910)
constexpr
#endif
inline ResultP cross_product(P1 const& p1, P2 const& p2)
{
    BOOST_CONCEPT_ASSERT((concepts::Point<ResultP>));
    BOOST_CONCEPT_ASSERT((concepts::ConstPoint<P1>));
    BOOST_CONCEPT_ASSERT((concepts::ConstPoint<P2>));

    return detail::cross_product<3>::apply<ResultP>(p1, p2);
}

/*!
\brief Computes the cross product of two vectors.
\details All vectors should have the same dimension, 3 or 2.
\ingroup arithmetic
\param p1 first vector
\param p2 second vector
\return the cross product vector

\qbk{[heading Examples]}
\qbk{[cross_product] [cross_product_output]}
*/
template
<
    typename P,
    std::enable_if_t
        <
            dimension<P>::value != 3
         || ! traits::make<P>::is_specialized,
            int
        > = 0
>
inline P cross_product(P const& p1, P const& p2)
{
    BOOST_CONCEPT_ASSERT((concepts::Point<P>));
    BOOST_CONCEPT_ASSERT((concepts::ConstPoint<P>));

    P result;
    detail::cross_product<dimension<P>::value>::apply(p1, p2, result);
    return result;
}

template
<
    typename P,
    std::enable_if_t
        <
            dimension<P>::value == 3
         && traits::make<P>::is_specialized,
            int
        > = 0
>
// workaround for VS2015
#if !defined(_MSC_VER) || (_MSC_VER >= 1910)
constexpr
#endif
inline P cross_product(P const& p1, P const& p2)
{
    BOOST_CONCEPT_ASSERT((concepts::Point<P>));
    BOOST_CONCEPT_ASSERT((concepts::ConstPoint<P>));

    return detail::cross_product<3>::apply<P>(p1, p2);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP


// Statistical Computing //
// Load typical packages

#include<cmath>
#include<iostream>
#include<iomanip>

// Typiccal C libraries
#include<cmath>
#include<cstdlib>

// C++ Data Structures
#include<set>
#include<map>
#include<vector>

// Include Eigen Package for Matrices
#include <Eigen/Core>

// Load Matrix615 header file to read in from file
#include "Matrix615.h"

using namespace std;
// we avoid using namespace Eigen to be able to clearly see where we are calling the Eigen Package

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

	// Steps corresponding to the commented code is associated with the following documents:

	// Tentative Read Matrix using Matrix615.h

	Matrix615<double> read_Design; // throw-away Matrices to read in data
	Matrix615<double> read_Y;
	Matrix615<double> read_ID;

	// Read the data in
	read_Design.readFromFile(argv[3]); // t
	read_Y.readFromFile(argv[2]);
	read_ID.readFromFile(argv[1]);

	// Step (1): Store the design matrix, response, and Id into an Eigen Matrix
	Eigen::MatrixXd	design_X;
	Eigen::MatrixXd outcome_Y;

	read_Design.cloneToEigen(design_X);
	read_Y.cloneToEigen(outcome_Y);

	// cout << Weights.rows() << endl << endl;
	// cout << Weights << endl;
	Eigen::MatrixXd Weights(outcome_Y.rows(), 1);
	Weights.setOnes();

	if (int (argc) == 5) {
		Matrix615<double> read_Weights;
		read_Weights.readFromFile(argv[4]);

		Eigen::MatrixXd Weights;
		read_Weights.cloneToEigen(Weights);
	}

	// else {
	// 	Eigen::MatrixXd Weights(outcome_Y.rows(), 1);
	// 	Weights.setOnes();
	// }

	// cout << design_X.rows() << " " << design_X.cols() << endl << endl;
	// cout << design_X << endl << endl << endl;
	// cout << outcome_Y.rows() << " " << outcome_Y.cols() << endl << endl;
	// cout << outcome_Y << endl << endl << endl;

	// Step (2): Count number of unique ID's, patients
	Eigen::MatrixXd ID_Mat;
	read_ID.cloneToEigen(ID_Mat);

	std::set<int> unique_ID; // to dynamically count n
	std::map<int, int> id_map; // to dynamically count m
	std::map<int, double> weight_map;

	for (int i = 0; i < int(ID_Mat.rows()); ++i) {
		unique_ID.insert(  ID_Mat(i,0) );
		id_map[ID_Mat(i, 0)] += 1;
		weight_map[ID_Mat(i, 0)] = Weights(i, 0);
	}

	int n = int( unique_ID.size() );
	// int m = int( id_map[ID_Mat(1,0)] );

	// q = (p + 1) : # number of paramters to be estimated using GEE
	int q = int ( design_X.cols() );

	// Correlation Structure

	// Step : Weights

	// Step (3): Initialize betas *********** Should we incorporate the beta's that come from a fast linear regression or no?

	Eigen::MatrixXd Betas_new(q , 1); // dimensions (p+1)  x 1
	Betas_new.setZero();

	// cout << "Betas ***********************" << endl;
	// cout << Betas_new << endl << endl;

	// Step (4): Initialize rho and phi (rho: off diagonals of correlation structure & phi: )
	double rho = 0.0;
	// double phi = 0.0;

	// Step (5): Set up variables for iteration convergence check

	// Should we make this dynamic?

	double diff_beta = 1; // updated difference in beta estimation {beta (new) - beta (old)}

	double diff_threshold = 0.00000010; // threshold for the update difference in betas

	int iteration_threshold = 1000; // threshold on how many iterations there will be

	// Step (6): Initialize the iteration
	int iteration_count = 0;

	// Step (7): Assign appropriate value to n*
	// double n_star = (0.5) * double(n) * double(m) * double(m - 1);

	// double n_star = (0.5) * double(n) * double(5) * double(5 - 1);

	// cout << " n* is " << n_star << endl;

	double n_star_sum = 0.0;

	// Step (8): Start the GEE Estimation

	Eigen::MatrixXd Betas_updated = Betas_new;

	Eigen::MatrixXd sandwhich_Mat;

	// cout << Betas_updated << endl << endl;

	while ( (diff_beta > diff_threshold) && (iteration_count < iteration_threshold) ) {
		// (1)
		Betas_updated = Betas_new;

		// (2) Vector EE ((p+1) x 1)
		Eigen::MatrixXd EE(q , 1);
		EE.setZero();

		// (3) Matrix GI ( (p+1) x (p+1) )
		// First derivative of EE; Model-based Variance
		Eigen::MatrixXd GI(q , q);
		GI.setZero();

		// (4) Matrix G ( (p+1) x (p+1) )
		// Meat of the Sandwhich Estimator
		Eigen::MatrixXd G(q , q);
		G.setZero();

		// (5) Initialize phi(sum) and tau(sum)
		double phi_sum = 0.0;
		double tau_sum = 0.0;

		// (6) Initialize start and end
		int start = 0;
		int end = -1;

		// inner loop in while loop Time-Complexity: O(n^2)??

		for (int i = 0; i < n; ++i) {

			int m = id_map[i + 1];
			start = end + 1;
			end = start + m - 1;

			//update n*
			n_star_sum = n_star_sum + (0.5) * double(m) * double(double(m) - 1.0);

			// assign mu_i ( m x (p+1) ) for ith person
			// Eigen.block(starting_row = , starting_col = , dim_row = , dim_col = )
			Eigen::MatrixXd mu_i = design_X.block(start, 0, m, q) * Betas_updated;

			// assign r_i (m x 1)
			Eigen::MatrixXd r_i = outcome_Y.block(start, 0, m, 1) - mu_i;

			// Loop to update phi-sum
			for (int j = 0; j < int(m); ++j) phi_sum = phi_sum + (r_i(j,0) * r_i(j,0));

			// Loop to update tau-sum
			for (int j = 0; j < int(m - 1); ++j) {
				for (int k = (j + 1); k < m; ++k) tau_sum = tau_sum + r_i(j,0) * r_i(k, 0);
			}

			// create R matrix (m x m) for each observation/patient
			Eigen::MatrixXd R(m, m);
			R.setConstant(rho); // off-diagonals will be rho
			for (int d = 0; d < int(m); ++d) R(d, d) = 1; //diagonals will be 1

			// update EE ((p+1) x 1)
			EE = EE + ((design_X.block(start, 0, m, q).transpose()) * (R.inverse() * (weight_map[i+1] * r_i) )); // weight_map

			// update GI ((p+1) x (p+1)) or (q x q)
			GI = GI + ((design_X.block(start, 0, m, q).transpose()) * R.inverse() *  design_X.block(start, 0, m, q));

			// update G ((p+1) x (p+1)) or (q x q)
			G = G + ((design_X.block(start, 0, m, q).transpose()) *
				( ((R.inverse()) * r_i) * (r_i.transpose()*(R.inverse())) ) * design_X.block(start, 0, m, q));
		}

		// Update beta using either Newton Raphson or Gradient Boosting *** we can add an if statement
		Betas_new = Betas_updated + ( GI.inverse() * EE);

		// calculate the difference in the betas
		diff_beta = (Betas_new - Betas_updated).norm();

		// update rho
		rho = ( (( double(n) - double(q) ) * tau_sum) / ( (double(n_star_sum) - double(q) ) * phi_sum) );

		sandwhich_Mat = GI.inverse() * G * GI.inverse();
		// increment iteration count
		iteration_count += 1;
	}

	cout << endl << "Estimated Betas are: " << endl;
	cout << Betas_new.transpose() << endl << endl << endl;

	// cout << "The sandwhich matrix is: " << endl << endl;
	// cout << sandwhich_Mat << endl << endl;

	cout << "The beta Robust S.E.'s are: " << endl << endl;
	cout << sandwhich_Mat.diagonal().array().sqrt() << endl << endl;

	return 0;

}


#define BOOST_TEST_MODULE "Test Manhattan Normalization class"

#include <boost/test/unit_test.hpp>

#include "distance/Manhattan.hpp"
#include "Exception.hpp"

using namespace genex;

#define TOLERANCE 1e-9

struct MockData
{
  data_t dat_1[5] = {1, 2, 3, 4, 5};
  data_t dat_2[5] = {11, 2, 3, 4, 5};
};

BOOST_AUTO_TEST_CASE( man_norm, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;
  TimeSeries ts_1(data.dat_1, 0, 0, 5);
  TimeSeries ts_2(data.dat_2, 0, 0, 5);

  Manhattan dist;

  data_t total = dist.init();

  for (int i = 0; i < ts_1.getLength(); i++) {
    total = dist.reduce(total, total, ts_1[i], ts_2[i]);
  }

  BOOST_TEST( dist.norm(total, ts_1, ts_2) == 2.0 );

  dist.clean(total);
}


#pragma once

#include <Eigen/Dense>

class CubicKernel
{
public:
	 double getRadius() { return m_radius; }
	 void setRadius(double val)
	{
		m_radius = val;
		const double pi = static_cast<double>(M_PI);

		const double h3 = m_radius*m_radius*m_radius;
		m_k = 8.0 / (pi*h3);
		m_l = 48.0 / (pi*h3);
		m_W_zero = W(Eigen::Vector3d::Zero());
	}

public:
	double W(Eigen::Vector3d const& r)
	{
		double res = 0.0;
		const double rl = r.norm();
		const double q = rl/m_radius;
		if (q <= 1.0)
		{
			if (q <= 0.5)
			{
				const double q2 = q*q;
				const double q3 = q2*q;
				res = m_k * (6.0*q3-6.0*q2+1.0);
			}
			else
			{
				auto _1mq = 1.0 - q;
				res = m_k * (2.0*_1mq*_1mq*_1mq);
			}
		}
		return res;
	}

	Eigen::Vector3d gradW(const Eigen::Vector3d &r)
	{
		using namespace Eigen;
		Vector3d res;
		const double rl = r.norm();
		const double q = rl / m_radius;
		if (q <= 1.0)
		{
			if (rl > 1.0e-6)
			{
				const Vector3d gradq = r * ((double) 1.0 / (rl*m_radius));
				if (q <= 0.5)
				{
					res = m_l*q*((double) 3.0*q - (double) 2.0)*gradq;
				}
				else
				{
					const double factor = 1.0 - q;
					res = m_l*(-factor*factor)*gradq;
				}
			}
		}
		else
			res.setZero();

		return res;
	}

	double W_zero()
	{
		return m_W_zero;
	}

private:
	double m_radius;
	double m_k;
	double m_l;
	double m_W_zero;
};


#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

namespace vpp
{

  // http://www.cse.psu.edu/~rcollins/CSE486/lecture19_6pp.pdf
  // Page 5.
  inline vfloat2 epipole_left(const Eigen::Matrix3f& F)
  {
    // F e_l = 0.
    Eigen::EigenSolver<Eigen::Matrix3d> es(F.cast<double>().transpose() * F.cast<double>());

    auto values = es.eigenvalues();
    auto vectors = es.eigenvectors();
    vdouble2 epipole;
    double min_ev = FLT_MAX;
    for (int i = 0; i < values.size(); i++)
      if (values[i].real() < min_ev)
      {
        min_ev = values[i].real();
        auto vc = vectors.col(i);
        epipole[0] = vc[0].real();
        epipole[1] = vc[1].real();
        epipole /= double(vc[2].real());
      }

    return epipole.cast<float>();
  }

  // http://www.cse.psu.edu/~rcollins/CSE486/lecture19_6pp.pdf
  // Page 5.
  inline vfloat2 epipole_right(const Eigen::Matrix3f& F)
  {
    // e_r F  = 0.
    Eigen::EigenSolver<Eigen::Matrix3d> es(F.cast<double>() * F.cast<double>().transpose());

    auto values = es.eigenvalues();
    auto vectors = es.eigenvectors();
    vdouble2 epipole;
    double min_ev = FLT_MAX;
    for (int i = 0; i < values.size(); i++)
      if (values[i].real() < min_ev)
      {
        min_ev = values[i].real();
        auto vc = vectors.col(i);
        epipole[0] = vc[0].real();
        epipole[1] = vc[1].real();
        epipole /= double(vc[2].real());
      }

    return epipole.cast<float>();
  }
}


/**
 * @file crossprod.cc
 * @brief NPDE homework CrossProd code
 * @author Unknown, Oliver Rietmann
 * @date 31.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "crossprod.h"

#include <Eigen/Geometry>
#include <iomanip>
#include <iostream>
#include <vector>

namespace CrossProd {

/* SAM_LISTING_BEGIN_0 */
void tab_crossprod() {
  // TO DO (13-1.e): solve the cross-product ODE with the implicit RK method
  // defined in solve_imp_mid. Tabulate the norms of the results at all steps.
  double T = 10.;
  int N = 128;
  // set data
  double c = 1.;
  Eigen::Vector3d y0(1., 1., 1.);
  Eigen::Vector3d a(1., 0., 0.);

  // define rhs
  auto f = [a, c](Eigen::Vector3d y) -> Eigen::Vector3d {
    return a.cross(y) + c * y.cross(a.cross(y));
  };
  // define Jacobian of rhs
  auto Jf = [a, c](Eigen::Vector3d y) -> Eigen::Matrix3d {
    Eigen::Matrix3d temp;
    temp << -c * (a(1) * y(1) + a(2) * y(2)),
        c * (2 * a(0) * y(1) - a(1) * y(0)) - a(2),
        a(1) + c * (2 * a(0) * y(2) - a(2) * y(0)),
        a(2) - c * (a(0) * y(1) - 2 * a(1) * y(0)),
        -c * (a(0) * y(0) + a(2) * y(2)),
        c * (2 * a(1) * y(2) - a(2) * y(1)) - a(0),
        -a(1) - c * (a(0) * y(2) - 2 * a(2) * y(0)),
        a(0) - c * (a(1) * y(2) - 2 * a(2) * y(1)),
        -c * (a(0) * y(0) + a(1) * y(1));
    return temp;
  };

  std::vector<Eigen::VectorXd> res_imp = solve_imp_mid(f, Jf, T, y0, N);

  std::cout << "1. Implicit midpoint method" << std::endl;
  std::cout << std::setw(10) << "t" << std::setw(15) << "norm(y(t))"
            << std::endl;

  for (int i = 0; i < N + 1; ++i) {
    std::cout << std::setw(10) << T * i / N << std::setw(15)
              << res_imp[i].norm() << std::endl;
  }
  /* SAM_LISTING_END_0 */

  /* SAM_LISTING_BEGIN_1 */
  // TO DO (13-1.g): solve the cross-product ODE with the implicit RK method
  // defined in solve_lin_mid. Tabulate the norms of the results at all steps.
  std::vector<Eigen::VectorXd> res_lin = solve_lin_mid(f, Jf, T, y0, N);

  std::cout << "\n2. Linear implicit midpoint method" << std::endl;
  std::cout << std::setw(10) << "t" << std::setw(15) << "norm(y(t))"
            << std::endl;
  for (int i = 0; i < N + 1; ++i) {
    std::cout << std::setw(10) << T * i / N << std::setw(15)
              << res_lin[i].norm() << std::endl;
  }
  /* SAM_LISTING_END_1 */
}

}  // namespace CrossProd


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

/**
 * \file libw/numeric/ublasx/operation/balance.cpp
 *
 * \brief Test case for matrix balance operation.
 *
 * \author Marco Guazzone, <marco.guazzone@gmail>
 *
 * <hr/>
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublasx/operations.hpp>

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

int main()
{
    // MATLAB/Octave:
    //   a = linspace(0, 2, 2)
    //   b = linspace(1, 2, 2)
    //   c = 2*a + 3*b
    ublas::vector<double> a = ublasx::linspace(0.0, 2.0, 2); // a = [0 2]
    ublas::vector<double> b = ublasx::linspace(1.0, 2.0, 2); // b = [1 2]
    ublas::vector<double> c = 2*a + 3*b; // c = [3 10]
    std::cout << "a = " << a << "\n";
    std::cout << "b = " << b << "\n";
    std::cout << "c = 2*a + 3*b = " << c << "\n";

    // MATLAB/Octave:
    //   A = rot90(2*eye(2))
    //   rank(A)
    //   B = inv(A)
    ublas::matrix<double> A = ublasx::rot90(2*ublas::identity_matrix<double>(2)); // A = [0 2; 2 0]
    ublas::matrix<double> B = ublasx::inv(A); // B = [0 0.5; 0.5 0]
    std::cout << "A = " << A << "\n";
    std::cout << "rank of A = " << ublasx::rank(A) << "\n";
    std::cout << "inverse of A = " << B << "\n";

    // MATLAB/Octave:
    //   C = reshape(linspace(1, 9, 9), 3, 3)
    //   D = pow2(C)
    //   E = cat(2, C, D)
    ublas::matrix<double> C = ublasx::reshape(ublasx::linspace(1.0, 9.0, 9), 3, 3); // C = [1 2 3; 4 5 6; 7 8 9]
    ublas::matrix<double> D = ublasx::pow2(C); // [2 4 8; 16 32 64; 128 256 512]
    ublas::matrix<double> E = ublasx::cat<2>(C, D); // [1 2 3 2 4 8; 4 5 6 16 32 64; 7 8 9 128 256 512]
    std::cout << "C = " << C << "\n";
    std::cout << "D = 2.^C = " << D << "\n";
    std::cout << "E = [C D] = " << E << "\n";
}


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

#include <fstream>
#include <boost/algorithm/string.hpp>
#include <numeric>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

typedef std::pair<nombre, nombre> paire;

ENREGISTRER_PROBLEME(107, "Minimal network") {
    // The following undirected network consists of seven vertices and twelve edges with a total weight of 243.
    //
    //
    // The same network can be represented by the matrix below.
    //
    //      	A	B	C	D	E	F	G
    //      A	-	16	12	21	-	-	-
    //      B	16	-	-	17	20	-	-
    //      C	12	-	-	28	-	31	-
    //      D	21	17	28	-	18	19	23
    //      E	-	20	-	18	-	-	11
    //      F	-	-	31	19	-	-	27
    //      G	-	-	-	23	11	27	-
    //
    // However, it is possible to optimise the network by removing some edges and still ensure that all points on the
    // network remain connected. The network which achieves the maximum saving is shown below. It has a weight of 93,
    // representing a saving of 243 − 93 = 150 from the original network.
    //
    // Using network.txt (right click and 'Save Link/Target As...'), a 6K text file containing a network with forty
    // vertices, and given in matrix form, find the maximum saving which can be achieved by removing redundant edges
    // whilst ensuring that the network remains connected.
    std::ifstream ifs("data/p107_network.txt");

    graphe::Kruskal::aretes A;
    nombre i = 0;
    std::string ligne;
    while (ifs >> ligne) {
        std::vector<std::string> strings;
        boost::split(strings, ligne, boost::is_any_of(","));

        nombre j = 0;
        for (auto &s: strings) {
            if (s != "-" && i < j)
                A.emplace_back(i, j, std::stoull(s));
            ++j;
        }
        ++i;
    }

    graphe::Kruskal kruskal(A);
    auto arbre_mini = kruskal.algorithme();

    auto somme_poids = [](const nombre &r, const graphe::Kruskal::arete &a) { return r + std::get<2>(a); };

    nombre resultat = std::accumulate(A.begin(), A.end(), 0ull, somme_poids) -
                      std::accumulate(arbre_mini.begin(), arbre_mini.end(), 0ull, somme_poids);
    return std::to_string(resultat);
}


// Created by dinies on 03/07/2018.

//Collection of static functions and useful types to perform geometric computations
#pragma once

#include <unistd.h>
#include <vector>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace MultiRobot{
  class MyMath{

  public:
    // static Eigen::Isometry2d v2t(const std::vector<double> &t_vec);
    // static std::vector<double> t2v(const Eigen::Isometry2d& t_transf);

    static Eigen::Isometry2d v2t(const Eigen::Vector3d &t_vec);
    static Eigen::Vector3d t2v(const Eigen::Isometry2d& t_transf);

    static void rotate2D( Eigen::Vector2d &t_point, const double t_angle_rad );

    static std::vector<double> vecSum(const std::vector<double> &t_first,const std::vector<double> &t_second);

    static std::vector<double> vecMultEleWise(const std::vector<double> &t_first,const std::vector<double> &t_second);

    static double boxMinusAngleRad(const double t_ref,const double t_actual);
    static double boxPlusAngleRad(const double t_ref,const double t_actual);
    static double computeAvg(const double t_prevAvg,const int t_numEntries,const double t_newValue);
  };
}


#include <boost/test/unit_test.hpp>
#include <shz/math/quaternion.hpp>

#define ROUND_ERROR 0.00001

BOOST_AUTO_TEST_CASE(quaternionConstructors)
{
	shz::math::quaternion<shz::math::f64> q1;
	q1.x = 1.0;
	q1.y = 0.0;
	q1.z = 0.0;
	q1.w = 1.0;
	BOOST_CHECK_EQUAL(q1.data[0], q1.x);
	BOOST_CHECK_EQUAL(q1.data[1], q1.y);
	BOOST_CHECK_EQUAL(q1.data[2], q1.z);
	BOOST_CHECK_EQUAL(q1.data[3], q1.w);

	shz::math::quaternion<shz::math::f64> q2(q1);
	BOOST_CHECK_EQUAL(q1.data[0], q2.x);
	BOOST_CHECK_EQUAL(q1.data[1], q2.y);
	BOOST_CHECK_EQUAL(q1.data[2], q2.z);
	BOOST_CHECK_EQUAL(q1.data[3], q2.w);

	shz::math::vector<shz::math::f64, 3> v;
	v.data[0] = 0.0;
	v.data[1] = 1.0;
	v.data[2] = 0.0;
	q2 = shz::math::quaternion<shz::math::f64>(v, shz::math::HALF_PI);
	BOOST_CHECK_CLOSE(0.0, q2.x, ROUND_ERROR);
	BOOST_CHECK_CLOSE(0.70710679664085752, q2.y, ROUND_ERROR);
	BOOST_CHECK_CLOSE(0.0, q2.z, ROUND_ERROR);
	BOOST_CHECK_CLOSE(0.70710679664085752, q2.w, ROUND_ERROR);

	q2 = shz::math::quaternion<shz::math::f64>(0.0, 1.0, 0.0, shz::math::HALF_PI);
	BOOST_CHECK_CLOSE(0.0, q2.x, ROUND_ERROR);
	BOOST_CHECK_CLOSE(0.70710679664085752, q2.y, ROUND_ERROR);
	BOOST_CHECK_CLOSE(0.0, q2.z, ROUND_ERROR);
	BOOST_CHECK_CLOSE(0.70710679664085752, q2.w, ROUND_ERROR);
}

BOOST_AUTO_TEST_CASE(quaternionToMatrix)
{
	shz::math::quaternion<shz::math::f64> q1(0.0, 1.0, 0.0, shz::math::HALF_PI);
	shz::math::matrix<shz::math::f64, 4, 4> matrix;

	q1.to_matrix(matrix);

	shz::math::matrix<shz::math::f32, 4, 4> m = shz::math::matrix<shz::math::f32, 4, 4>::from_rotation(0.f, 1.f, 0.f, shz::math::HALF_PI);

	BOOST_CHECK_CLOSE(matrix[0], m[0], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[1], m[1], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[2], m[2], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[3], m[3], ROUND_ERROR);

	BOOST_CHECK_CLOSE(matrix[4], m[4], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[5], m[5], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[6], m[6], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[7], m[7], ROUND_ERROR);

	BOOST_CHECK_CLOSE(matrix[8], m[8], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[9], m[9], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[10], m[10], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[11], m[11], ROUND_ERROR);

	BOOST_CHECK_CLOSE(matrix[12], m[12], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[13], m[13], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[14], m[14], ROUND_ERROR);
	BOOST_CHECK_CLOSE(matrix[15], m[15], ROUND_ERROR);

}

BOOST_AUTO_TEST_CASE(quaternionMuls)
{
	shz::math::quaternion<shz::math::f64> q1(0.0, 1.0, 0.0, shz::math::HALF_PI);
	shz::math::quaternion<shz::math::f64> q2(0.0, 1.0, 0.0, shz::math::HALF_PI);
	shz::math::quaternion<shz::math::f64> q3(0.0, 1.0, 0.0, shz::math::PI);

	shz::math::quaternion<shz::math::f64> q12 = q1 * q2;

	BOOST_CHECK_CLOSE(q12.x, q3.x, ROUND_ERROR);
	BOOST_CHECK_CLOSE(q12.y, q3.y, ROUND_ERROR);
	BOOST_CHECK_CLOSE(q12.z, q3.z, ROUND_ERROR);
	BOOST_CHECK_CLOSE(q12.w, q3.w, ROUND_ERROR);

	q12 = q1;
	q12 *= q2;

	BOOST_CHECK_CLOSE(q12.x, q3.x, ROUND_ERROR);
	BOOST_CHECK_CLOSE(q12.y, q3.y, ROUND_ERROR);
	BOOST_CHECK_CLOSE(q12.z, q3.z, ROUND_ERROR);
	BOOST_CHECK_CLOSE(q12.w, q3.w, ROUND_ERROR);
}

BOOST_AUTO_TEST_CASE(quaternionSlerp)
{
	shz::math::quaternion<shz::math::f64> q1(0.0, 1.0, 0.0, 0.0);
	shz::math::quaternion<shz::math::f64> q2(0.0, 1.0, 0.0, shz::math::HALF_PI/2);
	shz::math::quaternion<shz::math::f64> q3(0.0, 1.0, 0.0, shz::math::HALF_PI);

	auto half = shz::math::quaternion<shz::math::f64>::slerp(q1, q3, 0.5);

	BOOST_CHECK_CLOSE(half.x, q2.x, ROUND_ERROR);
	BOOST_CHECK_CLOSE(half.y, q2.y, ROUND_ERROR);
	BOOST_CHECK_CLOSE(half.z, q2.z, ROUND_ERROR);
	BOOST_CHECK_CLOSE(half.w, q2.w, ROUND_ERROR);

	half = shz::math::quaternion<shz::math::f64>::slerp(q1, q1, 0.5);
	BOOST_CHECK_CLOSE(half.x, q1.x, ROUND_ERROR);
	BOOST_CHECK_CLOSE(half.y, q1.y, ROUND_ERROR);
	BOOST_CHECK_CLOSE(half.z, q1.z, ROUND_ERROR);
	BOOST_CHECK_CLOSE(half.w, q1.w, ROUND_ERROR);
}


#include <ctime>
#include <gtest/gtest.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include "slick/math/so3.h"
#include "slick/test/unittest/util.h"

using namespace std;
using namespace Eigen;
using namespace slick;

// constructors tests ==========================================================
template <typename T>
void DefaultConstructor_Test() {
  // for double precision
  SO3Group<T> rot;
  EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 3, 3>::Identity(), rot.get_matrix(),
                     Gap<T>());
}

TEST(SO3Test, DefaultConstructor) {
  DefaultConstructor_Test<double>();
  DefaultConstructor_Test<float>();
}

template <typename T>
void FromMatrixConstructor_Test() {
  Eigen::Matrix<T, 3, 1> aaxis = Eigen::Matrix<T, 3, 1>::Random();
  aaxis.normalize();
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 3, 3> m;
  m = Eigen::AngleAxis<T>(angle, aaxis);

  SO3Group<T> rot(m);
  EXPECT_MATRIX_NEAR(m, rot.get_matrix(), Gap<T>());
}

TEST(SO3Test, FromMatrixConstructor) {
  FromMatrixConstructor_Test<double>();
  FromMatrixConstructor_Test<float>();
}

template <typename T>
void FromAngleAxisConstructor_Test() {
  Eigen::Matrix<T, 3, 1> aaxis = Eigen::Matrix<T, 3, 1>::Random();
  aaxis.normalize();
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 3, 3> m;
  m = Eigen::AngleAxis<T>(angle, aaxis);

  aaxis *= angle;
  SO3Group<T> rot(aaxis);
#if 0  // NOTE: Eigen implementation of rotation matrix is not as accurate as \
       // ours
    EXPECT_MATRIX_NEAR(m, rot.get_matrix(), Gap<T>());
    std::cout << "gt:\n" << m << std::endl;
    std::cout << "est:\n" << rot.get_matrix() << std::endl;

    std::cout << "(rot.inverse()*rot).get_matrix():\n" << (rot.inverse()*rot).get_matrix() << std::endl;
    EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 3, 3>::Identity(),
     (rot.inverse()*rot).get_matrix(), Gap<T>());

    std::cout << "m*m.transpose():\n" << m*m.transpose() << std::endl;
    EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 3, 3>::Identity(),
     m*m.transpose(), Gap<T>());
#else
  EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 3, 3>::Identity(),
                     (rot.inverse() * rot).get_matrix(), Gap<T>());
#endif
}

TEST(SO3Test, FromAngleAxisConstructor) {
  FromAngleAxisConstructor_Test<double>();
  FromAngleAxisConstructor_Test<float>();
}

// template<typename T>
// void FromListInitializerConstructor_Test() {
//   SO3Group<T> rot {1, 0, 0, 1};
//   EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 3, 3>::Identity(), rot.get_matrix(),
// Gap<T>());

//   EXPECT_NEAR(0.0, rot.ln(), Gap<T>());
// }

// TEST(SO3Test, FromListInitializerConstructor) {
//   FromListInitializerConstructor_Test<double>();
//   FromListInitializerConstructor_Test<float>();
// }

// so2 specific functions ======================================================

template <typename T>
void Ln_Test() {
  Eigen::Matrix<T, 3, 1> aaxis = Eigen::Matrix<T, 3, 1>::Random();
  aaxis.normalize();
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand()) / T(RAND_MAX) * 2 * M_PI;
  aaxis *= angle;

  SO3Group<T> rot(aaxis);
  auto est_axis = rot.ln();

  // std::cout << "gt: " << aaxis.transpose()
  //           << "  est:" << est_axis.transpose() << std::endl;

  T d = est_axis.dot(aaxis);
  if (d < 0) {
    auto a = -est_axis.norm() + 2 * M_PI;
    est_axis = -est_axis.normalized() * a;
  }
  EXPECT_MATRIX_NEAR(aaxis, est_axis, Gap<T>());
}

TEST(SO3Test, Ln) {
  Ln_Test<double>();
  Ln_Test<float>();
}

template <typename T>
void Inverse_Test() {
  Eigen::Matrix<T, 3, 1> aaxis = Eigen::Matrix<T, 3, 1>::Random();
  aaxis.normalize();
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());
  aaxis *= angle;
  SO3Group<T> rot(aaxis);
  Eigen::Matrix<T, 3, 3> m_inv = rot.get_matrix().inverse();
  EXPECT_MATRIX_NEAR(m_inv, rot.inverse().get_matrix(), Gap<T>());
}

TEST(SO3Test, Inverse) {
  Inverse_Test<double>();
  Inverse_Test<float>();
}

template <typename T>
void SO3RightHandMulOperator_Test() {
  Eigen::Matrix<T, 3, 1> aaxis = Eigen::Matrix<T, 3, 1>::Random();
  aaxis.normalize();
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

#if 0  // NOTE: Eigen::AngleAxis has less accuracy than ours
    Eigen::Matrix<T, 3, 3> mrot1;
    mrot1 = Eigen::AngleAxis<T>(angle, aaxis);

    aaxis *= angle;
    SO3Group<T> rot1(aaxis);

    aaxis = Eigen::Matrix<T, 3, 1>::Random();
    aaxis.normalize();
    std::srand(std::time(0));  // use current time as seed for random generator
    angle = T(std::rand());

    Eigen::Matrix<T, 3, 3> mrot2;
    mrot2 = Eigen::AngleAxis<T>(angle, aaxis);

    aaxis *= angle;
    SO3Group<T> rot2(aaxis);

    EXPECT_MATRIX_NEAR(mrot1 * mrot2, (rot1 * rot2).get_matrix(), Gap<T>());
#else
  aaxis *= angle;
  SO3Group<T> rot1(aaxis);
  Eigen::Matrix<T, 3, 1> t = -aaxis;
  SO3Group<T> rot2(t);

  EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 3, 3>::Identity(),
                     (rot1 * rot2).get_matrix(), Gap<T>());
#endif
}

TEST(SO3Test, SO3RightHandMulOperator) {
  SO3RightHandMulOperator_Test<double>();
  SO3RightHandMulOperator_Test<float>();
}

template <typename T>
void MatrixRightHandMulOperator_Test() {
  Eigen::Matrix<T, 3, 1> aaxis = Eigen::Matrix<T, 3, 1>::Random();
  aaxis.normalize();
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

#if 0  // NOTE: Eigen::AngleAxis has less accuracy than ours
    Eigen::Matrix<T, 3, 3> mrot1;
    mrot1 = Eigen::AngleAxis<T>(angle, aaxis);

    aaxis *= angle;
    SO3Group<T> rot1(aaxis);

    Eigen::Matrix<T, 3, 3> mrand = Eigen::Matrix<T, 3, 3>::Random();

    EXPECT_MATRIX_NEAR(mrot1 * mrand, rot1 * mrand, Gap<T>());
#else
  aaxis *= angle;
  SO3Group<T> rot1(aaxis);

  Eigen::Matrix<T, 3, 3> mrand = Eigen::Matrix<T, 3, 3>::Random();

  EXPECT_MATRIX_NEAR(rot1.get_matrix() * mrand, rot1 * mrand, Gap<T>());

#endif
}

TEST(SO3Test, MatrixRightHandMulOperator) {
  MatrixRightHandMulOperator_Test<double>();
  MatrixRightHandMulOperator_Test<float>();
}

template <typename T>
void MatrixLeftHandMulOperator_Test() {
  Eigen::Matrix<T, 3, 1> aaxis = Eigen::Matrix<T, 3, 1>::Random();
  aaxis.normalize();
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

#if 0  // NOTE: Eigen::AngleAxis has less accuracy than ours
    Eigen::Matrix<T, 3, 3> mrot1;
    mrot1 = Eigen::AngleAxis<T>(angle, aaxis);

    aaxis *= angle;
    SO3Group<T> rot1(aaxis);

    Eigen::Matrix<T, 3, 3> mrand = Eigen::Matrix<T, 3, 3>::Random();

    EXPECT_MATRIX_NEAR(mrand * mrot1, mrand * rot1, Gap<T>());
#else

  aaxis *= angle;
  SO3Group<T> rot1(aaxis);

  Eigen::Matrix<T, 3, 3> mrand = Eigen::Matrix<T, 3, 3>::Random();

  EXPECT_MATRIX_NEAR(mrand * rot1.get_matrix(), mrand * rot1, Gap<T>());
#endif
}

TEST(SO3Test, MatrixLeftHandMulOperator) {
  MatrixLeftHandMulOperator_Test<double>();
  MatrixLeftHandMulOperator_Test<float>();
}

int main(int argc, char** argv) {
  std::vector<char*> vars(argc+1);
  for (int i = 0; i < argc; ++i)
    vars[i] = argv[i];
  char ca[50];
  sprintf(ca, "--gtest_repeat=%d", NSAMPLES);
  vars[argc] = ca;
  argc++;
  ::testing::InitGoogleTest(&argc, &vars.front());
  return RUN_ALL_TESTS();
}


/**
 * @file mixedfemwave_test.cc
 * @author Erick Schulz
 * @date 24.07.2020
 * @copyright Developed at SAM, ETH Zurich
 */

#include "../mixedfemwave.h"

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

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

namespace MixedFEMWave::test {

TEST(MixedFEMWave_computeMQ, test) {
  // LOADING COARSE MESH
  auto mesh_factory_init = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader_init(std::move(mesh_factory_init),
                                 CURRENT_SOURCE_DIR
                                 "/../../meshes/unitsquare_unitest.msh");
  std::shared_ptr<lf::mesh::Mesh> mesh_p = reader_init.mesh();

  // Vector dofhandler for the finite element space Q
  lf::assemble::UniformFEDofHandler dofh_Q(mesh_p,
                                           {{lf::base::RefEl::kPoint(), 0},
                                            {lf::base::RefEl::kSegment(), 0},
                                            {lf::base::RefEl::kTria(), 2},
                                            {lf::base::RefEl::kQuad(), 2}});

  Eigen::SparseMatrix<double> M_Q_sparse = computeMQ(dofh_Q);
  Eigen::MatrixXd M_Q_dense = Eigen::MatrixXd(M_Q_sparse);
  Eigen::MatrixXd M_Q_dense_ref = Eigen::MatrixXd::Zero(8, 8);
  for (int i = 0; i < 8; i++) {
    M_Q_dense_ref(i, i) = 0.25;
  }

  double tol = 1.0e-8;
  Eigen::MatrixXd difference = M_Q_dense - M_Q_dense_ref;
  ASSERT_NEAR(0.0, difference.lpNorm<Eigen::Infinity>(), tol);
}

TEST(MixedFEMWave_computeMV, test) {
  auto rho = [](Eigen::Vector2d x) -> double { return 1.0; };
  // LOADING COARSE MESH
  auto mesh_factory_init = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader_init(std::move(mesh_factory_init),
                                 CURRENT_SOURCE_DIR
                                 "/../../meshes/unitsquare_unitest.msh");
  std::shared_ptr<lf::mesh::Mesh> mesh_p = reader_init.mesh();

  // Scalar finite element space for lowest-order Lagrangian finite elements
  std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space_V =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  Eigen::SparseMatrix<double> M_V_sparse = computeMV(fe_space_V, rho);
  Eigen::MatrixXd M_V_dense = Eigen::MatrixXd(M_V_sparse);
  Eigen::MatrixXd M_V_dense_ref = Eigen::MatrixXd::Zero(5, 5);
  for (int i = 0; i < 4; i++) {
    M_V_dense_ref(i, i) = 0.1 + 0.2 / 3.0;
  }
  M_V_dense_ref(4, 4) = 1.0 / 3.0;

  double tol = 1.0e-8;
  Eigen::MatrixXd difference = M_V_dense - M_V_dense_ref;
  ASSERT_NEAR(0.0, difference.lpNorm<Eigen::Infinity>(), tol);
}

TEST(MixedFEMWave_computeB, test) {
  auto rho = [](Eigen::Vector2d x) -> double { return 1.0; };
  // LOADING COARSE MESH
  auto mesh_factory_init = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader_init(std::move(mesh_factory_init),
                                 CURRENT_SOURCE_DIR
                                 "/../../meshes/unitsquare_unitest.msh");
  std::shared_ptr<lf::mesh::Mesh> mesh_p = reader_init.mesh();

  // Scalar finite element space for lowest-order Lagrangian finite elements
  std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space_V =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  // Scalar dofhandler as built along with the finite-element space for V
  const lf::assemble::DofHandler &dofh_V = fe_space_V->LocGlobMap();

  // Vector dofhandler for the finite element space Q
  lf::assemble::UniformFEDofHandler dofh_Q(mesh_p,
                                           {{lf::base::RefEl::kPoint(), 0},
                                            {lf::base::RefEl::kSegment(), 0},
                                            {lf::base::RefEl::kTria(), 2},
                                            {lf::base::RefEl::kQuad(), 2}});

  Eigen::SparseMatrix<double> B_sparse = computeB(dofh_V, dofh_Q);
  Eigen::MatrixXd B_dense = Eigen::MatrixXd(B_sparse);
  Eigen::MatrixXd B_dense_ref = Eigen::MatrixXd::Zero(8, 5);
  B_dense_ref(1, 4) = 0.5;
  B_dense_ref(2, 4) = 0.5;
  B_dense_ref(4, 4) = -0.5;
  B_dense_ref(7, 4) = -0.5;

  double tol = 1.0e-8;
  Eigen::MatrixXd difference = B_dense - B_dense_ref;
  ASSERT_NEAR(0.0, difference.lpNorm<Eigen::Infinity>(), tol);
}

}  // namespace MixedFEMWave::test


/**
 * @file TrajectoryGenerator.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-08
 */

#pragma once

#include <vector>
#include <Eigen/Eigen>
#include <yaml-cpp/yaml.h>

namespace momentumopt
{
  enum class InterpolationMethod {
    PiecewiseLinear,
    PiecewiseConstant,
  };

  // Class to generate trajectories around way-points in position, velocity and acceleration
  class Interpolator
  {
    public:
	  typedef Eigen::Matrix<double,Eigen::Dynamic,2> InterpMat;

    public:
	  Interpolator(){}
	  ~Interpolator(){}
      void initialize(const InterpMat& pos_des, const InterpMat& vel_des, const InterpMat& acc_des);
      void interpolate(const double time, double& position, double& velocity, double& acceleration);

    private:
      double cons_size_;
	  std::vector<double> time_span_;
	  Eigen::MatrixXd regression_matrix_;
	  Eigen::VectorXd regression_vector_, regression_coeffs_;
  };

  // Class to generate 3d continuous and smooth trajectories
  class TrajectoryGenerator
  {
    public:
	  TrajectoryGenerator(){};
      ~TrajectoryGenerator(){};
      void initialize(const double tini, const Eigen::Vector3d& ini_position,
                      const double tend, const Eigen::Vector3d& end_position,
                      const Eigen::Vector3d& via_position);
      void update(const double time);

	  const Eigen::Vector3d& desPos() const { return pos_des_; }
	  const Eigen::Vector3d& desVel() const { return vel_des_; }
	  const Eigen::Vector3d& desAcc() const { return acc_des_; }

    private:
	  Interpolator traj_interp_[3];
	  Eigen::Vector3d pos_des_, vel_des_, acc_des_;
  };
}