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

struct SPoint2D
{
	double x[2];
};

double ComputeAngle(const Eigen::Vector2d& input_one, const Eigen::Vector2d& input_two)
{
	//compute theta
	//when e_im.dot(e_i) = 1.0, theta = pi;
	const double r_sinT = input_one(0)*input_two(1) - input_one(1)*input_two(0);
	const double r_cosT = -input_one.dot(input_two);
	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& input_one, const Eigen::Vector2d& input_two)
{
	//compute 2.0 * tan(phi / 2.0)
	//phi = pi - theta
	return ComputeOmegaFromAngle(ComputeAngle(input_one, input_two));
}

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 rest_length)
{
	double tot = 0.0;
	const double len = rest_length / 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& rest_length)
{
	const Eigen::Vector2d diff = p1 - p2;
	return 0.5 * YA * rest_length
		* (diff.norm() / rest_length - 1)
		* (diff.norm() / rest_length - 1);
}

inline double computeSegmentBendingEnergy(const Eigen::Vector2d& pp, const Eigen::Vector2d& pc, const Eigen::Vector2d& pn,
	const double& alpha, const double& rest_length_p, const double& rest_length_n, 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) / (rest_length_p + rest_length_n);
}

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_initial_vars, SVar& io_vars, SCurve* io_curve, SSolverVars& io_solver_vars)
{
	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_initial_vars, 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_initial_vars = io_vars;

	initSolverVars(in_params, io_curve, io_initial_vars, io_solver_vars, true);
}

bool SecantLMMethodSingleUpdate(const SParameters* in_params, const SCurve* in_curve, const SVar& in_initial_vars, SSolverVars& io_solver_vars, SVar& solution)
{
	if(io_solver_vars.found || io_solver_vars.k > in_params->kmax)
		return true;

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

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

	double Fnew = 0.5 * ComputeEnergy(in_params, in_curve, io_solver_vars.xnew);
	double F = 0.5 * ComputeEnergy(in_params, in_curve, io_solver_vars.x);

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

	if(gain > 0)
	{
		io_solver_vars.x = io_solver_vars.xnew;
		Compute_f(in_params, in_curve, io_solver_vars.x, io_solver_vars.f);
		ComputeNumericalDerivative(in_params, in_curve, io_solver_vars.x, io_solver_vars.epsilon, io_solver_vars.B);
		io_solver_vars.g = io_solver_vars.B.transpose() * io_solver_vars.f;
		io_solver_vars.found = (io_solver_vars.g.lpNorm<Eigen::Infinity>() <= in_params->epsilon_1);
		printf("k: %d, gain: %f, |g|_inf: %f\n", io_solver_vars.k, gain, io_solver_vars.g.lpNorm<Eigen::Infinity>());
		io_solver_vars.mu = io_solver_vars.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_solver_vars.nu = 2.0;
	}
	else
	{
		io_solver_vars.mu = io_solver_vars.mu * io_solver_vars.nu;
		io_solver_vars.nu = io_solver_vars.nu * 2.0;
	}

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

	solution = io_solver_vars.x;

	return io_solver_vars.found || io_solver_vars.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_initial_vars, SSolverVars& io_solver_vars, SVar& solution)
{
	initSolverVars(in_params, in_curve, in_initial_vars, io_solver_vars);

	while(1)
	{
		if(SecantLMMethodSingleUpdate(in_params, in_curve, in_initial_vars, io_solver_vars, solution))
			break;
		UpdateCurveSubdivision(in_params, in_initial_vars, solution, in_curve, io_solver_vars);
	}

	printf("found in %d steps\n", io_solver_vars.k);
	solution = io_solver_vars.x;
	ShowFeaturePoints(in_curve, solution);
}


///////////////////////////////////////////////////////////////////////////////
// chi_square_summand.hpp                                                    //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_COMMON_CHISQ_SUMMAND_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_COMMON_CHISQ_SUMMAND_HPP_ER_2010
#include <boost/numeric/conversion/converter.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chi_square_statistic{

    // The sum over all cells of this quantity is a fraction, f, of Pearson's
    // Chi-square statistic. If (expected, observed) are expressed in counts,
    // f = 1. If they are expressed in frequencies, f = 1/n, where n is the
    // total number of counts.
    template<typename T1,typename T2,typename T3>
    T1  chi_square_summand(const T2& expected, const T3& observed)
    {
        typedef boost::numeric::converter<T1,T2> conv2_;
        typedef boost::numeric::converter<T1,T3> conv3_;
        T1 summand = conv2_::convert( expected ) - conv3_::convert( observed );
        summand *= summand;
        return summand / conv2_::convert( expected );
    }

}// pearson_chi_square_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif


#include "GL_Logger.h"
#include "KeySpline.h"
#include <Eigen/Geometry>
#include "Transform.h"
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/quaternion.hpp>
#include <GLFW/glfw3.h>

/**
 * Calculate the lenght of a segment using a Gaussian Quadrature
 * @param  u1 the first time point
 * @param  u2 the second point of the segemnt
 * @param  Gk 4 control poitns describing the spline
 * @return    the length of the segment.
 */
float gaussQuad3(float u1, float u2, const Eigen::MatrixXf & Gk, const Eigen::Matrix4f & B){
    float w[] = {5/9.0,8/9.0,5/9.0};
    float x[] = {-sqrtf(3/5.0),0,sqrtf(3/5.0)};
    float pointSummation = 0;
    for(int i = 0; i < 3; i++){
        float u = (u2-u1)/2.0 * x[i] + (u1 + u2)/2.0;
        Eigen::Vector4f du;
        du << 0,1,2*u,3*u*u;
        pointSummation+= w[i]*(Gk*B*du).norm();
    }
    return pointSummation * (u2-u1)/2.0;
}

//Calculate the fernet frame of a point.
glm::vec3 getNormalVector(const Eigen::Matrix4f & B, const Eigen::Vector3f points[],
    float u){
    Eigen::MatrixXf G(3,4);
    G << points[0](0), points[1](0), points[2](0), points[3](0),
         points[0](1), points[1](1), points[2](1), points[3](1),
         points[0](2), points[1](2), points[2](2), points[3](2);

    Eigen::Vector4f dUVec, d2UVec;
    dUVec << 0.0f, 1.0f, 2*u, 3*u*u;
    dUVec= dUVec.transpose();

    Eigen::Vector3f Tvec = (G*B*dUVec).normalized(); //T Vector
    return glm::vec3(Tvec(0),Tvec(1),Tvec(2));
}

KeySpline::KeySpline():
numSplines(0),
usTableDirty(true)
{
    setSplineType(CATMULL);
    vao.addAttribute(0,vertexBuffer);
};
KeySpline::~KeySpline(){};

void KeySpline::setSplineType(SplineType t){
    switch (t){
        case CATMULL:
            this->B << 0.0f, -1.0f,  2.0f, -1.0f,
                       2.0f,  0.0f, -5.0f,  3.0f,
                       0.0f,  1.0f,  4.0f, -3.0f,
                       0.0f,  0.0f, -1.0f,  1.0f;
            this->B*=0.5;
            break;
        case BSPLINE:
            this->B  << 1.0f, -3.0f,  3.0f, -1.0f,
                        4.0f,  0.0f, -6.0f,  3.0f,
                        1.0f,  3.0f,  3.0f, -3.0f,
                        0.0f,  0.0f,  0.0f,  1.0f;
            this->B /= 6.0f;
            break;
    }
    if(this->type != t){
        recalculateTable(6);
        this->type = t;
    }
}

void KeySpline::addNode(const SplineNode & node){
    nodePos.push_back(node.getPosition());
    nodeRot.push_back(node.getRotation());
    if(nodePos.size() >= 3){
        std::cout << "Increasing splines" << std::endl;
        numSplines++;
    }
    if(numSplines > 0){
           recalculateTable(6);
    }
    updateVBO = true;
}

//Replace with Geometry shader later
void KeySpline::draw(){
    if(updateVBO && numSplines > 0){

        Eigen::MatrixXf G(3,nodePos.size() + 1);
        Eigen::MatrixXf Gk(3,4);
        for(int i = 0; i < nodePos.size(); i++){
            G.block<3,1>(0,i) = nodePos[i];
        }
        G.block<3,1>(0,nodePos.size()) = nodePos.back();
        std::vector<glm::vec3>  positions;
        positions.clear();
        for(int k = 0; k < numSplines; k++){
            Gk = G.block<3,4>(0,k);
            int n = 32;
            for(int i = 0; i < n; i++){
                float u = i/(n-1.0f);
                Eigen::Vector4f uVec(1.0,u,u*u,u*u*u);
                Eigen::Vector3f P=Gk*B*uVec;
                positions.push_back(glm::vec3(P(0),P(1),P(2)));
            }
        }
        vertexBuffer.setData(positions);
        updateVBO = false;
    }
    if (numSplines > 0)
    {
        vao.bind();
        GL_Logger::LogError("Could not bind spline vao");

        glDrawArrays(GL_LINE_STRIP,0,vertexBuffer.getNumVerts());
        GL_Logger::LogError("Could not draw spline");
        vao.unbind();

    }
    //Drawing code here
}
void KeySpline::close(){
    nodePos.push_back(nodePos[0]);
    nodeRot.push_back(nodeRot[0]);
    //n-2,n-1,n,0
    nodePos.push_back(nodePos[1]);
    nodeRot.push_back(nodeRot[1]);
    //n-1,n,0,1
    nodePos.push_back(nodePos[2]);
    nodeRot.push_back(nodeRot[2]);
    //n,0,1,2
    numSplines+=3;
}

