/*
 * cost_structure.cpp
 *
 *  Created on: Jan 20, 2021
 *      Author: talhakavuncu
 */
#include <iostream>
#include <unsupported/Eigen/AdolcForward>
#include <adolc/adolc.h>
#include <Eigen/Dense>
#include "cost_structure.h"
#include <math.h>
namespace Unicycle_Cost

{
	adouble running_cost(const state_type_unicycle & x,const input_type_unicycle & u,const state_type_unicycle & x_goal)
	{

		adouble output,input_cost,state_cost;
		state_type_unicycle diff=x-x_goal;
	//	Eigen::Matrix<adouble,4,4> Q;
	//	Eigen::Matrix<adouble,2,2> R;
	//	Q<<1,0,0,0,
	//		0,1,0,0,
	//		0,0,1,0,
	//		0,0,0,1;

	//		Q<<0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
	//		R<<2,2,2,2;
	//	R<<1,0,0,1;
	//
	//	intermediate=inputs.transpose()*R*inputs;//+inputs[0]*states[2];//inputs.dot(inputs)*states.dot(states);
	//	output=diff.transpose()*Q*diff + intermediate;
		input_cost=u.dot(u);
		state_cost=diff.dot(diff);
		output=input_cost+state_cost;
		return output;
	};

	adouble terminal_cost(const state_type_unicycle & x,const input_type_unicycle & u,const state_type_unicycle & x_goal)
	{

		adouble output;
		state_type_unicycle diff=x-x_goal;

//		Eigen::Matrix<adouble,4,4> Q;
//		Q<<1,0,0,0,
//			0,1,0,0,
//			0,0,1,0,
//			0,0,0,1;
//		Eigen::Matrix<adouble,2,2> R;
//		R<<2,2,2,2;
//		R<<1,1,1,1;
//		intermediate=inputs.transpose()*R*inputs;//+inputs[0]*states[2];//inputs.dot(inputs)*states.dot(states);

		output=100*diff.dot(diff);
		return output;
	};

	adouble running_cost2(const state_type_unicycle2 & x,const input_type_unicycle2 & u,const state_type_unicycle2 & x_goal)
	{
		adouble output,input_cost,state_cost,ca_total;
		int row_size=1;
		int col_size=2;

		adouble d_prox=1.2;
//		adouble c_ij_costs[1][2]={{0}};
		std::vector<std::vector<adouble>> c_ij_costs(row_size,std::vector<adouble>(col_size));
		for(int i=0;i<row_size;++i)
			for(int j=i+1;j<col_size;++j)
				c_ij_cost(x.segment(i*4,2),x.segment(j*4,2),c_ij_costs[i][j],d_prox);

		calculate_total_c_ij_cost(c_ij_costs,ca_total);

		state_type_unicycle2 diff=x-x_goal;
		input_cost=u.dot(u);
		state_cost=diff.dot(diff);
		output=input_cost+state_cost+ca_total;
		return output;
	};
	adouble terminal_cost2(const state_type_unicycle2 & x,const input_type_unicycle2 & u,const state_type_unicycle2 & x_goal)
	{
		adouble output;
		state_type_unicycle2 diff=x-x_goal;
		output=25*diff.dot(diff);
		return output;
	};

	adouble running_cost3(const state_type_unicycle3 & x,const input_type_unicycle3 & u,const state_type_unicycle3 & x_goal)
	{
		adouble output,input_cost,state_cost,ca_total;
		int row_size=2;
		int col_size=3;

		adouble d_prox=0.6;
//		adouble c_ij_costs[1][2]={{0}};
		std::vector<std::vector<adouble>> c_ij_costs(row_size,std::vector<adouble>(col_size));
		for(int i=0;i<row_size;++i)
			for(int j=i+1;j<col_size;++j)
				c_ij_cost(x.segment(i*4,2),x.segment(j*4,2),c_ij_costs[i][j],d_prox);

		calculate_total_c_ij_cost(c_ij_costs,ca_total);
		state_type_unicycle3 diff=x-x_goal;
		input_cost=u.dot(u);
		state_cost=diff.dot(diff);
		output=input_cost+state_cost;
		return output;
	};
	adouble terminal_cost3(const state_type_unicycle3 & x,const input_type_unicycle3 & u,const state_type_unicycle3 & x_goal)
	{
		adouble output;
		state_type_unicycle3 diff=x-x_goal;
		output=diff.dot(diff);
		return output;
	};

	adouble running_cost4(const state_type_unicycle4 & x,const input_type_unicycle4 & u,const state_type_unicycle4 & x_goal)
	{
		adouble output,input_cost,state_cost,ca_total;
		int row_size=3;
		int col_size=4;

		adouble d_prox=0.6;
//		adouble c_ij_costs[1][2]={{0}};
		std::vector<std::vector<adouble>> c_ij_costs(row_size,std::vector<adouble>(col_size));
		for(int i=0;i<row_size;++i)
			for(int j=i+1;j<col_size;++j)
				c_ij_cost(x.segment(i*4,2),x.segment(j*4,2),c_ij_costs[i][j],d_prox);

		calculate_total_c_ij_cost(c_ij_costs,ca_total);
		state_type_unicycle4 diff=x-x_goal;
		input_cost=u.dot(u);
		state_cost=diff.dot(diff);
		output=input_cost+state_cost;
		return output;
	};
	adouble terminal_cost4(const state_type_unicycle4 & x,const input_type_unicycle4 & u,const state_type_unicycle4 & x_goal)
	{
		adouble output;
		state_type_unicycle4 diff=x-x_goal;
		output=diff.dot(diff);
		return output;
	};

	adouble running_cost6(const state_type_unicycle6 & x,const input_type_unicycle6 & u,const state_type_unicycle6 & x_goal)
	{
		adouble output,input_cost,state_cost,ca_total;
		int row_size=5;
		int col_size=6;

		adouble d_prox=0.6;
//		adouble c_ij_costs[1][2]={{0}};
		std::vector<std::vector<adouble>> c_ij_costs(row_size,std::vector<adouble>(col_size));
		for(int i=0;i<row_size;++i)
			for(int j=i+1;j<col_size;++j)
				c_ij_cost(x.segment(i*4,2),x.segment(j*4,2),c_ij_costs[i][j],d_prox);

		calculate_total_c_ij_cost(c_ij_costs,ca_total);
		state_type_unicycle6 diff=x-x_goal;
		input_cost=u.dot(u);
		state_cost=diff.dot(diff);
		output=input_cost+state_cost;
		return output;
	};
	adouble terminal_cost6(const state_type_unicycle6 & x,const input_type_unicycle6 & u,const state_type_unicycle6 & x_goal)
	{
		adouble output;
		state_type_unicycle6 diff=x-x_goal;
		output=diff.dot(diff);
		return output;
	};

	void c_ij_cost(const Eigen::Matrix<adouble,2,1> & x_i,const Eigen::Matrix<adouble,2,1> & x_j,
			adouble & cost,const adouble &d_prox)
	{
		Eigen::Matrix<adouble,2,1> diff;
		diff=x_i-x_j;
		adouble distance,distance_diff;
		distance=pow(diff.dot(diff),0.5);
		distance_diff=distance-d_prox;
		condassign(cost,distance_diff,adouble(0),1e3*pow(distance_diff,2));

	};

	void calculate_total_c_ij_cost(const std::vector<std::vector<adouble>> &c_ij_costs,adouble & total_cost)
	{
		for(int i=0;i<c_ij_costs.size();++i)
			for(int j=i+1;j<c_ij_costs[0].size();++j)
				total_cost+=c_ij_costs[i][j];
	};
};

//adouble terminal_cost_drone(const state_type_tensor & states,const input_type_tensor & inputs);
namespace Drone_Cost
{

	adouble running_cost(const state_type_drone & states,const input_type_drone & inputs,const state_type_drone & X_goal)
	{

		adouble output,input_cost,collision_cost,d_prox,distance,distance_diff;
		d_prox=3;
		Eigen::Matrix<adouble,3,1> avoid_pos,avoid_diff;
		avoid_pos<<5,-1,0;
//		avoid_diff<<
		state_type_drone diff;
		for (int i=0;i<3;++i)
			avoid_diff[i]=states[i]-avoid_pos[i];

		distance=pow(avoid_diff.dot(avoid_diff),0.5);
		distance_diff=distance-d_prox;
//		adouble zero=0;
//		adouble value=1e6*pow(distance_diff,2);
		condassign(collision_cost,distance_diff,adouble(0),1e3*pow(distance_diff,2));
//		for(int i=0;i<3;++i)
//			xyz_diff[i]=diff[i];

//		X_goal<<0,0,0, M_PI/2,0,0, 0,30,0, 0,0,0;
//		Eigen::Matrix<adouble,12,12> Q;
//		Q<<	1,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,1,0,0,0,0,0,0,0,0,0,
//			0,0,0,1/9,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,1,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,1/900,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,1,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,1;

//		diff=states-X_goal;
		input_cost=10*1/(mass*g/(4*C_T))*inputs.dot(inputs);
//		output=0.000001*diff.transpose()*Q*diff;//intermediate;
		diff=states-X_goal;
		output=100*diff.dot(diff)+input_cost+collision_cost;
//		output=1;
//		output=100*(1*diff[0]*diff[0]+1*diff[1]*diff[1]+1*diff[2]*diff[2]+100*diff[3]*diff[3]+100*diff[7]*diff[7]);
		return output;

	};

