//
//  gmatrix16float.cpp
//  GCommon
//
//  Created by David Coen on 2011 06 01
//  Copyright Pleasure seeking morons 2011. All rights reserved.
//

#include "gmatrix16float.h"

#include "gvector3float.h"
#include "gmathmatrix.h"
#include "gmath.h"
#include "gmatrix9float.h"

#include <boost/swap.hpp>

#define DSC_INLINE_MATRIX_MUL

/*
matching openGL documentation
		0_0, 0_1, 0_2, 0_3(x),
		1_0, 1_1, 1_2, 1_3(y),
		2_0, 2_1, 2_2, 2_3(z),
		3_0, 3_1, 3_2, 3_3
*/

/*static*/ const GMatrix16Float GMatrix16Float::sIdentity(1.0F, 0.0F, 0.0F, 0.0F,
														  0.0F, 1.0F, 0.0F, 0.0F,
														  0.0F, 0.0F, 1.0F, 0.0F,
														  0.0F, 0.0F, 0.0F, 1.0F);

//constructors
GMatrix16Float::GMatrix16Float(const GR32 in_data_0_0, const GR32 in_data_0_1, const GR32 in_data_0_2, const GR32 in_data_0_3,
	const GR32 in_data_1_0, const GR32 in_data_1_1, const GR32 in_data_1_2, const GR32 in_data_1_3,
	const GR32 in_data_2_0, const GR32 in_data_2_1, const GR32 in_data_2_2, const GR32 in_data_2_3,
	const GR32 in_data_3_0, const GR32 in_data_3_1, const GR32 in_data_3_2, const GR32 in_data_3_3
	)
{
	SetData(in_data_0_0, in_data_0_1, in_data_0_2, in_data_0_3,
		in_data_1_0, in_data_1_1, in_data_1_2, in_data_1_3,
		in_data_2_0, in_data_2_1, in_data_2_2, in_data_2_3,
		in_data_3_0, in_data_3_1, in_data_3_2, in_data_3_3
		);
	return;
}
GMatrix16Float::GMatrix16Float(const GR32* const in_data)
{
	for (GS32 index = 0; index < 16; ++index)
	{
		m_data[index] = in_data[index];
	}
	return;

}

GMatrix16Float::GMatrix16Float(const GMatrix16Float& in_src)
{
	(*this) = in_src;
	return;
}

GMatrix16Float::~GMatrix16Float()
{
	return;
}

//operators
const GMatrix16Float& GMatrix16Float::operator=(const GMatrix16Float& in_rhs)
{
	for (GS32 index = 0; index < 16; ++index)
	{
		m_data[index] = in_rhs.m_data[index];
	}
	return (*this);
}

const GMatrix16Float& GMatrix16Float::operator*=(const GR32 in_rhs)
{
	for (GS32 index = 0; index < 16; ++index)
	{
		m_data[index] *= in_rhs;
	}

	return (*this);
}

//public methods
void GMatrix16Float::Decompose(GVector3Float& out_position,
   GQuaternion4Float& out_rotation,
   GVector3Float& out_scale
   )
{
	return;
}

GMatrix16Float&  GMatrix16Float::TransposeSelf()
{
	std::swap(m_0_1, m_1_0);
	std::swap(m_0_2, m_2_0);
	std::swap(m_0_3, m_3_0);
	std::swap(m_1_2, m_2_1);
	std::swap(m_1_3, m_3_1);
	std::swap(m_2_3, m_3_2);

	return (*this);
}
GMatrix16Float& GMatrix16Float::InverseSelf()
{
	(*this) = ReturnInverse();
	return (*this);
}

const GMatrix16Float GMatrix16Float::ReturnInverse()const
{
	GMatrix16Float result;
	GMathMatrix<GR32>::Inverse4(&result.m_data[0], &m_data[0]);

	return result;
}

GMatrix9Float GMatrix16Float::GetRotation()const
{
	return GMatrix9Float(
		m_0_0, m_0_1, m_0_2,
		m_1_0, m_1_1, m_1_2,
		m_2_0, m_2_1, m_2_2
	);
}

void GMatrix16Float::SetRotation(const GMatrix9Float& in_rotation)
{
	m_0_0 = in_rotation.m_0_0;
	m_1_0 = in_rotation.m_1_0;
	m_2_0 = in_rotation.m_2_0;
	m_0_1 = in_rotation.m_0_1;
	m_1_1 = in_rotation.m_1_1;
	m_2_1 = in_rotation.m_2_1;
	m_0_2 = in_rotation.m_0_2;
	m_1_2 = in_rotation.m_1_2;
	m_2_2 = in_rotation.m_2_2;
	return;
}

//public accessors
void GMatrix16Float::SetData(const GR32 in_data_0_0, const GR32 in_data_0_1, const GR32 in_data_0_2, const GR32 in_data_0_3,
	const GR32 in_data_1_0, const GR32 in_data_1_1, const GR32 in_data_1_2, const GR32 in_data_1_3,
	const GR32 in_data_2_0, const GR32 in_data_2_1, const GR32 in_data_2_2, const GR32 in_data_2_3,
	const GR32 in_data_3_0, const GR32 in_data_3_1, const GR32 in_data_3_2, const GR32 in_data_3_3
	)
{
	m_0_0 = in_data_0_0;
	m_1_0 = in_data_1_0;
	m_2_0 = in_data_2_0;
	m_3_0 = in_data_3_0;

	m_0_1 = in_data_0_1;
	m_1_1 = in_data_1_1;
	m_2_1 = in_data_2_1;
	m_3_1 = in_data_3_1;

	m_0_2 = in_data_0_2;
	m_1_2 = in_data_1_2;
	m_2_2 = in_data_2_2;
	m_3_2 = in_data_3_2;

	m_0_3 = in_data_0_3;
	m_1_3 = in_data_1_3;
	m_2_3 = in_data_2_3;
	m_3_3 = in_data_3_3;

	return;
}

const GVector3Float GMatrix16Float::GetAt()const
{
	const GVector3Float result(
		m_0_2,
		m_1_2,
		m_2_2
		);
	return result;
}

const GVector3Float GMatrix16Float::GetUp()const
{
	const GVector3Float result(
		m_0_1,
		m_1_1,
		m_2_1
		);
	return result;
}

const GVector3Float GMatrix16Float::GetPosition()const
{
	const GVector3Float result(
		m_0_3,
		m_1_3,
		m_2_3
		);
	return result;
}

void GMatrix16Float::SetPosition(const GVector3Float& in_position)
{
	m_0_3 = in_position.m_x;
	m_1_3 = in_position.m_y;
	m_2_3 = in_position.m_z;
	return;
}

//global operators
const GMatrix16Float operator*(const GMatrix16Float& in_lhs, const GMatrix16Float& in_rhs)
{
	GR32 value[16];
#ifdef DSC_INLINE_MATRIX_MUL
	const GR32* const lhsData = in_lhs.GetData();
	const GR32* const rhsData = in_rhs.GetData();
	value[ 0] = (lhsData[ 0] * rhsData[ 0]) + (lhsData[ 1] * rhsData[ 4]) + (lhsData[ 2] * rhsData[ 8]) + (lhsData[ 3] * rhsData[12]);
	value[ 1] = (lhsData[ 0] * rhsData[ 1]) + (lhsData[ 1] * rhsData[ 5]) + (lhsData[ 2] * rhsData[ 9]) + (lhsData[ 3] * rhsData[13]);
	value[ 2] = (lhsData[ 0] * rhsData[ 2]) + (lhsData[ 1] * rhsData[ 6]) + (lhsData[ 2] * rhsData[10]) + (lhsData[ 3] * rhsData[14]);
	value[ 3] = (lhsData[ 0] * rhsData[ 3]) + (lhsData[ 1] * rhsData[ 7]) + (lhsData[ 2] * rhsData[11]) + (lhsData[ 3] * rhsData[15]);

	value[ 4] = (lhsData[ 4] * rhsData[ 0]) + (lhsData[ 5] * rhsData[ 4]) + (lhsData[ 6] * rhsData[ 8]) + (lhsData[ 7] * rhsData[12]);
	value[ 5] = (lhsData[ 4] * rhsData[ 1]) + (lhsData[ 5] * rhsData[ 5]) + (lhsData[ 6] * rhsData[ 9]) + (lhsData[ 7] * rhsData[13]);
	value[ 6] = (lhsData[ 4] * rhsData[ 2]) + (lhsData[ 5] * rhsData[ 6]) + (lhsData[ 6] * rhsData[10]) + (lhsData[ 7] * rhsData[14]);
	value[ 7] = (lhsData[ 4] * rhsData[ 3]) + (lhsData[ 5] * rhsData[ 7]) + (lhsData[ 6] * rhsData[11]) + (lhsData[ 7] * rhsData[15]);

	value[ 8] = (lhsData[ 8] * rhsData[ 0]) + (lhsData[ 9] * rhsData[ 4]) + (lhsData[10] * rhsData[ 8]) + (lhsData[11] * rhsData[12]);
	value[ 9] = (lhsData[ 8] * rhsData[ 1]) + (lhsData[ 9] * rhsData[ 5]) + (lhsData[10] * rhsData[ 9]) + (lhsData[11] * rhsData[13]);
	value[10] = (lhsData[ 8] * rhsData[ 2]) + (lhsData[ 9] * rhsData[ 6]) + (lhsData[10] * rhsData[10]) + (lhsData[11] * rhsData[14]);
	value[11] = (lhsData[ 8] * rhsData[ 3]) + (lhsData[ 9] * rhsData[ 7]) + (lhsData[10] * rhsData[11]) + (lhsData[11] * rhsData[15]);

	value[12] = (lhsData[12] * rhsData[ 0]) + (lhsData[13] * rhsData[ 4]) + (lhsData[14] * rhsData[ 8]) + (lhsData[15] * rhsData[12]);
	value[13] = (lhsData[12] * rhsData[ 1]) + (lhsData[13] * rhsData[ 5]) + (lhsData[14] * rhsData[ 9]) + (lhsData[15] * rhsData[13]);
	value[14] = (lhsData[12] * rhsData[ 2]) + (lhsData[13] * rhsData[ 6]) + (lhsData[14] * rhsData[10]) + (lhsData[15] * rhsData[14]);
	value[15] = (lhsData[12] * rhsData[ 3]) + (lhsData[13] * rhsData[ 7]) + (lhsData[14] * rhsData[11]) + (lhsData[15] * rhsData[15]);
#else
	GMathMatrix<GR32>::MatrixMul(
		in_lhs.GetData(),
		in_rhs.GetData(),
		4,
		4,
		4,
		&value[0]
		);
#endif
	return GMatrix16Float(&value[0]);
}