Transform KeySpline::transformAt(float s){
    assert(numSplines > 0);

    float kfloat;
    float uu = s;
    float u = std::modf(uu, &kfloat);
    int k = (int)std::floor(kfloat) % numSplines;
    Eigen::MatrixXf GPos(3,4);
    Eigen::MatrixXf GRot(4,4);
    std::vector<Eigen::Vector3f> cappedPos(nodePos);
    cappedPos.push_back(nodePos.back());
    for(int i = 0; i < 4; i++){
        GPos.block<3,1>(0,i) = cappedPos[k+i];
    }
    Eigen::Vector4f uVec(1.0,u,u*u,u*u*u);
    Eigen::Vector3f P= GPos * B * uVec;
    Transform t;
    t.setPosition(glm::vec3(P(0),P(1),P(2)));
    t.lookAlong(getNormalVector(B,&(cappedPos[k]),u));
    return t;
}

float KeySpline::sToU(float s)
{
    float sMod = std::fmod(s,usTable.back().second);
    if(usTableDirty)
    {
        recalculateTable(6);
        usTableDirty = false;
    }
    int i = 0;
    float last = 0;
    float cur = 0;

    while( i < (int)usTable.size() && sMod >= cur){
        i++;
        last = cur;
        cur = usTable[i].second;

    }
    if(i == 0){
        return 0;
    }
    float interpolationConst = (sMod-last)/(cur-last);
    return (1-interpolationConst)*usTable[i-1].first + interpolationConst*usTable[i].first;

}

void KeySpline::recalculateTable(int discretization){
    if(numSplines == 0)
        return;
    usTable.clear();
    int ncps = (int)nodePos.size();
    Eigen::MatrixXf G(3,ncps + 1);
    Eigen::MatrixXf Gk(3,4);

    usTable.push_back(std::make_pair(0.0f,0.0f));
    float s = 0;
    for(int i = 0; i < ncps; ++i) {
            G.block<3,1>(0,i) = nodePos[i];
    }
    //assign last node duplicate to cap the spline
    G.block<3,1>(0,ncps) = nodePos.back();
    ncps++;
    for(int k = 0; k < ncps - 3; ++k) {
        int n = discretization; // table resolution discretization
            // Gk is the 3x4 block starting at column k
        Gk = G.block<3,4>(0,k);
        for(int i = 0; i < n-1; ++i) {
            float u = i / (n - 1.0f);
            float u2 = u+1.0/(n - 1);
                // Compute spline point at u
            float ds = gaussQuad3(u, u2 , Gk, B);
            usTable.push_back(std::make_pair(u2+k,(s+=ds)));
        }
    }

}

int KeySpline::getNumNodes()
{
    return nodePos.size();
}


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2011 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

 //------------------ Author: Catarina Silva  -------------------------------------------------
// ------------------ E-mail: (catsilva20@gmail.com) ------------------------------------------

#include "attitudeintegration.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Astro-Core/EODE/eode.h>
#include <Astro-Core/statevector.h>

using namespace Eigen;

typedef Eigen::Matrix< double, 3, 3 > 	MyMatrix3d;
typedef Eigen::Matrix< double, 3, 1 > 	MyVector3d;
typedef Eigen::Matrix< double, 4, 4 > 	MyMatrix4d;
typedef Eigen::Matrix< double, 4, 1 > 	MyVector4d;

//Integrates the Euler equation and the quaternion rates equation.
//Inputs: initial body rates, inertia matrix and moments (now = 0)
//The propagation will use the Runge Kutta 4 integrator
//Outputs: final euler angles or quaternions

// WRITE THE REFERENCE OF THE FORMULAS

/**
  * Derivative of the Euler equation
  *
  * @param bodyRates        initial body rates
  * @param inertiaMatrix    inertia matrix
  *
  * @return derivBodyRates  body rates derivatives
  */
void derivEulerEquation (VectorXd bodyRates, double time, VectorXd inertiaANDmoments, VectorXd& derivBodyRates)
{
    double p = bodyRates[0];
    double q = bodyRates[1];
    double r = bodyRates[2];

    static double bodyCoeffs[9]={
        0,  -r,   q,
        r,   0,  -p,
       -q,   p,   0
    };
    static const Matrix3d bodyMatrix(bodyCoeffs);

    //the input of derivEulerEquation needs to be a VectorXd (in this case, 12elements vector).
    //This vector will include the components of the INERTIA MATRIX (the first 9 elements), and
    //the components of the external MOMENTS applied to the vehicle (last 3 elements)
    double I11 = inertiaANDmoments[0];
    double I12 = inertiaANDmoments[1];
    double I13 = inertiaANDmoments[2];
    double I21 = inertiaANDmoments[3];
    double I22 = inertiaANDmoments[4];
    double I23 = inertiaANDmoments[5];
    double I31 = inertiaANDmoments[6];
    double I32 = inertiaANDmoments[7];
    double I33 = inertiaANDmoments[8];

    //External Moments
    double M1 = inertiaANDmoments[9];
    double M2 = inertiaANDmoments[10];
    double M3 = inertiaANDmoments[11];

    Vector3d Moments(M1,M2,M3);

    static double inertiaCoeffs[9] = {
        I11, I12, I13,
        I21, I22, I23,
        I31, I32, I33

    };
    static const Matrix3d inertiaMatrix(inertiaCoeffs);

    //Euler's equation
    Matrix3d multipInertia= inertiaMatrix * inertiaMatrix.transpose();
    Matrix3d inertiaTimesBodyMatrix = bodyMatrix * multipInertia;

    derivBodyRates = Moments - inertiaTimesBodyMatrix * bodyRates;
}

/**
  * Propagate the Euler equation
  *
  * @param initbodyRates    initial body rates
  * @param time
  * @param timeStep
  * @param inertiaMatrix
  *
  * @return body rates      final body rates
  */
Vector3d propagateEulerEquation(const Vector3d& initbodyRates,
                                double time,
                                double timeStep,
                                const Matrix3d& inertiaMatrix)
{
    //Convert Vector3d to VectorSd so it can be fed to RK4
    VectorXd theBodyRates;
    theBodyRates << initbodyRates[0], initbodyRates[1],initbodyRates[2];

    //Convert the matrix3d of the inertia matrix to a vectorXd, so it can be an input in the intregator
    VectorXd inertiaMatrix_Vector;

    inertiaMatrix_Vector[0] = inertiaMatrix(0,0);
    inertiaMatrix_Vector[1] = inertiaMatrix(0,1);
    inertiaMatrix_Vector[2] = inertiaMatrix(0,2);
    inertiaMatrix_Vector[3] = inertiaMatrix(1,0);
    inertiaMatrix_Vector[4] = inertiaMatrix(1,1);
    inertiaMatrix_Vector[5] = inertiaMatrix(1,2);
    inertiaMatrix_Vector[6] = inertiaMatrix(2,0);
    inertiaMatrix_Vector[7] = inertiaMatrix(2,1);
    inertiaMatrix_Vector[8] = inertiaMatrix(2,2);

    //For now, a Runge Kutta 4 will be used. Later, this can be changed to a better integrator (Runge Kutta 7,8 ex.)
    rk4(theBodyRates, 3, time, timeStep, derivEulerEquation, inertiaMatrix_Vector);

    //Returns the body rates, after integration
    Vector3d integratedBodyRates;
    integratedBodyRates[0] = theBodyRates[0];
    integratedBodyRates[1] = theBodyRates[1];
    integratedBodyRates[2] = theBodyRates[2];
    return integratedBodyRates;
}

/**
  * This function calculates the quaternions rates
  *
  * @param initQuaternions  initial quaternions
  * @param parameters       body rates
  * @param time
  *
  * @return derivative      quaternions derivatives
  */

//This function calculates the derivatives of the quaternions
void derivQUATERNIONS (VectorXd initQuaternions, double time, VectorXd parameters, VectorXd& derivative)
{
    double p = parameters[0];
    double q = parameters[1];
    double r = parameters[2];

    static double bodyCoeffs[16]= {
        0, -p, -q, -r,
        p, 0, r, -q,
        q, -r, 0, p,
        r, q, -p, 0
    };
    static const MyMatrix4d bodyRatesMatrix(bodyCoeffs);
    derivative = bodyRatesMatrix * initQuaternions;
}

/**
  * This function integrates the quaternions rates
  *
  * @param quaternions  initial quaternions
  * @param time         time
  * @param timeStep     time step of the integration
  * @param bodyrates
  *
  * @return quats       final quaternions
  */

Quaterniond propagateQUATERNIONS(Quaterniond& quaternions,
                                 double time,
                                 double timeStep,
                                 Vector3d bodyrates)
{

    VectorXd parameters(3);
    parameters << bodyrates[0], bodyrates[1], bodyrates[2];

    VectorXd quats(4);
    quats << quaternions.coeffs().coeffRef(0), quaternions.coeffs().coeffRef(1),
    quaternions.coeffs().coeffRef(2), quaternions.coeffs().coeffRef(3);

    rk4 (quats, 4, time, timeStep, derivQUATERNIONS, parameters);

    //Transform VectorXd to quaternions
    Quaterniond propagatedQuaternion(quats(0),quats(1), quats(2),quats(3));

    return propagatedQuaternion;
}


#include <ros/ros.h>
#include "ros_utils.hpp"
#include "kinematics.hpp"
#include "estimators.hpp"
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include <Eigen/Dense>
#include <unistd.h>
#include <math.h>

#include <gtsam/config.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/inference/LabeledSymbol.h>

#include <gtsam_node/ArticulatedAngles.h>
#include <gtsam_node/CanData.h>
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/Twist.h"
#include "ackermann_msgs/AckermannDrive.h"