	adouble terminal_cost(const state_type_drone & states,const input_type_drone & inputs,const state_type_drone & X_goal)
	{
//		state_type_drone X_goal,diff;
		state_type_drone diff;
//		for (int i=0;i<12;++i)
//			diff[i]=states[i]-X_goal[i];
		diff=states-X_goal;
//		X_goal<<5,5,5, 0,0,0 ,0,0,0, 0,0,0;
//		X_goal<<0,0,0, M_PI/2,0,0, 0,30,0, 0,0,0;
//		diff=states-X_goal;
//		Eigen::Matrix<adouble,12,12> Q;
//		Q<<	1,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,1,0,0,0,0,0,0,0,0,0,
//			0,0,0,1/9,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,1,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,1/900,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,1,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,1;
		adouble output;
//		output=100*diff.transpose()*Q*diff;
		output=100*diff.dot(diff);
//		output=100*(1*diff[0]*diff[0]+1*diff[1]*diff[1]+1*diff[2]*diff[2]+100*diff[3]*diff[3]+100*diff[7]*diff[7]);
		return output;
	};

	adouble running_cost2(const state_type_drone2 & states,const input_type_drone2 & inputs,const state_type_drone2 & X_goal)
	{

		adouble output,input_cost,state_cost;

//		adouble d_prox_collision,collision_distance,distance_diff,collision_cost;
//		d_prox_collision=3;
//		Eigen::Matrix<adouble,3,1> avoid_pos,avoid_diff;
//		avoid_pos<<5,-1,0;
//		for (int i=0;i<3;++i)
//			avoid_diff[i]=states[i]-avoid_pos[i];
//
//		collision_distance=pow(avoid_diff.dot(avoid_diff),0.5);
//		distance_diff=collision_distance-d_prox_collision;
//		condassign(collision_cost,distance_diff,adouble(0),1e3*pow(distance_diff,2));

		adouble ca_distance_diff,ca_cost,ca_distance,ca_dprox;
		ca_dprox=0.5;
		Eigen::Matrix<adouble,3,1> pos_diff;
		for (int i=0;i<3;++i)
			pos_diff[i]=states[i]-states[i+12];

		ca_distance=pow(pos_diff.dot(pos_diff),0.5);
		ca_distance_diff=ca_distance-ca_dprox;
		condassign(ca_cost,ca_distance_diff,adouble(0),1e3*pow(ca_distance_diff,2));

		state_type_drone2 goal_diff;
		goal_diff=states-X_goal;
		input_cost=10*1/(mass*g/(4*C_T))*inputs.dot(inputs);
		state_cost=100*goal_diff.dot(goal_diff);
		output=state_cost+input_cost+ca_cost;
		return output;

	};

	adouble terminal_cost2(const state_type_drone2 & states,const input_type_drone2 & inputs,const state_type_drone2 & X_goal)
	{
		state_type_drone2 diff;
		diff=states-X_goal;
		adouble output;
		output=100*diff.dot(diff);
		return output;
	};

};

namespace Single_Integrator_Cost{

	adouble running_cost(const state_tensor & x, const input_tensor & u,const state_tensor & x_goal)
	{
		adouble output,input_cost,state_cost,collision_cost,d_prox,distance,distance_diff;
		state_tensor diff,avoid_pose,avoid_diff;
		d_prox=0.6;
		avoid_pose<<-0.5,0.5,0.5;
		avoid_diff=x-avoid_pose;

		distance=pow(avoid_diff.dot(avoid_diff),0.5);
		distance_diff=distance-d_prox;

		condassign(collision_cost,distance_diff,adouble(0),1e5*pow(distance_diff,2));

		diff=x-x_goal;
		state_cost=1e2*diff.dot(diff);
		input_cost=1e1*u.dot(u);

		output=state_cost+input_cost+collision_cost;

		return output;

	}
	adouble terminal_cost(const state_tensor & x, const input_tensor & u,const state_tensor & x_goal)
	{
		state_tensor diff=x-x_goal;
		adouble state_cost=1e2*diff.dot(diff);

		return state_cost;

	}

	adouble running_cost2(const state_tensor2 & x, const input_tensor2 & u,const state_tensor2 & x_goal)
	{
		adouble output,input_cost,state_cost;

//		adouble d_prox_collision,collision_distance,distance_diff,collision_cost;
//		d_prox_collision=3;
//		Eigen::Matrix<adouble,3,1> avoid_pos,avoid_diff;
//		avoid_pos<<5,-1,0;
//		for (int i=0;i<3;++i)
//			avoid_diff[i]=states[i]-avoid_pos[i];
//
//		collision_distance=pow(avoid_diff.dot(avoid_diff),0.5);
//		distance_diff=collision_distance-d_prox_collision;
//		condassign(collision_cost,distance_diff,adouble(0),1e3*pow(distance_diff,2));

		adouble ca_distance_diff,ca_cost,ca_distance,ca_dprox;
		ca_dprox=0.5;
		Eigen::Matrix<adouble,3,1> pos_diff;
		for (int i=0;i<3;++i)
			pos_diff[i]=x[i]-x[i+3];

		ca_distance=pow(pos_diff.dot(pos_diff),0.5);
		ca_distance_diff=ca_distance-ca_dprox;
		condassign(ca_cost,ca_distance_diff,adouble(0),1e5*pow(ca_distance_diff,2));

		state_tensor2 goal_diff;
		goal_diff=x-x_goal;
//		input_cost=0.12*1e1*u.dot(u);
		input_cost=0.5*1e1*u.dot(u);
		state_cost=1e-20*goal_diff.dot(goal_diff);
		output=state_cost+input_cost+ca_cost;
		return output;

	}
	adouble terminal_cost2(const state_tensor2 & x, const input_tensor2 & u,const state_tensor2 & x_goal)
	{
		state_tensor2 diff=x-x_goal;
		adouble state_cost=100*diff.dot(diff);

		return state_cost;
	}

}

namespace Double_Integrator_Cost{

	adouble running_cost(const state_tensor & x, const input_tensor & u,const state_tensor & x_goal)
	{
		adouble output,input_cost,state_cost,collision_cost,d_prox,distance,distance_diff;
		state_tensor diff;
		Eigen::Matrix<adouble,3,1> avoid_pos,avoid_diff;
		d_prox=1;
		avoid_pos<<1,1,1;
		avoid_diff=x.segment(0,3)-avoid_pos;
		distance=pow(avoid_diff.dot(avoid_diff),0.5);
		distance_diff=distance-d_prox;

		condassign(collision_cost,distance_diff,adouble(0),1e3*pow(distance_diff,2));

		diff=x-x_goal;
		state_cost=1e2*diff.dot(diff);
		input_cost=1*1e-2*u.dot(u);

		output=state_cost+input_cost+collision_cost;

		return output;

	}
	adouble terminal_cost(const state_tensor & x, const input_tensor & u,const state_tensor & x_goal)
	{
		state_tensor diff=x-x_goal;
		adouble state_cost=1e2*diff.dot(diff);

		return state_cost;

	}

//	adouble running_cost2(const state_tensor2 & x, const input_tensor2 & u,const state_tensor2 & x_goal)
//	{
//		adouble output,input_cost,state_cost;
//
////		adouble d_prox_collision,collision_distance,distance_diff,collision_cost;
////		d_prox_collision=3;
////		Eigen::Matrix<adouble,3,1> avoid_pos,avoid_diff;
////		avoid_pos<<5,-1,0;
////		for (int i=0;i<3;++i)
////			avoid_diff[i]=states[i]-avoid_pos[i];
////
////		collision_distance=pow(avoid_diff.dot(avoid_diff),0.5);
////		distance_diff=collision_distance-d_prox_collision;
////		condassign(collision_cost,distance_diff,adouble(0),1e3*pow(distance_diff,2));
//
//		adouble ca_distance_diff,ca_cost,ca_distance,ca_dprox;
//		ca_dprox=0.5;
//		Eigen::Matrix<adouble,3,1> pos_diff;
//		for (int i=0;i<3;++i)
//			pos_diff[i]=x[i]-x[i+3];
//
//		ca_distance=pow(pos_diff.dot(pos_diff),0.5);
//		ca_distance_diff=ca_distance-ca_dprox;
//		condassign(ca_cost,ca_distance_diff,adouble(0),1e5*pow(ca_distance_diff,2));
//
//		state_tensor2 goal_diff;
//		goal_diff=x-x_goal;
////		input_cost=0.12*1e1*u.dot(u);
//		input_cost=0.5*1e1*u.dot(u);
//		state_cost=1e-20*goal_diff.dot(goal_diff);
//		output=state_cost+input_cost+ca_cost;
//		return output;
//
//
//	}
//	adouble terminal_cost2(const state_tensor2 & x, const input_tensor2 & u,const state_tensor2 & x_goal)
//	{
//		state_tensor2 diff=x-x_goal;
//		adouble state_cost=100*diff.dot(diff);
//
//		return state_cost;
//	}

}

namespace Drone_First_Order_Cost
{

	adouble running_cost(const state_type_drone & states,const input_type_drone & inputs,const state_type_drone & X_goal)
	{
		adouble output,input_cost,collision_cost,d_prox,distance,distance_diff;
		d_prox=1.0;
		Eigen::Matrix<adouble,3,1> avoid_pos,avoid_diff;
		avoid_pos<<0.5,0.5,0.5;
//		avoid_diff<<
		state_type_drone diff;
		for (int i=0;i<3;++i)
			avoid_diff[i]=states[i]-avoid_pos[i];

		distance=pow(avoid_diff.dot(avoid_diff),0.5);
		distance_diff=distance-d_prox;
//		adouble zero=0;
//		adouble value=1e6*pow(distance_diff,2);
		condassign(collision_cost,distance_diff,adouble(0),1e6*pow(distance_diff,2));
//		for(int i=0;i<3;++i)
//			xyz_diff[i]=diff[i];

//		X_goal<<0,0,0, M_PI/2,0,0, 0,30,0, 0,0,0;
//		Eigen::Matrix<adouble,12,12> Q;
//		Q<<	1,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,1,0,0,0,0,0,0,0,0,0,
//			0,0,0,1/9,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,1,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,1/900,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,1,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,1;

//		diff=states-X_goal;
		input_cost=3e1*inputs.dot(inputs);
//		output=0.000001*diff.transpose()*Q*diff;//intermediate;
		diff=states-X_goal;
		output=100*diff.dot(diff)+input_cost+collision_cost;
//		output=1;
//		output=100*(1*diff[0]*diff[0]+1*diff[1]*diff[1]+1*diff[2]*diff[2]+100*diff[3]*diff[3]+100*diff[7]*diff[7]);
		return output;
	}
	adouble terminal_cost(const state_type_drone & states,const input_type_drone & inputs,const state_type_drone & X_goal)
	{
		adouble output;
		state_type_drone diff;
		diff=states-X_goal;
//		output=100*diff.transpose()*Q*diff;
		output=100*diff.dot(diff);
//		output=100*(1*diff[0]*diff[0]+1*diff[1]*diff[1]+1*diff[2]*diff[2]+100*diff[3]*diff[3]+100*diff[7]*diff[7]);
		return output;
	}