const GVector3Float GMatrix16FloatMultiplyNoTranslate(const GVector3Float& in_lhs, const GMatrix16Float& in_rhs)
{
#ifdef DSC_INLINE_MATRIX_MUL
	GR32 value[3];
	const GR32* const lhsData = in_lhs.GetData();
	const GR32* const rhsData = in_rhs.GetData();
	value[ 0] = (lhsData[ 0] * rhsData[ 0]) + (lhsData[ 1] * rhsData[ 4]) + (lhsData[ 2] * rhsData[ 8]);
	value[ 1] = (lhsData[ 0] * rhsData[ 1]) + (lhsData[ 1] * rhsData[ 5]) + (lhsData[ 2] * rhsData[ 9]);
	value[ 2] = (lhsData[ 0] * rhsData[ 2]) + (lhsData[ 1] * rhsData[ 6]) + (lhsData[ 2] * rhsData[10]);
#else
	GR32 source[4];
	GR32 value[4];
	//promote
	source[0] = in_lhs.m_x;
	source[1] = in_lhs.m_y;
	source[2] = in_lhs.m_z;
	source[3] = 0.0F;

	//< m, n > * < p, m > = < p, n >
	GMathMatrix<GR32>::MatrixMul(
		&source[0],
		in_rhs.GetData(),
		4, //m
		1, //n
		4, //p
		&value[0]
		);
#endif

	return GVector3Float(&value[0]);
}

//{
//	const GMatrix9Float subMatrix = in_lhs.GetRotation();
//	const GVector3Float result = subMatrix * in_rhs;
//	return result;
//}

const GVector3Float operator*(const GVector3Float& in_lhs, const GMatrix16Float& in_rhs)
{
#ifdef DSC_INLINE_MATRIX_MUL
	const GR32* const lhsData = in_lhs.GetData();
	const GR32* const rhsData = in_rhs.GetData();
	return GVector3Float(
		(lhsData[ 0] * rhsData[ 0]) + (lhsData[ 1] * rhsData[ 4]) + (lhsData[ 2] * rhsData[ 8]) + rhsData[12],
		(lhsData[ 0] * rhsData[ 1]) + (lhsData[ 1] * rhsData[ 5]) + (lhsData[ 2] * rhsData[ 9]) + rhsData[13],
		(lhsData[ 0] * rhsData[ 2]) + (lhsData[ 1] * rhsData[ 6]) + (lhsData[ 2] * rhsData[10]) + rhsData[14]
		);
#else
	GR32 source[4];
	GR32 value[4];
	//promote
	source[0] = in_lhs.m_x;
	source[1] = in_lhs.m_y;
	source[2] = in_lhs.m_z;
	source[3] = 1.0F;

	//< m, n > * < p, m > = < p, n >
	GMathMatrix<GR32>::MatrixMul(
		&source[0],
		in_rhs.GetData(),
		4, //m
		1, //n
		4, //p
		&value[0]
		);
	return GVector3Float(&value[0]);
#endif
}

/*
  from _Mathematics for computer graphics p.171
 mapping a pair of vectors onto another pair, u,x being one pair, a,y being another of unit vectors at same angle
 u.x = a.y = cos(\), with sin(\) != 0
 a roation matrix sending u,x to a,y given by
               a
 M = [u v w ][ b ]
               c
 d = | x * u | = | y * a | = | sin(\) |
 if 90deg = u to x, cos(\) = 0, sin(\) = 1
 v = ( x * u ) / d
 b = ( y * a ) / d
 w = ( x - u( cos(\) ) ) / d
 c = ( y - a( cos(\) ) ) / d
*/
const GMatrix16Float GMatrix16FloatConstructAtUp(
	const GVector3Float& in_targetAt,
	const GVector3Float& in_targetUp,
	const GVector3Float& in_baseAt,
	const GVector3Float& in_baseUp,
	const GVector3Float& in_position
	)
{
	////is this a stablity issue
	//const GR32 dotResultA = DotProduct(in_baseAt, in_targetAt);
	//const GR32 dotResultU = DotProduct(in_baseUp, in_targetUp);
	//if (((dotResultA < -0.999F) || (0.999F < dotResultA)) &&
	//	((dotResultU < -0.999F) || (0.999F < dotResultU)))
	//{
	//	GMatrix16Float result(GMatrix16Float::sIdentity);
	//	result.SetPosition(in_position);
	//	return result;
	//}

	const GVector3Float crossBaseUpAt = CrossProduct(in_baseUp, in_baseAt);
	const GVector3Float crossTargetUpAt = CrossProduct(in_targetUp, in_targetAt);

	return GMatrix16Float(
		(in_baseAt.m_x * in_targetAt.m_x) + (crossBaseUpAt.m_x * crossTargetUpAt.m_x) + (in_baseUp.m_x * in_targetUp.m_x),
		(in_baseAt.m_y * in_targetAt.m_x) + (crossBaseUpAt.m_y * crossTargetUpAt.m_x) + (in_baseUp.m_y * in_targetUp.m_x),
		(in_baseAt.m_z * in_targetAt.m_x) + (crossBaseUpAt.m_z * crossTargetUpAt.m_x) + (in_baseUp.m_z * in_targetUp.m_x),
		in_position.m_x,

		(in_baseAt.m_x * in_targetAt.m_y) + (crossBaseUpAt.m_x * crossTargetUpAt.m_y) + (in_baseUp.m_x * in_targetUp.m_y),
		(in_baseAt.m_y * in_targetAt.m_y) + (crossBaseUpAt.m_y * crossTargetUpAt.m_y) + (in_baseUp.m_y * in_targetUp.m_y),
		(in_baseAt.m_z * in_targetAt.m_y) + (crossBaseUpAt.m_z * crossTargetUpAt.m_y) + (in_baseUp.m_z * in_targetUp.m_y),
		in_position.m_y,

		(in_baseAt.m_x * in_targetAt.m_z) + (crossBaseUpAt.m_x * crossTargetUpAt.m_z) + (in_baseUp.m_x * in_targetUp.m_z),
		(in_baseAt.m_y * in_targetAt.m_z) + (crossBaseUpAt.m_y * crossTargetUpAt.m_z) + (in_baseUp.m_y * in_targetUp.m_z),
		(in_baseAt.m_z * in_targetAt.m_z) + (crossBaseUpAt.m_z * crossTargetUpAt.m_z) + (in_baseUp.m_z * in_targetUp.m_z),
		in_position.m_z,

		0.0F,
		0.0F,
		0.0F,
		1.0F
		);
}

void GMatrix16FloatDecomposeAtUp(
	GVector3Float& out_at,
	GVector3Float& out_up,
	GVector3Float& out_position,
	const GMatrix16Float& in_matrix
	)
{
	out_at = in_matrix.GetAt();
	out_up = in_matrix.GetUp();
	out_position = in_matrix.GetPosition();
	return;
}

/*
  http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
	matrix[ HMatrixR4::IndexGet( 0, 0 ) ] = c + axis_x * axis_x * t;
	matrix[ HMatrixR4::IndexGet( 1, 1 ) ] = c + axis_y * axis_y * t;
	matrix[ HMatrixR4::IndexGet( 2, 2 ) ] = c + axis_z * axis_z * t;

	HREAL tmp1 = axis_x * axis_y * t;
	HREAL tmp2 = axis_z * s;
	matrix[ HMatrixR4::IndexGet( 1, 0 ) ] = tmp1 + tmp2;
	matrix[ HMatrixR4::IndexGet( 0, 1 ) ] = tmp1 - tmp2;
	tmp1 = axis_x * axis_z * t;
	tmp2 = axis_y * s;
	matrix[ HMatrixR4::IndexGet( 2, 0 ) ] = tmp1 - tmp2;
	matrix[ HMatrixR4::IndexGet( 0, 2 ) ] = tmp1 + tmp2;
	tmp1 = axis_y * axis_z * t;
	tmp2 = axis_x * s;
	matrix[ HMatrixR4::IndexGet( 2, 1 ) ] = tmp1 + tmp2;
	matrix[ HMatrixR4::IndexGet( 1, 2 ) ] = tmp1 - tmp2;
*/
const GMatrix16Float GMatrix16FloatConstructAxisAngle(const GVector3Float& in_axis, const GR32 in_angleRad)
{
	const GVector3Float localAxis = Normalise(in_axis);
	const GR32 axis_x = localAxis.m_x;
	const GR32 axis_y = localAxis.m_y;
	const GR32 axis_z = localAxis.m_z;

	const GR32 c = GMath::Cos( in_angleRad );
	const GR32 s = GMath::Sin( in_angleRad );
	const GR32 t = 1.0F - c;

	const GR32 tmp1_01 = axis_x * axis_y * t;
	const GR32 tmp2_01 = axis_z * s;

	const GR32 tmp1_02 = axis_x * axis_z * t;
	const GR32 tmp2_02 = axis_y * s;

	const GR32 tmp1_21 = axis_y * axis_z * t;
	const GR32 tmp2_21 = axis_x * s;

	return GMatrix16Float(
		c + axis_x * axis_x * t,		tmp1_01 - tmp2_01,			tmp1_02 + tmp2_02,			0.0F,
		tmp1_01 + tmp2_01,				c + axis_y * axis_y * t,	tmp1_21 - tmp2_21,			0.0F,
		tmp1_02 - tmp2_02,				tmp1_21 + tmp2_21,			c + axis_z * axis_z * t,	0.0F,
		0.0F,							0.0F,						0.0F,						1.0F
		);
}

const GBOOL Valid(const GMatrix16Float& in_data)
{
	const GR32* const pData = in_data.GetData();
	if ((!GMath::Valid(pData[0])) ||
		(!GMath::Valid(pData[1])) ||
		(!GMath::Valid(pData[2])) ||
		(!GMath::Valid(pData[3])) ||
		(!GMath::Valid(pData[4])) ||
		(!GMath::Valid(pData[5])) ||
		(!GMath::Valid(pData[6])) ||
		(!GMath::Valid(pData[7])) ||
		(!GMath::Valid(pData[8])) ||
		(!GMath::Valid(pData[9])) ||
		(!GMath::Valid(pData[10])) ||
		(!GMath::Valid(pData[11])) ||
		(!GMath::Valid(pData[12])) ||
		(!GMath::Valid(pData[13])) ||
		(!GMath::Valid(pData[14])) ||
		(!GMath::Valid(pData[15])))
	{
		return false;
	}
	return true;
}