#include <visualization_msgs/Marker.h>
#include <cmath>

using namespace std;
using namespace gtsam;

class CircleFactor: public NoiseModelFactor1<Vector5> {

  Vector3 circle_;

 public:

  typedef boost::shared_ptr<CircleFactor> shared_ptr;

  CircleFactor(Key j, Vector3 circle, const SharedNoiseModel& model):
    NoiseModelFactor1<Vector5>(model, j), circle_(circle) {}

  virtual ~CircleFactor() {}

  Vector evaluateError(const Vector5& po,
                       boost::optional<Matrix&> H = boost::none) const override {

    /*
    double err = 1/(pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2));

    double ctrval = 0.05;
    double jX = (ctrval*(2*po[0] - 2*circle_[0]))/pow(pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2),2);
    double jY = (ctrval*(2*po[1] - 2*circle_[1]))/pow(pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2),2);

    if (H) (*H) = (Matrix(1, 5) << jX, jY, 0.0, 0.0, 0.0).finished();
    return (Vector(1) << err*ctrval).finished();
    /**/
    //
    double ctrval = 0.05;
    double err = (1/(pow(2,(ctrval*((pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2))/pow(circle_[2],2))))))-1;

    double jX = (-0.6931471*ctrval*(po[0]-circle_[0])*pow(2,(1-(ctrval*((pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2))/pow(circle_[2],2)) ))))/pow(circle_[2],2);
    double jY = (-0.6931471*ctrval*(po[1]-circle_[1])*pow(2,(1-(ctrval*((pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2))/pow(circle_[2],2)) ))))/pow(circle_[2],2);

    if (H) (*H) = (Matrix(1, 5) << jX, jY, 0.0, 0.0, 0.0).finished();
    //ROS_INFO_STREAM("errY: " << err << "");
    //ROS_INFO_STREAM("errYJ: " << jY << "");
    return (Vector(1) << err).finished();
    /**/
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new CircleFactor(*this))); }

};  // CircleFactor

class PolyPointFactor: public NoiseModelFactor2<Vector8,Vector5> {

  private:
    using This = PolyPointFactor;
    using Base = gtsam::NoiseModelFactor2<Vector8, Vector5>;

    Vector8 mpoly_;
    Vector5 mpoint_;
    gtsam::Key j_;
  public:

    typedef boost::shared_ptr<PolyPointFactor> shared_ptr;

    PolyPointFactor(Key i, Key j, Vector8 mpoly, Vector5 mpoint, const SharedNoiseModel& model)
        : Base(model, i, j), mpoly_(mpoly), mpoint_(mpoint),j_(j) {}

    virtual ~PolyPointFactor()
    {}

    double getPoly(const double& x, const Vector8& p8) const
    {
        double y;
        y = (p8[0]*pow(x,7))+(p8[1]*pow(x,6))+(p8[2]*pow(x,5))+(p8[3]*pow(x,4))+(p8[4]*pow(x,3))+(p8[5]*pow(x,2))+(p8[6]*x)+p8[7];

        return y; // order p0 x^7 -> p7 x^1
    }

    double diffPoly(const double& x, const Vector8& p8) const
    {
        double y;
        y = (7*p8[0]*pow(x,6))+(6*p8[1]*pow(x,5))+(5*p8[2]*pow(x,4))+(4*p8[3]*pow(x,3))+(3*p8[4]*pow(x,2))+(2*p8[5]*x)+p8[6];

        return y; // order p0 x^7 -> p7 x^1
    }

    double diff2Poly(const double& x, const Vector8& p8) const
    {
        double y;
        y = (42*p8[0]*pow(x,5))+(30*p8[1]*pow(x,4))+(20*p8[2]*pow(x,3))+(12*p8[3]*pow(x,2))+(6*p8[4]*x)+(2*p8[5]);

        return y; // order p0 x^7 -> p7 x^1
    }

     Vector evaluateError(const Vector8& p8, const Vector5& p5,
        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override
    {
        //PrintKey(j_);
        gtsam::Vector err(13);
        err = Vector::Zero(13);
        err[9] =  p5[1] - getPoly(p5[0],p8);
        //ROS_INFO_STREAM("errY: " << err[9] << "");

        double errYx = diffPoly(p5[0],p8);
        double errY2x = diff2Poly(p5[0],p8);
        double curve = abs(errY2x)/pow((pow(errYx,2)+1),1.5);
        double ctrVal = 1;
        double curveMax = 0.1;
        //err[11] =  1/(pow(2,ctrVal*(curveMax - curve)));
        err[9] = err[9] + 1/(pow(2,ctrVal*(curveMax - curve)));
        ROS_INFO_STREAM("Curve: " << curve << "");
        //ROS_INFO_STREAM("errCurve: " << err[11] << "");

        //curve = (abs(42*p0*x^5+30*p1*x^4+20*p2*x^3+12*p3*x^2+6*p4*x+2*p5)/(1+(7*p0*x^6+6*p1*x^5+5p2*x^4+4*p3*x^3+3*p4*x^2+2*p5*x+p6)^2)^(3/2))

        //errCurve = 1/2^(a*(m-(((abs(42*p0*x^5+30*p1*x^4+20*p2*x^3+12*p3*x^2+6*p4*x+2*p5)/(1+(7*p0*x^6+6*p1*x^5+5p2*x^4+4*p3*x^3+3*p4*x^2+2*p5*x+p6)^2)^(3/2))))

/**/
        gtsam::Vector crvJp(7);
        crvJp = Vector::Zero(7);
        double lnCtr = -0.6931471*ctrVal;
        double deno = (pow(2,ctrVal*(curveMax - curve)));
        double eqp1 = (errYx* abs(errY2x))/(pow( (pow(errYx,2)+1),2.5));
        double eqp2 = pow( (pow(errYx,2)+1) ,1.5) *abs(errY2x);

        crvJp[0] = (lnCtr*(( (21* pow(p5[0],6)* eqp1)  - ((42*pow(p5[0],5) *  errY2x)  / eqp2))))   / deno;

        crvJp[1] = (lnCtr*(( (18* pow(p5[0],5)* eqp1)  - ((30*pow(p5[0],4) *  errY2x)  / eqp2))))   / deno;

        crvJp[2] = (lnCtr*(( (15* pow(p5[0],4)* eqp1)  - ((20*pow(p5[0],3) *  errY2x)  / eqp2))))   / deno;

        crvJp[3] = (lnCtr*(( (12* pow(p5[0],3)* eqp1)  - ((12*pow(p5[0],2) *  errY2x)  / eqp2))))   / deno;

        crvJp[4] = (lnCtr*(( (9*  pow(p5[0],2)* eqp1)  - ((6*      p5[0]   *  errY2x)  / eqp2))))   / deno;

        crvJp[5] = (lnCtr*(( (6*      p5[0]*    eqp1)  - ((2*                 errY2x)  / eqp2))))   / deno;

        crvJp[6] = (lnCtr*   3*     errYx* abs(errY2x)) / (pow( (pow(errYx,2)+1),2.5)*deno);
        /*
        if (isnan(crvJp[0])) {crvJp[0] = 0.0;}
        if (isnan(crvJp[1])) {crvJp[1] = 0.0;}
        if (isnan(crvJp[2])) {crvJp[2] = 0.0;}
        if (isnan(crvJp[3])) {crvJp[3] = 0.0;}
        if (isnan(crvJp[4])) {crvJp[4] = 0.0;}
        if (isnan(crvJp[5])) {crvJp[5] = 0.0;}
        if (isnan(crvJp[6])) {crvJp[6] = 0.0;}

        /*
        ROS_INFO_STREAM("crvJp6: " << crvJp[6] << "");
        ROS_INFO_STREAM("crvJp5: " << crvJp[5] << "");
        ROS_INFO_STREAM("crvJp4: " << crvJp[4] << "");
        ROS_INFO_STREAM("crvJp3: " << crvJp[3] << "");
        ROS_INFO_STREAM("crvJp2: " << crvJp[2] << "");
        ROS_INFO_STREAM("crvJp1: " << crvJp[1] << "");
        ROS_INFO_STREAM("crvJp0: " << crvJp[0] << "");
        */

        if (H1) (*H1) = (Matrix(13, 8) <<   0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            -pow(p5[0],7)+crvJp[0], -pow(p5[0],6)+crvJp[1], -pow(p5[0],5)+crvJp[2], -pow(p5[0],4)+crvJp[3], -pow(p5[0],3)+crvJp[4], -pow(p5[0],2)+crvJp[5], -p5[0]+crvJp[6], -1.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished();
          //crvJp0, crvJp1, crvJp2, crvJp3, crvJp4, crvJp5, crvJp6, 0.0,
          //crvJp[0], crvJp[1], crvJp[2], crvJp[3], crvJp[4], crvJp[5], crvJp[6], 0.0,
        //(-1)*errYx
        //double kdot = (0.6931471*ctrVal)/(pow(2,ctrVal*( curveMax - curve)));
        //-pow(p5[0],7), -pow(p5[0],6), -pow(p5[0],5), -pow(p5[0],4), -pow(p5[0],3), -pow(p5[0],2), -p5[0], -1.0,
        if (H2) (*H2) = (Matrix(13, 5) <<   0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 1.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 1.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 1.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 1.0).finished();
        return err;
    }

    gtsam::NonlinearFactor::shared_ptr clone() const override {
      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new PolyPointFactor(*this))); }

};  // PolyPointFactor

class ppFactor: public NoiseModelFactor2<Vector5,Vector5> {