	adouble running_cost2(const state_type_drone2 & states,const input_type_drone2 & inputs,const state_type_drone2 & X_goal)
	{
		adouble output,input_cost,state_cost;

//		adouble d_prox_collision,collision_distance,distance_diff,collision_cost;
//		d_prox_collision=3;
//		Eigen::Matrix<adouble,3,1> avoid_pos,avoid_diff;
//		avoid_pos<<5,-1,0;
//		for (int i=0;i<3;++i)
//			avoid_diff[i]=states[i]-avoid_pos[i];
//
//		collision_distance=pow(avoid_diff.dot(avoid_diff),0.5);
//		distance_diff=collision_distance-d_prox_collision;
//		condassign(collision_cost,distance_diff,adouble(0),1e3*pow(distance_diff,2));

		adouble ca_distance_diff,ca_cost,ca_distance,ca_dprox;
		ca_dprox=0.30;
		Eigen::Matrix<adouble,3,1> pos_diff;
		for (int i=0;i<3;++i)
			pos_diff[i]=states[i]-states[i+6];

		ca_distance=pow(pos_diff.dot(pos_diff),0.5);
		ca_distance_diff=ca_distance-ca_dprox;
		condassign(ca_cost,ca_distance_diff,adouble(0),1e6*pow(ca_distance_diff,2));

		state_type_drone2 goal_diff;
		goal_diff=states-X_goal;
		input_cost=0.4e2*inputs.dot(inputs);
		state_cost=0.5*goal_diff.dot(goal_diff);
		output=state_cost+input_cost+ca_cost;
		return output;
	}
	adouble terminal_cost2(const state_type_drone2 & states,const input_type_drone2 & inputs,const state_type_drone2 & X_goal)
	{
		state_type_drone2 diff;
		diff=states-X_goal;
		adouble output;
		output=200*diff.dot(diff);
		return output;
	}
}


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>
#include <adept_source.h>
#include <adept.h>
using adept::adouble;

static float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

#define BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#define BOOST_NO_EXCEPTIONS
#include <iostream>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

#include <boost/throw_exception.hpp>

using namespace std;
using namespace boost::numeric::odeint;

#include <stdio.h>

double foobar(double t, uint64_t iters);

typedef boost::array< adouble , 1 > astate_type;

void alorenz( const astate_type &x , astate_type &dxdt , adouble t )
{
    const double a = 1.2;
    dxdt[0] = -a * x[0];
}

adouble afoobar(adouble t, uint64_t iters) {
    astate_type x = { 1.0 }; // initial conditions

    adouble start = 0.0;
    adouble step = t/adouble(iters);
    typedef controlled_runge_kutta< runge_kutta_dopri5< astate_type , typename astate_type::value_type , astate_type , adouble > > stepper_type;
    //typedef euler< astate_type , typename astate_type::value_type , astate_type , adouble > stepper_type;
    integrate_const( stepper_type(), alorenz , x , start , t, step );

    //x[0] += -1.2 * step * x[0];

    //printf("final result t=%f x(t)=%f, exp(-1.2* t)=%f\n", t, x[0], exp(- 1.2 * t));
    return x[0];
}

double afoobar_and_gradient(double xin, double& xgrad, uint64_t iters) {
    adept::Stack stack;
    adouble x = xin;
    stack.new_recording();
    adouble y = afoobar(x, iters);
    y.set_gradient(1.0);
    stack.compute_adjoint();
    xgrad = x.get_gradient();
    return y.value();
}