//cheap invert, no skew or scale support
const GMatrix16Float GMatrix16FloatInvertOrthogonal(const GMatrix16Float& in_src)
{
	const GVector3Float position = in_src.GetPosition();
	GMatrix16Float transpose(in_src);
	transpose.SetPosition(GVector3Float::sZero);
	transpose.TransposeSelf();
	transpose.SetPosition(-position);

	return transpose;
}


#include "Truss.hpp"
#include <armadillo>
#include <iomanip>

using namespace arma;

Truss::Joint::Joint() {
	x=y=0;
	fixedX=fixedY=false;
	externalX=externalY=0;
	connectionLen = 0;
}

Truss::Member::Member(){
	id = -1;
	joint1 = joint2 = NULL;
	length = 0;
}

Truss::Truss(int numJoints, int numMembers) {
	this->numJoints = numJoints;
	this->numMembers = numMembers;
	this->joints = new Joint[this->numJoints];
	this->members = new Member[this->numMembers];
	this->pin = &joints[0];
	this->normalJoint = &joints[1];
}

Truss::~Truss() {
	delete[] joints;
	delete[] members;
}

Truss::Joint* Truss::getJoints() {
	return this->joints;
}

Truss::Member* Truss::getMembers() {
	return this->members;
}

void Truss::solveGeneralSystem() {
	// assumes normal reaction force is horizontally or vertically aligned
	double momentAtPin = 0; // counterclockwise is positive
	// calculate moments about every joint
	for (int i = 2; i < this->numJoints; i++) {
		// if the joint has an external x (i.e. has any external force
		if (joints[i].externalY != 0) {
			momentAtPin += joints[i].x*joints[i].externalY;
			// do not need to calculate moment generated by x forces since external forces are only in y direction
//			momentAtPin -= joints[i].y*joints[i].externalX;
		}
	}

	if (normalJoint->x == 0) { // the reactions are vertically aligned
		 normalJoint->externalX = momentAtPin/normalJoint->y;
		 pin->externalX = -normalJoint->externalX;
		 pin->externalY = 0;
		 for (int i = 2; i < this->numJoints; i++) {
			 pin->externalY -= joints[i].externalY;
		 }
	}
	else { // the reactions forces are horizontally aligned
		normalJoint->externalY = -momentAtPin/normalJoint->x;
		pin->externalX=0;
		pin->externalY = 0;
		for (int i = 1; i < this->numJoints; i++) {
			pin->externalY -= joints[i].externalY;
		}
	}
}

void Truss::initialSolve(){
	for (int i = 0; i < numMembers; i++){
		members[i].length = pow(pow(members[i].joint1->x - members[i].joint2->x, 2) + pow(members[i].joint1->y - members[i].joint2->y, 2), 0.5);
//		cout << "m" << i << "L: "<< members[i].length << endl;
	}

	for (int i = 0; i < numJoints; i++) {
		for (int k = 0; k < joints[i].connections.size(); k++) {
			joints[i].connectionLen += members[joints[i].connections[k]].length;
//			cout << "joint " << i << "connectionLen: " << joints[i].connectionLen << endl;
		}
	}

	solveGeneralSystem();

	arma::mat equations = arma::mat(numJoints*2, numMembers, arma::fill::zeros);
	arma::vec external = arma::vec(numJoints*2, arma::fill::zeros);

	for (int i = 0; i < numJoints; i++){
		Joint * j = &joints[i];
		external(i*2) = -j->externalX;
		external(i*2+1) = -j->externalY;

		for(int k = 0; k < joints[i].connections.size(); k++) {
			equations(i * 2, joints[i].connections[k]) = (members[joints[i].connections[k]].joint1->x + members[j->connections[k]].joint2->x - 2 * j->x) / members[j->connections[k]].length;
			equations(i * 2 + 1, joints[i].connections[k]) = (members[joints[i].connections[k]].joint1->y + members[j->connections[k]].joint2->y - 2 * j->y) / members[j->connections[k]].length;
		}
	}
	//external.print();
	//cout << endl;
	//system("pause");
//	equations.print();
//	cout << endl;
//	external.print();
//	cout << endl;
//	system("pause");
	arma::vec forces = solve(equations, external);
//	forces.print();
	for (int i = 0; i < numMembers; i++){
		validForces.push_back(forces(i));
	}
}

bool Truss::solveInternal() {
	//cout << "solving internal" << endl;
	solveGeneralSystem();

	arma::mat equations = arma::mat(numJoints*2, numMembers, arma::fill::zeros);
	arma::vec external = arma::vec(numJoints*2, arma::fill::zeros);

	for (int i = 0; i < numJoints; i++) {
		Joint * j = &joints[i];
		external(i * 2) = -j->externalX;
		external(i * 2 + 1) = -j->externalY;

		for (int k = 0; k < joints[i].connections.size(); k++) {
			equations(i * 2, joints[i].connections[k]) = (members[joints[i].connections[k]].joint1->x + members[j->connections[k]].joint2->x - 2 * j->x) / members[j->connections[k]].length;
			equations(i * 2 + 1, joints[i].connections[k]) = (members[joints[i].connections[k]].joint1->y + members[j->connections[k]].joint2->y - 2 * j->y) / members[j->connections[k]].length;
		}
	}

	//equations.print();
	//cout << endl << "external" << endl;
	/*external.print();
	cout << endl;
	*/
	//external.print();
	//cout << endl;
	//system("pause");
	arma::vec forces = solve(equations, external);

//	forces.print();
//	cout << endl;*/

    bool solveValid = true;
    for (int i = 0; i < numMembers; i++){
    	if(fabs(forces(i)) >= fabs(validForces[i]) && fabs(forces(i)) > MAX_MEMBER_FORCE){
			//cout << "invalid forces" << endl;
			solveValid = false;
    		break;
    	}
    }

    if (solveValid){
		for (int i = 0; i < numMembers; i++){
			validForces[i] = forces(i);
		}
        return true;
    } else {
        return false;
    }
}

double * Truss::checkIfBetterState(bool xDir, int jointNum, double increment) {
//	cout << "checking new state with joint " << jointNum << " moving " << xDir << " by " << increment << endl;
	bool betterState = true;
	Joint * joint = &joints[jointNum];
	int numCon = joint->connections.size();
	double * newLengths = new double[numCon];
	double * oldLengths = NULL;
	double totalLen = 0;

	//changes x or y value based on the direction given
	if(xDir) {
        joint->x += increment;
    } else {
	    joint->y += increment;
	}

	for (int i = 0; i < numCon; i++){
		newLengths[i] = pow( pow(members[joint->connections[i]].joint1->x - members[joint->connections[i]].joint2->x , 2) + pow(members[joint->connections[i]].joint1->y - members[joint->connections[i]].joint2->y, 2 ) , 0.5);
		if(newLengths[i] > members[joint->connections[i]].length  && newLengths[i] > 3.00000000){
			betterState = false;
			break;
		}
//		cout << "length " << i << newLengths[i] << endl;
        totalLen += newLengths[i];
	}
//	cout << "previos length: " << joint->connectionLen << endl;
    if (totalLen >= joint->connectionLen){
        betterState = false;
    }
	if (!betterState) {
		for (int i = 0; i < numMembers; i++) {
			if (fabs(validForces[i]) >= MAX_MEMBER_FORCE) {
				betterState = true;
				break;
			}
		}
	}
	// if new state is better, copy length values into old length and
	if (betterState){
//		cout << "state is better" << endl;
		joint->connectionLen = totalLen;
		oldLengths = new double[numCon];
		for (int i = 0; i< numCon; i++){
		    //change all the member geometry
			oldLengths[i] = members[joint->connections[i]].length;
			members[joint->connections[i]].length = newLengths[i];
//			cout << "m" << i << "L: " << members[joint->connections[i]].length << endl;
		}

	}
	//reverts x or y value based on direction given if new state is not better
	else {
	//	cout << "state is worse. new: " <<totalLen << "\told:" <<joint->connectionLen << endl;
        if(xDir) {
            joint->x -= increment;
        } else {
			joint->y -= increment;
		}
	}

	delete [] newLengths;
	newLengths = NULL;

	return oldLengths;
}

void Truss::revertLengths(int jointNum, double * oldLengths){
	for (int i = 0; i < joints[jointNum].connections.size(); i++){
		members[joints[jointNum].connections[i]].length = oldLengths[i];
	}
}

void Truss::optimize(){
	initialSolve();
	bool systemChanged = false;
	long double movementIncrement = 0.0001;
	double * oldLengths = NULL;

	while (movementIncrement > MIN_MOVEMENT_INCREMENT){
		systemChanged = true;
		while (systemChanged){
			systemChanged = false;
			for (int i  = 1; i < numJoints; i++){

				//testing vertical movement
				if (!joints[i].fixedY) {
					oldLengths = checkIfBetterState(false, i, -movementIncrement);
					if (oldLengths != NULL) {
						if (solveInternal()) {
							systemChanged = true;
						}
						else {
							joints[i].y += movementIncrement;
							revertLengths(i, oldLengths);//move node back and adjust dimensions
							for (int j = 0; j < numJoints; j++) {
								joints[j].connectionLen = 0;
								for (int con = 0; con < joints[j].connections.size(); con++) {
									joints[j].connectionLen += members[joints[j].connections[con]].length;
								}
							}
							solveGeneralSystem();
						}
					}
					else {
						oldLengths = checkIfBetterState(false, i, movementIncrement);
						if (oldLengths != NULL) {
							if (solveInternal()) {
								systemChanged = true;
							}
							else {
								joints[i].y -= movementIncrement;
								revertLengths(i, oldLengths);
								for (int j = 0; j < numJoints; j++) {
									joints[j].connectionLen = 0;
									for (int con = 0; con < joints[j].connections.size(); con++) {
										joints[j].connectionLen += members[joints[j].connections[con]].length;
									}
								}
								solveGeneralSystem();
							}
						}
					}
					if (oldLengths != NULL) {
						delete[] oldLengths;
						oldLengths = NULL;
					}

				}

				//testing horizontal movement
				if (!joints[i].fixedX){
					oldLengths = checkIfBetterState(true, i, movementIncrement*pow(-1, i));
					if (oldLengths != NULL) {
					    if(solveInternal()) {
                            systemChanged = true;
                        } else {
							joints[i].x -= movementIncrement* pow(-1, i);
					    	revertLengths(i, oldLengths);//move node back and adjust dimensions
							for (int j = 0; j < numJoints; j++) {
								joints[j].connectionLen = 0;
								for (int con = 0; con < joints[j].connections.size(); con++) {
									joints[j].connectionLen += members[joints[j].connections[con]].length;
								}
							}
							solveGeneralSystem();
					    }
					} else {
						oldLengths = checkIfBetterState(true, i, -movementIncrement* pow(-1, i));
                        if (oldLengths != NULL){
                            if (solveInternal()){
                                systemChanged = true;
                            } else {
                                joints[i].x += movementIncrement*pow(-1, i);
                            	revertLengths(i, oldLengths);
								for (int j = 0; j < numJoints; j++) {
									joints[j].connectionLen = 0;
									for (int con = 0; con < joints[j].connections.size(); con++) {
										joints[j].connectionLen += members[joints[j].connections[con]].length;
									}
								}
								solveGeneralSystem();
                            }
                        }
					}
					if(oldLengths != NULL){
						delete [] oldLengths;
						oldLengths = NULL;
					}

				}

				//both movement directions tested
				//output(cout);
//				system("pause");
			}

			//all joints have been moved
		}
		//system did not change on last iteration
		movementIncrement /= 2;
		cout << "Movement Increment Changed to: " << movementIncrement << endl;
	}
	//movement increment <0.01
}