  private:
    using This = ppFactor;
    using Base = gtsam::NoiseModelFactor2<Vector5, Vector5>;
  public:
    typedef boost::shared_ptr<ppFactor> shared_ptr;
    ppFactor(Key i, Key j, Vector5 pointA, Vector5 pointB, const SharedNoiseModel& model)
        : Base(model, i, j) {}

    virtual ~ppFactor()
    {}
     Vector evaluateError(const Vector5& pA, const Vector5& pB,
        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override
    {

        gtsam::Vector err(10);
        err = Vector::Zero(10);
        double dis = 0.1;
        err[0] =  pA[0] - pB[0] + dis ;
        err[5] =  pB[0] - pA[0] - dis ;

        if (H1) (*H1) = (Matrix(10, 5) <<   1.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            -1.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0).finished();

        if (H2) (*H2) = (Matrix(10, 5) <<   -1.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            1.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0).finished();
        return err;
    }

    gtsam::NonlinearFactor::shared_ptr clone() const override {
      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new ppFactor(*this))); }

};  // point<->point Factor

double getPolyY(const double& x, const Vector8& p8)  //only for illustration purposes
{
    double y;
    y = (p8[0]*pow(x,7))+(p8[1]*pow(x,6))+(p8[2]*pow(x,5))+(p8[3]*pow(x,4))+(p8[4]*pow(x,3))+(p8[5]*pow(x,2))+(p8[6]*x)+ p8[7];
    return y; // order x^7 -> x^0
}

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

    using namespace gtsam_node;
    using namespace ast;
    using namespace ast::ros;
    ::ros::init(argc, argv, "gtsam_node");
    NodeHandle nh;

    /*--------------------------------------------| Get Params |---------------------------------------------*/
    CarParams carParams;
    MheParams mheParams;
    ParamsIn(carParams, mheParams, nh); //TODO: change mhe to graph (just old name )
    /*-------------------------------------------------------------------------------------------------------*/

    /*-----------------------------------------------| Rviz |-------------------------------------------------*/
    ::ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("GTSAM_points", 10);
    /*--------------------------------------------------------------------------------------------------------*/

    ROS_INFO_STREAM("Estimator loop rate: "<< mheParams.loopRate <<"");

    sleep(2);
    ::ros::Rate loop_rate(mheParams.loopRate);
    while(::ros::ok())
    {
        auto now = ::ros::Time::now();
        /*---------------------------------| Rviz initialization |--------------------------------------------*/
        visualization_msgs::Marker points, pointsInit, PointOpt, line_strip, circleObst;
        circleObst.header.frame_id = PointOpt.header.frame_id = pointsInit.header.frame_id = points.header.frame_id = line_strip.header.frame_id = "/gtsam_frame";
        circleObst.header.stamp = points.header.stamp = line_strip.header.stamp = PointOpt.header.stamp = pointsInit.header.stamp = now;
        circleObst.ns = pointsInit.ns = PointOpt.ns = points.ns = line_strip.ns = "points_and_lines";
        circleObst.action = pointsInit.action = PointOpt.action = points.action = line_strip.action = visualization_msgs::Marker::ADD;
        circleObst.pose.orientation.w = pointsInit.pose.orientation.w = PointOpt.pose.orientation.w = points.pose.orientation.w = line_strip.pose.orientation.w = 1.0;

        points.id = 0; line_strip.id = 1; pointsInit.id = 2; PointOpt.id = 3; circleObst.id = 4;
        points.type = visualization_msgs::Marker::POINTS;
        line_strip.type = visualization_msgs::Marker::LINE_STRIP;
        pointsInit.type = visualization_msgs::Marker::POINTS;
        PointOpt.type = visualization_msgs::Marker::POINTS;
        circleObst.type = visualization_msgs::Marker::LINE_STRIP;
        // POINTS markers use x and y scale for width/height respectively
        points.scale.x = 0.2; points.scale.y = 0.2;  points.scale.z = 0.0;
        pointsInit.scale.x = 0.05; pointsInit.scale.y = 0.05; pointsInit.scale.z = 0.0;
        PointOpt.scale.x = 0.05; PointOpt.scale.y = 0.05; PointOpt.scale.z = 0.0;
        points.color.g = 1.0; points.color.a = 1.0;
        line_strip.color.b = 1.0; line_strip.color.a = 1.0;
        line_strip.scale.x = 0.02; line_strip.scale.y = 0.02; line_strip.scale.z = 0.0;
        circleObst.scale.x = 0.01;circleObst.scale.y = 0.01;circleObst.scale.z = 0.0; circleObst.color.a = 1.0;
        circleObst.color.b = 1.0; circleObst.color.g = 1.0;
        pointsInit.color.r = 1.0; pointsInit.color.a = 1.0;
        PointOpt.color.r = 1.0; PointOpt.color.b = 1.0; PointOpt.color.a = 1.0;
        /*--------------------------------------------------------------------------------------------------------*/

        NonlinearFactorGraph graph;
        /*---------------------------------------| polynomal node |-----------------------------------------------*/
        gtsam::Vector polyPriorSigmas(8);
        polyPriorSigmas << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ;
        auto priorPolyNoise = gtsam::noiseModel::Diagonal::Sigmas(polyPriorSigmas);
        gtsam::Vector polyParams(8);
        polyParams << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ; // order x^7 -> x^0
        //polyParams << 0.001, -0.036, 0.294, -0.808, -0.505, 4.54, -2.492, 1.500; // order x^7 -> x^0
        //graph.addPrior(Symbol('l', 0), polyParams, priorPolyNoise);
        /*--------------------------------------------------------------------------------------------------------*/

        /*--------------------------------| polynomal<->point factor |--------------------------------------------*/
        gtsam::Vector polyPointSigmas(13);
        polyPointSigmas << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ;
        auto polyPointNoise = gtsam::noiseModel::Diagonal::Sigmas(polyPointSigmas);

        gtsam::Vector po1(5), po2(5), poj(5) ;
        po1 << -3.0, 3.0, 0.0, 0.5, 0.0;
        poj << 0.0, 0.0, 0.0, 0.5, 0.0;
        po2 << 3.0, -3.0, 0.0, 0.5, 0.0;
        gtsam::Vector poPriorSigmas(5);
        poPriorSigmas << 0.0, 0.0, 0.0, 0.0, 0.0;
        auto priorPoNoise = gtsam::noiseModel::Diagonal::Sigmas(poPriorSigmas);
        graph.addPrior(Symbol('p', 0), po1, priorPoNoise);   //add prior on first point
        graph.addPrior(Symbol('p', 59), po2, priorPoNoise);   //add prior on first point
        geometry_msgs::Point pois;
        pois.x = po1[0];
        pois.y = po1[1];
        points.points.push_back(pois);
        pois.x = po2[0];
        pois.y = po2[1];
        points.points.push_back(pois);

        std::vector<gtsam::Vector5> poses;
        for(size_t j = 0; j < 60; ++j) // create vector of poses
        {
          poses.push_back(poj);
        }

        for(size_t j = 0; j < poses.size(); ++j) //add polynoaml<->point factors to the graph
        {
          graph.push_back(boost::make_shared<PolyPointFactor>(Symbol('l', 0), Symbol('p', j), polyParams, poses[j], polyPointNoise));
          ROS_INFO_STREAM("key: "<< j <<"");
        }
        /*--------------------------------------------------------------------------------------------------------*/

        /*-----------------------------------| point<->point factor |---------------------------------------------*/
        gtsam::Vector ppSigmas(10);
        ppSigmas << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        auto ppNoise = gtsam::noiseModel::Diagonal::Sigmas(ppSigmas);

        for (size_t j = 0; j < (poses.size()-1); ++j)   //add distance factor in x direction between each neighbouring point
        {
          graph.push_back(boost::make_shared<ppFactor>(Symbol('p', j), Symbol('p', j+1), poses[j], poses[j+1], ppNoise));
        }
        /*--------------------------------------------------------------------------------------------------------*/

        /*--------------------------------------| obstacle  factor |----------------------------------------------*/
        auto circleNoise = noiseModel::Diagonal::Sigmas(Vector1(0.0));  // 10cm std on x,y
        gtsam::Vector circle0(3),circle1(3); //x, y, r
        circle0 <<  0.5, 2.6, 0.5;
        circle1 << 0.2, 0.0, 0.6;

        for(size_t j = 0; j < poses.size(); ++j)
        {
          graph.push_back(boost::make_shared<CircleFactor>(Symbol('p', j), circle0, circleNoise));
          graph.push_back(boost::make_shared<CircleFactor>(Symbol('p', j), circle1, circleNoise));
        }
        /*--------------------------------------------------------------------------------------------------------*/
        ROS_INFO_STREAM("/////////////////////////////////////////////////////////");
        //graph.print("\nFactor Graph:\n");  // cout the graph

        /*----------------------------| graph optimization initialization |----------------------------------------*/
        Values initialEstimate;

        gtsam::Vector po0init(8);
        //po0init << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        po0init << 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0;
        initialEstimate.insert(Symbol('l', 0), po0init);

        geometry_msgs::Point poisInit;
        gtsam::Vector po1init(5);
        po1init << -3.1, 3.1, 0.0, 0.5, 0.0;
        poisInit.x = po1init[0];
        poisInit.y = po1init[1];
        pointsInit.points.push_back(poisInit);

        for (size_t j = 0; j < poses.size(); ++j)   //points.size()
        {
          initialEstimate.insert(Symbol('p', j), po1init);
        }
        //initialEstimate.print("\nInitial Estimate:\n");  // print
        /*--------------------------------------------------------------------------------------------------------*/

        /*-------------------------------------| optimize the graph  |---------------------------------------------*/
        LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
        Values result = optimizer.optimize();
        //result.print("Final Result:\n");
        geometry_msgs::Point poisOpt;

        Vector8 polyUpdate = result.at<Vector8>(Symbol('l', 0));
        for (size_t j = 0; j < poses.size(); ++j)   //points.size()
        {
          Vector5 x_update = result.at<Vector5>(Symbol('p', j));
          poisOpt.x = x_update[0];
          poisOpt.y = x_update[1];
          PointOpt.points.push_back(poisOpt);
        }

        Marginals marginals(graph, result);
        /*
        cout << "polynomial covariance:\n" << marginals.marginalCovariance(Symbol('l', 0)) << endl;
        cout << "p1 covariance:\n" << marginals.marginalCovariance(Symbol('p', 0)) << endl;
        cout << "p2 covariance:\n" << marginals.marginalCovariance(Symbol('p', 1)) << endl;
        cout << "p3 covariance:\n" << marginals.marginalCovariance(Symbol('p', 2)) << endl;
        cout << "p4 covariance:\n" << marginals.marginalCovariance(Symbol('p', 3)) << endl;
        cout << "p5 covariance:\n" << marginals.marginalCovariance(Symbol('p', 4)) << endl;
        /**/
        /*--------------------------------------------------------------------------------------------------------*/

        /*------------------------------------| graph result to rviz  |-------------------------------------------*/

        auto circle1bst = circleObst;
        circle1bst.id = 5;
        for (double i = 0; i < 100; ++i)
        {
        geometry_msgs::Point p;
        p.x = ((double)i/10) - 5;
        p.y = getPolyY(p.x, polyUpdate);
        p.z = 0;
        line_strip.points.push_back(p);
        }
        for (double i = 0; i < (2 * M_PI); i = i + (M_PI/50))
        {
        geometry_msgs::Point p;
        p.x = circle0[2] * cos(i) + circle0[0];
        p.y = circle0[2] * sin(i) + circle0[1];
        p.z = 0;
        circleObst.points.push_back(p);
        }

        for (double i = 0; i < (2 * M_PI); i = i + (M_PI/50))
        {
        geometry_msgs::Point p;
        p.x = circle1[2] * cos(i) + circle1[0];
        p.y = circle1[2] * sin(i) + circle1[1];
        p.z = 0;
        circle1bst.points.push_back(p);
        }

        marker_pub.publish(circleObst);
        marker_pub.publish(circle1bst);
        marker_pub.publish(line_strip);
        marker_pub.publish(points);
        //marker_pub.publish(pointsInit);
        marker_pub.publish(PointOpt);

        /*--------------------------------------------------------------------------------------------------------*/
        auto loopTime =  ::ros::Time::now() - now;
        ROS_INFO_STREAM("loop time" << loopTime.toSec());

        ::ros::spinOnce();
        loop_rate.sleep();

    }

    return 0;
}