void adept_sincos(double inp, uint64_t iters) {
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = foobar(inp, iters);

  gettimeofday(&end, NULL);
  printf("Adept real %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  adept::Stack stack;
 // stack.new_recording();
  adouble resa = afoobar(inp, iters);
  double res = resa.value();

  gettimeofday(&end, NULL);
  printf("Adept forward %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = 0;
  afoobar_and_gradient(inp, res2, iters);

  gettimeofday(&end, NULL);
  printf("Adept combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}


// #include <stan/math.hpp>
// #include <boost/math/tools/promotion.hpp>

// using namespace stan::math;

namespace rosenbrock_model_namespace {

template <typename T0__>
Eigen::Matrix<stan::promote_args_t<T0__>, -1, 1>
my_rosenbrock(const Eigen::Matrix<T0__, -1, 1>& xy, std::ostream* pstream__)
{
    // throw std::logic_error("not implemented");  // this should never be called
    // typedef typename boost::math::tools::promote_args<T0__>::type T;
    if (xy.size() > 2) {
        throw std::logic_error("This function is implemented only for input of size 2.");
    }
    using T = stan::return_type_t<T0__>;
    T x = xy(0);
    T y = xy(1);
    T res = pow((1 - x), 2) + (100 * pow((y - pow(x, 2)), 2));
    Eigen::Matrix<T, -1, 1> out(1);
    out(0) = res;
    return out;
}

} // rosenbrock_model_namespace


// Copyright Nick Thompson, 2017

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

#include <iostream>
#include <vector>
#include <array>
#include <cmath>
#include <boost/math/interpolators/catmull_rom.hpp>
#include <boost/math/constants/constants.hpp>

using std::sin;
using std::cos;
using boost::math::catmull_rom;

int main()
{
    std::cout << "This shows how to use Boost's Catmull-Rom spline to create an Archimedean spiral.\n";

    // The Archimedean spiral is given by r = a*theta. We have set a = 1.
    std::vector<std::array<double, 2>> spiral_points(500);
    double theta_max = boost::math::constants::pi<double>();
    for (size_t i = 0; i < spiral_points.size(); ++i)
    {
        double theta = ((double) i/ (double) spiral_points.size())*theta_max;
        spiral_points[i] = {theta*cos(theta), theta*sin(theta)};
    }

    auto archimedean = catmull_rom<std::array<double,2>>(std::move(spiral_points));
    double max_s = archimedean.max_parameter();
    std::cout << "Max s = " << max_s << std::endl;
    for (double s = 0; s < max_s; s += 0.01)
    {
        auto p = archimedean(s);
        double x = p[0];
        double y = p[1];
        double r = sqrt(x*x + y*y);
        double theta = atan2(y/r, x/r);
        std::cout << "r = " << r << ", theta = " << theta << ", r - theta = " << r - theta << std::endl;
    }

    return 0;
}


// ----------------------------------------------------------------------------
// tropter: test_double_pendulum.cpp
// ----------------------------------------------------------------------------
// Copyright (c) 2017 tropter authors
//
// 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.
// ----------------------------------------------------------------------------

#define CATCH_CONFIG_MAIN
#include <catch.hpp>
#include "testing_optimalcontrol.h"

#include <tropter/tropter.h>
#include <Eigen/LU>

using Eigen::Ref;
using Eigen::VectorXd;
using Eigen::RowVectorXd;
using Eigen::Vector2d;
using Eigen::MatrixXd;
using Eigen::Matrix2d;

using namespace tropter;

constexpr double PI = 3.14159;

/// This class template defines the dynamics of a double pendulum. To create
/// an actual optimal control problem, one must derive from this class
/// template and define boundary conditions, cost terms, etc.
template<typename T>
class DoublePendulum : public tropter::Problem<T> {
public:
    constexpr static const double g = 9.81;
    double L0 = 1;
    double L1 = 1;
    double m0 = 1;
    double m1 = 1;

    void calc_differential_algebraic_equations(
            const Input<T>& in, Output<T> out) const override final {
        const auto& x = in.states;
        const auto& tau = in.controls;
        const auto& q0 = x[0];
        const auto& q1 = x[1];
        const auto& u0 = x[2];
        const auto& u1 = x[3];
        const auto& L0 = this->L0;
        const auto& L1 = this->L1;
        const auto& m0 = this->m0;
        const auto& m1 = this->m1;
        out.dynamics[0] = u0;
        out.dynamics[1] = u1;

        const T z0 = m1 * L0 * L1 * cos(q1);
        const T M01 = m1 * L1*L1 + z0;
        Matrix2<T> M;
        M << m0 * L0*L0 + m1 * (L0*L0 + L1*L1) + 2*z0,    M01,
             M01,                                         m1 * L1*L1;
        Vector2<T> V(-u1 * (2 * u0 + u1),
                     u0 * u0);
        V *= m1*L0*L1*sin(q1);
        Vector2<T> G(g * ((m0 + m1) * L0 * cos(q0) + m1 * L1 * cos(q0 + q1)),
                     g * m1 * L1 * cos(q0 + q1));

        //Vector2<T> drag(-10 * u0, -10 * u1);
        // TODO xdot.tail<2>() =
        out.dynamics.tail(2) = M.inverse() * (tau - (V + G));
    }
};

/// The optimal solution for a double pendulum to swing from horizontal to
/// vertical (up) in minimum time is for the links to fall down first and
/// for the first link to rotate clockwise. The second link flips around +360
/// degrees to reach the Cartesian coordinates (0, 2).
template<typename T>
class DoublePendulumSwingUpMinTime : public DoublePendulum<T> {
public:
    DoublePendulumSwingUpMinTime()
    {
        this->set_time(0, {0, 5}); //{0, 5});
        // TODO fix allowing final bounds to be unconstrained.
        this->add_state("q0", {-10, 10}, {0});
        this->add_state("q1", {-10, 10}, {0});
        this->add_state("u0", {-50, 50}, {0}, {0});
        this->add_state("u1", {-50, 50}, {0}, {0});
        this->add_control("tau0", {-50, 50});
        this->add_control("tau1", {-50, 50});
        this->add_cost("target", 0);
    }
    void calc_cost(
            int, const CostInput<T>& in, T& cost) const override {
        // TODO a final state constraint probably makes more sense.
        const auto& q0 = in.final_states[0];
        const auto& q1 = in.final_states[1];
        const auto& L0 = this->L0;
        const auto& L1 = this->L1;
        Vector2<T> actual_location(L0 * cos(q0) + L1 * cos(q0 + q1),
                L0 * sin(q0) + L1 * sin(q0 + q1));
        const Vector2<T> desired_location(0, 2);
        cost = 1000.0 * (actual_location - desired_location).squaredNorm() +
                0.001 * in.final_time;
    }

    static void run_test(std::string solver, std::string hessian_approx,
            std::string transcription, int N = 100) {
        auto ocp = std::make_shared<DoublePendulumSwingUpMinTime<T>>();
        DirectCollocationSolver<T> dircol(ocp, transcription, solver, N);
        std::string jacobian_approx;
        if (hessian_approx == "exact") {
            jacobian_approx = hessian_approx;
        } else {
            jacobian_approx = "finite-difference-values";
        }
        dircol.get_opt_solver().set_jacobian_approximation(jacobian_approx);
        dircol.get_opt_solver().set_hessian_approximation(hessian_approx);
        dircol.get_opt_solver().set_sparsity_detection("random");
        tropter::Iterate guess;
        const int Nguess = 2;
        guess.time.setLinSpaced(Nguess, 0, 1);
        // Give a hint.
        ocp->set_state_guess(guess, "q0",
                Eigen::RowVectorXd::LinSpaced(Nguess, 0, -3./2.*PI));
        ocp->set_state_guess(guess, "q1",
                Eigen::RowVectorXd::LinSpaced(Nguess, 0, 2*PI));
        ocp->set_state_guess(guess, "u0", Eigen::RowVectorXd::Zero(Nguess));
        ocp->set_state_guess(guess, "u1", Eigen::RowVectorXd::Zero(Nguess));
        ocp->set_control_guess(guess, "tau0",
                Eigen::RowVectorXd::LinSpaced(Nguess, -50, 50));
        ocp->set_control_guess(guess, "tau1",
                Eigen::RowVectorXd::LinSpaced(Nguess, 50, -50));
        Solution solution = dircol.solve(guess);
        solution.write("double_pendulum_horizontal_to_vertical_solution.csv");
        // Check the final states.
        INFO(solution.states);
        TROPTER_REQUIRE_EIGEN(solution.states.rightCols<1>(),
                Eigen::Vector4d(-3./2.*PI, 2*PI, 0, 0), 1e-3);
        // Check the controls, which are bang-bang.
        testAlmostEqual(solution.controls.topLeftCorner<1, 40>(),
                Eigen::RowVectorXd::Constant(40, -50), 1e-2);
        testAlmostEqual(solution.controls.topRightCorner<1, 40>(),
                Eigen::RowVectorXd::Constant(40,  50), 1e-2);
        testAlmostEqual(solution.controls.bottomLeftCorner<1, 15>(),
                Eigen::RowVectorXd::Constant(15,  50), 1e-2);
        testAlmostEqual(solution.controls.bottomRightCorner<1, 15>(),
                Eigen::RowVectorXd::Constant(15, -50), 1e-2);
    }
};

TEST_CASE("Double pendulum swing up in minimum time.",
          "[trapezoidal][hermite-simpson]")
{
    SECTION("IPOPT") {
        SECTION("Finite differences, limited-memory Hessian, trapezoidal") {
            DoublePendulumSwingUpMinTime<double>::run_test("ipopt",
                    "limited-memory", "trapezoidal");
        }
        // This test passes but it's just really slow:
        //SECTION("Finite differences, exact Hessian, trapezoidal") {
        //    DoublePendulumSwingUpMinTime<double>::run_test("ipopt",
        //            "exact", "trapezoidal");
        //}
        SECTION("ADOL-C, trapezoidal") {
            DoublePendulumSwingUpMinTime<adouble>::run_test("ipopt",
                    "limited-memory", "trapezoidal");
        }
        SECTION("Finite differences, limited-memory Hessian, hermite-simpson") {
            DoublePendulumSwingUpMinTime<double>::run_test("ipopt",
                "limited-memory", "hermite-simpson", 50);
        }
        //SECTION("Finite differences, exact Hessian, hermite-simpson") {
        //    DoublePendulumSwingUpMinTime<double>::run_test("ipopt",
        //            "exact", "hermite-simpson");
        //}
        SECTION("ADOL-C, hermite-simpson") {
            DoublePendulumSwingUpMinTime<adouble>::run_test("ipopt",
                "limited-memory", "hermite-simpson", 50);
        }
    }
    // Does not give desired answer (not fully bang-bang controls):
    // #if defined(TROPTER_WITH_SNOPT)
    // SECTION("SNOPT") {
    //     SECTION("ADOL-C, trapezoidal") {
    //         DoublePendulumSwingUpMinTime<adouble>::run_test("snopt",
    //                 "limited-memory", "trapezoidal");
    //     }
    //     SECTION("ADOL-C, hermite-simpson") {
    //         DoublePendulumSwingUpMinTime<adouble>::run_test("snopt",
    //                 "limited-memory", "hermite-simpson");
    //     }
    // }
    // #endif
}

template<typename T>
class DoublePendulumCoordinateTracking : public DoublePendulum<T> {
public:
    DoublePendulumCoordinateTracking()
    {
        this->set_time(0, 1);
        // TODO fix allowing final bounds to be unconstrained.
        this->add_state("q0", {-10, 10});
        this->add_state("q1", {-10, 10});
        this->add_state("u0", {-50, 50});
        this->add_state("u1", {-50, 50});
        this->add_control("tau0", {-100, 100});
        this->add_control("tau1", {-100, 100});
        this->add_cost("tracking", 1);
    }
    void calc_cost(
            int, const CostInput<T>& in, T& cost) const override {
        cost = in.integral;
    }
    void calc_cost_integrand(
            int, const Input<T>& in, T& integrand) const override {

        const auto& time = in.time;
        const auto& states = in.states;

        VectorX<T> desired(2);
        desired << (time / 1.0) * 0.50 * PI,
                (time / 1.0) * 0.25 * PI;
        integrand = (states.head(2) - desired).squaredNorm();
    }

    static Solution run_test(const std::string& solver,
            const std::string& hessian_approx,
            const std::string& transcription, int N = 50) {
        auto ocp = std::make_shared<DoublePendulumCoordinateTracking<T>>();
        DirectCollocationSolver<T> dircol(ocp, transcription, solver, N);
        // Using an exact Hessian seems really important for this problem
        // (solves in only 20 iterations). Even a limited-memory problem started
        // from the solution using an exact Hessian does not converge.
        std::string jacobian_approx;
        if (hessian_approx == "exact") {
            jacobian_approx = hessian_approx;
        } else {
            jacobian_approx = "finite-difference-values";
        }
        dircol.get_opt_solver().set_jacobian_approximation(jacobian_approx);
        dircol.get_opt_solver().set_hessian_approximation(hessian_approx);
        dircol.get_opt_solver().set_sparsity_detection("random");
        Solution solution = dircol.solve();
        //dircol.print_constraint_values(solution);
        solution.write("double_pendulum_coordinate_tracking.csv");

        TROPTER_REQUIRE_EIGEN(solution.states.row(0),
                Eigen::RowVectorXd::LinSpaced(solution.time.size(), 0,
                    0.50 * PI), 1e-3);
        TROPTER_REQUIRE_EIGEN(solution.states.row(1),
                Eigen::RowVectorXd::LinSpaced(solution.time.size(), 0,
                    0.25 * PI), 1e-3);

        return solution;
    }
};

/// This class template defines the dynamics of a double pendulum using an
/// implicit formulation. To create an actual optimal control problem, one must
/// derive from this class template and define boundary conditions, cost terms,
/// etc.
template<typename T>
class ImplicitDoublePendulum : public tropter::Problem<T> {
public:
    constexpr static const double g = 9.81;
    double L0 = 1;
    double L1 = 1;
    double m0 = 1;
    double m1 = 1;
    void calc_differential_algebraic_equations(
            const Input<T>& in, Output<T> out) const override final {
        const auto& x = in.states;
        const auto& udot = in.controls.template head<2>();
        const auto& tau = in.controls.template tail<2>();
        const auto& q0 = x[0];
        const auto& q1 = x[1];
        const auto& u0 = x[2];
        const auto& u1 = x[3];
        const auto& L0 = this->L0;
        const auto& L1 = this->L1;
        const auto& m0 = this->m0;
        const auto& m1 = this->m1;
        out.dynamics[0] = u0;
        out.dynamics[1] = u1;
        out.dynamics[2] = udot[0];
        out.dynamics[3] = udot[1];

        if (out.path.size() != 0) {
            const T z0 = m1 * L0 * L1 * cos(q1);
            const T M01 = m1 * L1*L1 + z0;
            Matrix2<T> M;
            M << m0 * L0*L0 + m1 * (L0*L0 + L1*L1) + 2*z0,    M01,
                    M01,                                         m1 * L1*L1;
            Vector2<T> V(-u1 * (2 * u0 + u1),
                         u0 * u0);
            V *= m1*L0*L1*sin(q1);
            Vector2<T> G(g * ((m0 + m1) * L0 * cos(q0) + m1 * L1 * cos(q0 + q1)),
                         g * m1 * L1 * cos(q0 + q1));

            out.path = M * udot + V + G - tau;
        }
    }
};

template<typename T>
class ImplicitDoublePendulumCoordinateTracking : public
                                                 ImplicitDoublePendulum<T> {
public:
    ImplicitDoublePendulumCoordinateTracking() {
        this->set_time(0, 1);
        // TODO fix allowing final bounds to be unconstrained.
        this->add_state("q0", {-10, 10});
        this->add_state("q1", {-10, 10});
        this->add_state("u0", {-50, 50});
        this->add_state("u1", {-50, 50});
        this->add_control("udot0", {-100, 100});
        this->add_control("udot1", {-100, 100});
        this->add_control("tau0", {-100, 100});
        this->add_control("tau1", {-100, 100});
        this->add_cost("tracking", 1);
        this->add_path_constraint("u0", 0);
        this->add_path_constraint("u1", 0);
    }
    void calc_cost(
            int, const CostInput<T>& in, T& cost) const override {
        cost = in.integral;
    }
    void calc_cost_integrand(
            int, const Input<T>& in, T& integrand) const override {

        const auto& time = in.time;
        const auto& states = in.states;

        VectorX<T> desired(2);
        desired << (time / 1.0) * 0.50 * PI,
                   (time / 1.0) * 0.25 * PI;
        integrand = (states.template head<2>() - desired).squaredNorm();
    }
    static Solution run_test(const std::string& solver,
            const std::string& hessian_approx,
            const std::string& transcription, int N = 50) {
        auto ocp =
                std::make_shared<ImplicitDoublePendulumCoordinateTracking<T>>();
        DirectCollocationSolver<T> dircol(ocp, transcription, solver, N);
        std::string jacobian_approx;
        if (hessian_approx == "exact") {
            jacobian_approx = hessian_approx;
        } else {
            jacobian_approx = "finite-difference-values";
        }
        dircol.get_opt_solver().set_jacobian_approximation(jacobian_approx);
        dircol.get_opt_solver().set_hessian_approximation(hessian_approx);
        dircol.get_opt_solver().set_sparsity_detection("random");
        dircol.get_opt_solver().set_advanced_option_string
                ("print_timing_statistics", "yes");
        Solution solution = dircol.solve();
        // dircol.print_constraint_values(solution);
        solution.write("implicit_double_pendulum_coordinate_tracking.csv");

        TROPTER_REQUIRE_EIGEN(solution.states.row(0),
                Eigen::RowVectorXd::LinSpaced(solution.time.size(), 0,
                    0.50 * PI), 1e-3);
        TROPTER_REQUIRE_EIGEN(solution.states.row(1),
                Eigen::RowVectorXd::LinSpaced(solution.time.size(), 0,
                    0.25 * PI), 1e-3);

        return solution;
    }
};

TEST_CASE("Double pendulum coordinate tracking",
        "[trapezoidal][hermite-simpson][implicitdynamics]")
{
    SECTION("IPOPT, trapezoidal") {
        // Make sure the solutions from the implicit and explicit
        // formulations are similar.

        // The explicit solution takes 20 iterations whereas the implicit
        // solution takes 25 iterations.
        const auto explicit_solution =
                DoublePendulumCoordinateTracking<adouble>::
                run_test("ipopt", "exact", "trapezoidal");

        const auto implicit_solution =
                ImplicitDoublePendulumCoordinateTracking<adouble>::
                run_test("ipopt", "exact", "trapezoidal");

        TROPTER_REQUIRE_EIGEN(explicit_solution.time,
                implicit_solution.time, 1e-10);
        // q0 and q1
        TROPTER_REQUIRE_EIGEN(explicit_solution.states.bottomRows(2),
                implicit_solution.states.bottomRows(2), 1e-2);
        // u0 and u1
        TROPTER_REQUIRE_EIGEN(explicit_solution.states.bottomRows(2),
                implicit_solution.states.bottomRows(2), 1e-2);
        // tau0 and tau1
        // The controls have the same shape but have a pretty large error
        // between them.
        // The peak magnitude of the torques is about 10-30 N-m, so a tolerance
        // of 5.0 N-m means the shape of the torques is preserved.
        CAPTURE(explicit_solution.controls);
        CAPTURE(implicit_solution.controls.bottomRows(2));
        TROPTER_REQUIRE_EIGEN_ABS(explicit_solution.controls,
                implicit_solution.controls.bottomRows(2), 5.0);

        // Finite differences.
        // -------------------
        // Check that finite differences are correct.
        OCPDerivativesComparison<DoublePendulumCoordinateTracking> c;
        c.findiff_hessian_step_size = 1e-5;
        c.gradient_error_tolerance = 1e-5;
        c.jacobian_error_tolerance = 1e-5;
        c.hessian_error_tolerance = 1e-2;
        c.compare();

        OCPDerivativesComparison<ImplicitDoublePendulumCoordinateTracking> ci;
        ci.findiff_hessian_step_size = 1e-5;
        ci.gradient_error_tolerance = 1e-5;
        ci.jacobian_error_tolerance = 1e-5;
        ci.hessian_error_tolerance = 1e-2;
        ci.compare();

        DoublePendulumCoordinateTracking<double>:: run_test("ipopt", "exact",
            "trapezoidal");
        ImplicitDoublePendulumCoordinateTracking<double>::
        run_test("ipopt", "exact", "trapezoidal");

        // The following do not converge:
        // EXIT: Maximum number of iterations exceeded.
        // DoublePendulumCoordinateTracking<adouble>::
        // run_test("ipopt", "limited-memory");
        // EXIT: Solved to Acceptable Level, "Restoration phase is called at
        // almost feasible point, but acceptable point from iteration 810 could
        // be restored." After 812 iterations. But solution is pretty wrong.
        // DoublePendulumCoordinateTracking<double>::
        // run_test("ipopt", "limited-memory");
        // EXIT: Maximum number of iterations exceeded.
        // ImplicitDoublePendulumCoordinateTracking<adouble>::
        // run_test("ipopt", "limited-memory");
        // EXIT: Restoration failed after 235 iterations.
        // ImplicitDoublePendulumCoordinateTracking<double>::
        // run_test("ipopt", "limited-memory");

    }
    SECTION("IPOPT, hermite-simpson") {
        // Make sure the solutions from the implicit and explicit
        // formulations are similar.

        // The explicit solution takes 20 iterations whereas the implicit
        // solution takes 25 iterations.
        const auto explicit_solution =
            DoublePendulumCoordinateTracking<adouble>::
            run_test("ipopt", "exact", "hermite-simpson", 25);

        const auto implicit_solution =
            ImplicitDoublePendulumCoordinateTracking<adouble>::
            run_test("ipopt", "exact", "hermite-simpson", 25);

        TROPTER_REQUIRE_EIGEN(explicit_solution.time,
            implicit_solution.time, 1e-10);
        // q0 and q1
        TROPTER_REQUIRE_EIGEN(explicit_solution.states.bottomRows(2),
            implicit_solution.states.bottomRows(2), 1e-2);
        // u0 and u1
        TROPTER_REQUIRE_EIGEN(explicit_solution.states.bottomRows(2),
            implicit_solution.states.bottomRows(2), 1e-2);
        // tau0 and tau1
        CAPTURE(explicit_solution.controls);
        CAPTURE(implicit_solution.controls.bottomRows(2));
        // TODO this fails since control become zero at the midpoint for
        // Hermite-Simpson transcription.
        //TROPTER_REQUIRE_EIGEN_ABS(explicit_solution.controls,
        //    implicit_solution.controls.bottomRows(2), 5.0);

        DoublePendulumCoordinateTracking<double>::run_test("ipopt", "exact",
            "hermite-simpson", 25);
        ImplicitDoublePendulumCoordinateTracking<double>::
            run_test("ipopt", "exact", "hermite-simpson", 25);
    }
    /*
    #if defined(TROPTER_WITH_SNOPT)
    SECTION("SNOPT") {
        // TODO SNOPT will get the correct solution if the initial guess is the
        // solution from IPOPT, but otherwise, the solution is off (u0/u1
        // dynamics violation is 1e-7?).
        //ImplicitDoublePendulumCoordinateTracking<adouble>::run_test("snopt",
        //        "limited-memory");
        const auto explicit_solution =
                DoublePendulumCoordinateTracking<adouble>::run_test("ipopt");
        auto ocp = std::make_shared<DoublePendulumCoordinateTracking<adouble>>();
        tropter::Iterate guess;

        const int N = 100;
        guess.time.setLinSpaced(N, 0, 1);
        ocp->set_state_guess(guess, "q0",
                Eigen::RowVectorXd::LinSpaced(N, 0, 0.5*PI));
        ocp->set_state_guess(guess, "q1",
                Eigen::RowVectorXd::LinSpaced(N, 0, 0.25*PI));
        ocp->set_state_guess(guess, "u0", Eigen::RowVectorXd::Zero(N));
        ocp->set_state_guess(guess, "u1", Eigen::RowVectorXd::Zero(N));
        ocp->set_control_guess(guess, "tau0", Eigen::RowVectorXd::Zero(N));
        ocp->set_control_guess(guess, "tau1", Eigen::RowVectorXd::Zero(N));
        DirectCollocationSolver<adouble> dircol(ocp, "trapezoidal", "snopt", N);
        Solution solution = dircol.solve(guess);
        dircol.print_constraint_values(solution);
        solution.write("double_pendulum_coordinate_tracking_snopt.csv");
    }
    #endif
    */
}

// TODO include acceleration and deceleration.


// solving A * X = B
// A symmetric in packed format
// driver function sysv()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/driver/spsv.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include "utils.h"

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

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

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<real_t, ublas::column_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;

typedef
  ublas::symmetric_matrix<real_t, ublas::lower, ublas::column_major> symml_t;
typedef
  ublas::symmetric_matrix<cmplx_t, ublas::lower, ublas::column_major> csymml_t;

typedef
  ublas::symmetric_matrix<real_t, ublas::upper, ublas::column_major> symmu_t;
typedef
  ublas::symmetric_matrix<cmplx_t, ublas::upper, ublas::column_major> csymmu_t;

template <typename M>
void init_symm2 (M& m) {
  for (int i = 0; i < m.size1(); ++i)
    for (int j = i; j < m.size1(); ++j)
      m (i, j) = m (j, i) = 1 + j - i;
}

int main (int argc, char **argv) {
  size_t n = 0;
  if (argc > 1) {
    n = atoi(argv [1]);
  }

  cout << endl;

  cout << "real symmetric\n" << endl;

  if (n <= 0) {
    cout << "n -> ";
    cin >> n;
  }
  if (n < 5) n = 5;
  cout << "min n = 5" << endl << endl;
  size_t nrhs = 2;
  symml_t sal (n, n);   // symmetric matrix
  symmu_t sau (n, n);   // symmetric matrix
  m_t x (n, nrhs);
  m_t bl (n, nrhs), bu (n, nrhs);  // RHS matrices

  std::vector<fortran_int_t> ipiv (n);

  init_symm2 (sal);
  print_m (sal, "sal");
  cout << endl;

  init_symm2 (sau);
  print_m (sau, "sau");
  cout << endl;

  for (int i = 0; i < x.size1(); ++i) {
    x (i, 0) = 1.;
    x (i, 1) = 2.;
  }
  bl = prod (sal, x);
  bu = prod (sau, x);

  print_m (bl, "bl");
  cout << endl;
  print_m (bu, "bu");
  cout << endl;

  symml_t sal1 (sal);   // for part 2
  symmu_t sau1 (sau);
  m_t bl1 (bl), bu1 (bu);

//  lapack::spsv (sal, bl);
//  no ipiv less version is currently provided, so fall back to using ipiv
  lapack::spsv (sal, ipiv, bl);
  print_m (bl, "xl");
  cout << endl;

//  lapack::spsv (sau, bu);
//  no ipiv less version is currently provided, so fall back to using ipiv
  lapack::spsv (sau, ipiv, bu);
  print_m (bu, "xu");
  cout << endl;

  // part 2

  int err = lapack::spsv (sal1, ipiv, bl1);
  print_m (sal1, "sal1 factored");
  cout << endl;
  print_v (ipiv, "ipiv");
  cout << endl;
  print_m (bl1, "xl1");
  cout << endl;

  err = lapack::spsv (sau1, ipiv, bu1);
  print_m (sau1, "sau1 factored");
  cout << endl;
  print_v (ipiv, "ipiv");
  cout << endl;
  print_m (bu1, "xu1");
  cout << endl;
  cout << endl;

  //////////////////////////////////////////////////////////
  cout << "\n==========================================\n" << endl;
  cout << "complex symmetric\n" << endl;

  csymml_t scal (n, n);   // symmetric matrix
  csymmu_t scau (n, n);   // symmetric matrix
  cm_t cx (n, 1);
  cm_t cbl (n, 1), cbu (n, 1);  // RHS

  init_symm2 (scal);
  init_symm2 (scau);
  scal *= cmplx_t (1, 1);
  scau *= cmplx_t (1, -0.5);

  print_m (scal, "scal");
  cout << endl;
  print_m (scau, "scau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, -1);
  print_m (cx, "cx");
  cout << endl;
  cbl = prod (scal, cx);
  cbu = prod (scau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

//  int ierr = lapack::spsv (scal, cbl);
//  no ipiv less version is currently provided, so fall back to using ipiv
  int ierr = lapack::spsv (scal, ipiv, cbl);
  if (ierr == 0)
    print_m (cbl, "cxl");
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

  ierr = lapack::spsv (scau, ipiv, cbu);
  if (ierr == 0) {
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (cbu, "cxu");
  }
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

}


#pragma once
#include <vector>
#include "mass_matrix.hpp"
#include <Eigen/Sparse>
#include "opposite_volumes.hpp"

namespace mtao { namespace geometry { namespace mesh {
    template <typename VertexDerived, typename SimplexDerived>
        auto cot_laplacian( const Eigen::MatrixBase<VertexDerived>& V, const Eigen::MatrixBase<SimplexDerived>& S) {
            using Scalar = typename VertexDerived::Scalar;
            using Triplet = Eigen::Triplet<double>;
            std::vector<Triplet> trips;
            int mV = V.cols();
            Eigen::SparseMatrix<Scalar> L(mV,mV);
            assert(S.rows() == 3);
            auto OV = opposite_volumes(V,S);
            std::vector<double> diag(mV,0);
            for(int fi = 0; fi < S.cols(); ++fi) {
                auto&& le = OV.col(fi).array();
                auto&& f = S.col(fi);

                double s = (le.sum()) / 2.0;
                double area = std::sqrt(s * (s-le).prod());
                double d = le.prod() / (2 * area);

                auto Sa = le/d;

                for(int i = 0; i < 3; ++i) {
                    double a = le((i+0)%3);
                    double b = le((i+1)%3);
                    double c = le((i+2)%3);
                    double ca = ( b*b + c*c - a*a ) / ( 2 * b*c );
                    double v = -.5 * ca / Sa(i);

                    //int A = f((i+0)%3);
                    int B = f((i+1)%3);
                    int C = f((i+2)%3);
                    trips.emplace_back(B,C,v);
                    trips.emplace_back(C,B,v);
                    diag[B] += -v;
                    diag[C] += -v;
                }

            }
            for(size_t i = 0; i < diag.size(); ++i) {
                trips.emplace_back(i,i,diag[i]);
            }

            L.setFromTriplets(trips.begin(),trips.end());
            return L;
        }

    template <typename VertexDerived, typename SimplexDerived>
        auto strong_cot_laplacian( const Eigen::MatrixBase<VertexDerived>& V, const Eigen::MatrixBase<SimplexDerived>& S) {
            using Scalar = typename VertexDerived::Scalar;
            auto DM = mass_matrix(V,S).array().eval();
            DM = (DM.abs() > 1e-6).select(1/DM,0);
            Eigen::SparseMatrix<Scalar> L = DM.matrix().asDiagonal() * cot_laplacian(V,S);
            return L;
        }
}
}
}


/**
 * @Copyright 2020, Wuhan University of Technology
 * @Author: Pengwei Zhou
 * @Date: 2021/2/10 下午9:11
 * @FileName: registration.hpp
 * @Description: T-LOAM frontend Lidar odometry
 * @License: See LICENSE for the license information
 */

#ifndef TLOAM_REGISTRATION_HPP
#define TLOAM_REGISTRATION_HPP

#include <thread>
#include <deque>
#include <mutex>
#include <future>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <yaml-cpp/yaml.h>
#include <omp.h>

#include <open3d/Open3D.h>

#include "third_party/sophus/so3.hpp"
#include "third_party/sophus/se3.hpp"

#include "tloam/models/utils/work_space_path.h"
#include "tloam/models/registration/registration_interface.hpp"
#include "tloam/open3d/open3d_to_ros.hpp"

namespace tloam{

class PointToPointErr : public ceres::SizedCostFunction<3, 6>{
public:
  PointToPointErr(Eigen::Vector3d& source_, Eigen::Vector3d& target_, double& weight_, double* cost_);

  ~PointToPointErr() override { }

  virtual bool Evaluate(
    double const* const* parameters,
    double* residuals,
    double** jacobians
  ) const;

public:
  Eigen::Vector3d target;
  Eigen::Vector3d source;
  double weight;
  mutable double* cost;
};

class PointToLineErr : public ceres::SizedCostFunction<3, 6>{
public:
  PointToLineErr(
    Eigen::Vector3d& curr_point_,
    Eigen::Vector3d& line_point_a_,
    Eigen::Vector3d& line_point_b_,
    double& weight_, double* cost_
  );

  ~PointToLineErr() override { }

  virtual bool Evaluate(
    double const* const* parameters,
    double* residuals,
    double** jacobians
  ) const;

public:
  Eigen::Vector3d curr_point;
  Eigen::Vector3d line_point_a;
  Eigen::Vector3d line_point_b;
  double weight;
  mutable double* cost;
};

class PointToPlaneErr : public ceres::SizedCostFunction<1, 6>{
public:
  PointToPlaneErr(Eigen::Vector3d curr_point_, Eigen::Vector3d& unit_norm_, double& devia_, double& weight_, double* cost_);

  ~PointToPlaneErr() override { }

  virtual bool Evaluate(
    double const* const* parameters,
    double* residuals,
    double** jacobians
  ) const;

public:
  Eigen::Vector3d curr_point;
  Eigen::Vector3d unit_norm;
  double devia;
  double weight;
  mutable double* cost;
};

class PlaneToPlaneErr : public ceres::SizedCostFunction<3, 6>{
public:
  PlaneToPlaneErr(
    Eigen::Vector3d& source_point_,
    Eigen::Matrix3d& source_covs_,
    Eigen::Vector3d& target_point_,
    Eigen::Matrix3d& target_covs_,
    double& weight_, double* cost_
  );

  ~PlaneToPlaneErr() override { }

  virtual bool Evaluate(
    double const* const* parameters,
    double* residuals,
    double** jacobians
  ) const;

public:
  Eigen::Vector3d source_point;
  Eigen::Matrix3d source_covs;
  Eigen::Vector3d target_point;
  Eigen::Matrix3d target_covs;
  double weight;
  mutable double* cost;
};

class PoseSE3Parameterization : public ceres::LocalParameterization {
public:
  PoseSE3Parameterization() = default;
  ~PoseSE3Parameterization() override = default;

  bool Plus(const double* x, const double* delta, double* x_plus_delta) const override;
  bool ComputeJacobian(const double* x, double* jacobian) const override;
  int GlobalSize() const override{
    return 6;
  }
  int LocalSize() const override{
    return 6;
  }
};

// local registration based on truncated least squares method
class LocalRegistration : public RegistrationInterface{
public:
  enum Factor{
    planar = 2,
    planarEdge = 3,
    planarEdgeSphere = 4
  };
  LocalRegistration(const YAML::Node& config_node);
  ~LocalRegistration() override;

  void initConfig(const YAML::Node& node);

  Eigen::Isometry3d getTransform(void);

  Eigen::Isometry3d getPoseIncrement(void);

  bool setInputSource(Frame& cloud_in_) override;
  bool setInputTarget(Frame& cloud_in_) override;
  bool scanMatching(Frame& out_result_, Eigen::Isometry3d& predict_pose_, Eigen::Isometry3d& result_pose_) override;

  std::pair<double, double> getFitnessScore() override;

  void resetKDTree(void);

protected:
  /**
   * @brief fit the best plane from the point cloud set
   * @param point_set_, input point set
   * @return plane model
   */
  Eigen::Vector4d fitBestPlane(std::vector<Eigen::Vector3d>& point_set_);

  /**
   * @brief calculate the covariance matrix
   * @param cloud_in_, input point cloud
   * @param out_covs_, output covariance matrix
   * @return true if success otherwise false
   */
  bool calculateCov(
    const std::shared_ptr<open3d::geometry::PointCloud2>& cloud_in_,
    std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>& out_covs_
  );

  /**
   * @brief add edge features factor to ceres residual function
   * @param scan_edge_, current frame edge features
   * @param submap_edge_, edge_features in submap
   * @param edge_weights_, the weight value of each feature
   * @param edge_residuals_, the residual value of each feature
   * @param problem_, ceres problem
   * @param loss_function_, cerese loss function
   * @return true if success otherwise false
   */
  bool addEdgeCostFactor(
    const std::shared_ptr<open3d::geometry::PointCloud2>& scan_edge_,
    const std::shared_ptr<open3d::geometry::PointCloud2>& submap_edge_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& edge_weights_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& edge_residuals_,
    ceres::Problem& problem_,
    ceres::LossFunction* loss_function_
  );

  /**
   * @brief add sphere features factor to ceres residual function
   * @param scan_sphere_, current frame sphere features
   * @param submap_sphere_, sphere features in submap
   * @param sphere_weights_, the weight value of each feature
   * @param sphere_residuals_, the residual value of each feature
   * @param problem_, ceres problem
   * @param loss_function_, ceres loss function
   * @return true if success otherwise false
   */
  bool addSphereCostFactor(
    const std::shared_ptr<open3d::geometry::PointCloud2>& scan_sphere_,
    const std::shared_ptr<open3d::geometry::PointCloud2>& submap_sphere_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& sphere_weights_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& sphere_residuals_,
    ceres::Problem& problem_,
    ceres::LossFunction* loss_function_
  );

  /**
   * @brief add planar features factor to ceres residual function point_to_plane
   * @param scan_plane_, current frame planar features
   * @param submap_plane_, planar features in submap
   * @param plane_weights_, the weight of each feature
   * @param plane_residuals_, the residual value of each feature
   * @param problem, ceres problem
   * @param loss_function, ceres loss function
   * @return true if success otherwise false
   */
  bool addSurfCostFactor(
    const std::shared_ptr<open3d::geometry::PointCloud2>& scan_plane_,
    const std::shared_ptr<open3d::geometry::PointCloud2>& submap_plane_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& plane_weights_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& plane_residuals_,
    ceres::Problem& problem,
    ceres::LossFunction* loss_function
  );

  /**
   * @brief add planar features factor to ceres residual function plane_to_plane
   * @param scan_plane_, current frame planar features
   * @param submap_sphere_, planar features in submap
   * @param scan_plane_covs_, scan_plane covariance
   * @param submap_plane_covs_, submap_plane_ covariance
   * @param plane_weights_, the weight of each feature
   * @param plane_residuals_, the residual value of each feature
   * @param problem, ceres problem
   * @param loss_function, ceres loss function
   * @return true if success otherwise false
   */
  bool addSurfCostFactor2(
    const std::shared_ptr<open3d::geometry::PointCloud2>& scan_plane_,
    const std::shared_ptr<open3d::geometry::PointCloud2>& submap_plane_,
    std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>& scan_plane_covs_,
    std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>& submap_plane_covs_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& plane_weights_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& plane_residuals_,
    ceres::Problem& problem,
    ceres::LossFunction* loss_function
  );

  /**
   * @brief add ground features factor to ceres residual function
   * @param scan_ground_, current frame ground features
   * @param submap_ground_, ground features in submap
   * @param ground_weights_, the weight of each feature
   * @param ground_residuals_, the residual of each feature
   * @param problem, ceres problem
   * @param loss_function, ceres loss function
   * @return true if success otherwise false
   */
  bool addGroundCostFactor(
    const std::shared_ptr<open3d::geometry::PointCloud2>& scan_ground_,
    const std::shared_ptr<open3d::geometry::PointCloud2>& submap_ground_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& ground_weights_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& ground_residuals_,
    ceres::Problem& problem,
    ceres::LossFunction* loss_function
  );

  /**
   * @brief add ground features factor to ceres residual function
   * @param scan_ground_, current frame ground features
   * @param submap_ground_, ground features in submap
   * @param scan_ground_covs_, scan_ground covariance
   * @param submap_ground_covs_, submap_ground covariance
   * @param ground_weights_, the weight of each feature
   * @param ground_residuals_, the residual of each feature
   * @param problem, ceres problem
   * @param loss_function, ceres loss function
   * @return true if success otherwise false
   */
  bool addGroundCostFactor2(
      const std::shared_ptr<open3d::geometry::PointCloud2>& scan_ground_,
      const std::shared_ptr<open3d::geometry::PointCloud2>& submap_ground_,
      std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>& scan_ground_covs_,
      std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>& submap_ground_covs_,
      Eigen::Matrix<double, 1, Eigen::Dynamic>& ground_weights_,
      Eigen::Matrix<double, 1, Eigen::Dynamic>& ground_residuals_,
      ceres::Problem& problem,
      ceres::LossFunction* loss_function
  );

  /**
   * @brief update corresponding features weights
   * @param weights_, output weights
   * @param residuals_, feature residuals
   * @param totalSize_, feature size
   * @param noise_bound_sq_, noise upper bound
   * @param th1_, gnc control threshold, the weight is assigned one if the rq less than the th1
   * @param th2_, gnc control threshold, the weight is assigned zero if the rq more than the th2
   * @param mu_, gnc control parameters
   * @return true if success otherwise false
   */
  bool updateWeight(
    Eigen::Matrix<double, 1, Eigen::Dynamic>& weights_,
    Eigen::Matrix<double, 1, Eigen::Dynamic>& residuals_,
    double totalSize_, double noise_bound_sq_,
    double th1_, double th2_,
    double mu_
  );

private:
  // Note with optimized pose, the front 3d represents translation, and the back 3D represents rotation.
  double parameters[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  Eigen::Map<Eigen::Matrix<double, 6, 1>> se3_pose_ = Eigen::Map<Eigen::Matrix<double, 6, 1>>(parameters);
  int num_threads_;
  int k_corr;

  int factor_num;
  double edge_dist_thres, sphere_dist_thres, planar_dist_thres, ground_dist_thres, edge_dir_thres, fitness_thres;
  int edge_maxnum, sphere_maxnum, planar_maxnum, ground_maxnum;
  std::mutex factor_mutex;

  int max_iterations, nr_iterations;
  double cost_threshold, gnc_factor, noise_bound;

  Eigen::Isometry3d curr_frame_pose;
  Eigen::Isometry3d last_frame_pose;

  std::shared_ptr<open3d::geometry::PointCloud2> scan_planar;
  std::shared_ptr<open3d::geometry::PointCloud2> scan_edge;
  std::shared_ptr<open3d::geometry::PointCloud2> scan_sphere;
  std::shared_ptr<open3d::geometry::PointCloud2> scan_ground;

  std::shared_ptr<open3d::geometry::PointCloud2> submap_planar;
  std::shared_ptr<open3d::geometry::PointCloud2> submap_edge;
  std::shared_ptr<open3d::geometry::PointCloud2> submap_sphere;
  std::shared_ptr<open3d::geometry::PointCloud2> submap_ground;

  std::shared_ptr<open3d::geometry::KDTreeFlann> planar_kd_tree;
  std::shared_ptr<open3d::geometry::KDTreeFlann> edge_kd_tree;
  std::shared_ptr<open3d::geometry::KDTreeFlann> sphere_kd_tree;
  std::shared_ptr<open3d::geometry::KDTreeFlann> ground_kd_tree;

  std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> source_planar_covs;
  std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> submap_planar_covs;

  std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> source_ground_covs;
  std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> submap_ground_covs;

};

// waiting for code coming
class GlobalRegistration : public RegistrationInterface{
  GlobalRegistration();
  ~GlobalRegistration() override;

  bool setInputSource(Frame& cloud_in_) override;
  bool setInputTarget(Frame& cloud_in_) override;
  bool scanMatching(Frame& out_result_, Eigen::Isometry3d& predict_pose_, Eigen::Isometry3d& result_pose_) override;
  std::pair<double, double> getFitnessScore() override;
};

}
#endif //TLOAM_REGISTRATION_HPP


#pragma once

#include <boost/range.hpp>
#include <cmath>
#include <math/Matrix.hpp>
#include <math/Vec.hpp>

template <class T>
Matrix<4, 4, T> frustum(T left, T right, T bottom, T top, T near, T far) {
	T a = 2 * near / (right - left),
		b = 2 * near / (top - bottom),
		c = (right + left) / (right - left),
		d = (top + bottom) / (top - bottom),
		e = - (far + near) / (far - near),
		f = -2 * far * near / (far - near);

	T out[] = {
		a, 0, 0,  0,
		0, b, 0,  0,
		c, d, e, -1,
		0, 0, f,  0
	};
	return Matrix<4, 4, T>(boost::begin(out), boost::end(out));
}

template <class T>
Matrix<4, 4, T> ortho(T max_x, T max_y) {
	T a = 1.0f / max_x;
	T b = 1.0f / max_y;

	T out[] = {
		a, 0,  0, 0,
		0, b,  0, 0,
		0, 0, -1, 0,
		0, 0,  0, 1
	};
	return Matrix<4, 4, T>(boost::begin(out), boost::end(out));
}

template <class T>
Matrix<4, 4, T> identity_matrix() {
	T out[] = {
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1
	};

	return Matrix<4, 4, T>(boost::begin(out), boost::end(out));
}

template <class T>
Matrix<3, 3, T> identity_matrix3() {
	T out[] = {
		1, 0, 0,
		0, 1, 0,
		0, 0, 1
	};

	return Matrix<3, 3, T>(out);
}

template <class T>
Matrix<4, 4, T> perspective(T near, T far, T aspect_ratio, T field_of_view) {
	T height = std::tan(field_of_view / 2) * near,
		width = aspect_ratio * height;

	return frustum(-width, width, -height, height, near, far);
}

template <class T>
Matrix<4, 4, T> rotate_around_z(T angle) {
	T s = std::sin(angle);
	T c = std::cos(angle);

	T out[] = {
		 c, s, 0, 0,
		-s, c, 0, 0,
		 0, 0, 1, 0,
		 0, 0, 0, 1
	};

	return Matrix<4, 4, T>(boost::begin(out), boost::end(out));
}

template <class T>
Matrix<4, 4, T> translate_matrix(Vec<3, T> const & position) {
	T values[] = {
		1,             0,             0,             0,
		0,             1,             0,             0,
		0,             0,             1,             0,
		position.x(),  position.y(),  position.z(),  1
	};

	return Matrix<4, 4, T>(values);
}


/** \file ublas_cholesky.hpp \brief Cholesky decomposition */
/*
 -   begin                : 2005-08-24
 -   copyright            : (C) 2005 by Gunter Winkler, Konstantin Kutzkow
                            2011-2013 by Ruben Martinez-Cantin <rmcantin@unizar.es>
 -   email                : guwi17@gmx.de

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library 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
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

*/

#ifndef _H_CHOLESKY_HPP_
#define _H_CHOLESKY_HPP_

#include <cassert>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>

#include <boost/numeric/ublas/triangular.hpp>

namespace bayesopt
{
  namespace utils
  {
    namespace ublas = boost::numeric::ublas;

    /** \brief decompose the symmetric positive definit matrix A into product L L^T.
     *
     * \param MATRIX type of input matrix
     * \param TRIA type of lower triangular output matrix
     * \param A square symmetric positive definite input matrix (only the lower triangle is accessed)
     * \param L lower triangular output matrix
     * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
     */
    template < class MATRIX, class TRIA >
    size_t cholesky_decompose(const MATRIX& A, TRIA& L)
    {
      using namespace ublas;

      typedef typename MATRIX::value_type T;

      assert( A.size1() == A.size2() );
      assert( A.size1() == L.size1() );
      assert( A.size2() == L.size2() );

      const size_t n = A.size1();

      for (size_t k=0 ; k < n; k++) {

	double qL_kk = A(k,k) - inner_prod( project( row(L, k), range(0, k) ),
					    project( row(L, k), range(0, k) ) );

	if (qL_kk <= 0) {
	  return 1 + k;
	} else {
	  double L_kk = sqrt( qL_kk );
	  L(k,k) = L_kk;

	  matrix_column<TRIA> cLk(L, k);
	  project( cLk, range(k+1, n) )
	    = ( project( column(A, k), range(k+1, n) )
		- prod( project(L, range(k+1, n), range(0, k)),
			project(row(L, k), range(0, k) ) ) ) / L_kk;
	}
      }
      return 0;
    }

    /** \brief decompose the symmetric positive definit matrix A into product L L^T.
     *
     * \param MATRIX type of matrix A
     * \param A input: square symmetric positive definite matrix (only the lower triangle is accessed)
     * \param A output: the lower triangle of A is replaced by the cholesky factor
     * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
     */
    template < class MATRIX >
    size_t cholesky_decompose(MATRIX& A)
    {
      using namespace ublas;
      typedef typename MATRIX::value_type T;

      const MATRIX& A_c(A);

      const size_t n = A.size1();

      for (size_t k=0 ; k < n; k++) {

	double qL_kk = A_c(k,k) - inner_prod( project( row(A_c, k), range(0, k) ),
					      project( row(A_c, k), range(0, k) ) );

	if (qL_kk <= 0) {
	  return 1 + k;
	} else {
	  double L_kk = sqrt( qL_kk );

	  matrix_column<MATRIX> cLk(A, k);
	  project( cLk, range(k+1, n) )
	    = ( project( column(A_c, k), range(k+1, n) )
		- prod( project(A_c, range(k+1, n), range(0, k)),
			project(row(A_c, k), range(0, k) ) ) ) / L_kk;
	  A(k,k) = L_kk;
	}
      }
      return 0;
    }

#if 0
    using namespace ublas;

    // Operations:
    //  n * (n - 1) / 2 + n = n * (n + 1) / 2 multiplications,
    //  n * (n - 1) / 2 additions

    // Dense (proxy) case
    template<class E1, class E2>
    void inplace_solve (const matrix_expression<E1> &e1, vector_expression<E2> &e2,
                        lower_tag, column_major_tag) {
      std::cout << " is_lc ";
      typedef typename E2::size_type size_type;
      typedef typename E2::difference_type difference_type;
      typedef typename E2::value_type value_type;

      BOOST_UBLAS_CHECK (e1 ().size1 () == e1 ().size2 (), bad_size ());
      BOOST_UBLAS_CHECK (e1 ().size2 () == e2 ().size (), bad_size ());
      size_type size = e2 ().size ();
      for (size_type n = 0; n < size; ++ n) {
#ifndef BOOST_UBLAS_SINGULAR_CHECK
	BOOST_UBLAS_CHECK (e1 () (n, n) != value_type/*zero*/(), singular ());
#else
	if (e1 () (n, n) == value_type/*zero*/())
	  singular ().raise ();
#endif
	value_type t = e2 () (n) / e1 () (n, n);
	e2 () (n) = t;
	if (t != value_type/*zero*/()) {
	  project( e2 (), range(n+1, size) )
	    .minus_assign( t * project( column( e1 (), n), range(n+1, size) ) );
	}
      }
    }
#endif

    /** \brief decompose the symmetric positive definit matrix A into product L L^T.
     *
     * \param MATRIX type of matrix A
     * \param A input: square symmetric positive definite matrix (only the lower triangle is accessed)
     * \param A output: the lower triangle of A is replaced by the cholesky factor
     * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
     */
    template < class MATRIX >
    size_t incomplete_cholesky_decompose(MATRIX& A)
    {
      using namespace ublas;

      typedef typename MATRIX::value_type T;

      // read access to a const matrix is faster
      const MATRIX& A_c(A);

      const size_t n = A.size1();

      for (size_t k=0 ; k < n; k++) {

	double qL_kk = A_c(k,k) - inner_prod( project( row( A_c, k ), range(0, k) ),
					      project( row( A_c, k ), range(0, k) ) );

	if (qL_kk <= 0) {
	  return 1 + k;
	} else {
	  double L_kk = sqrt( qL_kk );

	  // aktualisieren
	  for (size_t i = k+1; i < A.size1(); ++i) {
	    T* Aik = A.find_element(i, k);

	    if (Aik != 0) {
	      *Aik = ( *Aik - inner_prod( project( row( A_c, k ), range(0, k) ),
					  project( row( A_c, i ), range(0, k) ) ) ) / L_kk;
	    }
	  }

	  A(k,k) = L_kk;
	}
      }

      return 0;
    }

    /** \brief solve system L L^T x = b inplace
     *
     * \param L a triangular matrix
     * \param x input: right hand side b; output: solution x
     */
    template < class TRIA, class MATRIX >
    void
    cholesky_solve(const TRIA& L, MATRIX& x, ublas::lower)
    {
      using namespace ublas;
      //   ::inplace_solve(L, x, lower_tag(), typename TRIA::orientation_category () );
      inplace_solve(L, x, lower_tag() );
      inplace_solve(trans(L), x, upper_tag());
    }

    /****** Added by Ruben Martinez-Cantin. 2011 ***********/

    /** \brief Computes the inverse matrix of a symmetric positive definite matrix
     *
     * \param M original matrix
     * \param inverse inverse of M
     * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
     */
    template < class Min, class Mout >
    size_t
    inverse_cholesky(const Min& M, Mout& inverse)
    {
      typedef typename Mout::value_type value_type;

      size_t size = M.size1();
      Min L(size,size);
      size_t res = cholesky_decompose(M, L);

      if (res != 0) return res;

      inverse.assign(ublas::identity_matrix<value_type>(size));
      cholesky_solve(L,inverse,ublas::lower());

      return 0;
    }

    /** \brief decompose the symmetric positive definit matrix A into product L L^T.
     *
     * \param MATRIX type of input matrix
     * \param TRIA type of lower triangular output matrix
     * \param A square symmetric positive definite input matrix (only the lower triangle is accessed)
     * \param L lower triangular output matrix
     */
    template < class TRIA, class VECTOR >
    void cholesky_add_row(TRIA& L, const VECTOR& v)
    {
      using namespace ublas;
      typedef typename TRIA::value_type T;

      assert( L.size1() == L.size2() );
      assert( L.size1()+1 == v.size() );

      const size_t n = v.size();

      L.resize(n,n);
      double L_j;

      for (size_t j = 0; j < n-1; ++j)
	{
	  L_j = v(j) - inner_prod(project (row(L, j), range(0,j)),
				  project (row(L,n-1), range(0,j)));
	  L(n-1,j) = (L_j) / L(j,j);
	}
      L_j = v(n-1) - inner_prod(project (row(L, n-1), range(0,n-1)),
				project (row(L,n-1), range(0,n-1)));
      L(n-1,n-1) = sqrt(L_j);
      return;
    }

  } //namespace utils

} // namespace bayesopt

#endif