void Truss::output(ostream & out) const{
	for(int i  = 0; i < numJoints; i++){
		out << "Joint " << i << ":  (" << joints[i].x << ", " << joints[i].y << ")" << endl;
	}
	out << endl;
	double totalLength = 0;
	for(int i  = 0; i < numMembers; i++){
		totalLength += members[i].length;
		out << "Member " << setw(4) << i << ": " << setw(20) << this->validForces[i] << " kN" << "\tLength: " << members[i].length << endl;
	}
	out << endl << "Total Length: " << totalLength << endl;
}

void Truss::makeCSV(ostream & out) const {
	for (int i = 0; i < numJoints; i++) {
		out <<  joints[i].x-0.1 << "," << joints[i].y << endl;
	}
}


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

#include <ceres/ceres.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include "ceres-error-terms/parameterization/pose-param-jpl.h"

struct CostFunctor {
  explicit CostFunctor(const pose::Transformation& reference)
      : reference_(reference) {}
  template <typename T>
  bool operator()(const T* const x, T* residual) const {
    typedef kindr::minimal::RotationQuaternionTemplate<T> QuaternionT;

    // x is currently passed from x.coeffs() Eigen function that returns
    // (x, y, z, w) JPL convention so we need to map it first to Eigen
    // Quaternion and then construct a kindr quaternion on top
    const Eigen::Map<const Eigen::Quaternion<T>> current_rot(x);
    const Eigen::Map<const Eigen::Matrix<T, 3, 1>> current_pos(x + 4);
    Eigen::Map<Eigen::Matrix<T, 6, 1>> error(residual);

    QuaternionT error_quaternion = common::signedQuaternionProductHamilton(
        QuaternionT(current_rot),
        QuaternionT(
            reference_.getRotation().inverse().toImplementation().cast<T>()));

    error.head(3) = Eigen::Matrix<T, 3, 1>(
        T(2.0) * error_quaternion.x(), T(2.0) * error_quaternion.y(),
        T(2.0) * error_quaternion.z());
    error.tail(3) = current_pos - reference_.getPosition().cast<T>();

    return true;
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  pose::Transformation reference_;
};

TEST(JplPoseParameterization, SimpleMinimization) {
  // Initial state values.
  pose::Quaternion x_rot(1, 0, 0, 0);
  pose::Position3D x_pos(1, 3.7, 0);
  // Create contiguous pose memory.
  Eigen::Matrix<double, 7, 1> x_pose;
  x_pose << x_rot.toImplementation().coeffs(), x_pos;

  // Reference transformation the solver should arrive at.
  pose::Quaternion reference_rot(sqrt(2) / 2, sqrt(2) / 2, 0, 0);
  pose::Position3D reference_pos(0.2, -0.3, 0.7);
  pose::Transformation reference(reference_pos, reference_rot);

  ceres::Problem problem;
  ceres::CostFunction* cost_function =
      new ceres::AutoDiffCostFunction<CostFunctor, 6, 7>(
          new CostFunctor(reference));
  problem.AddResidualBlock(cost_function, NULL, x_pose.data());
  ceres::LocalParameterization* pose_parameterization =
      new ceres_error_terms::JplPoseParameterization;
  problem.SetParameterization(x_pose.data(), pose_parameterization);

  ceres::Solver::Options options;
  options.minimizer_progress_to_stdout = false;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  pose::Quaternion out_rot(
      Eigen::Map<Eigen::Quaternion<double>>(x_pose.head(4).data()));
  Eigen::Vector3d out_pos(x_pose.tail(3));
  EXPECT_NEAR_KINDR_QUATERNION(out_rot, reference_rot, 1e-6);
  EXPECT_NEAR_EIGEN(out_pos, reference_pos, 1e-10);
  LOG(INFO) << summary.BriefReport() << std::endl;
}

MAPLAB_UNITTEST_ENTRYPOINT


/**
 * @date Wed Jan 30 17:25:28 CET 2013
 * @author Ivana Chingovska <ivana.chingovska@idiap.ch>
 *
 * @brief GLCMProp implementation
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <bob.core/array_copy.h>
#include <bob.core/assert.h>
#include <boost/make_shared.hpp>

#include <bob.ip.base/GLCM.h>

static double sqr(const double x)
{
  return x*x;
}

bob::ip::base::GLCMProp::GLCMProp(){ }

bob::ip::base::GLCMProp::~GLCMProp() { }

const blitz::Array<double,3> bob::ip::base::GLCMProp::normalize_glcm(const blitz::Array<double,3>& glcm) const
{
   blitz::firstIndex i;
   blitz::secondIndex j;
   blitz::thirdIndex k;
   blitz::Array<double, 2> summations_temp(blitz::sum(glcm(i, k, j), k));
   blitz::Array<double, 1> summations(blitz::sum(summations_temp(j,i), j));
   blitz::Array<double,3> res(glcm / summations(k));
   return res;
}

const blitz::TinyVector<int,1> bob::ip::base::GLCMProp::get_prop_shape(const blitz::Array<double,3>& glcm) const
{
  blitz::TinyVector<int,1> res;
  res(0) = glcm.extent(2);
  return res;
}

void bob::ip::base::GLCMProp::angular_second_moment(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = blitz::pow2(glcm_norm(rall, rall, l));
    prop(l) = blitz::sum(mat); // angular second moment
  }
}

void bob::ip::base::GLCMProp::energy(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  //do the computation of the feature
  angular_second_moment(glcm, prop);
  prop = blitz::sqrt(prop);
}

void bob::ip::base::GLCMProp::variance(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(sqr(i-blitz::mean(mat))*mat);
  }
}

void bob::ip::base::GLCMProp::contrast(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum((i-j)*(i-j)*mat);
  }
  /*
  //as done in [1]
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double contrast = 0;
    for (int t=0; t < glcm_norm.extent(0) - 1; ++t) // iterate through all the levels
    {
      contrast += t*t*blitz::sum(blitz::where(abs(i-j)==t, mat, 0));
    }
    prop(l) = contrast;
  }
  */
}

void bob::ip::base::GLCMProp::auto_correlation(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(i*j*mat);
  }
}

void bob::ip::base::GLCMProp::correlation(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double mean_x = blitz::sum(i*mat);
    double mean_y = blitz::sum(j*mat);
    double std_x = sqrt(blitz::sum(sqr(i-mean_x)*mat));
    double std_y = sqrt(blitz::sum(sqr(j-mean_y)*mat));
    prop(l) = (blitz::sum(i*j*mat) - mean_x*mean_y) / (std_x * std_y);
  }
}

void bob::ip::base::GLCMProp::correlation_m(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double mean_x = blitz::sum(i*mat);
    double mean_y = blitz::sum(j*mat);
    double std_x = sqrt(blitz::sum(sqr(i-mean_x)*mat));
    double std_y = sqrt(blitz::sum(sqr(j-mean_y)*mat));
    prop(l) = blitz::sum(((i-mean_x) * (j-mean_x) * mat) / (std_x * std_y));
  }
}

void bob::ip::base::GLCMProp::inv_diff_mom(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(mat / (1 + sqr(i-j)));
  }
}

void bob::ip::base::GLCMProp::sum_avg(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double sum_avg = 0;
    for (int t = 0; t < 2 * glcm_norm.extent(0) - 1; t++) // iterate through all the levels
    {
      sum_avg += t * blitz::sum(blitz::where(i+j==t, mat, 0));
    }
    prop(l) = sum_avg;
  }
}

void bob::ip::base::GLCMProp::sum_var(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  blitz::Array<double,1>& prop_sum_entropy(prop);
  sum_entropy(glcm, prop_sum_entropy);
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double sum_var = 0;
    for (int t = 0; t < 2 * glcm_norm.extent(0) -1; t++) // iterate through all the levels
    {
      sum_var += sqr(t-prop_sum_entropy(l)) * blitz::sum(blitz::where(i+j==t, mat, 0));
    }
    prop(l) = sum_var;
  }
}

void bob::ip::base::GLCMProp::sum_entropy(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double sum_entropy = 0;
    for (int t = 0; t < 2 * glcm_norm.extent(0) - 1; t++) // iterate through all grey levels
    {
      sum_entropy += blitz::sum(blitz::where(i+j==t, mat, 0)) * log(blitz::sum(blitz::where(i+j==t, mat, 0)) + std::numeric_limits<double>::min());
    }
    prop(l) = -sum_entropy;
  }
}

void bob::ip::base::GLCMProp::entropy(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = -blitz::sum(mat * blitz::log(mat + std::numeric_limits<double>::min())); // small numeric value is added to avoid 0 as an argument to the logarithm
  }
}

void bob::ip::base::GLCMProp::diff_var(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double diff_var = 0;
    for (int t = 0; t < glcm_norm.extent(0); t++) // iterate through all grey levels
    {
      diff_var +=  t * t * blitz::sum(blitz::where(abs(i-j)==t, mat, 0));
    }
    prop(l) = diff_var;
  }
}

void bob::ip::base::GLCMProp::diff_entropy(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double diff_entropy = 0;
    for (int t = 0; t < glcm_norm.extent(0); t++) // iterate through all grey levels
    {
      diff_entropy += blitz::sum(blitz::where(abs(i-j)==t, mat, 0)) * log(blitz::sum(blitz::where(abs(i-j)==t, mat, 0)) + std::numeric_limits<double>::min());
    }
    prop(l) = -diff_entropy;
  }
}

void bob::ip::base::GLCMProp::dissimilarity(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(abs(i-j)*mat);
  }
}

void bob::ip::base::GLCMProp::homogeneity(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(mat / (1 + abs(i-j)));
  }
}