/*
gtsam::Vector crvJp(7);
        crvJp = Vector::Zero(7);
        crvJp[0] = (-0.6931471*ctrVal*(((21* pow(p5[0],6)* errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((42*pow(p5[0],5) *  pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[0])) {crvJp[0] = 0.0;}
        crvJp[1] = (-0.6931471*ctrVal*(((18* pow(p5[0],5)* errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((30*pow(p5[0],4) *  pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[1])) {crvJp[1] = 0.0;}
        crvJp[2] = (-0.6931471*ctrVal*(((15* pow(p5[0],4)* errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((20*pow(p5[0],3) *  pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[2])) {crvJp[2] = 0.0;}
        crvJp[3] = (-0.6931471*ctrVal*(((12* pow(p5[0],3)* errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((12*pow(p5[0],2) *  pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[3])) {crvJp[3] = 0.0;}
        crvJp[4] = (-0.6931471*ctrVal*(((9*  pow(p5[0],2)* errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((6*      p5[0]   *  pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[4])) {crvJp[4] = 0.0;}
        crvJp[5] = (-0.6931471*ctrVal*(((6*      p5[0]*    errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((2*                 pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[5])) {crvJp[5] = 0.0;}
        crvJp[6] = (-0.6931471*ctrVal*   3*                errYx*  sqrt( pow(errYx,2)  +1))                                                                                /(abs(errY2x)*pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[6])) {crvJp[6] = 0.0;}
        */


#include <iostream>
#define ARMA_DONT_USE_WRAPPER
#include <armadillo>
#include <cmath>

using namespace std;
using namespace arma;

// g++ mat2.cpp -o mat2 -std=c++11 -O2 -larmadillo -framework Accelerate

int
main(int argc, char** argv)
  {
      float pi=M_PI;
      float eu=std::exp(1.0);
      float raiz= sqrt(2);
      float raiz1= pow(pi,0.3333);
      float coseno = cos(pi/4);
      float cdiag = pow(7,0.3333);

      mat A(10,10, fill::zeros); //A(fila, columna)
      cout << "A.n_rows: " << A.n_rows << endl;
      cout << "A.n_cols: " << A.n_cols << endl;
      A.rows(0,4) += pi;
      A.rows(5,9) += eu;
      A.save("A1.txt", raw_ascii);

      mat B(10,10, fill::zeros);
      cout << "B.n_rows: " << B.n_rows << endl;
      cout << "B.n_cols: " << B.n_cols << endl;
      B.col(0) += raiz;
      B.col(2) += raiz;
      B.col(4) += raiz;
      B.col(8) += raiz;
      B.col(1) += raiz1;
      B.col(6) += raiz1;
      B.col(7) += raiz1;
      B.col(3) += coseno;
      B.col(5) += coseno;
      B.col(9) += coseno;
      B.save("B1.txt", raw_ascii);

      mat C = repelem(A, 1, 1);
      C.diag() += cdiag;
      C.save("C1.txt", raw_ascii);

      mat D(10,10, fill::randu);
      mat E(10,10, fill::zeros );

      E += pow((A.t()+B),-1 ) + (pow(A,-1)*pow(B,0.3333)*pow(D,0.2)) - (pow(D,-1)*B.t()*(A.t()*pow(C,-1))) + D*(cos(C)*sin(A)+pow(sin(B),2)) ;
      E.save("E1.txt", raw_ascii);

      mat F(10,10, fill::zeros);

      F += exp(C)*D + exp(A)*sin(A) + (sqrt(pow(pi, 3))/exp(pi))*cos(D) - log10(sqrt(pi/8))*pow(cos(B),4) + pow(sin(D.t()*pow(C,3)*A),-1)+ E*(cos(D)+sin(D));
      F.save("F1.txt", raw_ascii);

      cout << "C.n_rows: " << C.n_rows << endl;
      cout << "C.n_cols: " << C.n_cols << endl;
      cout << "D.n_rows: " << D.n_rows << endl;
      cout << "D.n_cols: " << D.n_cols << endl;
      cout << "E.n_rows: " << E.n_rows << endl;
      cout << "E.n_cols: " << E.n_cols << endl;
      cout << "F.n_rows: " << F.n_rows << endl;
      cout << "F.n_cols: " << F.n_cols << endl;

      return 0;

  }


#pragma once

#include <list>
#include <vector>
#include <NTL/ZZ.h>
#include <math/mpz_class.hh>
#include <math.h>
#include <algorithm>    // std::min
#include <float.h>
#include <limits.h>
using namespace std;
struct encnum
{
	int  exponent;
	mpz_class mantissa;
	//double plaintext;
};
class Paillier {
 public:
    Paillier(const std::vector<mpz_class> &pk, gmp_randstate_t state);
    std::vector<mpz_class> pubkey() const { return { n, g }; }
    encnum encrypt_f(double plaintext);
    std::vector  <std::vector<encnum>> encryptMatrix(const std::vector < std::vector<double> > &p);
    encnum encode( double plaintext) const;

    encnum sub_f(const encnum &a, const encnum &b) const;
    encnum add_f(const encnum &a, const encnum &b) const;
    std::vector  <std::vector<encnum>> addMatrix(const std::vector < std::vector<encnum> > &c0, const std::vector < std::vector<encnum> > &c1) ;
    std::vector  <std::vector<encnum>> multConstMatrixbyMatrix(const std::vector < std::vector<double> > &c, const std::vector < std::vector<encnum> > &mat) ;
    std::vector  <std::vector<encnum>> multMatrixbyConstMatrix(const std::vector < std::vector<encnum> > &mat , const std::vector < std::vector<double> > &c) ;

    encnum decrease_exponent_too(const encnum &y,const int new_exp) const;
    encnum constMult_f(const double &m, const encnum &c) const;
	vector < vector<encnum> > multMatrixbyConst( double alpha,const vector < vector<encnum> > &mat) ;

    mpz_class encrypt(const mpz_class &plaintext);
    mpz_class add(const mpz_class &c0, const mpz_class &c1) const;
    mpz_class sub(const mpz_class &c0, const mpz_class &c1) const;
    mpz_class constMult(const mpz_class &m, const mpz_class &c) const;
    mpz_class constMult(long m, const mpz_class &c) const;
    mpz_class constMult(const mpz_class &c, long m) const { return constMult(m,c); };
    mpz_class scalarize(const mpz_class &c);
    void refresh(mpz_class &c);
    mpz_class random_encryption();

    mpz_class dot_product(const std::vector<mpz_class> &c, const std::vector<mpz_class> &v);
    mpz_class dot_product(const std::vector<mpz_class> &c, const std::vector<long> &v);
    void rand_gen(size_t niter = 100, size_t nmax = 1000);
    double logbase(double base, double x) const;

    void printMatrix(std::vector  <std::vector<double>> &Mat);

    const double precision = 0.00000001 ; //0.00000001 0.001
    bool acc_per = false;
    int max_exponent = 0;
	//int exponent;
	//int int_rep;
    int BASE = 10;
    double LOG2_BASE = log2(BASE);
    double FLOAT_MANTISSA_BITS = LDBL_MANT_DIG;
    encnum zero_enc;
   // double max_int = n.get_d()/3 -1;
    //double nsquare = n.get_d() * n.get_d() ;
 protected:
    /* Public key */
    const mpz_class n, g;

    /* Randomness state */
    gmp_randstate_t _randstate;

    /* Cached values */
    const uint nbits;
    const mpz_class n2;
    bool good_generator;

    /* Pre-computed randomness */
    std::list<mpz_class> rqueue;
};

class Paillier_priv : public Paillier {
 public:
    Paillier_priv(const std::vector<mpz_class> &sk, gmp_randstate_t state);
    std::vector<mpz_class> privkey() const { return { p, q, g, a }; }
    void find_crt_factors();

    // if a !=0, and if you are encrypting using the private key, use this function
    // 75% speedup
    mpz_class encrypt(const mpz_class &plaintext);

    // no speedup compared to the fast_encrypt
    mpz_class fast_encrypt_precompute(const mpz_class &plaintext);

    mpz_class decrypt(const mpz_class &ciphertext) const;
    static std::vector<mpz_class> keygen(gmp_randstate_t state, uint nbits = 1024, uint abits = 256);
    double decrypt_f(encnum x) const;
    std::vector< std::vector<double> > decryptMatrix(const std::vector < std::vector<encnum> > &c);

 protected:
    /* Private key, including g from public part; n=pq */
    const mpz_class p, q;
    const mpz_class a;      /* non-zero for fast mode */

    /* Cached values */
    const bool fast;
    const mpz_class p2, q2;
    mpz_class e_p2, e_q2;
    const mpz_class two_p, two_q;
    const mpz_class pinv, qinv;
    const mpz_class hp, hq;
};

class Paillier_priv_fast : public Paillier_priv {
public:
    Paillier_priv_fast(const std::vector<mpz_class> &sk, gmp_randstate_t state);
    void precompute_powers();
    mpz_class compute_g_star_power(const mpz_class &x);
    static std::vector<mpz_class> keygen(gmp_randstate_t state, uint nbits = 1024);

    mpz_class encrypt(const mpz_class &plaintext);
private:
    const mpz_class g_star_;
    const mpz_class phi_n;
    const mpz_class phi_n2;
    const uint phi_n2_bits;
    std::vector<mpz_class> g_star_powers_p_;
    std::vector<mpz_class> g_star_powers_q_;
};


/*
 * ex1x.cpp
 *
 *  Created on: 25.04.2017
 *
 *  Author: Daniel Rehfeldt
 *
 *  Compile: g++ ex1x.cpp -o ex1x -O3 -std=c++14 -Wall
 */

#include <vector>
#include <cmath>
#include <limits>
#include <iostream>
#include <fstream>
#include <cassert>
#include <boost/spirit/include/qi.hpp>

int main(int argc, char* argv[])
{
    // check number of parameters
    if( argc != 2 )
    {
       // tell the user how to run the program
       std::cerr << "Usage: " << argv[0] << " filename" << std::endl;
       exit(EXIT_FAILURE);
    }

    /* start of definitions for geometric mean */

    std::vector<double> valvec[2];
    double geomean[2] {0.0, 0.0};
    int geomeancount[2] {0, 0};

    // get number of multiplications in geo mean computation guaranteed to be without underflow; unfortunately, constexpr is not supported here by c++14
    const int nmults = std::log2(std::numeric_limits<double>::min()) / std::log2(0.5);

    assert(nmults > 0);
    assert(std::ilogb(std::numeric_limits<double>::max()) + 1 <= std::numeric_limits<int>::max() / nmults); // guard against overflow of exponent

    // compute logarithm of product of values entries in range [start, end)
    auto subsetlog = [](auto start, auto end, std::vector<double> & values)
    {
       // split values into mantissa and exponent
       int exponentsum = 0;
       double mantissaprod = 1.0;
       for( auto j = start; j < end; j++ )
       {
            int ex;
            mantissaprod *= std::frexp(values[j], &ex);
            exponentsum += ex;
       }
       return std::log2(mantissaprod) + exponentsum;
    };

    /* end of definitions for geometric mean */

    unsigned int nlines = 0;

    std::ifstream file (argv[1], std::ifstream::in);
    std::string strline;

    // read line by line
    while( getline(file, strline) )
    {
       nlines++;

       using namespace boost::spirit;
       using qi::int_;
       using qi::double_;
       using qi::phrase_parse;
       using ascii::space;

       double value;
       int location;

       // iterator which will be changed by parse method
       auto it = strline.begin();

       // parse line as int;int;double and ignore whitespaces
       bool success = phrase_parse(it, strline.end(),
             int_ >> ';' >> int_[([&location](int i){ location = i; })] >> ';' >> double_[([&value](double i){ value = i; })]
                      , space);

       if( success && it == strline.end() )
       {
          if( location < 1 || location > 2 || value <= 0 || std::isnan(value) || value <= 0 )
             continue;

          // finally, store the value
          valvec[--location].push_back(value);

          // update geo mean?
          assert(valvec[location].size() <= std::numeric_limits<int>::max());
          assert(int(valvec[location].size()) - geomeancount[location] <= nmults);
          if( int(valvec[location].size()) - geomeancount[location] == nmults )
          {
             geomean[location] += subsetlog(geomeancount[location], int(valvec[location].size()), valvec[location]);
             geomeancount[location] = int(valvec[location].size());
          }
       }
    }

    file.close();

    // update and print geo mean
    for( int i = 0; i < 2; i++ )
    {
      geomean[i] += subsetlog(geomeancount[i], int(valvec[i].size()), valvec[i]);
      geomean[i] /= double(valvec[i].size());
      geomean[i] = std::pow(2.0, geomean[i]);

      std::cout << "Valid values Loc" << i + 1 <<  " " << valvec[i].size() << " with geo mean: " << geomean[i] << std::endl;
    }
    std::cout << "File: " << argv[1] << " with " << nlines << " lines " << std::endl;

    return EXIT_SUCCESS;
}


// NOTE: this is a subset sum problem
#include <algorithm>
#include <fstream>
#include <iostream>
#include <iterator>
#include <numeric>
#include <string>
#include <vector>

#include <boost/range/adaptor/reversed.hpp>
#include <boost/range/irange.hpp>

// this recursive function will go through all possible combinations of weights, starting with the last weight in the list,
// which is indicated by parameter 'last', until the sum (or what's left of it) is reached
template<typename Set, typename Sets, typename W, typename T, typename N>
auto find_subset(Set&& subset, Sets&& subsets, const W& weights, const T& tbl_has_sum, N row, N sum_left) -> void {

	const auto only_one_element_left = (row == 0);

	// first base case: only one weight left (first of the weights), sum is non-zero
	// and the final weight to be added to reach 'sum_left' is the the first weight
	if(only_one_element_left && (sum_left > 0) && tbl_has_sum[row][sum_left]) {

		subset.push_back(weights[row]);

		subsets.push_back(subset);

	} else if(only_one_element_left && (sum_left == 0)) {

		// second base case: only one weight left, there's no more sum left
		// to add to, so the current subset can be added to the collection
		subsets.push_back(subset);

	} else {

		const auto prev_row = (row - 1);

		// first recursive case: the matrix tells us whether or not sum_left
		// can be formed without the current weight but with all previous weights
		if(tbl_has_sum[prev_row][sum_left]) {

			// there are potentially two successive calls to find_subset, which means the passed subset will be modified
			// both times - but on the same recursion level we need the subset to be the same, so the first call will get
			// a copy, and when the recursion for that copy is finished, which at that point will be modified, we can continue
			// to the second call with the original that we started the recursion with
			auto new_subset = subset;

			find_subset(new_subset, subsets, weights, tbl_has_sum, prev_row, sum_left);
		}

		const auto weight = weights[row];

		const auto new_sum_left = (sum_left - weight);

		// second recursive case: to make access to the matrix safe, we need to check first,
		// if the current weight can be added without going over the sum we need - if the first
		// check is successful, the matrix will tell us, whether or not the previous row can
		// create the sum 'sum_left - weight' - if the answer is 'yes', we can add the current
		// weight to the current subset and make another recursive call
		if((sum_left >= weight) && tbl_has_sum[prev_row][new_sum_left]) {

			subset.push_back(weight);

			find_subset(subset, subsets, weights, tbl_has_sum, prev_row, new_sum_left);
		}
	}
}