void bob::ip::base::GLCMProp::cluster_prom(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double mean_x = blitz::sum(i*mat);
    double mean_y = blitz::sum(j*mat);
    prop(l) = blitz::sum(pow(i + j - mean_x - mean_y, 4) * mat);
  }
}

void bob::ip::base::GLCMProp::cluster_shade(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    double mean_x = blitz::sum(i*mat);
    double mean_y = blitz::sum(j*mat);
    prop(l) = blitz::sum(pow(i + j - mean_x - mean_y, 3) * mat);
  }
}

void bob::ip::base::GLCMProp::max_prob(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::max(mat);
  }
}

void bob::ip::base::GLCMProp::inf_meas_corr1(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  blitz::Array<double,1>& prop_entropy(prop);
  entropy(glcm, prop_entropy); //calculate the entropy
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);

    blitz::Array<double,1> marg_prob_i(blitz::sum(mat,j)); // marginal probability of first dimension (i.e. row-wise sum)
    blitz::Array<double,1> marg_prob_j(blitz::sum(mat(j,i),j)); // marginal probability of second dimension (i.e. column-wise sum)

    double hxy1 = -blitz::sum(mat * blitz::log(marg_prob_i(i) * marg_prob_j(j) + std::numeric_limits<double>::min())); // small numeric value is added to avoid 0 as an argument to the logarithm
    double px_entropy = -blitz::sum(marg_prob_i * blitz::log(marg_prob_i + std::numeric_limits<double>::min()));
    double py_entropy = -blitz::sum(marg_prob_j * blitz::log(marg_prob_j + std::numeric_limits<double>::min()));
    prop(l) = (prop_entropy(l) - hxy1) / std::max(px_entropy, py_entropy);
  }
}

void bob::ip::base::GLCMProp::inf_meas_corr2(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{

  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  blitz::Array<double,1>& prop_entropy(prop);
  entropy(glcm, prop_entropy); //calculate the entropy
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    blitz::Array<double,1> marg_prob_i(blitz::sum(mat,j)); // marginal probability of first dimension (i.e. row-wise sum)
    blitz::Array<double,1> marg_prob_j(blitz::sum(mat(j,i),j)); // marginal probability of second dimension (i.e. column-wise sum)

    double hxy2 = -blitz::sum(marg_prob_i(i) * marg_prob_j(j) * blitz::log(marg_prob_i(i) * marg_prob_j(j) + std::numeric_limits<double>::min())); // small numeric value is added to avoid 0 as an argument to the logarithm
    prop(l) = sqrt(1 - exp(-2 * (hxy2 - prop_entropy(l))));
  }

}

void bob::ip::base::GLCMProp::inv_diff(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  homogeneity(glcm, prop);
}

void bob::ip::base::GLCMProp::inv_diff_norm(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(mat / (1 + (abs(i-j) / (double)mat.extent(0)) ));
  }
}

void bob::ip::base::GLCMProp::inv_diff_mom_norm(const blitz::Array<double,3>& glcm, blitz::Array<double,1>& prop) const
{
  // check if the size of the output matrix is as expected
  blitz::TinyVector<int,1> shape(get_prop_shape(glcm));
  bob::core::array::assertSameShape(prop, shape);

  // normalize the input GLCM matrix
  blitz::Array<double,3> glcm_norm = normalize_glcm(glcm);

  blitz::Array<double,2> mat(glcm.extent(0), glcm.extent(1)); // auxiliary matrix that will be used for glcm matrix for one particular offset

  blitz::Range rall = blitz::Range::all();
  blitz::firstIndex i;
  blitz::secondIndex j;

  //do the computation of the feature
  for (int l=0; l < glcm_norm.extent(2); ++l)
  {
    mat = glcm_norm(rall, rall, l);
    prop(l) = blitz::sum(mat / (1 + (sqr(i-j) / sqr(mat.extent(0)))));
  }
}


#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT

#include <unistd.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include "image_segmentation_utils.h"
#include <getopt.h>

using namespace cv;

int main(int argc, char *argv[]) {
	int o;
	char* imagePath;
	int numNodes = 5000;
	char* outputPath;
    std::string logFile;
    bool log_file_is_set = false;
	int option_idx;

	struct option long_options[] = {
        {"input", required_argument, NULL, 'i'},
        {"pixel", required_argument, NULL, 'n'},
        {"output", required_argument, NULL, 'o'},
        {"log", required_argument, NULL, 'l'},
    };

	if (argc == 1) {
		printf("Usage: image_segmentation <command>\n\nCommands:\n  -i --input <path>\tInput file path\n"
				"  -n --pixel <num_pixels>\tNumber of pixels of the output picture (5000 by default)\n"
				"  -o --output <path>\tOutput file path (prefer .bmp extension to avoid opencv bug)\n"
				"  -l --log <path>\tLog file path \n");
		return 0;
	}
    const char *optstring = "i:n:o:l:h";
    while ((o = getopt_long(argc, argv, optstring, long_options, &option_idx)) != -1) {
        switch (o) {
            case 'i':
				imagePath = optarg;
                break;
            case 'n':
                numNodes = atoi(optarg);
				break;
            case 'o':
				outputPath = optarg;
				break;
			case 'l':
				log_file_is_set = true;
				logFile = optarg;
				break;
			default:
				printf("Usage: image_segmentation <command>\n\nCommands:\n  -i --input <path>\tInput file path\n"
						"  -n --pixel <num_pixels>\tNumber of pixels of the output picture (5000 by default)\n"
						"  -o --output <path>\tOutput file path (prefer .bmp extension to avoid opencv bug)\n"
						"  -l --log <path>\tLog file path \n");
				return 0;
		}
	}
	Mat image = imread(imagePath, 0);
	double lambda = 3.0;

	/* Scaling the picture */
	double scale = std::sqrt(numNodes / (double) (image.rows * image.cols));
	Mat scaled_image;
	Size scaled_size = Size(std::round(scale * image.cols), std::round(scale * image.rows));
	resize(image, scaled_image, Size(), scale, scale);

	int numPixels = scaled_image.rows * scaled_image.cols;
	DenseMatrix I;
	cv2eigen(scaled_image, I);

	/* Rescale the value of each pixel between [0, 1] */
	I.array() = I.array() / 256.0;
	DenseMatrix unary_cost;
	SparseMatrix binary_cost;

	double sigma = 0.1;
	double b = 0.6;
	double f1 = 0.2;
	double f2 = 0.2;

	get_unary_cost(I, sigma, b, f1, f2, unary_cost);
	get_binary_cost(I, binary_cost); /* Actually calculating round(lambda * W) in this so we can get the
									  * binary cost directly */

	unary_cost.array() = unary_cost.array().round();

	SparseMatrix _A;
	DenseVector _b;
	get_A_b_from_cost(unary_cost, binary_cost, _A, _b);

	printf("Finished generating matrices, starting algorithm\n");
	Solution sol;
	DenseVector x0 = DenseVector::Zero(_b.rows());
	LPboxADMMsolver solver;
	solver.ADMM_bqp_unconstrained_init();

	if (log_file_is_set) {
		solver.set_log_file(logFile);
		printf("Writing log output to %s\n", logFile.c_str());
	}
	solver.ADMM_bqp_unconstrained_legacy(_A.cols(), _A, _b, x0, sol);
	printf("Algorithm finishes, saving picture\n");

	/* Reshape the solution to fit the size of the image */
	DenseMatrix reshaped_mat = Eigen::Map<DenseMatrix>(sol.x_sol->data(), I.rows(), I.cols());

	/* Setting the location with value 1 to be white */
	DenseIntMatrix r_mat = (reshaped_mat.array() >= 0.5).matrix().cast<int>() * 255;
	Mat res_mat(r_mat.rows(), r_mat.cols(), CV_8UC1);
	eigen2cv(r_mat, res_mat);
	imwrite(outputPath, res_mat);

	delete sol.best_sol;
	delete sol.x_sol;
	delete sol.y1;
	delete sol.y2;
	return 1;
}


#ifndef BFGS_HPP_
#define BFGS_HPP_

#include <Eigen/Core>

/**
 * @file BFGS.hpp
 */

namespace rwlibs { namespace algorithms {

    /** @addtogroup algorithms */
    /*@{*/

    /**
     * @brief BFGS is a class including the BFGS minimization algorithm.
     *
     * The BFGS minimization algorithm has been implemented as described in "Numerical Optimization
     * - by Jorge Nocedal and Stephen J. Wright" chapter 6+3. \sa \ref bfgsExample.cpp
     * "../example/bfgsApp/bfgsExample.cpp"
     */
    class BFGS
    {
      public:
        //! Vector type used in the minimazation algorithm.
        typedef Eigen::VectorXd vector;
        //! MAtrix type used in the minimazation algorithm.
        typedef Eigen::MatrixXd matrix;

        /**
         * @brief Minimisation function struct.
         */
        struct BFGS_function_struct
        {
            /** Function pointer to the static minimization function @f$f(vec{x})@f$. */
            double (*f) (const vector* x, void* params);
            /** Function pointer to the static minimization function @f$df(vec{x})@f$. */
            void (*df) (const vector* x, void* params, vector* g);
            /** Void pointer to optional data that the minimization function might require */
            void* params;
        };

        /**
         * @brief Optimization status.
         */
        enum OPTM_STATUS {
            /** Indicating a problem with the numerical precision when evaluating the gradient. */
            GRADIENTWARNING = 0,
            /** Indicating a successfully minimization. */
            SUCCESS
        };

        /**
         * @brief Minimize a function using the BFGS algorithm.
         * @param startguess Start guess for the minimizer parameters. Replaced with minima solution
         * at end of minimization.
         * @param function BFGS_function_struct including pointers to the minimization function f,
         * df and a void pointer to other data for the minimization function.
         * @param tolerance Indicating when an acceptable minima has been found by evaluating if @f$
         * tolerance>||\Delta f(x)||_2 @f$.
         * @param iterationLimit Maximum number of iterations for the BFGS algorithm.
         * @param initialStepsize Initial step size for the BFGS algorithm.
         * @param c1 Value used to ensure the "strong Wolfe conditions" are satisfied with the value
         * c1. See "Numerical Optimization - by Jorge Nocedal and Stephen J. Wright" chapter 3.
         * Typical value = 1e-4.
         * @param c2 Value used to ensure the "strong Wolfe conditions" are satisfied with the value
         * c2. See "Numerical Optimization - by Jorge Nocedal and Stephen J. Wright" chapter 3.
         * Typical value = 0.9.
         * @param alphamax Maximum stepsize used in iterations. Typical value of 1.0 is used to
         * produce superlinear convergence of the overall algorithm.
         * @return GRADIENTWARNING on numerically precision problems SUCCESS when a minima is found.
         */
        static int optimizer (vector& startguess, BFGS_function_struct function, double tolerance,
                              unsigned int iterationLimit, double initialStepsize = 1.0,
                              double c1 = 1e-4, double c2 = 0.9, double alphamax = 1.0);