// this function is based on: https://stackoverflow.com/a/45427013/699211
template<typename T, typename N>
auto find_subsets(const T& weights, N sum) {

	auto subsets = std::vector<T>{};

	// in dynamic programming, the first row of the lookup matrix
	// represents zero elements (empty subsets), so normally such
	// a matrix would have (n + 1) rows, but since we won't do anything
	// with that particular row, we might just as well stick to n rows
	const auto rows = weights.size();

	using VectorBool = std::vector<std::uint8_t>;
	using Matrix = std::vector<VectorBool>;

	// todo: rename lookup

	// this is a truth table - matrix[i][s] returns true or false to indicate the answer to the question:
	// "using the first i items in the array can we find a subset sum to s?"
	auto tbl_has_sum = Matrix(rows);

	const auto columns = (sum + 1);

	for(auto& row : tbl_has_sum) {
		row = VectorBool(columns);
		// first column represents subset sum = 0, which is true for all sets
		row.front() = true;
	}

	// first element of the weights container
	const auto first = weights.front();

	// if the first element of weights is <= sum, then the set containing that weight will have it as subset sum,
	// allowing to check if it's possible to combine that weight with any of the subset sums of the previous row
	// to see if we can create the subset sum we are looking for
	if (first <= sum) {
		tbl_has_sum.front()[first] = true;
	}

	for(const auto row : boost::irange({1}, rows)) {

		for(const auto col : boost::irange({}, columns)) {

			auto& cell = tbl_has_sum[row][col];

			const auto weight = weights[row];
			const auto& prev_row = tbl_has_sum[row-1];
			const auto cell_prev_row = prev_row[col];

			// ensure we don't accidentally go out of bounds
			if(weight <= col) {
				// this checks if the subset sum 'col' can be formed by
				// a) the previous set of weights (cell_prev_row) or
				// b) if subset sum 'col-weight' is formable by the previous set
				// this is the implementation of the following logic:
				// set X with subset-sum k has subset-sum k1 if subset X1 has subset-sum (k - k1)
				cell = (cell_prev_row || prev_row[col-weight]);
			} else {
				cell = cell_prev_row;
			}
		}
	}

	// start recursion
	find_subset(T{}, subsets, weights, tbl_has_sum, (rows - 1), sum);

	return subsets;
}

template<typename T>
auto find_min_sets(const T& subsets) {

	using value_type = decltype(subsets.size());
	auto min_size = std::numeric_limits<value_type>::max();

	auto min_sets = T{};

	for(const auto& subset : subsets) {

		const auto subset_size = subset.size();

		// we don't need any sets that have size > min_size
		// so if a smaller set is found, the min_sets collection
		// can immediately be replaced
        if(subset_size < min_size) {
        	min_size = subset_size;
        	min_sets = {subset};
        } else if(subset_size == min_size) {
        	min_sets.push_back(subset);
        }
	}

	return min_sets;
}

// =======================================
// SOME TEMPLATE NONSENSE FOR DEDUCING THE
// INNERMOST TYPE OF A NESTED CONTAINER
// =======================================
template<typename T, typename = std::void_t<>>
struct innermost_value_type {
    using type = T;
};

template<typename T>
struct innermost_value_type<T, std::void_t<typename T::value_type>> {
    using type = typename innermost_value_type<typename T::value_type>::type;
};

template<typename T>
using innermost_value_type_t = typename innermost_value_type<T>::type;
// =======================================
// END OF TEMPLATE NONSENSE
// =======================================

template<typename T>
auto find_min_quantum_entanglement(const T& min_sets) {

	using ValueType = innermost_value_type_t<T>;

	auto min_value = std::numeric_limits<ValueType>::max();

	for(const auto& min_set : min_sets) {

		const auto begin = min_set.begin();
		const auto end   = min_set.end();
		const auto init  = ValueType{1};
		const auto binop = std::multiplies{};

		const auto new_value = std::accumulate(begin, end, init, binop);

		min_value = std::min(min_value, new_value);
	}

	return min_value;
}

int main() {

	const auto filename = std::string{"weights.txt"};
	auto file = std::fstream{filename};

	if(file.is_open()) {

		auto weights = std::vector<std::uint64_t>{};

		using NumericType = innermost_value_type_t<decltype(weights)>;

		using stream_iterator = std::istream_iterator<NumericType>;

		const auto begin = stream_iterator{file};
		const auto end   = stream_iterator{};
		const auto init  = NumericType{};
		const auto binop = [&weights] (auto acc, auto weight) {
			weights.push_back(weight);
			return (acc + weight);
		};

		const auto total_weight = std::accumulate(begin, end, init, binop);

		const auto n_groups = 3;

		const auto sum_per_group = (total_weight / n_groups);

		std::sort(weights.begin(), weights.end());

		const auto subsets = find_subsets(weights, sum_per_group);

		const auto min_sets = find_min_sets(subsets);

		std::cout << find_min_quantum_entanglement(min_sets) << std::endl;

	} else {
		std::cerr << "Error! Could not open \"" << filename << "\"!" << std::endl;
	}

	return 0;
}


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

#pragma once

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

namespace fluid {
namespace algorithm {

class NNActivations
{

public:
  enum class Activation { kLinear, kSigmoid, kReLU, kTanh };

  using ArrayXXd = Eigen::ArrayXXd;
  using ActivationsMap =
      std::map<Activation, std::function<void(Eigen::Ref<Eigen::ArrayXXd>,
                                              Eigen::Ref<Eigen::ArrayXXd>)>>;

  static ActivationsMap& activation()
  {
    static ActivationsMap _funcs = {
        {Activation::kLinear,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = in;
         }},
        {Activation::kSigmoid,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = 1 / (1 + (-in).exp());
         }},
        {Activation::kReLU,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = in.max(0);
         }},
        {Activation::kTanh,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = (in.exp() - (-in).exp()) / (in.exp() + (-in).exp());
         }},
    };
    return _funcs;
  }

  // derivative from output of activation
  static ActivationsMap& derivative()
  {
    static ActivationsMap _funcs = {
        {Activation::kLinear,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = ArrayXXd::Ones(in.rows(), in.cols());
         }},
        {Activation::kSigmoid,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = in * (1 - in);
         }},
        {Activation::kReLU,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = (in > 0).cast<double>();
         }},
        {Activation::kTanh,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = 1 - in.square();
         }}};
    return _funcs;
  }
};
} // namespace algorithm
} // namespace fluid


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2018 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Classic image feature extraction algorithms                              *
***************************************************************************/

#define cimg_display 0
#include <CImg.h>
#include <CImgMATLAB.hpp>
#include <EigenMATLAB.hpp>
#include <ScalarMATLAB.hpp>
#include <STDMATLAB.hpp>
#include <vector>
#include <cmath>
#include <omp.h>

#define INF 1e+300

using namespace cimg_library;

class ImageFeatureExtraction
{
public:
	template<typename T>
	static CImg<double> FirstOrderCentralMoments(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius);
	template<typename T>
	static CImg<double> LocalMedianMAD(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius);
	template<typename T>
	static CImg<double> LocalEnergy(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius);
	template<typename T>
	static CImg<double> LocalEntropy(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius, const int nbins);
}

template<typename T>
CImg<double> ImageFeatureExtraction::FirstOrderCentralMoments(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius)
{
	CImg<double> F(image.width(), image.height(), image.depth(), 7);

    #pragma omp parallel for
	for (int x = 0; x < image.width(); ++x)
	{
		for (int y = 0; y < image.height(); ++y)
		{
			for (int z = 0; z < image.depth(); ++z)
			{
				for (int f = 0; f < 7; ++f)
					F(x, y, z, f) = 0;

				if (!mask(x, y, z))
					continue;

				// Local Mean
				int n = 0;
				double mu = 0;
				for (int xx = -radius[0]; xx <= radius[0]; ++xx)
				{
					const int xr = x + xx;
					if (xr < 0 || xr >= image.width())
						continue;

					for (int yy = -radius[1]; yy <= radius[1]; ++yy)
					{
						const int yr = y + yy;
						if (yr < 0 || yr >= image.height())
							continue;
						for (int zz = -radius[2]; zz <= radius[2]; ++zz)
						{
							const int zr = z + zz;
							if (zr < 0 || zr >= image.depth())
								continue;

							if (!mask(xr, yr, zr))
								continue;

							mu += image(xr, yr, zr);
							++n;
						}
					}
				}
				mu = n == 0 ? image(x, y, z) : mu / (double) n;

				// Variance, Skewness, Kurtosis, Energy0, Energy1
				n = 0;
				double var = 0;
				double skw = 0;
				double krt = 0;
				double energy0 = 0;
				double energy1 = 0;
				double max = -INF;
				double min = INF;
				for (int xx = -radius[0]; xx <= radius[0]; ++xx)
				{
					const int xr = x + xx;
					if (xr < 0 || xr >= image.width())
						continue;

					for (int yy = -radius[1]; yy <= radius[1]; ++yy)
					{
						const int yr = y + yy;
						if (yr < 0 || yr >= image.height())
							continue;

						for (int zz = -radius[2]; zz <= radius[2]; ++zz)
						{
							const int zr = z + zz;
							if (zr < 0 || zr >= image.depth())
								continue;

							if (!mask(xr, yr, zr))
								continue;

							const double value = image(xr, yr, zr);
							const double diff = value - mu;

							var += (diff * diff);
                            skw += (diff * diff * diff);
                            krt += (diff * diff * diff * diff);
							energy0 += (value * value);
							energy1 += value;
							max = value > max ? value : max;
							min = value < min ? value : min;
							++n;
						}
					}
				}
				F(x, y, z, 0) = mu;
				F(x, y, z, 1) = n <= 1 ? 0 : var / (double) (n - 1);
				F(x, y, z, 2) = n <= 0 || var == 0 ? 0 : (skw / (double) n) / pow(var / (double) n, 1.5);
				F(x, y, z, 3) = n <= 0 || var == 0 ? 0 : ((krt / (double) n) / pow(var / (double) n, 2)) - 3;
				F(x, y, z, 4) = n == 0 ? 0 : energy0 / (double) n;
				F(x, y, z, 5) = n == 0 ? 0 : (energy1 * energy1) / (double) (n * n);
				F(x, y, z, 6) = max - min;
			}
		}
	}
	return F;
}