      private:
        BFGS () {}

        static void colDotRow (vector& colvec, vector& rowvec, matrix& result);

        static double lineSearch (BFGS_function_struct function, vector& xk, vector& pk, double c1,
                                  double c2, double alphamax);

        static double phiGradient (BFGS_function_struct function, vector& xk, vector& pk,
                                   double alpha, vector& tempArray, double phi_alpha, double eps);

        static double zoom (double& alphalow, double& alphahigh, double& phi_alphalow,
                            double& dphi_alphalow, double& phi_alphahigh,
                            BFGS_function_struct function, vector& xk, vector& pk,
                            vector& tempArray, double phi_alpha_zero, double dphi_alpha_zero,
                            double c1, double c2, double eps);

        static double quadraticInterpolation (double phi_alpha_lo, double dphi_alpha_lo,
                                              double alpha_lo, double phi_alpha_hi,
                                              double alpha_hi);
    };
    /** \example bfgsExample.cpp
     * Example of using the BFGS optimization algorithm for finding a minimum in the Rosenbrock
     * function.
     *
     * The Rosenbrock function are defined by: @f$ f(x, y) = (1-x)^2 + 100(y-x^2)^2 @f$
     */

    /*@}*/
}}     // namespace rwlibs::algorithms
#endif /* BFGS_HPP_ */


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Melman, J. Propagate software, J.C.P.Melman@tudelft.nl, 2010.
 *      NASA, Goddard Spaceflight Center. Orbit Determination Toolbox (ODTBX), NASA - GSFC Open
 *          Source Software, http://opensource.gsfc.nasa.gov/projects/ODTBX/, last accessed:
 *          31st January, 2012.
 *      ESA, GTOP Toolbox, http://www.esa.int/gsp/ACT/doc/INF/Code/globopt/GTOPtoolbox.rar.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h"

#include <Eigen/Core>

#include <map>
#include <limits>

#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Basics/testMacros.h"

#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Basics/basicTypedefs.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/keplerPropagatorTestData.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

using namespace orbital_element_conversions;

//! Test 1: Comparison of propagateKeplerOrbit() output with benchmark data from (Melman, 2010).
BOOST_AUTO_TEST_CASE( testPropagateKeplerOrbit_Eccentric_Melman )
{
    // Load benchmark data.
    // This data originates from J. Melman and is generated by the software package Propagate.

    // Create propagation history map for benchmark data to be stored in.
    PropagationHistory benchmarkKeplerPropagationHistory = getMelmanBenchmarkData( );

    // Propagate to final state in Keplerian elements.
    Eigen::Vector6d computedFinalStateInKeplerianElements
            = propagateKeplerOrbit(
                benchmarkKeplerPropagationHistory.begin( )->second,
                benchmarkKeplerPropagationHistory.rbegin( )->first -
                benchmarkKeplerPropagationHistory.begin( )->first,
                getMelmanEarthGravitationalParameter( ) );

    // Check that computed results match expected results.
    BOOST_CHECK_CLOSE_FRACTION(
                benchmarkKeplerPropagationHistory.rbegin( )->second( 5 ),
                basic_mathematics::computeModulo( computedFinalStateInKeplerianElements( 5 ),
                                                  2.0 * mathematical_constants::PI ), 1.0e-8 );
}

//! Test 2: Comparison of kepprop2b() test output from (GSFC, 2012) using modulo option.
BOOST_AUTO_TEST_CASE( testPropagateKeplerOrbit_Eccentric_kepprop2b_modulo )
{
    // Create expected propagation history.
    PropagationHistory expectedPropagationHistory = getODTBXBenchmarkData( );

    // Set Earth gravitational parameter [m^3 s^-2].
    const double earthGravitationalParameter = 398600.4415e9;

    // Set time step for ODTBX benchmark data.
    const double timeStep = 8640.0;

    // Compute propagation history.
    PropagationHistory computedPropagationHistory;
    computedPropagationHistory[ 0.0 ] = expectedPropagationHistory[ 0.0 ];

    for ( unsigned int i = 1; i < expectedPropagationHistory.size( ); i++ )
    {
        computedPropagationHistory[ static_cast< double >( i ) * timeStep ]
                = propagateKeplerOrbit(
                    computedPropagationHistory[ static_cast< double >( i - 1 ) * timeStep ],
                    timeStep, earthGravitationalParameter  );

        computedPropagationHistory[ static_cast< double >( i ) * timeStep ]( 5 )
                = basic_mathematics::computeModulo(
                    computedPropagationHistory[ static_cast< double >( i ) * timeStep ]( 5 ),
                    2.0 * mathematical_constants::PI );

        // Check that computed results match expected results.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    computedPropagationHistory[ static_cast< double >( i ) * timeStep ],
                    expectedPropagationHistory[ static_cast< double >( i ) * timeStep ],
                    1.0e-13 );
    }
}

//! Test 3: Comparison of kepprop2b() test output from (GSFC, 2012), propagating backwards.
BOOST_AUTO_TEST_CASE( testPropagateKeplerOrbit_Eccentric_kepprop2b_backwards )
{
    // Create expected propagation history.
    PropagationHistory expectedPropagationHistory = getODTBXBenchmarkData( );

    // Set Earth gravitational parameter [m^3 s^-2].
    const double earthGravitationalParameter = 398600.4415e9;

    // Set time step for ODTBX benchmark data.
    const double timeStep = 8640.0;

    // Compute propagation history.
    PropagationHistory computedPropagationHistory;
    computedPropagationHistory[ 10.0 * 8640.0 ] = expectedPropagationHistory[ 10.0 * 8640.0 ];

    for ( int i = expectedPropagationHistory.size( ) - 2; i >= 0; i-- )
    {
        computedPropagationHistory[ static_cast< double >( i ) * timeStep ]
                = propagateKeplerOrbit(
                    computedPropagationHistory[ static_cast< double >( i + 1 ) * timeStep ],
                    -timeStep, earthGravitationalParameter );

        computedPropagationHistory[ static_cast< double >( i ) * timeStep ]( 5 )
                = basic_mathematics::computeModulo(
                    computedPropagationHistory[ static_cast< double >( i ) * timeStep ]( 5 ),
                    2.0 * mathematical_constants::PI );

        // Check that computed results match expected results.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    computedPropagationHistory[ static_cast< double >( i ) * timeStep ],
                    expectedPropagationHistory[ static_cast< double >( i ) * timeStep ],
                    1.0e-13 );
    }
}

//! Test 4: Comparison of hyperbolic kepler propagation with that of GTOP.
BOOST_AUTO_TEST_CASE( testPropagateKeplerOrbit_hyperbolic_GTOP )
{
    // Load the expected propagation history.
    PropagationHistory expectedPropagationHistory = getGTOPBenchmarkData( );

    // Set the time step for the GTOP benchmark data to 100 days.
    const double timeStep = 86400.0 * 100.0;

    // Compute propagation history.
    PropagationHistory computedPropagationHistory;
    computedPropagationHistory[ 0.0 ] = expectedPropagationHistory[ 0.0 ];

    for ( unsigned int i = 1; i < expectedPropagationHistory.size( ); i++ )
    {
        // Compute next entry.
        computedPropagationHistory[ static_cast< double >( i ) * timeStep ] =
                propagateKeplerOrbit(
                    computedPropagationHistory[ static_cast< double >( i - 1 ) * timeStep ],
                    timeStep, getGTOPGravitationalParameter( ) );

        // Check that computed results match expected results.
        BOOST_CHECK_CLOSE_FRACTION(
                    computedPropagationHistory[ static_cast< double >( i ) * timeStep ]( 5 ),
                    expectedPropagationHistory[ static_cast< double >( i ) * timeStep ]( 5 ),
                    1.0e-15 );
    }
}

//! Test 5: Unit test that failed on versions that caused the old modulo function to crash.
BOOST_AUTO_TEST_CASE( testPropagateKeplerOrbit_FunctionFailingOnOldModuloFunction )
{
    // Set expected true anomaly.
    const double expectedTrueAnomaly = -3.1245538487052089;

    // Set the propagation time.
    const double propagationTime = 8651869.8944624383;

    // Set the gravitational parameter (of the Sun).
    const double gravitationalParameter = 1.32712428e20;

    // Set initial Keplerian elements.
    Eigen::Vector6d keplerElements;
    keplerElements << 56618890355.593132, 0.99961601437304082, 1.0238269559089248,
            3.1526292818328812, 1.5807574453453865, 3.1478950321924795;

    // Propagate Keplerian elements.
    keplerElements = propagateKeplerOrbit(
                keplerElements, propagationTime, gravitationalParameter );

    // Check that computed results match expected results.
    BOOST_CHECK_CLOSE_FRACTION( keplerElements( trueAnomalyIndex ),
                                expectedTrueAnomaly,
                                1.0e-15 );
}

//! Test 6. Propagation test using ODTBX test Kepler elements.
BOOST_AUTO_TEST_CASE( testMeanAnomalyAgainstMeanMotion )
{
    std::vector< double > doubleErrors;
    // Test using double parameters.
    {
        double gravitationalParameter = 398600.4415e9;
        Eigen::Vector6d initialStateInKeplerianElements;

        initialStateInKeplerianElements << 42165.3431351313e3, 0.26248354351331, 0.30281462522101,
                4.71463172847351, 4.85569272927819, 2.37248926702153;
        double timeStep = 600.0;
        double meanMotion = std::sqrt( gravitationalParameter /
                                       std::pow( initialStateInKeplerianElements( 0 ), 3.0 ) );

        double initialMeanAnomaly = convertEccentricAnomalyToMeanAnomaly(
                    convertTrueAnomalyToEccentricAnomaly(
                        initialStateInKeplerianElements( 5 ), initialStateInKeplerianElements( 1 ) ),
                    initialStateInKeplerianElements( 1 ) );

        double propagationTime, propagatedMeanAnomaly;

        Eigen::Vector6d propagatedKeplerElements;

        for( int i = -25; i < 26; i++ )
        {
            propagationTime = static_cast< double >( i ) * timeStep;
            propagatedKeplerElements = propagateKeplerOrbit(
                        initialStateInKeplerianElements, propagationTime, gravitationalParameter );
            propagatedMeanAnomaly = convertEccentricAnomalyToMeanAnomaly(
                        convertTrueAnomalyToEccentricAnomaly(
                            propagatedKeplerElements( 5 ), initialStateInKeplerianElements( 1 ) ),
                        initialStateInKeplerianElements( 1 ) );
            doubleErrors.push_back( meanMotion * propagationTime - ( propagatedMeanAnomaly - initialMeanAnomaly ) );
        }
    }

    std::vector< double > longDoubleErrors;
    // Test using long double parameters.
    {
        long double gravitationalParameter = 398600.4415e9L;
        Eigen::Matrix< long double, 6, 1 > initialStateInKeplerianElements;

        initialStateInKeplerianElements << 42165.3431351313e3L, 0.26248354351331L, 0.30281462522101L,
                4.71463172847351L, 4.85569272927819L, 2.37248926702153L;
        long double timeStep = 600.0L;
        long double meanMotion = std::sqrt( gravitationalParameter /
                                            ( initialStateInKeplerianElements( 0 ) *
                                              initialStateInKeplerianElements( 0 ) *
                                              initialStateInKeplerianElements( 0 ) ) );

        long double initialMeanAnomaly = convertEccentricAnomalyToMeanAnomaly< long double >(
                    convertTrueAnomalyToEccentricAnomaly< long double >(
                        initialStateInKeplerianElements( 5 ), initialStateInKeplerianElements( 1 ) ),
                    initialStateInKeplerianElements( 1 ) );

        long double propagationTime, propagatedMeanAnomaly;

        Eigen::Matrix< long double, 6, 1 > propagatedKeplerElements;

        for( int i = -25; i < 26; i++ )
        {
            propagationTime = static_cast< long double >( i ) * timeStep;

            propagatedKeplerElements = propagateKeplerOrbit< long double >(
                        initialStateInKeplerianElements, propagationTime, gravitationalParameter );
            propagatedMeanAnomaly = convertEccentricAnomalyToMeanAnomaly< long double >(
                        convertTrueAnomalyToEccentricAnomaly< long double >(
                            propagatedKeplerElements( 5 ), initialStateInKeplerianElements( 1 ) ),
                        initialStateInKeplerianElements( 1 ) );

            longDoubleErrors.push_back( static_cast< double >(
                                            meanMotion * propagationTime -
                                            ( propagatedMeanAnomaly - initialMeanAnomaly ) ) );
        }
    }

    for( unsigned int i = 0; i < doubleErrors.size( ); i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( static_cast< double >( longDoubleErrors.at( i ) ) ),
                           static_cast< double >( 5.0 * std::numeric_limits< long double >::epsilon( ) ) );
        BOOST_CHECK_SMALL( std::fabs( static_cast< double >( doubleErrors.at( i ) ) ),
                           static_cast< double >( 5.0 * std::numeric_limits< double >::epsilon( ) ) );
    }
}
} // namespace unit_tests
} // namespace tudat


/**
 * BRIValue (Boost Rational with Infinity Value) class for Discrete Event Simulation purposes
 * Copyright (C) 2016  Laouen Mayal Louan Belloli
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef BRIVALUE_H
#define BRIVALUE_H

#include <iostream>
#include <string>
#include <boost/rational.hpp>

using namespace std;

class BRIValue {

  private:
    boost::rational<int> _value;
    bool _inf;
    bool _possitive;

  public:
    BRIValue() : _inf(false) {}
    BRIValue(int n) : _value(n), _inf(false) {}
    BRIValue(int n, int d) : _value(n,d), _inf(false) {}

    static BRIValue infinity() noexcept {
      BRIValue f;
      f._inf=true;
      f._possitive=true;
      return f;
    }

    static BRIValue minusInfinity() noexcept {
      BRIValue f;
      f._inf=true;
      f._possitive=false;
      return f;
    }

    BRIValue& operator=(const BRIValue& o) noexcept {
      this->_value = o._value;
      this->_inf = o._inf;
      this->_possitive = o._possitive;
      return *this;
    }

    /* Aritmetical operators */

    BRIValue& operator+=(const BRIValue& o) noexcept {
      this->_value += o._value;
      if (o._inf) {
        this->_inf = o._inf;
        this->_possitive = o._possitive; // + * {+/-} = {+/-}
      };
      return *this;
    }

    BRIValue& operator-=(const BRIValue& o) noexcept {
      this->_value -= o._value;
      if (!this->_inf && o._inf) {
        this->_inf = o._inf;
        this->_possitive = !o._possitive; // - * {+/-} = {-/+}
      } else if (o._inf && (this->_possitive == o._possitive)) { //-inf-(-inf) = -inf+inf = 0 = inf-inf
        this->_inf = false;
        this->_value = boost::rational<int>(0);
      }
      return *this;
    }

    BRIValue& operator/=(const BRIValue& o) noexcept {
      if (!this->inf) {
        if (o._inf) {
          this->_value = boost::rational<int>(0);
        } else {
          this->_value /= o._value;
        }
      }
      return *this;
    }

    BRIValue& operator*=(const BRIValue& o) noexcept {
      if (!this->inf && o._inf) {
        this->_inf = o._inf;
        this->_possitive = o._possitive;
      } else if (o._inf && (this->_possitive == o._possitive)) { // (+ * + = -) and (- * - = +)
        this->_possitive = !this->_possitive;
      } else {
        this->_value *= o._value;
      }
      return *this;
    }

    BRIValue& operator--() noexcept {
      this->_value -= boost::rational<int>(1);
      return *this;
    }

    BRIValue& operator++() noexcept {
      this->_value += boost::rational<int>(1);
      return *this;
    }

    string naturalDisplay() {
      if (this->_inf) {
        if (this->_possitive)
          return "inf";
        else
          return "-inf";
      }

      return to_string(_value.numerator()) + "/" + to_string(_value.denominator());
    }
};

inline BRIValue operator+(const BRIValue lhs, const BRIValue& rhs) noexcept {
  BRIValue res = lhs;
  res += rhs;
  return res;
}

inline BRIValue operator-(const BRIValue lhs, const BRIValue& rhs) noexcept {
  BRIValue res = lhs;
  res -= rhs;
  return res;
}

inline BRIValue operator/(const BRIValue lhs, const BRIValue& rhs) noexcept {
  BRIValue res = lhs;
  res /= rhs;
  return res;
}

inline bool operator==(const BRIValue& lhs, const BRIValue& rhs) noexcept {

  if (lhs._inf && rhs._inf) return (lhs._possitive == rhs._possitive);
  else if (lhs._inf || rhs._inf) return false;
  return (lhs._value == rhs._value);
}

inline bool operator!=(const BRIValue& lhs, const BRIValue& rhs) noexcept {
  return !operator==(lhs,rhs);
}

inline bool operator<(const BRIValue& lhs, const BRIValue& rhs) noexcept {
  if (lhs._inf && lhs._possitive) return false;
  else if (lhs._inf && !lhs._possitive) return !(rhs._inf && !rhs._possitive);
  else if (rhs._inf && rhs._possitive) return true;
  else if (rhs._inf) return false;
  return (lhs._value < rhs._value);
}

inline bool operator>(const BRIValue& lhs, const BRIValue& rhs) noexcept {
  return  operator< (rhs,lhs);
}

inline bool operator<=(const BRIValue& lhs, const BRIValue& rhs) noexcept {
  return !operator> (lhs,rhs);
}

inline bool operator>=(const BRIValue& lhs, const BRIValue& rhs) noexcept {
  return !operator< (lhs,rhs);
}

inline std::ostream& operator<<(std::ostream& os, const BRIValue& t) noexcept {

  if (t._inf) {
    if (t._possitive)
      os << "inf";
    else
      os << "-inf";
  } else {
    os << t._value;
  }
  return os;
}

inline std::istream& operator>>std::istream& is, BRIValue& rhs) noexcept {
  string a;
  int n,d;
  is >> a;
  if (a == "inf") rhs = BRIValue::infinity();
  else if (a == "-inf") = BRIValue::minusInfinity();
  else {
    n = std::stoi(a.substr(0, a.find_last_of("/")));
    d = std::stoi(a.substr(a.find_last_of("/")+1));
    rhs = BRIValue(n,d);
  }
  return is;
}

  //TODO: Chack this specialization
  // Specialize numeric_limits
namespace std {
  template<>
  class numeric_limits<BRIValue>{
  public:
    static constexpr bool is_specialized = true;
    static BRIValue min() noexcept { return BRIValue(-1,1) * BRIValue{numeric_limits<int>::max(), numeric_limits<int>::min()}; }
    static BRIValue max() noexcept { return BRIValue{numeric_limits<int>::max(), numeric_limits<int>::min()}; }
    static BRIValue lowest() noexcept { return BRIValue(-1,1) * BRIValue{numeric_limits<int>::max(), numeric_limits<int>::min()}; }

    static constexpr bool is_signed = true;
    static constexpr bool is_integer = false;
    static constexpr bool is_exact = true;
    static BRIValue epsilon() noexcept { return BRIValue{1,1} - BRIValue{numeric_limits<int>::max(), numeric_limits<int>::max() - 1}; }
    static BRIValue round_error() noexcept { return BRIValue(0); }

    static constexpr int  min_exponent = numeric_limits<int>::min(); // trash_value
    static constexpr int  min_exponent10 = min_exponent/radix; // trash_value
    static constexpr int  max_exponent = numeric_limits<int>::max(); // trash_value
    static constexpr int  max_exponent10 = max_exponent/radix; // trash_value

    static constexpr bool has_infinity = true;
    static constexpr bool has_quiet_NaN = false;
    static constexpr bool has_signaling_NaN = false;
    static constexpr float_denorm_style has_denorm = denorm_indeterminate;
    static constexpr bool has_denorm_loss = false;
    static BRIValue infinity() noexcept { return BRIValue::infinity(); }

    static constexpr bool is_iec559 = false;
    static constexpr bool is_bounded = false;
    static constexpr bool is_modulo = false;

    static constexpr bool traps = false;
    static constexpr bool tinyness_before = false;
  };
}

#endif // BRIVALUE_H


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

// LOCAL
#include "CameraProjection.h"
#include "vtkEigenTools.h"

// STD
#include <iostream>
#include <fstream>
#include <sstream>

// BOOST
#include <boost/algorithm/string.hpp>