template<typename T>
CImg<double> ImageFeatureExtraction::LocalEntropy(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius, const int nbins)
{
	CImg<double> F(image.width(), image.height(), image.depth());
	const double ratio = 256 / (double) nbins;

	#pragma omp parallel for
	for (int x = 0; x < image.width(); ++x)
	{
		for (int y = 0; y < image.height(); ++y)
		{
			for (int z = 0; z < image.depth(); ++z)
			{
				F(x, y, z) = 0;

				if (!mask(x, y, z))
					continue;

				int n = 0;
				std::vector<int> bins(nbins, 0);
				for (int xx = -radius[0]; xx <= radius[0]; ++xx)
				{
					const int xr = x + xx;
					if (xr < 0 || xr >= image.width())
						continue;

					for (int yy = -radius[1]; yy <= radius[1]; ++yy)
					{
						const int yr = y + yy;
						if (yr < 0 || yr >= image.height())
							continue;

						for (int zz = -radius[2]; zz <= radius[2]; ++zz)
						{
							const int zr = z + zz;
							if (zr < 0 || zr >= image.depth())
								continue;

							if (!mask(xr, yr, zr))
								continue;

							bins[std::floor((double) image(xr, yr, zr) / ratio)]++;
							n++;
						}
					}
				}

				if (n == 0)
					continue;

				double H = 0;
				for (int i = 0; i < nbins; i++)
				{
					const double p = bins[i] / (double) n;
					if (p > 0)
						H += p * log(p);
				}
				F(x, y, z) = -H;
			}
		}
	}
	return F;
}

template<typename T>
CImg<double> ImageFeatureExtraction::LocalMedianMAD(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius)
{
	const int N = (2 * radius[0] + 1) * (2 * radius[1] + 1) * (2 * radius[2] + 1);
	const int mid = (int) std::floor((double) N / 2.0);

	CImg<double> F(image.width(), image.height(), image.depth(), 2);

	#pragma omp parallel for
	for (int x = 0; x < image.width(); ++x)
	{
		for (int y = 0; y < image.height(); ++y)
		{
			for (int z = 0; z < image.depth(); ++z)
			{
				F(x, y, z, 0) = 0;
				F(x, y, z, 1) = 0;

				if (!mask(x, y, z))
					continue;

				std::vector<double> patch;
				for (int xx = -radius[0]; xx <= radius[0]; ++xx)
				{
					const int xr = x + xx;
					if (xr < 0 || xr >= image.width())
					{
						patch.push_back(image(x, y, z));
						continue;
					}

					for (int yy = -radius[1]; yy <= radius[1]; ++yy)
					{
						const int yr = y + yy;
						if (yr < 0 || yr >= image.height())
						{
							patch.push_back(image(x, y, z));
							continue;
						}

						for (int zz = -radius[2]; zz <= radius[2]; ++zz)
						{
							const int zr = z + zz;
							if (zr < 0 || zr >= image.depth())
							{
								patch.push_back(image(x, y, z));
								continue;
							}

							patch.push_back(image(xr, yr, zr));
						}
					}
				}
				// Sort data and get median value
				std::sort(patch.begin(), patch.end());
				double median = patch[mid];
				F(x, y, z, 0) = median;

				// Compute MAD
				std::vector<double> residues;
				for (int l = 0; l < N; l++)
					residues.push_back(std::abs(patch[l] - median));

				// Sort data and get median value
				std::sort(residues.begin(), residues.end());
				F(x, y, z, 1) = residues[mid];
			}
		}
	}
	return F;
}


/******************************************************************************
Created By : Zhang Zhimin
Created On : 2012/11/22
Purpose    :
********************************************************************************/

#pragma once

#include <skynet/config.hpp>
#include <skynet/utility/math.hpp>
#include <skynet/utility/serialization.hpp>
#include <skynet/utility/eigen_lib.hpp>

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

namespace skynet{namespace statistics{

	template <typename T>
	class gauss_function;

	//template <typename T>
	template <>
	class gauss_function<float> : public unary_function<float, float>{
		static_assert(std::is_floating_point<float>::value, "the normal function template T should be float point type.");
	public:
		typedef float			value_type;
		typedef value_type		point_type;

		gauss_function() {}

		gauss_function(value_type mean, value_type sigma): m_mean(mean), m_sigma(sigma) {
			init();
		}

		void mean(value_type mean)			{ m_mean = mean; init(); }
		value_type mean() const				{ return m_mean; }
		void sigma(value_type sigma)		{ m_sigma = sigma; init(); }
		value_type sigma() const			{ return m_sigma; }
		void covariance(value_type cov)		{ m_sigma = sqrt(cov); init(); }
		value_type covariance() const		{ return sqr(m_sigma); }

		value_type operator()(const value_type & x) const{
			return	exp(-sqr(x-m_mean)/m_2sqr_sigma) / m_sigma_sqrt_2pi;
		}

		template <typename Archive>
		void serialize(Archive &ar, unsigned int){
			ar & boost::serialization::make_nvp("mean", m_mean);
			ar & boost::serialization::make_nvp("sigma", m_sigma);

			init();
		}

	protected:
		void init() {
			m_sigma_sqrt_2pi = m_sigma * sqrt(2 * PI);
			m_2sqr_sigma = 2 * sqr(m_sigma);
		}

	private:
		value_type		m_mean;
		value_type		m_sigma;

		value_type		m_sigma_sqrt_2pi;
		value_type		m_2sqr_sigma;
	};

	template <>
	class gauss_function<double> : public unary_function<double, double>{
		static_assert(std::is_floating_point<double>::value, "the normal function template T should be double point type.");
	public:
		typedef double			value_type;
		typedef value_type		point_type;

		gauss_function(value_type mean, value_type sigma): m_mean(mean), m_sigma(sigma) {
			init();
		}

		void mean(value_type mean)			{ m_mean = mean; init(); }
		value_type mean() const				{ return m_mean; }
		void sigma(value_type sigma)		{ m_sigma = sigma; init(); }
		value_type sigma() const			{ return m_sigma; }
		value_type covariance() const		{ return sqr(m_sigma); }

		value_type operator()(const value_type & x) const{
			return	exp(-sqr(x-m_mean)/m_2sqr_sigma) / m_sigma_sqrt_2pi;
		}

	protected:
		void init() {
			m_sigma_sqrt_2pi = m_sigma * sqrt(2 * PI);
			m_2sqr_sigma = 2 * sqr(m_sigma);
		}

	private:
		value_type		m_mean;
		value_type		m_sigma;

		value_type		m_sigma_sqrt_2pi;
		value_type		m_2sqr_sigma;
	};

	template <>
	class gauss_function<Eigen::VectorXd> : public std::unary_function<Eigen::VectorXd, double>{
	public:
		typedef Eigen::VectorXd			value_type;
		typedef Eigen::MatrixXd			matrix_type;

		gauss_function(){}

		gauss_function(const gauss_function & g): m_mean(g.m_mean), m_covariance(g.m_covariance), m_inv_cov(g.m_inv_cov), m_scale(g.m_scale){}

		void attach(matrix_type  feature_matrix){
			m_mean.resize(feature_matrix.rows());
			m_mean.setZero();
			for (int i = 0; i < feature_matrix.cols(); ++i){
				m_mean += feature_matrix.col(i);
			}

			m_mean /= feature_matrix.cols();

			for (int i = 0; i < feature_matrix.cols(); ++i){
				feature_matrix.col(i) -= m_mean;
			}

			m_covariance = feature_matrix * feature_matrix.transpose();
			m_covariance /= feature_matrix.cols();
			m_inv_cov = m_covariance.inverse();

			m_scale = sqrt(m_covariance.determinant()) * sqrt(std::pow((2*PI), m_mean.rows()));
		}

		result_type	 operator()(const argument_type &x) const{
			auto x_sub_mean = x - m_mean;
			auto dis = -0.5 * (x_sub_mean.transpose() * m_inv_cov * x_sub_mean);
			double dis_d = dis.value();
			return std::exp(dis_d)/m_scale;
		}

		void mean(const value_type &mean)		{ m_mean = mean; }
		value_type mean() const					{ return m_mean; }

		template <typename Archive>
		void serialize(Archive &ar, const unsigned int &){
			 ar & boost::serialization::make_nvp("mean_feature", m_mean);
			 ar & boost::serialization::make_nvp("covariance", m_covariance);
			 ar & boost::serialization::make_nvp("inv_covariance", m_inv_cov);
			 ar & boost::serialization::make_nvp("scale", m_scale);
		}

	private:
		value_type						m_mean;

		matrix_type						m_covariance;
		matrix_type						m_inv_cov;
		double							m_scale;

	};

}}


/**
 * @file OrientationUtils.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 <Eigen/Geometry>

namespace momentumopt {

  /** computes the error between two quaternions by rotating the desired orientation
   *  back by the current orientation, and taking the logarithm of the result.
   */
  Eigen::Vector3d orientationError(const Eigen::Quaternion<double>& desired_orientation,
                                   const Eigen::Quaternion<double>& current_orientation);

  /** computation of the required angular velocity to move from a current orientation
   *  to a desired orientation in a given time interval. Angular velocity can be returned
   *  in body or world coordinates.
   */
  Eigen::Vector3d requiredAngularVelocity(const Eigen::Quaternion<double>& desired_orientation,
                                          const Eigen::Quaternion<double>& current_orientation,
                                          const double& time_step, bool in_body_coordinates=false);

  /** integration of a quaternion given an angular velocity given either in
   *  body or world coordinates and a time interval for integration
   */
  Eigen::Quaternion<double> integrateAngularVelocity(const Eigen::Quaternion<double>& current_orientation,
                                                     const Eigen::Vector3d& angular_velocity,
                                                     const double& time_step, bool in_body_coordinates=false);

}