//-----------------------------------------------------------------------------
Eigen::Vector3d GetRGBColourFromReflectivity(double v, double vmin, double vmax)
{
   Eigen::Vector3d c(1.0, 1.0, 1.0); // white
   double dv;
   if (v < vmin)
      v = vmin;
   if (v > vmax)
      v = vmax;
   dv = vmax - vmin;

   if (v < (vmin + 0.25 * dv)) {
      c[2] = 0;
      c[1] = 4 * (v - vmin) / dv;
   } else if (v < (vmin + 0.5 * dv)) {
      c[2] = 0;
      c[0] = 1 + 4 * (vmin + 0.25 * dv - v) / dv;
   } else if (v < (vmin + 0.75 * dv)) {
      c[2] = 4 * (v - vmin - 0.5 * dv) / dv;
      c[0] = 0;
   } else {
      c[1] = 1 + 4 * (vmin + 0.75 * dv - v) / dv;
      c[0] = 0;
   }
   return 255.0 * c;
}

//----------------------------------------------------------------------------
void LoadCameraParamsFromCSV(std::string filename, Eigen::VectorXd& W)
{
  // Load file and check that the file is opened
  std::ifstream file(filename.c_str());
  if (!file.is_open())
  {
    std::cout << "Error: could not load file: " << filename << std::endl;
    return;
  }

  std::string line;
  std::getline(file, line);
  std::vector<std::string> values;
  boost::algorithm::split(values, line, boost::is_any_of(","));
  // initialize the parameters
  W = Eigen::VectorXd(values.size(), 1);
  for (int i = 0; i < values.size(); ++i)
  {
    W(i) = std::atof(values[i].c_str());
  }
  return;
}

//----------------------------------------------------------------------------
void WriteCameraParamsCSV(std::string filename, Eigen::VectorXd& W)
{
  // Load file and check that the file is opened
  std::ofstream file(filename.c_str());
  if (!file.is_open())
  {
    std::cout << "Error: could not open file: " << filename << std::endl;
    return;
  }
  for (int i = 0; i < W.size(); ++i)
  {
    file << W(i) << ",";
  }
  file.close();
  return;
}

//----------------------------------------------------------------------------
Eigen::Vector2d FisheyeProjection(const Eigen::Matrix<double, 15, 1>& W,
                                  const Eigen::Vector3d& X,
                                  bool shouldClip)
{
  // Get rotation matrix
  Eigen::Matrix3d R = RollPitchYawToMatrix(W(0), W(1), W(2));
  Eigen::Vector3d T(W(3), W(4), W(5));

  // Express the 3D point in the camera reference frame
  Eigen::Vector3d Xcam = R.transpose() * (X - T);

  // check that the point is not behind the camera plane
  if (shouldClip && (Xcam(2) < 0))
  {
    return Eigen::Vector2d(-1, -1);
  }

  // Project the 3D point in the plan
  Eigen::Vector2d Xp1(Xcam(0) / Xcam(2), Xcam(1) / Xcam(2));

  // Undistorded the projected image
  double r = Xp1.norm();
  double theta = std::atan(r);
  double thetad = theta * (1 + W(11) * std::pow(theta, 2) + W(12) * std::pow(theta, 4) +
                           W(13) * std::pow(theta, 6) + W(14) * std::pow(theta, 8));
   Eigen::Vector2d Xp1d = (thetad / r) * Xp1;

   // Create current intrinsic parameters
   Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
   K(0, 0) = W(6);
   K(1, 1) = W(7);
   K(0, 2) = W(8);
   K(1, 2) = W(9);
   K(0, 1) = W(10);
   K(2, 2) = 1;

   // Express the point in the pixel coordinates
   Eigen::Vector3d Xp1dh(Xp1d(0), Xp1d(1), 1);
   Eigen::Vector3d Xpix = K * Xp1dh;
   return Eigen::Vector2d(Xpix(0) / Xpix(2), Xpix(1) / Xpix(2));
}

//----------------------------------------------------------------------------
Eigen::Vector2d BrownConradyPinholeProjection(const Eigen::Matrix<double, 17, 1>& W,
                                              const Eigen::Vector3d& X,
                                              bool shouldClip)
{
  // Get rotation matrix
  Eigen::Matrix3d R = RollPitchYawToMatrix(W(0), W(1), W(2));
  Eigen::Vector3d T(W(3), W(4), W(5));

  // Express the 3D point in the camera reference frame
  Eigen::Vector3d Xcam = R.transpose() * (X - T);

  // check that the point is not behind the camera plane
  if (shouldClip && (Xcam(2) < 0))
  {
    return Eigen::Vector2d(-1, -1);
  }

  // Project the 3D point in the plan
  Eigen::Vector2d Xp1(Xcam(0) / Xcam(2), Xcam(1) / Xcam(2));

  // Undistorded the projected image
  double r = Xp1.norm();
  double k1 = W(11); double k2 = W(12);
  double p1 = W(13); double p2 = W(14);
  double p3 = W(15); double p4 = W(16);

  double xdist = Xp1(0) + Xp1(0) * (k1 * std::pow(r, 2) + k2 * std::pow(r, 4)) +
                 (p1 * (std::pow(r, 2) + 2 * std::pow(Xp1(0), 2)) +
                  2 * p2 * Xp1(0) * Xp1(1)) * (1 + p3 * std::pow(r, 2) + p4 * std::pow(r, 4));
  double ydist = Xp1(1) + Xp1(1) * (k1 * std::pow(r, 2) + k2 * std::pow(r, 4)) +
                 (2 * p1 * Xp1(0) * Xp1(1) + p2 * (std::pow(r, 2) + 2 * std::pow(Xp1(1), 2))) *
                 (1 + p3 * std::pow(r, 2) + p4 * std::pow(r, 4));
  Eigen::Vector2d Xp1d(xdist, ydist);

   // Create current intrinsic parameters
   Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
   K(0, 0) = W(6);
   K(1, 1) = W(7);
   K(0, 2) = W(8);
   K(1, 2) = W(9);
   K(0, 1) = W(10);
   K(2, 2) = 1;

   // Express the point in the pixel coordinates
   Eigen::Vector3d Xp1dh(Xp1d(0), Xp1d(1), 1);
   Eigen::Vector3d Xpix = K * Xp1dh;
   return Eigen::Vector2d(Xpix(0) / Xpix(2), Xpix(1) / Xpix(2));
}


#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <iostream>
#include <random> // Requires C++ 11

#include <SymGEigsSolver.h>
#include <MatOp/SparseSymMatProd.h>
#include <MatOp/SparseRegularInverse.h>

using namespace Spectra;

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;
typedef Eigen::SparseMatrix<double> SpMatrix;

// Generate random sparse matrix
SpMatrix sprand(int size, double prob = 0.5)
{
    SpMatrix mat(size, size);
    std::default_random_engine gen;
    gen.seed(0);
    std::uniform_real_distribution<double> distr(-1.0, 1.0);
    for(int i = 0; i < size; i++)
    {
        for(int j = 0; j < size; j++)
        {
            if(distr(gen) < prob)
                mat.insert(i, j) = distr(gen);
        }
    }
    return mat;
}

void gen_sparse_data(int n, SpMatrix& A, SpMatrix& B)
{
    // Eigen solver only uses the lower triangle of A,
    // so we don't need to make A symmetric here.
    A = sprand(n, 0.1);
    B = A.transpose() * A;
    // To make sure B is positive definite
    for(int i = 0; i < n; i++)
        B.coeffRef(i, i) += 0.1;
}

template <int SelectionRule>
void run_test(const SpMatrix& A, const SpMatrix& B, int k, int m, bool allow_fail = false)
{
    typedef SparseSymMatProd<double> OpType;
    typedef SparseRegularInverse<double> BOpType;
    OpType op(A);
    BOpType Bop(B);
    SymGEigsSolver<double, SelectionRule, OpType, BOpType, GEIGS_REGULAR_INVERSE> eigs(&op, &Bop, k, m);
    eigs.init();
    int nconv = eigs.compute(100); // maxit = 100 to reduce running time for failed cases
    int niter = eigs.num_iterations();
    int nops  = eigs.num_operations();

    if(allow_fail)
    {
        if( eigs.info() != SUCCESSFUL )
        {
            WARN( "FAILED on this test" );
            std::cout << "nconv = " << nconv << std::endl;
            std::cout << "niter = " << niter << std::endl;
            std::cout << "nops  = " << nops  << std::endl;
            return;
        }
    } else {
        INFO( "nconv = " << nconv );
        INFO( "niter = " << niter );
        INFO( "nops  = " << nops );
        REQUIRE( eigs.info() == SUCCESSFUL );
    }

    Vector evals = eigs.eigenvalues();
    Matrix evecs = eigs.eigenvectors();

    Matrix resid = A.template selfadjointView<Eigen::Lower>() * evecs -
                   B.template selfadjointView<Eigen::Lower>() * evecs * evals.asDiagonal();
    const double err = resid.array().abs().maxCoeff();

    INFO( "||AU - BUD||_inf = " << err );
    REQUIRE( err == Approx(0.0) );
}

void run_test_sets(const SpMatrix& A, const SpMatrix& B, int k, int m)
{
    SECTION( "Largest Magnitude" )
    {
        run_test<LARGEST_MAGN>(A, B, k, m);
    }
    SECTION( "Largest Value" )
    {
        run_test<LARGEST_ALGE>(A, B, k, m);
    }
    SECTION( "Smallest Magnitude" )
    {
        run_test<SMALLEST_MAGN>(A, B, k, m, true);
    }
    SECTION( "Smallest Value" )
    {
        run_test<SMALLEST_ALGE>(A, B, k, m);
    }
    SECTION( "Both Ends" )
    {
        run_test<BOTH_ENDS>(A, B, k, m);
    }
}

TEST_CASE("Generalized eigensolver of sparse symmetric real matrix [10x10]", "[geigs_sym]")
{
    std::srand(123);

    // Eigen solver only uses the lower triangle
    SpMatrix A, B;
    gen_sparse_data(10, A, B);
    int k = 3;
    int m = 6;

    run_test_sets(A, B, k, m);
}

TEST_CASE("Generalized eigensolver of sparse symmetric real matrix [100x100]", "[geigs_sym]")
{
    std::srand(123);

    // Eigen solver only uses the lower triangle
    SpMatrix A, B;
    gen_sparse_data(100, A, B);
    int k = 10;
    int m = 20;

    run_test_sets(A, B, k, m);
}

// Too time-consuming
/*
TEST_CASE("Generalized eigensolver of sparse symmetric real matrix [1000x1000]", "[geigs_sym]")
{
    std::srand(123);

    // Eigen solver only uses the lower triangle
    SpMatrix A, B;
    gen_sparse_data(1000, A, B);
    int k = 20;
    int m = 50;

    run_test_sets(A, B, k, m);
}
*/