#include "ros/ros.h"
#include "geometry_msgs/Pose2D.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Point.h"
#include "distributed_connectivity_controller/DistConnectivityMsgs.h"
#include "dynamic_reconfigure/server.h"
#include "distributed_connectivity_controller/ControlVariablesConfig.h"
#include <math.h>
#include <algorithm>
#include <vector>
#include "ros/console.h"
#include "stdio.h"
#include <LinearMath/btQuaternion.h>
#include <LinearMath/btMatrix3x3.h>
#include "nav_msgs/Odometry.h"
#include <Eigen/Dense>
#include <Eigen/LU>

#define NUM_ROBOTS 6

#define PI	3.1415926535
#define rho	133.334
#define rho1	0.7
#define rho2	2.3

#define dT		0.001

#define alphaOverBar 3.0
#define alphaUnderBar 0

using namespace std;
using namespace Eigen;

std::string robot1_name;
std::string robot2_name;
std::string robot3_name;
std::string robot4_name;
std::string robot5_name;
std::string robot6_name;

double x[NUM_ROBOTS] = {0.0};
double y[NUM_ROBOTS] = {0.0};
double theta[NUM_ROBOTS] = {0.0};

bool power = true;

double psi( double x )
{
	if ( x <= rho1 ) {
		return 1;
	} else if ( rho1 < x && x < rho2 ) {
		return ( exp(-1/(rho2-x)) / ( exp(-1/(rho2-x)) + exp(1/(rho1-x)) ) );
	} else if ( rho2 <= x ) {
		return 0;
	}
	return 0;
}

double psiPrime( double d )
{
	if ( d <= rho1 ) {
		return 0;
	} else if ( rho1 < d && d < rho2 ) {
		return (-0.5)*( pow((d-rho1),(-2))+pow((d-rho2),(-2)) ) / (1+  cosh(  (1/(-d+rho1))+(1/(-d+rho2)) )   );
	} else if ( rho2 <= d ) {
		return 0;
	}
	return 0;
}

void reconfigureCallback(distributed_connectivity_controller::ControlVariablesConfig &config, uint32_t level) {
	//k = config.k;
	//ktheta = config.ktheta;
	power = config.power;
	//xd = config.xd;
	//yd = config.yd;
}

void Callbackzero(const nav_msgs::Odometry::ConstPtr& msg)
{
	double pitch, roll;
	x[0] = msg->pose.pose.position.x * 0.001;
	y[0] = msg->pose.pose.position.y * 0.001;

	// get rotation
	btQuaternion q = btQuaternion(msg->pose.pose.orientation.x,
											msg->pose.pose.orientation.y,
											msg->pose.pose.orientation.z,
											msg->pose.pose.orientation.w);

   btMatrix3x3(q).getEulerYPR(theta[0], pitch, roll); // convert to radians
}

void Callbackone(const nav_msgs::Odometry::ConstPtr& msg)
{
	double pitch, roll;
	x[1] = msg->pose.pose.position.x * 0.001;
	y[1] = msg->pose.pose.position.y * 0.001;

	// get rotationp
	btQuaternion q = btQuaternion(msg->pose.pose.orientation.x,
											msg->pose.pose.orientation.y,
											msg->pose.pose.orientation.z,
											msg->pose.pose.orientation.w);

   btMatrix3x3(q).getEulerYPR(theta[1], pitch, roll); // convert to radians
}

void Callbacktwo(const nav_msgs::Odometry::ConstPtr& msg)
{
	double pitch, roll;
	x[2] = msg->pose.pose.position.x * 0.001;
	y[2] = msg->pose.pose.position.y * 0.001;

	// get rotation
	btQuaternion q = btQuaternion(msg->pose.pose.orientation.x,
											msg->pose.pose.orientation.y,
											msg->pose.pose.orientation.z,
											msg->pose.pose.orientation.w);

   btMatrix3x3(q).getEulerYPR(theta[2], pitch, roll); // convert to radians
}

void Callbackthree(const nav_msgs::Odometry::ConstPtr& msg)
{
	double pitch, roll;
	x[3] = msg->pose.pose.position.x * 0.001;
	y[3] = msg->pose.pose.position.y * 0.001;

	// get rotation
	btQuaternion q = btQuaternion(msg->pose.pose.orientation.x,
											msg->pose.pose.orientation.y,
											msg->pose.pose.orientation.z,
											msg->pose.pose.orientation.w);

   btMatrix3x3(q).getEulerYPR(theta[3], pitch, roll); // convert to radians
}

void Callbackfour(const nav_msgs::Odometry::ConstPtr& msg)
{
	double pitch, roll;
	x[4] = msg->pose.pose.position.x * 0.001;
	y[4] = msg->pose.pose.position.y * 0.001;

	// get rotation
	btQuaternion q = btQuaternion(msg->pose.pose.orientation.x,
											msg->pose.pose.orientation.y,
											msg->pose.pose.orientation.z,
											msg->pose.pose.orientation.w);

   btMatrix3x3(q).getEulerYPR(theta[4], pitch, roll); // convert to radians
}

void Callbackfive(const nav_msgs::Odometry::ConstPtr& msg)
{
	double pitch, roll;
	x[5] = msg->pose.pose.position.x * 0.001;
	y[5] = msg->pose.pose.position.y * 0.001;

	// get rotation
	btQuaternion q = btQuaternion(msg->pose.pose.orientation.x,
											msg->pose.pose.orientation.y,
											msg->pose.pose.orientation.z,
											msg->pose.pose.orientation.w);

   btMatrix3x3(q).getEulerYPR(theta[5], pitch, roll); // convert to radians
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "control");

	ros::NodeHandle n;
	ros::NodeHandle private_n("~");

	private_n.param<std::string>("robot1name", robot1_name, "gandhi");
	private_n.param<std::string>("robot2name", robot2_name, "suleyman");
	private_n.param<std::string>("robot3name", robot3_name, "alexander");
	private_n.param<std::string>("robot4name", robot4_name, "hiawatha");
	private_n.param<std::string>("robot5name", robot5_name, "darius");
	private_n.param<std::string>("robot6name", robot6_name, "caesar");

	dynamic_reconfigure::Server<distributed_connectivity_controller::ControlVariablesConfig> server;
  	dynamic_reconfigure::Server<distributed_connectivity_controller::ControlVariablesConfig>::CallbackType f;
  	f = boost::bind(&reconfigureCallback, _1, _2);
 	server.setCallback(f);

 	ros::Subscriber subzero = n.subscribe(  robot1_name + "/odom", 10, Callbackzero);
	ros::Subscriber subone =  n.subscribe(  robot2_name + "/odom", 10, Callbackone);
	ros::Subscriber subtwo =  n.subscribe(  robot3_name + "/odom", 10, Callbacktwo);
	ros::Subscriber subthree =n.subscribe(  robot4_name + "/odom", 10, Callbackthree);
	ros::Subscriber subfour = n.subscribe(  robot5_name + "/odom", 10, Callbackfour);
	ros::Subscriber subfive = n.subscribe(  robot6_name + "/odom", 10, Callbackfive);

	ros::Publisher pub = n.advertise<distributed_connectivity_controller::DistConnectivityMsgs>("/connectivity", 10);

	ros::Rate loop_rate(100); // 100 Hz

	ros::spinOnce();

	VectorXd v2[2] = {VectorXd::Random(NUM_ROBOTS), VectorXd::Random(NUM_ROBOTS)};
	VectorXd y1[2] = {VectorXd::Random(NUM_ROBOTS), VectorXd::Random(NUM_ROBOTS)};
	VectorXd y2[2] = {VectorXd::Random(NUM_ROBOTS), VectorXd::Random(NUM_ROBOTS)};
	VectorXd w1[2] = {VectorXd::Random(NUM_ROBOTS), VectorXd::Random(NUM_ROBOTS)};
	VectorXd w2[2] = {VectorXd::Random(NUM_ROBOTS), VectorXd::Random(NUM_ROBOTS)};

	VectorXd dv2[2] = {VectorXd::Random(NUM_ROBOTS), VectorXd::Random(NUM_ROBOTS)};
	VectorXd dy1[2] = {VectorXd::Random(NUM_ROBOTS), VectorXd::Random(NUM_ROBOTS)};
	VectorXd dy2[2] = {VectorXd::Random(NUM_ROBOTS), VectorXd::Random(NUM_ROBOTS)};
	VectorXd dw1[2] = {VectorXd::Random(NUM_ROBOTS), VectorXd::Random(NUM_ROBOTS)};
	VectorXd dw2[2] = {VectorXd::Random(NUM_ROBOTS), VectorXd::Random(NUM_ROBOTS)};

	VectorXd Lv2 = VectorXd::Random(NUM_ROBOTS);
	VectorXd Ly1 = VectorXd::Random(NUM_ROBOTS);
	VectorXd Lw1 = VectorXd::Random(NUM_ROBOTS);
	VectorXd Ly2 = VectorXd::Random(NUM_ROBOTS);
	VectorXd Lw2 = VectorXd::Random(NUM_ROBOTS);

	while (ros::ok())
	{
		double adjMatrix[NUM_ROBOTS][NUM_ROBOTS];
		double adjMatrixunw[NUM_ROBOTS][NUM_ROBOTS];
		int Ni[NUM_ROBOTS]={0};
		int Nind[NUM_ROBOTS][NUM_ROBOTS]={{0}};
		MatrixXd P2 = ArrayXXd::Zero(2,1);
		MatrixXd P3 = ArrayXXd::Zero(3,2);
		MatrixXd P4 = ArrayXXd::Zero(4,3);
		MatrixXd P5 = ArrayXXd::Zero(5,4);
		MatrixXd P6 = ArrayXXd::Zero(6,5);
		MatrixXd M = ArrayXXd::Zero(NUM_ROBOTS-1,NUM_ROBOTS-1);
		MatrixXd L = ArrayXXd::Zero(NUM_ROBOTS,NUM_ROBOTS);
		MatrixXd Lunw = ArrayXXd::Zero(NUM_ROBOTS,NUM_ROBOTS);

		P2(0,0) = -0.707106781186547;
		P2(1,0) =  0.707106781186547;

		P3(0,0) = -0.577350269189626;
		P3(0,1) = -0.577350269189626;
		P3(1,0) =  0.788675134594813;
		P3(1,1) = -0.211324865405187;
		P3(2,0) = -0.211324865405187;
		P3(2,1) =  0.788675134594813;

		P4(0,0) = -0.5;
		P4(0,1) = -0.5;
		P4(0,2) = -0.5;
		P4(1,0) =  0.833333333333333;
		P4(1,1) = -0.166666666666667;
		P4(1,2) = -0.166666666666667;
		P4(2,0) = -0.166666666666667;
		P4(2,1) =  0.833333333333333;
		P4(2,2) = -0.166666666666667;
		P4(3,0) = -0.166666666666667;
		P4(3,1) = -0.166666666666667;
		P4(3,2) =  0.833333333333333;

		double P_N5[5][4] ={	{1/sqrt(2),-(1/sqrt(6)),-1/(2.*sqrt(3)),-1/(2.*sqrt(5))},
   								{0,sqrt(0.6666666666666666),-1/(2.*sqrt(3)),-1/(2.*sqrt(5))},
   								{0,0,sqrt(3)/2.,-1/(2.*sqrt(5))},
   								{0,0,0,2/sqrt(5)},
   								{-(1/sqrt(2)),-(1/sqrt(6)),-1/(2.*sqrt(3)),-1/(2.*sqrt(5))}
   							};

   		for (int i = 0; i < 5; i++)
   		{
   			for (int j = 0; j < 4; j++)
   			{
   				P5(i,j) = P_N5[i][j];
   			}
   		}

		double P_N6[6][5] =		{{1/sqrt(2), 	-1/sqrt(6.0), -1/2.0/sqrt(3), 	-1/2.0/sqrt(5), 	-1/sqrt(30)},
								{0.0,        	sqrt(2.0/3),  -1/2.0/sqrt(3), 	-1/2.0/sqrt(5), 	-1/sqrt(30)},
								{0.0, 		 	0.0, 		sqrt(3.0)/2,    	-1/(2*sqrt(5)), -1/sqrt(30)},
								{0.0, 			0.0, 		0.0, 			2/sqrt(5), 		-1/sqrt(30)},
								{0.0, 			0.0, 		0.0,			0.0, 			sqrt(5.0/6)},
								{-1/sqrt(2), 	-1/sqrt(6), -1/2.0/sqrt(3), 	-1/2.0/sqrt(5), 	-1/sqrt(30)}};

		MatrixXd P = ArrayXXd::Zero(NUM_ROBOTS,NUM_ROBOTS-1);

		P(0,0) = P_N6[0][0];
		P(0,1) = P_N6[0][1];
		P(0,2) = P_N6[0][2];
		P(0,3) = P_N6[0][3];
		P(0,4) = P_N6[0][4];
		P(1,0) = P_N6[1][0];
		P(1,1) = P_N6[1][1];
		P(1,2) = P_N6[1][2];
		P(1,3) = P_N6[1][3];
		P(1,4) = P_N6[1][4];
		P(2,0) = P_N6[2][0];
		P(2,1) = P_N6[2][1];
		P(2,2) = P_N6[2][2];
		P(2,3) = P_N6[2][3];
		P(2,4) = P_N6[2][4];
		P(3,0) = P_N6[3][0];
		P(3,1) = P_N6[3][1];
		P(3,2) = P_N6[3][2];
		P(3,3) = P_N6[3][3];
		P(3,4) = P_N6[3][4];
		P(4,0) = P_N6[4][0];
		P(4,1) = P_N6[4][1];
		P(4,2) = P_N6[4][2];
		P(4,3) = P_N6[4][3];
		P(4,4) = P_N6[4][4];
		P(5,0) = P_N6[5][0];
		P(5,1) = P_N6[5][1];
		P(5,2) = P_N6[5][2];
		P(5,3) = P_N6[5][3];
		P(5,4) = P_N6[5][4];

		P6 = P;

		// Compute the agency matrix of the whole network
		for (int i = 0; i < NUM_ROBOTS; i++)
		{
			for (int j = 0; j < NUM_ROBOTS; j++)
			{
				adjMatrix[i][j]=0.0;
				if(i!=j){
					adjMatrix[i][j] = psi( sqrt(pow(x[j]-x[i], 2) + pow(y[j]-y[i], 2)) );
					if (adjMatrix[i][j] >0){
						adjMatrixunw[i][j] = 1;
					}
				}
				//adjMatrix[i][j] = ( sqrt(pow(x[j]-x[i], 2) + pow(y[j]-y[i], 2)) );
				if((adjMatrix[i][j] > 0) || (i==j))
				{
					Nind[i][Ni[i]] =j;Ni[i]++;
				}

			}
			Ni[i]--;// to account for the Ni++ when i==j

		}

	/*	ROS_INFO("positions");
		ROS_INFO("%f, %f, %f, %f\n",  x[0], x[1], x[2], x[3] );
		ROS_INFO("%f, %f, %f, %f\n",  y[0], y[1], y[2], y[3] );

		ROS_INFO("adj");
		ROS_INFO("%f, %f, %f, %f\n", adjMatrix[0][0], adjMatrix[0][1], adjMatrix[0][2], adjMatrix[0][3]);
		ROS_INFO("%f, %f, %f, %f\n", adjMatrix[1][0], adjMatrix[1][1], adjMatrix[1][2], adjMatrix[1][3]);
		ROS_INFO("%f, %f, %f, %f\n", adjMatrix[2][0], adjMatrix[2][1], adjMatrix[2][2], adjMatrix[2][3]);
		ROS_INFO("%f, %f, %f, %f\n", adjMatrix[3][0], adjMatrix[3][1], adjMatrix[3][2], adjMatrix[3][3]);

		ROS_INFO("start\n");
		ROS_INFO("%d, %d, %d, %d\n", Ni[0], Ni[1], Ni[2], Ni[3]);
		ROS_INFO("%d, %d, %d, %d\n", Nind[0][0], Nind[0][1], Nind[0][2], Nind[0][3]);
		ROS_INFO("%d, %d, %d, %d\n", Nind[1][0], Nind[1][1], Nind[1][2], Nind[1][3]);
		ROS_INFO("%d, %d, %d, %d\n", Nind[2][0], Nind[2][1], Nind[2][2], Nind[2][3]);
		ROS_INFO("%d, %d, %d, %d\n", Nind[3][0], Nind[3][1], Nind[3][2], Nind[3][3]);
		*/
		for (int i = 0; i < NUM_ROBOTS; i++)
		{
			for (int j = 0; j < NUM_ROBOTS; j++)
			{
				L(i,j)=0;
				Lunw(i,j)=0;
				if ( i == j ) {
					double rowSum = 0;
					double rowSumunw = 0;
					for (int k = 0; k < NUM_ROBOTS; k++) {
						rowSum += adjMatrix[i][k];
						rowSumunw += adjMatrixunw[i][k];
					}
					L(i,j) = rowSum - adjMatrix[i][j];
					Lunw(i,j) = rowSumunw - adjMatrixunw[i][j];
				} else {
					L(i,j) = -adjMatrix[i][j];
					Lunw(i,j) = -adjMatrixunw[i][j];
				}
			}
		}

		M = P.transpose() * L * P;

		ROS_INFO("P");
		ROS_INFO("%f, %f, %f, %f, %f\n", P(0,0), P(0,1), P(0,2), P(0,3), P(0,4));
		ROS_INFO("%f, %f, %f, %f, %f\n", P(1,0), P(1,1), P(1,2), P(1,3), P(1,4));
		ROS_INFO("%f, %f, %f, %f, %f\n", P(2,0), P(2,1), P(2,2), P(2,3), P(2,4));
		ROS_INFO("%f, %f, %f, %f, %f\n", P(3,0), P(3,1), P(3,2), P(3,3), P(3,4));
		ROS_INFO("%f, %f, %f, %f, %f\n", P(4,0), P(4,1), P(4,2), P(4,3), P(4,4));
		ROS_INFO("%f, %f, %f, %f, %f\n", P(5,0), P(5,1), P(5,2), P(5,3), P(5,4));

		ROS_INFO("L");
		ROS_INFO("%f, %f, %f, %f, %f, %f\n", L(0,0), L(0,1), L(0,2), L(0,3), L(0,4), L(0,5));
		ROS_INFO("%f, %f, %f, %f, %f, %f\n", L(1,0), L(1,1), L(1,2), L(1,3), L(1,4), L(1,5));
		ROS_INFO("%f, %f, %f, %f, %f, %f\n", L(2,0), L(2,1), L(2,2), L(2,3), L(2,4), L(2,5));
		ROS_INFO("%f, %f, %f, %f, %f, %f\n", L(3,0), L(3,1), L(3,2), L(3,3), L(3,4), L(3,5));
		ROS_INFO("%f, %f, %f, %f, %f, %f\n", L(4,0), L(4,1), L(4,2), L(4,3), L(4,4), L(4,5));
		ROS_INFO("%f, %f, %f, %f, %f, %f\n", L(5,0), L(5,1), L(5,2), L(5,3), L(5,4), L(5,5));

		ROS_INFO("M");
		ROS_INFO("%f, %f, %f, %f, %f\n", M(0,0), M(0,1), M(0,2), M(0,3), M(0,4));
		ROS_INFO("%f, %f, %f, %f, %f\n", M(1,0), M(1,1), M(1,2), M(1,3), M(1,4));
		ROS_INFO("%f, %f, %f, %f, %f\n", M(2,0), M(2,1), M(2,2), M(2,3), M(2,4));
		ROS_INFO("%f, %f, %f, %f, %f\n", M(3,0), M(3,1), M(3,2), M(3,3), M(3,4));
		ROS_INFO("%f, %f, %f, %f, %f\n", M(4,0), M(4,1), M(4,2), M(4,3), M(4,4));

		// Construct the individual adjacency matrices
		// adjacency 0
		double adjMatrix_0[Ni[0]+1][Ni[0]+1];
		for(int i = 0; i < Ni[0]+1;  i++)
		{
			for(int j = 0; j < Ni[0]+1;  j++)
			{
				adjMatrix_0[i][j] = 0;
			}
		}
		for(int i=0; i< Ni[0]+1;i++)
		{
			for(int j=0; j< Ni[0]+1;j++)
			{
				adjMatrix_0[i][j] = adjMatrix[Nind[0][i]][Nind[0][j]];
			}
		}

		// adjacency 1
		double adjMatrix_1[Ni[1]+1][Ni[1]+1];
		for(int i = 0; i < Ni[1]+1;  i++)
		{
			for(int j = 0; j < Ni[1]+1;  j++)
			{
				adjMatrix_1[i][j] = 0;
			}
		}
		for(int i=0; i< Ni[1]+1;i++)
		{
			for(int j=0; j< Ni[1]+1;j++)
			{
				adjMatrix_1[i][j] = adjMatrix[(Nind[1][i])][(Nind[1][j])];
			}
		}

		// adjacency 2
		double adjMatrix_2[Ni[2]+1][Ni[2]+1];
		for(int i = 0; i < Ni[2]+1;  i++)
		{
			for(int j = 0; j < Ni[2]+1;  j++)
			{
				adjMatrix_2[i][j] = 0;
			}
		}
		for(int i=0; i< Ni[2]+1;i++)
		{
			for(int j=0; j< Ni[2]+1;j++)
			{
				adjMatrix_2[i][j] = adjMatrix[(Nind[2][i])][(Nind[2][j])];
			}
		}

		// adjacency 3
		double adjMatrix_3[Ni[3]+1][Ni[3]+1];
		for(int i = 0; i < Ni[3]+1;  i++)
		{
			for(int j = 0; j < Ni[3]+1;  j++)
			{
				adjMatrix_3[i][j] = 0;
			}
		}
		for(int i=0; i< Ni[3]+1;i++)
		{
			for(int j=0; j< Ni[3]+1;j++)
			{
				adjMatrix_3[i][j] = adjMatrix[Nind[3][i]][Nind[3][j]];
			}
		}

		// adjacency 4
		double adjMatrix_4[Ni[4]+1][Ni[4]+1];
		for(int i = 0; i < Ni[4]+1;  i++)
		{
			for(int j = 0; j < Ni[4]+1;  j++)
			{
				adjMatrix_4[i][j] = 0;
			}
		}
		for(int i=0; i< Ni[4]+1;i++)
		{
			for(int j=0; j< Ni[4]+1;j++)
			{
				adjMatrix_4[i][j] = adjMatrix[Nind[4][i]][Nind[4][j]];
			}
		}

		// adjacency 5
		double adjMatrix_5[Ni[5]+1][Ni[5]+1];
		for(int i = 0; i < Ni[5]+1;  i++)
		{
			for(int j = 0; j < Ni[5]+1;  j++)
			{
				adjMatrix_5[i][j] = 0;
			}
		}
		for(int i=0; i< Ni[5]+1;i++)
		{
			for(int j=0; j< Ni[5]+1;j++)
			{
				adjMatrix_5[i][j] = adjMatrix[Nind[5][i]][Nind[5][j]];
			}
		}
/*
		ROS_INFO("adj_3");
		ROS_INFO("%f, %f, %f, %f\n", adjMatrix_3[0][0], adjMatrix_3[0][1], adjMatrix_3[0][2], adjMatrix_3[0][3]);
		ROS_INFO("%f, %f, %f, %f\n", adjMatrix_3[1][0], adjMatrix_3[1][1], adjMatrix_3[1][2], adjMatrix_3[1][3]);
		ROS_INFO("%f, %f, %f, %f\n", adjMatrix_3[2][0], adjMatrix_3[2][1], adjMatrix_3[2][2], adjMatrix_3[2][3]);
		ROS_INFO("%f, %f, %f, %f\n", adjMatrix_3[3][0], adjMatrix_3[3][1], adjMatrix_3[3][2], adjMatrix_3[3][3]);

		ROS_INFO("begin");
		ROS_INFO("%d, %d, %d, %d\n", adjMatrix_3[0][0], adjMatrix_3[0][1], adjMatrix_3[0][2], adjMatrix_3[0][3]);
		ROS_INFO("%d, %d, %d, %d\n", adjMatrix_3[1][0], adjMatrix_3[1][1], adjMatrix_3[1][2], adjMatrix_3[1][3]);
		ROS_INFO("%d, %d, %d, %d\n", adjMatrix_3[2][0], adjMatrix_3[2][1], adjMatrix_3[2][2], adjMatrix_3[2][3]);
		ROS_INFO("%d, %d, %d, %d\n", adjMatrix_3[3][0], adjMatrix_3[3][1], adjMatrix_3[3][2], adjMatrix_3[3][3]);		*/

		// Construct the Lks and Mks
		// Need to check if these initialize to zero or garbage !!
		MatrixXd L0 = ArrayXXd::Zero(Ni[0]+1,Ni[0]+1);
		MatrixXd M0 = ArrayXXd::Zero(Ni[0],Ni[0]);
		MatrixXd L1 = ArrayXXd::Zero(Ni[1]+1,Ni[1]+1);
		MatrixXd M1 = ArrayXXd::Zero(Ni[1],Ni[1]);
		MatrixXd L2 = ArrayXXd::Zero(Ni[2]+1,Ni[2]+1);
		MatrixXd M2 = ArrayXXd::Zero(Ni[2],Ni[2]);
		MatrixXd L3 = ArrayXXd::Zero(Ni[3]+1,Ni[3]+1);
		MatrixXd M3 = ArrayXXd::Zero(Ni[3],Ni[3]);

		MatrixXd L4 = ArrayXXd::Zero(Ni[4]+1,Ni[4]+1);
		MatrixXd M4 = ArrayXXd::Zero(Ni[4],Ni[4]);
		MatrixXd L5 = ArrayXXd::Zero(Ni[5]+1,Ni[5]+1);
		MatrixXd M5 = ArrayXXd::Zero(Ni[5],Ni[5]);

		// L0
		for (int i = 0; i < Ni[0]+1; i++)
		{
			for (int j = 0; j < Ni[0]+1; j++)
			{	L0(i,j)=0;
				if ( i == j ) {
					double rowSum = 0;
					for (int k = 0; k < Ni[0]+1; k++) {
						rowSum += adjMatrix_0[i][k];
					}
					L0(i,j) = rowSum - adjMatrix_0[i][j];
				} else {
					L0(i,j) = -adjMatrix_0[i][j];
				}
			}
		}

		// L1
		for (int i = 0; i < Ni[1]+1; i++)
		{
			for (int j = 0; j < Ni[1]+1; j++)
			{	L1(i,j)=0;
				if ( i == j ) {
					double rowSum = 0;
					for (int k = 0; k < Ni[1]+1; k++) {
						rowSum += adjMatrix_1[i][k];
					}
					L1(i,j) = rowSum - adjMatrix_1[i][j];
				} else {
					L1(i,j) = -adjMatrix_1[i][j];
				}
			}
		}

		// L2
		for (int i = 0; i < Ni[2]+1; i++)
		{
			for (int j = 0; j < Ni[2]+1; j++)
			{	L2(i,j)=0;
				if ( i == j ) {
					double rowSum = 0;
					for (int k = 0; k < Ni[2]+1; k++) {
						rowSum += adjMatrix_2[i][k];
					}
					L2(i,j) = rowSum - adjMatrix_2[i][j];
				} else {
					L2(i,j) = -adjMatrix_2[i][j];
				}
			}
		}
		// L3
		for (int i = 0; i < Ni[3]+1; i++)
		{
			for (int j = 0; j < Ni[3]+1; j++)
			{	L3(i,j)=0;
				if ( i == j ) {
					double rowSum = 0;
					for (int k = 0; k < Ni[3]+1; k++) {
						rowSum += adjMatrix_3[i][k];
					}
					L3(i,j) = rowSum - adjMatrix_3[i][j];
				} else {
					L3(i,j) = -adjMatrix_3[i][j];
				}
			}
		}

		// L4
		for (int i = 0; i < Ni[4]+1; i++)
		{
			for (int j = 0; j < Ni[4]+1; j++)
			{	L4(i,j)=0;
				if ( i == j ) {
					double rowSum = 0;
					for (int k = 0; k < Ni[4]+1; k++) {
						rowSum += adjMatrix_4[i][k];
					}
					L4(i,j) = rowSum - adjMatrix_4[i][j];
				} else {
					L4(i,j) = -adjMatrix_4[i][j];
				}
			}
		}

		// L5
		for (int i = 0; i < Ni[5]+1; i++)
		{
			for (int j = 0; j < Ni[5]+1; j++)
			{	L5(i,j)=0;
				if ( i == j ) {
					double rowSum = 0;
					for (int k = 0; k < Ni[5]+1; k++) {
						rowSum += adjMatrix_5[i][k];
					}
					L5(i,j) = rowSum - adjMatrix_5[i][j];
				} else {
					L5(i,j) = -adjMatrix_5[i][j];
				}
			}
		}

		//M0
		if (Ni[0] == 1)
			M0 = P2.transpose() * L0 * P2;

		if (Ni[0] == 2)
			M0 = P3.transpose() * L0 * P3;

		if (Ni[0] == 3)
			M0 = P4.transpose() * L0 * P4;

		if (Ni[0] == 4)
			M0 = P5.transpose() * L0 * P5;

		if (Ni[0] == 5)
			M0 = P6.transpose() * L0 * P6;

		//M1
		if (Ni[1] == 1)
			M1 = P2.transpose() * L1 * P2;

		if (Ni[1] == 2)
			M1 = P3.transpose() * L1 * P3;

		if (Ni[1] == 3)
			M1 = P4.transpose() * L1 * P4;

		if (Ni[1] == 4)
			M1 = P5.transpose() * L1 * P5;

		if (Ni[1] == 5)
			M1 = P6.transpose() * L1 * P6;

		//M2
		if (Ni[2] == 1)
			M2 = P2.transpose() * L2 * P2;

		if (Ni[2] == 2)
			M2 = P3.transpose() * L2 * P3;

		if (Ni[2] == 3)
			M2 = P4.transpose() * L2 * P4;

		if (Ni[2] == 4)
			M2 = P5.transpose() * L2 * P5;

		if (Ni[2] == 5)
			M2 = P6.transpose() * L2 * P6;

		//M3
		if (Ni[3] == 1)
			M3 = P2.transpose() * L3 * P2;

		if (Ni[3] == 2)
			M3 = P3.transpose() * L3 * P3;

		if (Ni[3] == 3)
			M3 = P4.transpose() * L3 * P4;

		if (Ni[3] == 4)
			M3 = P5.transpose() * L3 * P5;

		if (Ni[3] == 5)
			M3 = P6.transpose() * L3 * P6;

		//M4
		if (Ni[4] == 1)
			M4 = P2.transpose() * L4 * P2;

		if (Ni[4] == 2)
			M4 = P3.transpose() * L4 * P3;

		if (Ni[4] == 3)
			M4 = P4.transpose() * L4 * P4;

		if (Ni[4] == 4)
			M4 = P5.transpose() * L4 * P5;

		if (Ni[4] == 5)
			M4 = P6.transpose() * L4 * P6;

		//M5
		if (Ni[5] == 1)
			M5 = P2.transpose() * L5 * P2;

		if (Ni[5] == 2)
			M5 = P3.transpose() * L5 * P3;

		if (Ni[5] == 3)
			M5 = P4.transpose() * L5 * P4;

		if (Ni[5] == 4)
			M5 = P5.transpose() * L5 * P5;

		if (Ni[5] == 5)
			M5 = P6.transpose() * L5 * P6;

	   	// We have computed all M_k and L_ks;
	   	// Now we compute the dLk_dx and dLk_dx

		MatrixXd dLdx_0 = ArrayXXd::Zero(Ni[0]+1,Ni[0]+1);
		MatrixXd dLdx_1 = ArrayXXd::Zero(Ni[1]+1,Ni[1]+1);
		MatrixXd dLdx_2 = ArrayXXd::Zero(Ni[2]+1,Ni[2]+1);
		MatrixXd dLdx_3 = ArrayXXd::Zero(Ni[3]+1,Ni[3]+1);
		MatrixXd dLdx_4 = ArrayXXd::Zero(Ni[4]+1,Ni[4]+1);
		MatrixXd dLdx_5 = ArrayXXd::Zero(Ni[5]+1,Ni[5]+1);

		MatrixXd dLdy_0 = ArrayXXd::Zero(Ni[0]+1,Ni[0]+1);
		MatrixXd dLdy_1 = ArrayXXd::Zero(Ni[1]+1,Ni[1]+1);
		MatrixXd dLdy_2 = ArrayXXd::Zero(Ni[2]+1,Ni[2]+1);
		MatrixXd dLdy_3 = ArrayXXd::Zero(Ni[3]+1,Ni[3]+1);
		MatrixXd dLdy_4 = ArrayXXd::Zero(Ni[4]+1,Ni[4]+1);
		MatrixXd dLdy_5 = ArrayXXd::Zero(Ni[5]+1,Ni[5]+1);

		MatrixXd dMdx_0 = ArrayXXd::Zero(Ni[0],Ni[0]);
		MatrixXd dMdx_1 = ArrayXXd::Zero(Ni[1],Ni[1]);
		MatrixXd dMdx_2 = ArrayXXd::Zero(Ni[2],Ni[2]);
		MatrixXd dMdx_3 = ArrayXXd::Zero(Ni[3],Ni[3]);
		MatrixXd dMdx_4 = ArrayXXd::Zero(Ni[4],Ni[4]);
		MatrixXd dMdx_5 = ArrayXXd::Zero(Ni[5],Ni[5]);

		MatrixXd dMdy_0 = ArrayXXd::Zero(Ni[0],Ni[0]);
		MatrixXd dMdy_1 = ArrayXXd::Zero(Ni[1],Ni[1]);
		MatrixXd dMdy_2 = ArrayXXd::Zero(Ni[2],Ni[2]);
		MatrixXd dMdy_3 = ArrayXXd::Zero(Ni[3],Ni[3]);
		MatrixXd dMdy_4 = ArrayXXd::Zero(Ni[4],Ni[4]);
		MatrixXd dMdy_5 = ArrayXXd::Zero(Ni[5],Ni[5]);

		// Calculate dLdx and dLdy for each robot 0
		int currind = 0;
		for(int i = 0; i<Ni[currind]+1; i++)
		{
			for (int j=i+1; j<Ni[currind]+1; j++)
			{
				dLdx_0(i,j)=0;
				dLdy_0(i,j)=0;
				double D = sqrt( pow(x[ Nind[currind][i] ] - x[  Nind[currind][j] ], 2) + pow(y[ Nind[currind][i] ] - y[ Nind[currind][j] ], 2));
				double dAdD = psiPrime(D);

				//whatever =x[i] - x[j];

				if( currind == Nind[currind][i] ) {
					double dDdx = (x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = (y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_0(i,j) = dAdD*dDdx;
            		dLdy_0(i,j) = dAdD*dDdy;

				}

				if( currind == Nind[currind][j]  ) {
					double dDdx = -(x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = -(y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_0(i,j) = dAdD*dDdx;
            		dLdy_0(i,j) = dAdD*dDdy;

				}

            	dLdx_0(j,i) = dLdx_0(i,j);
            	dLdy_0(j,i) = dLdy_0(i,j);

			}
		}

		for(int i = 0; i < Ni[currind]+1; i++)
		{
			double sumx = 0;
			double sumy = 0;
			for(int j = 0; j<Ni[currind]+1; j++)
			{
				sumx += dLdx_0(i,j);
				sumy += dLdy_0(i,j);
			}
			dLdx_0(i,i) = -sumx;
            dLdy_0(i,i) = -sumy;
		}

		// Calculate dLdx and dLdy for robot 1
		currind = 1;
		for(int i = 0; i<Ni[currind]+1; i++)
		{
			for (int j=i+1; j<Ni[currind]+1; j++)
			{
				dLdx_1(i,j)=0;
				dLdy_1(i,j)=0;
				double D = sqrt( pow(x[ Nind[currind][i] ] - x[  Nind[currind][j] ], 2) + pow(y[ Nind[currind][i] ] - y[ Nind[currind][j] ], 2));
				double dAdD = psiPrime(D);

				//whatever =x[i] - x[j];

				if( currind == Nind[currind][i] ) {
					double dDdx = (x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = (y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_1(i,j) = dAdD*dDdx;
            		dLdy_1(i,j) = dAdD*dDdy;

				}

				if( currind == Nind[currind][j]  ) {
					double dDdx = -(x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = -(y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_1(i,j) = dAdD*dDdx;
            		dLdy_1(i,j) = dAdD*dDdy;

				}

            	dLdx_1(j,i) = dLdx_1(i,j);
            	dLdy_1(j,i) = dLdy_1(i,j);

			}
		}

		for(int i = 0; i < Ni[currind]+1; i++)
		{
			double sumx = 0;
			double sumy = 0;
			for(int j = 0; j<Ni[currind]+1; j++)
			{
				sumx += dLdx_1(i,j);
				sumy += dLdy_1(i,j);
			}
			dLdx_1(i,i) = -sumx;
            dLdy_1(i,i) = -sumy;
		}
		// Finished dLdx_1 dLdy_1

		// Calculate dLdx and dLdy for robot 2
		currind = 2;
		for(int i = 0; i<Ni[currind]+1; i++)
		{
			for (int j=i+1; j<Ni[currind]+1; j++)
			{
				dLdx_2(i,j)=0;
				dLdy_2(i,j)=0;
				double D = sqrt( pow(x[ Nind[currind][i] ] - x[  Nind[currind][j] ], 2) + pow(y[ Nind[currind][i] ] - y[ Nind[currind][j] ], 2));
				double dAdD = psiPrime(D);

				//whatever =x[i] - x[j];

				if( currind == Nind[currind][i] ) {
					double dDdx = (x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = (y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_2(i,j) = dAdD*dDdx;
            		dLdy_2(i,j) = dAdD*dDdy;

				}

				if( currind == Nind[currind][j]  ) {
					double dDdx = -(x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = -(y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_2(i,j) = dAdD*dDdx;
            		dLdy_2(i,j) = dAdD*dDdy;

				}

            	dLdx_2(j,i) = dLdx_2(i,j);
            	dLdy_2(j,i) = dLdy_2(i,j);

			}
		}

		for(int i = 0; i < Ni[currind]+1; i++)
		{
			double sumx = 0;
			double sumy = 0;
			for(int j = 0; j<Ni[currind]+1; j++)
			{
				sumx += dLdx_2(i,j);
				sumy += dLdy_2(i,j);
			}
			dLdx_2(i,i) = -sumx;
            dLdy_2(i,i) = -sumy;
		}
		// Finished dLdx_2 dLdy_2

		// Calculate dLdx and dLdy for robot 3
		currind = 3;
		for(int i = 0; i<Ni[currind]+1; i++)
		{
			for (int j=i+1; j<Ni[currind]+1; j++)
			{
				dLdx_3(i,j)=0;
				dLdy_3(i,j)=0;
				double D = sqrt( pow(x[ Nind[currind][i] ] - x[  Nind[currind][j] ], 2) + pow(y[ Nind[currind][i] ] - y[ Nind[currind][j] ], 2));
				double dAdD = psiPrime(D);

				//whatever =x[i] - x[j];

				if( currind == Nind[currind][i] ) {
					double dDdx = (x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = (y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_3(i,j) = dAdD*dDdx;
            		dLdy_3(i,j) = dAdD*dDdy;

				}

				if( currind == Nind[currind][j]  ) {
					double dDdx = -(x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = -(y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_3(i,j) = dAdD*dDdx;
            		dLdy_3(i,j) = dAdD*dDdy;

				}

            	dLdx_3(j,i) = dLdx_3(i,j);
            	dLdy_3(j,i) = dLdy_3(i,j);

			}
		}

		for(int i = 0; i < Ni[currind]+1; i++)
		{
			double sumx = 0;
			double sumy = 0;
			for(int j = 0; j<Ni[currind]+1; j++)
			{
				sumx += dLdx_3(i,j);
				sumy += dLdy_3(i,j);
			}
			dLdx_3(i,i) = -sumx;
            dLdy_3(i,i) = -sumy;
		}
		// Finished dLdx_3 dLdy_3

		// Calculate dLdx and dLdy for robot 4
		currind = 4;
		for(int i = 0; i<Ni[currind]+1; i++)
		{
			for (int j=i+1; j<Ni[currind]+1; j++)
			{
				dLdx_4(i,j)=0;
				dLdy_4(i,j)=0;
				double D = sqrt( pow(x[ Nind[currind][i] ] - x[  Nind[currind][j] ], 2) + pow(y[ Nind[currind][i] ] - y[ Nind[currind][j] ], 2));
				double dAdD = psiPrime(D);

				//whatever =x[i] - x[j];

				if( currind == Nind[currind][i] ) {
					double dDdx = (x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = (y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_4(i,j) = dAdD*dDdx;
            		dLdy_4(i,j) = dAdD*dDdy;

				}

				if( currind == Nind[currind][j]  ) {
					double dDdx = -(x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = -(y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_4(i,j) = dAdD*dDdx;
            		dLdy_4(i,j) = dAdD*dDdy;

				}

            	dLdx_4(j,i) = dLdx_4(i,j);
            	dLdy_4(j,i) = dLdy_4(i,j);

			}
		}

		for(int i = 0; i < Ni[currind]+1; i++)
		{
			double sumx = 0;
			double sumy = 0;
			for(int j = 0; j<Ni[currind]+1; j++)
			{
				sumx += dLdx_4(i,j);
				sumy += dLdy_4(i,j);
			}
			dLdx_4(i,i) = -sumx;
            dLdy_4(i,i) = -sumy;
		}
		// Finished dLdx_4 dLdy_4

		// Calculate dLdx and dLdy for robot 5
		currind = 5;
		for(int i = 0; i<Ni[currind]+1; i++)
		{
			for (int j=i+1; j<Ni[currind]+1; j++)
			{
				dLdx_5(i,j)=0;
				dLdy_5(i,j)=0;
				double D = sqrt( pow(x[ Nind[currind][i] ] - x[  Nind[currind][j] ], 2) + pow(y[ Nind[currind][i] ] - y[ Nind[currind][j] ], 2));
				double dAdD = psiPrime(D);

				//whatever =x[i] - x[j];

				if( currind == Nind[currind][i] ) {
					double dDdx = (x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = (y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_5(i,j) = dAdD*dDdx;
            		dLdy_5(i,j) = dAdD*dDdy;

				}

				if( currind == Nind[currind][j]  ) {
					double dDdx = -(x[  Nind[currind][i] ]-x[  Nind[currind][j] ]) / D;
					double dDdy = -(y[  Nind[currind][i] ]-y[  Nind[currind][j] ]) / D;

					dLdx_5(i,j) = dAdD*dDdx;
            		dLdy_5(i,j) = dAdD*dDdy;

				}

            	dLdx_5(j,i) = dLdx_5(i,j);
            	dLdy_5(j,i) = dLdy_5(i,j);

			}
		}

		for(int i = 0; i < Ni[currind]+1; i++)
		{
			double sumx = 0;
			double sumy = 0;
			for(int j = 0; j<Ni[currind]+1; j++)
			{
				sumx += dLdx_5(i,j);
				sumy += dLdy_5(i,j);
			}
			dLdx_5(i,i) = -sumx;
            dLdy_5(i,i) = -sumy;
		}
		// Finished dLdx_5 dLdy_5

		if (Ni[0] == 1){
			dMdx_0 = P2.transpose() * dLdx_0 * P2;
			dMdy_0 = P2.transpose() * dLdy_0 * P2;
		}
		if (Ni[0] == 2){
			dMdx_0 = P3.transpose() * dLdx_0 * P3;
			dMdy_0 = P3.transpose() * dLdy_0 * P3;
		}
		if (Ni[0] == 3){
			dMdx_0 = P4.transpose() * dLdx_0 * P4;
			dMdy_0 = P4.transpose() * dLdy_0 * P4;
		}
		if (Ni[0] == 4){
			dMdx_0 = P5.transpose() * dLdx_0 * P5;
			dMdy_0 = P5.transpose() * dLdy_0 * P5;
		}
		if (Ni[0] == 5){
			dMdx_0 = P6.transpose() * dLdx_0 * P6;
			dMdy_0 = P6.transpose() * dLdy_0 * P6;
		}

		if (Ni[1] == 1){
			dMdx_1 = P2.transpose() * dLdx_1 * P2;
			dMdy_1 = P2.transpose() * dLdy_1 * P2;
		}
		if (Ni[1] == 2){
			dMdx_1 = P3.transpose() * dLdx_1 * P3;
			dMdy_1 = P3.transpose() * dLdy_1 * P3;
		}
		if (Ni[1] == 3){

			dMdx_1 = P4.transpose() * dLdx_1 * P4;
			dMdy_1 = P4.transpose() * dLdy_1 * P4;
		}
		if (Ni[1] == 4){
			dMdx_1 = P5.transpose() * dLdx_1 * P5;
			dMdy_1 = P5.transpose() * dLdy_1 * P5;
		}
		if (Ni[1] == 5){

			dMdx_1 = P6.transpose() * dLdx_1 * P6;
			dMdy_1 = P6.transpose() * dLdy_1 * P6;
		}

		if (Ni[2] == 1){
			dMdx_2 = P2.transpose() * dLdx_2 * P2;
			dMdy_2 = P2.transpose() * dLdy_2 * P2;
		}
		if (Ni[2] == 2){
			dMdx_2 = P3.transpose() * dLdx_2 * P3;
			dMdy_2 = P3.transpose() * dLdy_2 * P3;
		}
		if (Ni[2] == 3){
			dMdx_2 = P4.transpose() * dLdx_2 * P4;
			dMdy_2 = P4.transpose() * dLdy_2 * P4;
		}
		if (Ni[2] == 4){
			dMdx_2 = P5.transpose() * dLdx_2 * P5;
			dMdy_2 = P5.transpose() * dLdy_2 * P5;
		}
		if (Ni[2] == 5){
			dMdx_2 = P6.transpose() * dLdx_2 * P6;
			dMdy_2 = P6.transpose() * dLdy_2 * P6;
		}

		if (Ni[3] == 1){
			dMdx_3 = P2.transpose() * dLdx_3 * P2;
			dMdy_3 = P2.transpose() * dLdy_3 * P2;
		}
		if (Ni[3] == 2){
			dMdx_3 = P3.transpose() * dLdx_3 * P3;
			dMdy_3 = P3.transpose() * dLdy_3 * P3;
		}
		if (Ni[3] == 3){
			dMdx_3 = P4.transpose() * dLdx_3 * P4;
			dMdy_3 = P4.transpose() * dLdy_3 * P4;
		}
		if (Ni[3] == 4){
			dMdx_3 = P5.transpose() * dLdx_3 * P5;
			dMdy_3 = P5.transpose() * dLdy_3 * P5;
		}
		if (Ni[3] == 5){
			dMdx_3 = P6.transpose() * dLdx_3 * P6;
			dMdy_3 = P6.transpose() * dLdy_3 * P6;
		}

		if (Ni[4] == 1){
			dMdx_4 = P2.transpose() * dLdx_4 * P2;
			dMdy_4 = P2.transpose() * dLdy_4 * P2;
		}
		if (Ni[4] == 2){
			dMdx_4 = P3.transpose() * dLdx_4 * P3;
			dMdy_4 = P3.transpose() * dLdy_4 * P3;
		}
		if (Ni[4] == 3){
			dMdx_4 = P4.transpose() * dLdx_4 * P4;
			dMdy_4 = P4.transpose() * dLdy_4 * P4;
		}
		if (Ni[4] == 4){
			dMdx_4 = P5.transpose() * dLdx_4 * P5;
			dMdy_4 = P5.transpose() * dLdy_4 * P5;
		}
		if (Ni[4] == 5){
			dMdx_4 = P6.transpose() * dLdx_4 * P6;
			dMdy_4 = P6.transpose() * dLdy_4 * P6;
		}

		if (Ni[5] == 1){
			dMdx_5 = P2.transpose() * dLdx_5 * P2;
			dMdy_5 = P2.transpose() * dLdy_5 * P2;
		}
		if (Ni[5] == 2){
			dMdx_5 = P3.transpose() * dLdx_5 * P3;
			dMdy_5 = P3.transpose() * dLdy_5 * P3;
		}
		if (Ni[5] == 3){
			dMdx_5 = P4.transpose() * dLdx_5 * P4;
			dMdy_5 = P4.transpose() * dLdy_5 * P4;
		}
		if (Ni[5] == 4){
			dMdx_5 = P5.transpose() * dLdx_5 * P5;
			dMdy_5 = P5.transpose() * dLdy_5 * P5;
		}
		if (Ni[5] == 5){
			dMdx_5 = P6.transpose() * dLdx_5 * P6;
			dMdy_5 = P6.transpose() * dLdy_5 * P6;
		}

		double trMx_0 = (M0.inverse()*dMdx_0).trace();
		double trMx_1 = (M1.inverse()*dMdx_1).trace();
		double trMx_2 = (M2.inverse()*dMdx_2).trace();
		double trMx_3 = (M3.inverse()*dMdx_3).trace();
		double trMx_4 = (M4.inverse()*dMdx_4).trace();
		double trMx_5 = (M5.inverse()*dMdx_5).trace();

		double trMy_0 = (M0.inverse()*dMdy_0).trace();
		double trMy_1 = (M1.inverse()*dMdy_1).trace();
		double trMy_2 = (M2.inverse()*dMdy_2).trace();
		double trMy_3 = (M3.inverse()*dMdy_3).trace();
		double trMy_4 = (M4.inverse()*dMdy_4).trace();
		double trMy_5 = (M5.inverse()*dMdy_5).trace();

		double detM = M.determinant();
		double Vcgain[NUM_ROBOTS] = {0.0};

		// find eigenvalues
		double lambda = 0;

		EigenSolver<MatrixXd> es(M);

		double eigenvalues[] = { es.eigenvalues()[0].real(),
								es.eigenvalues()[1].real(),
								 es.eigenvalues()[2].real(),
								es.eigenvalues()[3].real(),
								 es.eigenvalues()[4].real() };

		vector<double> eigenList (eigenvalues, eigenvalues + sizeof(eigenvalues) / sizeof(double) );

		sort(eigenList.begin(), eigenList.end());

		lambda = eigenList[0];

		// assign smallest to lambda2

		// Integrators

		Lv2 = L*v2[0];
		Ly1 = Lunw*y1[0];
		Lw1 = Lunw*w1[0];
		Ly2 = Lunw*y2[0];
		Lw2 = Lunw*w2[0];
		//ROS_INFO("L[0][0] = %f", Lunw(0, 0));
		//ROS_INFO("Ly2 = %f, %f, %f, %f", Ly2[0], Ly2[1], Ly2[2],Ly2[3]);
		double lambda2[NUM_ROBOTS] = {0.0};
		double k1,k2,k3,gamma,kp,ki;
		k1 = 18;
		k2 = 3;
		k3 = 60;
		kp = 250;
		ki = 22;
		gamma = 25;
		for (int i = 0; i < NUM_ROBOTS; i++) {

			dv2[1](i) = -k1*y1[0](i) - k2*Lv2(i) - k3*( (y2[0](i) - 1) *v2[0](i));
			dy1[1](i) = gamma*(v2[0](i)-y1[0](i)) - kp*Ly1(i)+ki*Lw1(i);
			dw1[1](i) = -ki*Ly1(i);
			dy2[1](i) = gamma*((v2[0](i)*v2[0](i))-y2[0](i)) -kp*Ly2(i) +ki*Lw2(i);
			dw2[1](i) = -ki*Ly2(i);

			//ROS_INFO("dy2[0](0) = %f", dy2[0](0));
			//ROS_INFO("dy2[1](0) = %f", dy2[1](0));
			v2[1](i) = v2[0](i) + (dT/2)*(dv2[1](i) + dv2[0](i));
			y1[1](i) = y1[0](i) + (dT/2)*(dy1[1](i) + dy1[0](i));
			w1[1](i) = w1[0](i) + (dT/2)*(dw1[1](i) + dw1[0](i));
			y2[1](i) = y2[0](i) + (dT/2)*(dy2[1](i) + dy2[0](i));
			w2[1](i) = w2[0](i) + (dT/2)*(dw2[1](i) + dw2[0](i));
			//ROS_INFO("y2[0](0) = %f", y2[0](0));
			lambda2[i] = (1-y2[1](i))*(k3/k2);

			dv2[0](i) = dv2[1](i);
			dy1[0](i) = dy1[1](i);
			dw1[0](i) = dw1[1](i);
			dy2[0](i) = dy2[1](i);
			dw2[0](i) = dw2[1](i);

			v2[0](i) = v2[1](i);
			y1[0](i) = y1[1](i);
			w1[0](i) = w1[1](i);
			y2[0](i) = y2[1](i);
			w2[0](i) = w2[1](i);
		}
		// End Integrators

		Vcgain[0] = 4*(((pow(alphaOverBar,2)-pow(alphaUnderBar,2))*(pow(lambda2[0], 2)-pow(alphaOverBar,2)))
				     /(pow(pow(lambda2[0], 2)-pow(alphaUnderBar,2),3)))*lambda2[0];

		Vcgain[1] = 4*(((pow(alphaOverBar,2)-pow(alphaUnderBar,2))*(pow(lambda2[1], 2)-pow(alphaOverBar,2)))
				     /(pow(pow(lambda2[1], 2)-pow(alphaUnderBar,2),3)))*lambda2[1];

		Vcgain[2] = 4*(((pow(alphaOverBar,2)-pow(alphaUnderBar,2))*(pow(lambda2[2], 2)-pow(alphaOverBar,2)))
				     /(pow(pow(lambda2[2], 2)-pow(alphaUnderBar,2),3)))*lambda2[2];

		Vcgain[3] = 4*(((pow(alphaOverBar,2)-pow(alphaUnderBar,2))*(pow(lambda2[3], 2)-pow(alphaOverBar,2)))
				     /(pow(pow(lambda2[3], 2)-pow(alphaUnderBar,2),3)))*lambda2[3];

		Vcgain[4] = 4*(((pow(alphaOverBar,2)-pow(alphaUnderBar,2))*(pow(lambda2[4], 2)-pow(alphaOverBar,2)))
				     /(pow(pow(lambda2[4], 2)-pow(alphaUnderBar,2),3)))*lambda2[4];

		Vcgain[5] = 4*(((pow(alphaOverBar,2)-pow(alphaUnderBar,2))*(pow(lambda2[5], 2)-pow(alphaOverBar,2)))
				     /(pow(pow(lambda2[5], 2)-pow(alphaUnderBar,2),3)))*lambda2[5];

		double u_conn_x[NUM_ROBOTS]={Vcgain[0]*trMx_0,Vcgain[1]*trMx_1,Vcgain[2]*trMx_2,Vcgain[3]*trMx_3,Vcgain[4]*trMx_4,Vcgain[5]*trMx_5};
		double u_conn_y[NUM_ROBOTS]={Vcgain[0]*trMy_0,Vcgain[1]*trMy_1,Vcgain[2]*trMy_2,Vcgain[3]*trMy_3,Vcgain[4]*trMy_4,Vcgain[5]*trMy_5};

		/////////////////////////////////////////////////////////////
		// Now begin calculation of formation and collision avoidance

		MatrixXd CAMatrix_x(NUM_ROBOTS,NUM_ROBOTS);
		MatrixXd CAMatrix_y(NUM_ROBOTS,NUM_ROBOTS);

		CAMatrix_x <<	0,0,0,0,0,0,
				 		0,0,0,0,0,0,
					 	0,0,0,0,0,0,
					 	0,0,0,0,0,0,
					 	0,0,0,0,0,0,
				 		0,0,0,0,0,0;

		CAMatrix_y <<	0,0,0,0,0,0,
				 		0,0,0,0,0,0,
					 	0,0,0,0,0,0,
					 	0,0,0,0,0,0,
					 	0,0,0,0,0,0,
				 		0,0,0,0,0,0;

		double u_ca_x[NUM_ROBOTS]={0.0};
		double u_ca_y[NUM_ROBOTS]={0.0};

		double u_fc_x[NUM_ROBOTS]={0.0};
		double u_fc_y[NUM_ROBOTS]={0.0};

		//double xd[NUM_ROBOTS]={ 1.0, 0.0, -1.0,  1.0,  0.0, -0.9};
		//double yd[NUM_ROBOTS]={ 0.5, 0.5,  0.5, -0.5, -0.5, -0.5};

		//double xd[NUM_ROBOTS]={ 0.5457, 1.9729, 1.237,-0.7, -0.3, -0.9};
		double xd[NUM_ROBOTS]={ -1.6, 1.9729, 1.237,0.5, -0.3, -0.9};
		double yd[NUM_ROBOTS]={-1.5440,-1.5410,-1.539,-1.54, -1.54, -1.54};

		xd[0] = x[0];
		yd[0] = y[0];
		//double xd[4]={ 0.7, 0.7,-0.7,-0.7};
		//double yd[4]={-0.7, 0.7, 0.7,-0.7};
		//double xd[4]={ 0.5457, -0.7, 1.237,1.8};
		//double yd[4]={-1.5740,-0.7,-1.539,-1.6};

		double R = 	0.7;
		double r =  0.400;

		// Collision Avoidance for each robot starts here:
		for(int i = 0; i<NUM_ROBOTS; i++)
		{
			for (int j=i+1; j<NUM_ROBOTS; j++)
			{
				double D = sqrt( pow(x[i] - x[j], 2) + pow(y[i] - y[j], 2));
				if(D<R && D>r){
					CAMatrix_x(i,j) = -4*(pow(R,2)-pow(r,2))*(pow(D,2)-pow(R,2))/pow((pow(D,2)-pow(r,2)),3)*(x[i]-x[j]);
		        	CAMatrix_y(i,j) = -4*(pow(R,2)-pow(r,2))*(pow(D,2)-pow(R,2))/pow((pow(D,2)-pow(r,2)),3)*(y[i]-y[j]);
            	}
            	else{
            		CAMatrix_x(i,j) = 0;
            		CAMatrix_y(i,j) = 0;
            	}
            	CAMatrix_x(j,i)=-CAMatrix_x(i,j);
	       		CAMatrix_y(j,i)=-CAMatrix_y(i,j);
			}
		}

		for(int i = 0; i < NUM_ROBOTS; i++)
		{
			double sumx = 0;
			double sumy = 0;

			for (int j=0; j<NUM_ROBOTS; j++)
			{
				sumx = sumx + CAMatrix_x(i,j);
				sumy = sumy + CAMatrix_y(i,j);
			}

			u_ca_x[i] = sumx;
			u_ca_y[i] = sumy;
			// sneak in the formation control

			u_fc_x[i] = -(x[i]-xd[i]);
			u_fc_y[i] = -(y[i]-yd[i]);

		}

		// Collision Avoidance and Formation Control ends here

		//////////////////////////////////////////////////////////////
		//////////////  Send Data  ///////////////////////////////////

		distributed_connectivity_controller::DistConnectivityMsgs msg;
		for(int i =0; i < NUM_ROBOTS; i++)
		{
			msg.u_conn_x[i] = u_conn_x[i];
			msg.u_conn_y[i] = u_conn_y[i];
			msg.u_ca_x[i] = u_ca_x[i];
			msg.u_ca_y[i] = u_ca_y[i];
			msg.u_fc_x[i] = u_fc_x[i];
			msg.u_fc_y[i] = u_fc_y[i];
			msg.lambda2[i] = lambda2[i];
		}

		msg.detM = detM;

		msg.lambda = lambda;

		msg.power = power;

		pub.publish(msg);

		ros::spinOnce();

		loop_rate.sleep();
	}

}


#include <boost/random/uniform_int.hpp>


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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.
*/

/*!\file NE....cpp
  \brief \ref EMNE_MULTIBODY - C++ input file, Time-Stepping version - O.B.

  A multibody example.
  Direct description of the model.
  Simulation with a Time-Stepping scheme.
*/

#include "SiconosKernel.hpp"
#include "KneeJointR.hpp"
#include "PrismaticJointR.hpp"
#include <boost/math/quaternion.hpp>
using namespace std;

/* Given a position of a point in the Inertial Frame and the configuration vector q of a solid
 * returns a position in the spatial frame.
 */
void fromInertialToSpatialFrame(double *positionInInertialFrame, double *positionInSpatialFrame, SP::SiconosVector  q  )
{
double q0 = q->getValue(3);
double q1 = q->getValue(4);
double q2 = q->getValue(5);
double q3 = q->getValue(6);

::boost::math::quaternion<double>    quatQ(q0, q1, q2, q3);
::boost::math::quaternion<double>    quatcQ(q0, -q1, -q2, -q3);
::boost::math::quaternion<double>    quatpos(0, positionInInertialFrame[0], positionInInertialFrame[1], positionInInertialFrame[2]);
::boost::math::quaternion<double>    quatBuff;

//perform the rotation
quatBuff = quatQ * quatpos * quatcQ;

positionInSpatialFrame[0] = quatBuff.R_component_2()+q->getValue(0);
positionInSpatialFrame[1] = quatBuff.R_component_3()+q->getValue(1);
positionInSpatialFrame[2] = quatBuff.R_component_4()+q->getValue(2);

}
void tipTrajectories(SP::SiconosVector  q, double * traj, double length)
{
  double positionInInertialFrame[3];
  double positionInSpatialFrame[3];
  // Output the position of the tip of beam1
  positionInInertialFrame[0]=length/2;
  positionInInertialFrame[1]=0.0;
  positionInInertialFrame[2]=0.0;

  fromInertialToSpatialFrame(positionInInertialFrame, positionInSpatialFrame, q  );
  traj[0] = positionInSpatialFrame[0];
  traj[1] = positionInSpatialFrame[1];
  traj[2] = positionInSpatialFrame[2];

  // std::cout <<  "positionInSpatialFrame[0]" <<  positionInSpatialFrame[0]<<std::endl;
  // std::cout <<  "positionInSpatialFrame[1]" <<  positionInSpatialFrame[1]<<std::endl;
  // std::cout <<  "positionInSpatialFrame[2]" <<  positionInSpatialFrame[2]<<std::endl;

  positionInInertialFrame[0]=-length/2;
  fromInertialToSpatialFrame(positionInInertialFrame, positionInSpatialFrame, q  );
  traj[3]= positionInSpatialFrame[0];
  traj[4] = positionInSpatialFrame[1];
  traj[5] = positionInSpatialFrame[2];
}

int main(int argc, char* argv[])
{
  try
  {

    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int nDof = 3;
    unsigned int qDim = 7;
    unsigned int nDim = 6;
    double t0 = 0;                   // initial computation time
    double T = 10.0;                  // final computation time
    double h = 0.01;                // time step
    int N = 1000;
    double L1 = 1.0;
    double theta = 1.0;              // theta for MoreauJeanOSI integrator
    double g = 9.81; // Gravity
    double m = 1.;

    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    FILE * pFile;
    pFile = fopen("data.h", "w");
    if (pFile == NULL)
    {
      printf("fopen exampleopen filed!\n");
      fclose(pFile);
    }

    cout << "====> Model loading ..." << endl << endl;

    // -- Initial positions and velocities --

    //First DS
    SP::SiconosVector q10(new SiconosVector(qDim));
    SP::SiconosVector v10(new SiconosVector(nDim));
    SP::SimpleMatrix I1(new SimpleMatrix(3, 3));
    v10->zero();
    I1->eye();
    I1->setValue(0, 0, 0.1);
    // Initial position of the center of gravity CG1
    (*q10)(0) = 0.5 * L1 / sqrt(2.0);
    (*q10)(1) = 0;
    (*q10)(2) = -0.5 * L1 / sqrt(2.0);
    // Initial orientation (a quaternion that gives the rotation w.r.t the spatial frame)
    // angle of the rotation Pi/4
    double angle = M_PI / 4;
    SiconosVector V1(3);
    V1.zero();
    // vector of the rotation (Y-axis)
    V1.setValue(0, 0);
    V1.setValue(1, 1);
    V1.setValue(2, 0);
    // construction of the quaternion
    q10->setValue(3, cos(angle / 2));
    q10->setValue(4, V1.getValue(0)*sin(angle / 2));
    q10->setValue(5, V1.getValue(1)*sin(angle / 2));
    q10->setValue(6, V1.getValue(2)*sin(angle / 2));

    // -- The dynamical system --
    SP::NewtonEulerDS beam1(new NewtonEulerDS(q10, v10, m, I1));
    // -- Set external forces (weight) --
    SP::SiconosVector weight(new SiconosVector(nDof));
    (*weight)(2) = -m * g;
    beam1->setFExtPtr(weight);

    // --------------------
    // --- Interactions ---
    // --------------------
    SP::NonSmoothLaw nslaw1(new EqualityConditionNSL(KneeJointR::numberOfConstraints()));

    SP::SiconosVector P(new SiconosVector(3));
    P->zero();
    // Building the first knee joint for beam1
    // input  - the concerned DS : beam1
    //        - a point in the spatial frame (absolute frame) where the knee is defined P
    SP::NewtonEulerR relation1(new KneeJointR(beam1, P));

    SP::Interaction inter1(new Interaction(KneeJointR::numberOfConstraints(), nslaw1, relation1));

    // -------------
    // --- Model ---
    // -------------
    SP::Model myModel(new Model(t0, T));
    // add the dynamical system in the non smooth dynamical system
    myModel->nonSmoothDynamicalSystem()->insertDynamicalSystem(beam1);
    // link the interaction and the dynamical system
    myModel->nonSmoothDynamicalSystem()->link(inter1, beam1);
    // ------------------
    // --- Simulation ---
    // ------------------

    // -- (1) OneStepIntegrators --
    SP::MoreauJeanCombinedProjectionOSI OSI(new MoreauJeanCombinedProjectionOSI(theta));

    // -- (2) Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));

    // -- (3) one step non smooth problem
    SP::OneStepNSProblem osnspb(new MLCP());
    SP::OneStepNSProblem osnspb_pos(new MLCPProjectOnConstraints(SICONOS_MLCP_ENUM));

    // -- (4) Simulation setup with (1) (2) (3)

    SP::TimeSteppingCombinedProjection s(new TimeSteppingCombinedProjection(t, OSI, osnspb, osnspb_pos));
    s->setProjectionMaxIteration(1000);
    s->setConstraintTolUnilateral(1e-08);

    // =========================== End of model definition ===========================

    // ================================= Computation =================================

    // --- Simulation initialization ---

    cout << "====> Initialisation ..." << endl << endl;
    myModel->initialize(s);

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 15 + 7;
    SimpleMatrix dataPlot(N, outputSize);
    SimpleMatrix beam1Plot(2,3*N);

    SP::SiconosVector q1 = beam1->q();
    SP::SiconosVector y= inter1->y(0);
    SP::SiconosVector ydot= inter1->y(1);

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;
    // ==== Simulation loop - Writing without explicit event handling =====
    int k = 0;
    boost::progress_display show_progress(N);

    boost::timer time;
    time.restart();
    SP::SiconosVector yAux(new SiconosVector(3));
    yAux->setValue(0, 1);
    SP::SimpleMatrix Jaux(new SimpleMatrix(3, 3));
    Index dimIndex(2);
    Index startIndex(4);
    fprintf(pFile, "double T[%d*%d]={", N + 1, outputSize);
    double beamTipTrajectories[6];

    for (k = 0; k < N-1; k++)
    {
      // solve ...
      //s->newtonSolve(1e-4, 50);

      s->advanceToEvent();
      // --- Get values to be plotted ---
      dataPlot(k, 0) =  s->nextTime();

      dataPlot(k, 1) = (*q1)(0);
      dataPlot(k, 2) = (*q1)(1);
      dataPlot(k, 3) = (*q1)(2);
      dataPlot(k, 4) = (*q1)(3);
      dataPlot(k, 5) = (*q1)(4);
      dataPlot(k, 6) = (*q1)(5);
      dataPlot(k, 7) = (*q1)(6);
      dataPlot(k, 8) = y->norm2();
      dataPlot(k, 9) = ydot->norm2();

      tipTrajectories(q1,beamTipTrajectories,L1);
      beam1Plot(0,3*k) = beamTipTrajectories[0];
      beam1Plot(0,3*k+1) = beamTipTrajectories[1];
      beam1Plot(0,3*k+2) = beamTipTrajectories[2];
      beam1Plot(1,3*k) = beamTipTrajectories[3];
      beam1Plot(1,3*k+1) = beamTipTrajectories[4];
      beam1Plot(1,3*k+2) = beamTipTrajectories[5];

      for (unsigned int jj = 0; jj < outputSize; jj++)
      {
        if ((k || jj))
          fprintf(pFile, ",");
        fprintf(pFile, "%f", dataPlot(k, jj));
      }
      fprintf(pFile, "\n");
       s->nextStep();
      //s->processEvents();
      ++show_progress;
    }
    fprintf(pFile, "};");
    cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
    cout << "Computation Time " << time.elapsed()  << endl;

    // --- Output files ---
    cout << "====> Output file writing ..." << endl;
    dataPlot.resize(k, outputSize);
    ioMatrix::write("NE_1DS_1Knee_MLCP_MoreauJeanCombinedProjection.dat", "ascii", dataPlot, "noDim");
    ioMatrix::write("NE_1DS_1Knee_MLCP_beam1.dat", "ascii", beam1Plot, "noDim");

    SimpleMatrix dataPlotRef(dataPlot);
    dataPlotRef.zero();
    ioMatrix::read("NE_1DS_1Knee_MLCP_MoreauJeanCombinedProjection.ref", "ascii", dataPlotRef);
    std::cout << "Error w.r.t. reference file : " << (dataPlot - dataPlotRef).normInf() << std::endl;

    if ((dataPlot - dataPlotRef).normInf() > 1e-7)
    {
      //(dataPlot - dataPlotRef).display();
      std::cout << "Warning. The results is rather different from the reference file." << std::endl;
      return 1;
    }

    fclose(pFile);
  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in NE_...cpp" << endl;
  }

}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_FUNCTIONAL_GEOMETRY_HPP
#define BOOST_COMPUTE_FUNCTIONAL_GEOMETRY_HPP

#include <boost/compute/type_traits.hpp>
#include <boost/compute/functional/detail/macros.hpp>

namespace boost {
namespace compute {

BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(cross, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(dot, typename scalar_type<T>::type (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(distance, typename scalar_type<T>::type (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fast_distance, typename scalar_type<T>::type (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(length, typename scalar_type<T>::type (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fast_length, typename scalar_type<T>::type (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(normalize, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fast_normalize, T (T), class T)

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_FUNCTIONAL_GEOMETRY_HPP


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/sec.hpp>
#include <boost/simd/function/fast.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/sqrt_2.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pio_4.hpp>

STF_CASE_TPL (" sec",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sec;

  using r_t = decltype(sec(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(sec(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(sec(-bs::Pi<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(-bs::Pio_4<T>()), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Pi<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Pio_4<T>()), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Zero<T>()), bs::One<r_t>(), 0.5);
}
STF_CASE_TPL (" sec fast_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sec;

  using r_t = decltype(sec(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::fast_(sec)(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(sec)(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(sec)(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::fast_(sec)(-bs::Pi<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(sec)(-bs::Pio_4<T>()), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(sec)(bs::Pi<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(sec)(bs::Pio_4<T>()), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(sec)(bs::Zero<T>()), bs::One<r_t>(), 0.5);
}

STF_CASE_TPL (" sec clipped_small_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bst = bs::tag;
  namespace bd = boost::dispatch;
  using bs::sec;

  using r_t = decltype(sec(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(sec(bs::Inf<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Minf<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Nan<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(sec(-bs::Pi<T>(), bst::clipped_small_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(-bs::Pio_4<T>(), bst::clipped_small_), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Pi<T>(), bst::clipped_small_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Pio_4<T>(), bst::clipped_small_), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Zero<T>(), bst::clipped_small_), bs::One<r_t>(), 0.5);
}

STF_CASE_TPL (" sec clipped_medium_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bst = bs::tag;
  namespace bd = boost::dispatch;
  using bs::sec;

  using r_t = decltype(sec(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(sec(bs::Inf<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Minf<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Nan<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(sec(-bs::Pi<T>(), bst::clipped_medium_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(-bs::Pio_4<T>(), bst::clipped_medium_), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Pi<T>(), bst::clipped_medium_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Pio_4<T>(), bst::clipped_medium_), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(sec(bs::Zero<T>(), bst::clipped_medium_), bs::One<r_t>(), 0.5);
}


#include <geometry_core/geometry.h>
#include <geometry_core/geometry_builder.h>
#include <geometry_core/scope.h>

#include <gtest/gtest.h>

#include <boost/qvm/map_vec_mat.hpp>
#include <boost/qvm/vec.hpp>
#include <boost/qvm/vec_operations.hpp>

using namespace pagoda;
using GeometryType = GeometryBase<>;

TEST(Scope, test_scope_construction)
{
	Scope s;

	EXPECT_TRUE(s.GetPosition() == (Vec3F{0, 0, 0}));
	EXPECT_TRUE(s.GetSize() == (Vec3F{0, 0, 0}));
	EXPECT_TRUE(boost::qvm::col<0>(s.GetRotation()) == (Vec3F{1, 0, 0}));
	EXPECT_TRUE(boost::qvm::col<1>(s.GetRotation()) == (Vec3F{0, 1, 0}));
	EXPECT_TRUE(boost::qvm::col<2>(s.GetRotation()) == (Vec3F{0, 0, 1}));
}

TEST(Scope, when_constructing_from_box_points_should_create_a_correct_scope)
{
	// clang-format off
	std::array<Vec3F, 8> boxPoints = {
	    Vec3F{0, 0, 0}, Vec3F{0, 2, 0}, Vec3F{-1, 0, 0}, Vec3F{-1, 1, 0},
        Vec3F{0, 0, 3}, Vec3F{0, 2, 3}, Vec3F{-1, 0, 3}, Vec3F{-1, 1, 3},
	};
	// clang-format on

	Scope s(boxPoints);

	EXPECT_TRUE(s.GetPosition() == (Vec3F{0, 0, 0}));
	EXPECT_TRUE(s.GetSize() == (Vec3F{2, 1, 3}));
	EXPECT_TRUE(boost::qvm::col<0>(s.GetRotation()) == (Vec3F{0, 1, 0}));
	EXPECT_TRUE(boost::qvm::col<1>(s.GetRotation()) == (Vec3F{-1, 0, 0}));
	EXPECT_TRUE(boost::qvm::col<2>(s.GetRotation()) == (Vec3F{0, 0, 1}));
}

TEST(Scope, when_constructing_from_geometry_and_constrained_rotation_should_create_a_correct_scope)
{
	auto geometry = std::make_shared<GeometryType>();
	GeometryBuilderT<GeometryType> builder(geometry);

	// Bottom face
	builder.AddPoint(Vec3F{0, 0, 0});
	builder.AddPoint(Vec3F{1, 0, 0});
	builder.AddPoint(Vec3F{1, 1, 0});
	builder.AddPoint(Vec3F{0, 1, 0});

	// Top face
	builder.AddPoint(Vec3F{0, 0, 1});
	builder.AddPoint(Vec3F{1, 0, 1});
	builder.AddPoint(Vec3F{1, 1, 1});
	builder.AddPoint(Vec3F{0, 1, 1});

	auto bottomFace = builder.StartFace(4);
	auto topFace = builder.StartFace(4);
	typename GeometryBuilderT<GeometryType>::FaceBuilder sideFaces[] = {
	    builder.StartFace(4),
	    builder.StartFace(4),
	    builder.StartFace(4),
	    builder.StartFace(4),
	};

	for (auto i : {3, 2, 1, 0})
	{
		bottomFace.AddIndex(i);
	}
	for (auto i : {4, 5, 6, 7})
	{
		topFace.AddIndex(i);
	}
	bottomFace.CloseFace();
	topFace.CloseFace();

	uint32_t i = 0;
	std::vector<std::vector<uint32_t>> sideFaceIndices;
	sideFaceIndices.push_back({0, 1, 5, 4});
	sideFaceIndices.push_back({1, 2, 6, 5});
	sideFaceIndices.push_back({2, 3, 7, 6});
	sideFaceIndices.push_back({3, 0, 4, 7});
	for (auto &f : sideFaceIndices)
	{
		for (auto index : f)
		{
			sideFaces[i].AddIndex(index);
		}
		sideFaces[i++].CloseFace();
	}

	auto s = Scope::FromGeometryAndConstrainedRotation(geometry, boost::qvm::diag_mat(XYZ(Vec3F{1, 1, 1})));
	EXPECT_TRUE(s.GetPosition() == (Vec3F{0, 0, 0}));
	EXPECT_TRUE(s.GetSize() == (Vec3F{1, 1, 1}));
	EXPECT_TRUE(boost::qvm::col<0>(s.GetRotation()) == (Vec3F{1, 0, 0}));
	EXPECT_TRUE(boost::qvm::col<1>(s.GetRotation()) == (Vec3F{0, 1, 0}));
	EXPECT_TRUE(boost::qvm::col<2>(s.GetRotation()) == (Vec3F{0, 0, 1}));
}


/**

Copyright (c) 2016, Borella Jocelyn, Karrenbauer Oliver, Meißner Pascal
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#pragma once

#include <ros/ros.h>
#include <math.h>
#include <boost/shared_ptr.hpp>
#include <eigen3/Eigen/Geometry>

#include "asr_robot_model_services/GetDistance.h"
#include "asr_robot_model_services/GetPose.h"
#include "asr_robot_model_services/RobotStateMessage.h"

#define RAD_TO_DEG 180.0 / M_PI
#define DEG_TO_RAD M_PI / 180.0

namespace directSearchWS {

class PoseHelper {

private:
    static boost::shared_ptr<PoseHelper> instancePtr;
    static double viewPointDistance;
    static int distanceFuncParam;
    static double viewCenterPositionDistanceThreshold;
    static double viewCenterOrientationRadDistanceThreshold;

    ros::ServiceClient getDistanceServiceClient;
    ros::ServiceClient getRobotPoseServiceClient;
    ros::ServiceClient getCameraPoseServiceClient;

    template <typename T>
    static T waitForParam(ros::NodeHandle n, std::string param);

    //calc distance functions
    double calcDistPositionWithNBV(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);
    double calcDistPositionEucl(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);

    Eigen::Vector3d calculateViewCenterPoint(const geometry_msgs::Pose &pose);

    Eigen::Vector3d convertPositionToVec(const geometry_msgs::Pose &pose) {
        return Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z);
    }

    geometry_msgs::Pose convertVecToPosition(const Eigen::Vector3d &vec)  {
        geometry_msgs::Pose pose;
        pose.position.x = vec[0];
        pose.position.y = vec[1];
        pose.position.z = vec[2];
        return pose;
    }

    Eigen::Quaterniond convertPoseQuatToQuat(const geometry_msgs::Pose &pose) {
        return Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
    }

    geometry_msgs::Pose getOriginPose() {
        geometry_msgs::Pose origin;
        origin.position.x = 0;
        origin.position.y = 0;
        origin.position.z = 0;
        origin.orientation.x = 0;
        origin.orientation.y = 0;
        origin.orientation.z = 0;
        origin.orientation.w = 1;
        return origin;
    }

    PoseHelper();

public:

    static boost::shared_ptr<PoseHelper> getInstance();
    static void resetInstance();

    geometry_msgs::Pose getCurrentRobotPose();
    geometry_msgs::Pose getCurrentCameraPose();

    double calculateDistance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);
    double calcAngularDistanceInRad(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);

    bool checkViewCenterPointsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);

    bool checkPosesAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double positionThreshold, const double orientationRadThreshold);
    bool checkPositionsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double positionThreshold);
    bool checkOrientationsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double orientationRadThreshold);

};

typedef boost::shared_ptr<PoseHelper> PoseHelperPtr;

}


#include "mesh_selector.hpp"

#include <Eigen/SparseCore>
#include <igl/Timer.h>

namespace ipc::rigid {

Eigen::SparseMatrix<size_t>
map_vertex_pairs_to_edge(size_t num_vertices, const Eigen::MatrixXi& E)
{
    // TODO: this could be faster using a non std unordered map
    std::vector<Eigen::Triplet<size_t>> vertices_to_edge_triplets;
    vertices_to_edge_triplets.reserve(E.rows());

    for (size_t i = 0; i < E.rows(); i++) {
        vertices_to_edge_triplets.emplace_back(
            std::min(E(i, 0), E(i, 1)), std::max(E(i, 0), E(i, 1)), i);
    }

    Eigen::SparseMatrix<size_t> vertices_to_edge(num_vertices, num_vertices);
    vertices_to_edge.setFromTriplets(
        vertices_to_edge_triplets.begin(), vertices_to_edge_triplets.end());
    vertices_to_edge.makeCompressed();

    return vertices_to_edge;
}

MeshSelector::MeshSelector(
    size_t num_vertices, const Eigen::MatrixXi& E, const Eigen::MatrixXi& F)
{
    init_vertex_to_edge(num_vertices, E);
    init_vertex_to_face(num_vertices, F);

    if (F.rows()) {
        auto vertices_to_edge = map_vertex_pairs_to_edge(num_vertices, E);
        init_face_to_edge(F, vertices_to_edge);
        init_edge_to_face(E.rows());
    }

    init_codim_vertices_to_vertices(num_vertices, E);
    init_codim_edges_to_edges(E.rows());
}

// Map from vertices to the minimum index edge containing the vertex.
void MeshSelector::init_vertex_to_edge(
    size_t num_vertices, const Eigen::MatrixXi& E)
{
    m_vertex_to_edge.resize(num_vertices, std::numeric_limits<size_t>::max());
    for (size_t i = 0; i < E.rows(); i++) {
        for (size_t j = 0; j < E.cols(); j++) {
            m_vertex_to_edge[E(i, j)] = std::min(m_vertex_to_edge[E(i, j)], i);
        }
    }
}

// Map from vertices to the minimum index face containing the vertex.
void MeshSelector::init_vertex_to_face(
    size_t num_vertices, const Eigen::MatrixXi& F)
{
    m_vertex_to_face.resize(num_vertices, std::numeric_limits<size_t>::max());
    for (size_t i = 0; i < F.rows(); i++) {
        for (size_t j = 0; j < F.cols(); j++) {
            m_vertex_to_face[F(i, j)] = std::min(m_vertex_to_face[F(i, j)], i);
        }
    }
}

// Each row is a face with the three indices of the edges.
void MeshSelector::init_face_to_edge(
    const Eigen::MatrixXi& F,
    const Eigen::SparseMatrix<size_t>& vertices_to_edge)
{
    m_faces_to_edges.resize(F.rows(), 3);
    for (size_t i = 0; i < F.rows(); i++) {
        for (size_t j = 0; j < F.cols(); j++) {
            m_faces_to_edges(i, j) = vertices_to_edge.coeff(
                std::min(F(i, j), F(i, (j + 1) % 3)),
                std::max(F(i, j), F(i, (j + 1) % 3)));
        }
    }
}

void MeshSelector::init_edge_to_face(const size_t num_edges)
{
    assert(m_faces_to_edges.size());

    if (!num_edges) {
        return;
    }

    m_edge_to_face.resize(num_edges, std::numeric_limits<size_t>::max());
    for (size_t i = 0; i < m_faces_to_edges.rows(); i++) {
        for (size_t j = 0; j < m_faces_to_edges.cols(); j++) {
            m_edge_to_face[m_faces_to_edges(i, j)] =
                std::min(m_edge_to_face[m_faces_to_edges(i, j)], i);
        }
    }
}

void MeshSelector::init_codim_vertices_to_vertices(
    size_t num_vertices, const Eigen::MatrixXi& E)
{
    std::vector<bool> is_vertex_codim(num_vertices, true);
    for (size_t i = 0; i < E.rows(); i++) {
        for (size_t j = 0; j < E.cols(); j++) {
            is_vertex_codim[E(i, j)] = false;
        }
    }
    size_t num_codim_vertices =
        std::count(is_vertex_codim.begin(), is_vertex_codim.end(), true);
    m_codim_vertices_to_vertices.reserve(num_codim_vertices);
    for (size_t i = 0; i < is_vertex_codim.size(); i++) {
        if (is_vertex_codim[i]) {
            m_codim_vertices_to_vertices.push_back(i);
        }
    }
}

void MeshSelector::init_codim_edges_to_edges(size_t num_edges)
{
    std::vector<bool> is_edge_codim(num_edges, true);
    for (size_t i = 0; i < m_faces_to_edges.rows(); i++) {
        for (size_t j = 0; j < m_faces_to_edges.cols(); j++) {
            is_edge_codim[m_faces_to_edges(i, j)] = false;
        }
    }
    size_t num_codim_edges =
        std::count(is_edge_codim.begin(), is_edge_codim.end(), true);
    m_codim_edges_to_edges.reserve(num_codim_edges);
    for (size_t i = 0; i < is_edge_codim.size(); i++) {
        if (is_edge_codim[i]) {
            m_codim_edges_to_edges.push_back(i);
        }
    }
}

} // namespace ipc::rigid


#pragma once
#ifndef TEST_NTL_HPP
#define TEST_NTL_HPP
#include <NTL/GF2X.h>
#include <string>
#include <stdio.h>
#include <inttypes.h>
#include <ctype.h>

static inline void to_hexstring(std::string& str, const NTL::GF2X& poly)
{
    long d = deg(poly);
    long m = d / 64;
    char buff[200];
    str = "";
    for (long c = m; c >= 0; c--) {
        uint64_t mask = 1;
        uint64_t p = 0;
        for(int i = 0; i < 64; i++) {
            if (IsOne(coeff(poly, 64 * c + i))) {
                p |= mask;
            }
            mask <<= 1;
        }
        sprintf(buff, "%016" PRIx64, p);
        str = str + buff;
    }
}

static inline void to_binstring(std::string& str, const NTL::GF2X& poly)
{
    long d = deg(poly);
    long m = d / 64;
    str = "";
    for (int c = 0; c <= m; c++) {
        for(int i = 0; i < 64; i++) {
            if (IsOne(coeff(poly, 64 * c + i))) {
                str += '1';
            } else {
                str += '0';
            }
        }
    }
    reverse(str.begin(), str.end());
}

static inline void hexto_poly(NTL::GF2X& poly, const std::string& str)
{
    NTL::GF2X x1;
    SetCoeff(x1, 1, 1);
    unsigned long len = str.length();
    poly = 0;
    for (unsigned int i = 0; i < len; i++) {
        char c = static_cast<char>(tolower(static_cast<char>(str[i])));
        int d;
        if (isdigit(c)) {
            d = c - '0';
        } else if (isxdigit(c)) {
            d = c - 'a' + 10;
        }
        int mask = 8;
        for (int j = 0; j < 4; j++) {
            poly *= x1;
            if (d & mask) {
                SetCoeff(poly, 0, 1);
            } else {
                SetCoeff(poly, 0, 0);
            }
            mask = mask >> 1;
        }
    }
}

static inline void ntl_print(const char *s, const NTL::GF2X& x)
{
    std::string str;
    to_binstring(str, x);
    long d = deg(x);
    printf("%s %ld,%s\n", s, d, str.c_str());
    fflush(stdout);
}

#if defined(DEBUG)
#define PRT(s, x) ntl_print(s, x)
#define PUTS(s) puts(s)
#else
#define PRT(s, x)
#define PUTS(s)
#endif

#endif // TEST_NTL_HPP


#pragma once

#include <cstddef>
#include <limits>
#include <Eigen/Dense>

namespace cilantro {
    // CRTP base class
    template <class ICPInstanceT, class TransformT, class CorrespondenceSearchEngineT, class ResidualVectorT>
    class IterativeClosestPointBase {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef TransformT Transformation;
        typedef typename TransformT::Scalar PointScalar;
        typedef CorrespondenceSearchEngineT CorrespondenceSearchEngine;
        typedef typename CorrespondenceSearchEngineT::SearchResult CorrespondenceSearchResults;
        typedef ResidualVectorT ResidualVector;

        IterativeClosestPointBase(CorrespondenceSearchEngine &corr_engine,
                                  size_t max_iter = 15,
                                  PointScalar conv_tol = (PointScalar)1e-5)
                : max_iterations_(max_iter),
                  iterations_(0),
                  convergence_tol_(conv_tol),
                  last_delta_norm_(std::numeric_limits<PointScalar>::infinity()),
                  correspondence_search_engine_(corr_engine)
        {}

        inline CorrespondenceSearchEngine& correspondenceSearchEngine() { return correspondence_search_engine_; }

        inline size_t getMaxNumberOfIterations() const { return max_iterations_; }

        inline ICPInstanceT& setMaxNumberOfIterations(size_t max_iter) {
            max_iterations_ = max_iter;
            return *static_cast<ICPInstanceT*>(this);
        }

        inline size_t getNumberOfPerformedIterations() const { return iterations_; }

        inline PointScalar getConvergenceTolerance() const { return convergence_tol_; }

        inline ICPInstanceT& setConvergenceTolerance(PointScalar conv_tol) {
            convergence_tol_ = conv_tol;
            return *static_cast<ICPInstanceT*>(this);
        }

        inline const Transformation& getInitialTransformation() const { return transform_init_; }

        inline ICPInstanceT& setInitialTransformation(const Transformation& tform_init) {
            transform_init_ = tform_init;
            return *static_cast<ICPInstanceT*>(this);
        }

        inline Transformation& initialTransformation() { return transform_init_; }

        inline PointScalar getLastUpdateNorm() const { return last_delta_norm_; }

        // Main ICP loop
        ICPInstanceT& estimateTransformation() {
            ICPInstanceT& icp_instance = *static_cast<ICPInstanceT*>(this);

            transform_ = transform_init_;
            iterations_ = 0;
            last_delta_norm_ = std::numeric_limits<PointScalar>::infinity();
            icp_instance.initializeComputation();

            while (iterations_ < max_iterations_) {
                // Update correspondences_
                icp_instance.updateCorrespondences();
                // Update transform_ and last_delta_norm_ based on correspondences_
                icp_instance.updateEstimate();

                iterations_++;
                if (last_delta_norm_ < convergence_tol_) break;
            }

            return icp_instance;
        }

        inline ICPInstanceT& estimateTransformation(size_t max_iter, PointScalar conv_tol) {
            max_iterations_ = max_iter;
            convergence_tol_ = conv_tol;
            return estimateTransformation();
        }

        inline const CorrespondenceSearchResults& getCorrespondenceSearchResults() const { return correspondences_; }

        inline const Transformation& getTransformation() const { return transform_; }

        inline const ICPInstanceT& getTransformation(Transformation& tform) const {
            tform = transform_;
            return *static_cast<const ICPInstanceT*>(this);
        }

        inline ResidualVector getResiduals() { return static_cast<ICPInstanceT*>(this)->computeResiduals(); }

        inline bool hasConverged() const { return last_delta_norm_ < convergence_tol_; }

    protected:
        size_t max_iterations_;
        size_t iterations_;
        PointScalar convergence_tol_;
        PointScalar last_delta_norm_;

        CorrespondenceSearchEngine& correspondence_search_engine_;
        CorrespondenceSearchResults correspondences_;

        Transformation transform_init_;
        Transformation transform_;

        // Default implementation
        inline void initializeComputation() {}
    };
}


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_INTERLEAVE_EVEN_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_INTERLEAVE_EVEN_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/swar/functions/interleave_even.hpp>
#include <boost/simd/swar/functions/details/shuffle.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::interleave_even_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<single_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(__m128 const a0, __m128 const a1) const
    {
      return _mm_unpacklo_ps( details::shuffle<0, 2, 0, 2>(a0,a0)
                            , details::shuffle<0, 2, 0, 2>(a1,a1)
                            );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::interleave_even_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<double_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(__m128d const a0, __m128d const a1) const
    {
      return _mm_unpacklo_pd(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::interleave_even_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<type32_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<type32_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(__m128i const a0, __m128i const a1) const
    {
      return _mm_unpacklo_epi32 ( details::shuffle<0, 2, 0, 2>(a0,a0)
                                , details::shuffle<0, 2, 0, 2>(a1,a1)
                                );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::interleave_even_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<type64_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<type64_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(A0 const& a0, A1 const& a1) const
    {
      return _mm_unpacklo_epi64(a0, a1);
    }
  };
} } }

#endif
#endif


// Boost.Geometry Index
// Unit Test

// Copyright (c) 2015 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// Enable enlargement of Values' bounds by epsilon in the rtree
// for Points and Segments
#define BOOST_GEOMETRY_INDEX_EXPERIMENTAL_ENLARGE_BY_EPSILON

#include <vector>

#include <rtree/test_rtree.hpp>

#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/polygon.hpp>

template <typename Params>
void test_rtree(unsigned vcount)
{
    typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;

    std::vector<point_t> values;

    double eps = std::numeric_limits<double>::epsilon();
    values.push_back(point_t(eps/2, eps/2));

    for ( unsigned i = 1 ; i < vcount ; ++i )
    {
        values.push_back(point_t(i, i));
    }

    point_t qpt(0, 0);

    BOOST_CHECK(bg::intersects(qpt, values[0]));

    {
        bgi::rtree<point_t, Params> rt(values);

        std::vector<point_t> result;
        rt.query(bgi::intersects(qpt), std::back_inserter(result));
        BOOST_CHECK(result.size() == 1);

        rt.remove(values.begin() + vcount/2, values.end());

        result.clear();
        rt.query(bgi::intersects(qpt), std::back_inserter(result));
        BOOST_CHECK(result.size() == 1);
    }

    {
        bgi::rtree<point_t, Params> rt;
        rt.insert(values);

        std::vector<point_t> result;
        rt.query(bgi::intersects(qpt), std::back_inserter(result));
        BOOST_CHECK(result.size() == 1);

        rt.remove(values.begin() + vcount/2, values.end());

        result.clear();
        rt.query(bgi::intersects(qpt), std::back_inserter(result));
        BOOST_CHECK(result.size() == 1);
    }
}

template <int Max, int Min>
void test_rtree_all()
{
    int pow = Max;
    for (int l = 0 ; l < 3 ; ++l)
    {
        pow *= Max;
        int vcount = (pow * 8) / 10;

        //std::cout << Max << " " << Min << " " << vcount << std::endl;

        test_rtree< bgi::linear<Max, Min> >(vcount);
        test_rtree< bgi::quadratic<Max, Min> >(vcount);
        test_rtree< bgi::rstar<Max, Min> >(vcount);
    }
}

int test_main(int, char* [])
{
    test_rtree_all<2, 1>();
    test_rtree_all<4, 1>();
    test_rtree_all<4, 2>();
    test_rtree_all<5, 3>();

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/combinatorial/include/functions/anp.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( anp_real,  NT2_SIMD_REAL_TYPES)
{
  using nt2::anp;
  using nt2::tag::anp_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(anp(nt2::Inf<vT>(), nt2::Inf<vT>()), nt2::Nan<vT>());
  NT2_TEST_EQUAL(anp(nt2::Nan<vT>(), nt2::Nan<vT>()), nt2::Nan<vT>());
#endif
  NT2_TEST_EQUAL(anp(nt2::One<vT>(), nt2::One<vT>()), nt2::One<vT>());
  NT2_TEST_EQUAL(anp(nt2::Zero<vT>(), nt2::Zero<vT>()), nt2::One<vT>());
}


////////////////////////////////////////////////////////////////////////////////
// SurfaceSampler.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Sample from piecewise linear fields on a triangulated surface by
//  evaluating the field at the closest point to the query.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  05/01/2019 23:55:30
////////////////////////////////////////////////////////////////////////////////
#ifndef SURFACE_SAMPLER_HH
#define SURFACE_SAMPLER_HH
#include <memory>
#include <stdexcept>
#include <Eigen/Dense>

// Forward declare AABB data structure
struct SurfaceAABB;

struct SurfaceSampler {
    SurfaceSampler(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F);

    // P: (#points x dim) matrix of stacked query point row vectors
    // fieldValues (X x fieldDim) matrix of stacked per-vertex or per-triangle field values
    template<class PLField>
    PLField sample(const Eigen::MatrixXd &P, PLField &fieldValues) const {
        bool isPerTri = fieldValues.rows() == m_F.rows();
        if (!isPerTri && (fieldValues.rows() != m_V.rows())) throw std::runtime_error("Invalid piecewise linear/constant field size");

        Eigen::VectorXi  I;
        Eigen::MatrixX3d B;
        closestTriAndBaryCoords(P, I, B);

        const size_t np = P.rows();
        PLField outSamples(np, fieldValues.cols());
        for (size_t i = 0; i < np; ++i) {
            int tri = I[i];
            if (isPerTri)
                outSamples.row(i) = fieldValues.row(tri);
            else {
                outSamples.row(i) = B(i, 0) * fieldValues.row(m_F(tri, 0))
                                  + B(i, 1) * fieldValues.row(m_F(tri, 1))
                                  + B(i, 2) * fieldValues.row(m_F(tri, 2));
            }
        }

        return outSamples;
    }

    // P: (#points x dim) matrix of stacked query point row vectors
    // I: index of closest triangle for each query point
    // B: (#points x 3) matrix of stacked barycentric coordinate vectors for each query point
    void closestTriAndBaryCoords(const Eigen::MatrixXd &P, Eigen::VectorXi &I, Eigen::MatrixX3d &B) const;

    ~SurfaceSampler(); // Needed because m_surfaceAABB is a smart pointer to an incomplete type.
private:
    std::unique_ptr<SurfaceAABB> m_surfaceAABB;
    Eigen::MatrixXd m_V;
    Eigen::MatrixXi m_F;
};

#endif /* end of include guard: SURFACE_SAMPLER_HH */


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/boost/mpl/list.hpp>

#include <boost/hana/comparable/laws.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/list/instance.hpp>

#include <boost/mpl/list.hpp>
using namespace boost::hana;

template <int i> struct x;

template <int ...i>
constexpr boost::mpl::list<x<i>...> mpl_list{};

int main() {
    BOOST_HANA_CONSTANT_ASSERT(
        Comparable::laws::check(
            list(
                mpl_list<>, mpl_list<0>, mpl_list<0, 1>,
                mpl_list<0, 1, 2>, mpl_list<1, 0, 2>
            )
        )
    );
}


#ifndef POLIMIDL_LAYERS_INTERNAL_AVG_POOLING_HPP
#define POLIMIDL_LAYERS_INTERNAL_AVG_POOLING_HPP

#include <chrono>

#include <Eigen/Dense>

#include "../alignment.hpp"
#include "../layer.hpp"

namespace polimidl {
namespace layers {
namespace internal {
template <typename type_t, typename components, typename kernel,
          typename stride>
class avg_pooling : public layer<type_t, components> {
 public:
  avg_pooling() : batch_size_(1) {}

  static std::size_t output_rows(std::size_t input_rows) {
    return ((input_rows - kernel::rows) / stride::rows) + 1;
  }
  static std::size_t output_columns(std::size_t input_columns) {
    return ((input_columns - kernel::columns) / stride::columns) + 1;
  }
  static std::size_t temporary_size(std::size_t input_rows,
                                    std::size_t input_columns,
                                    unsigned int number_of_workers) {
     return cell_size * number_of_workers;
  }

  template <typename input_t, typename temporary_t, typename output_t,
            typename scheduler_t>
  void operator()(input_t input, temporary_t temporary, output_t output,
                  std::size_t input_rows, std::size_t input_columns,
                  const scheduler_t& scheduler) const {
    if (input_rows == kernel::rows && input_columns == kernel::columns) {
      matrix_input_t val(&input[0]);
      matrix_output_t out(&output[0]);
      out = val.rowwise().mean();
      return;
    }
    const std::size_t output_rows = avg_pooling::output_rows(input_rows);
    const std::size_t output_columns =
        avg_pooling::output_columns(input_columns);
    const std::size_t cells = output_rows * output_columns;
    const unsigned int number_of_workers = scheduler.number_of_workers();

    for (std::size_t start_cell = 0; start_cell < cells;
        start_cell += batch_size_) {
      const std::size_t end_cell = std::min(cells, start_cell + batch_size_);
      scheduler.schedule([=] (std::size_t worker) {
        auto worker_temporary = temporary.slice(worker, number_of_workers);
        for (std::size_t cell = start_cell; cell < end_cell; ++cell) {
          const std::size_t row = cell / output_columns;
          const std::size_t column = cell % output_columns;
          for (std::size_t kernel_row = 0; kernel_row < kernel::rows;
               ++kernel_row) {
            auto i_input = &input[components::value * (
                input_columns * (stride::rows * row  + kernel_row) +
                stride::columns * column)];
            auto i_temporary = &worker_temporary[
                components::value * kernel::columns * kernel_row];
            std::copy_n(i_input, components::value * kernel::columns,
                        i_temporary);
          }
          matrix_input_t val(&worker_temporary[0]);
          auto i_output = &output[components::value * cell];
          matrix_output_t out(i_output);
          out = val.rowwise().mean();
        }
      });
    }
    scheduler.wait();
  }

  template <typename input_t, typename temporary_t, typename output_t,
            typename scheduler_t>
  void optimize_for(input_t input, temporary_t temporary, output_t output,
                  std::size_t input_rows, std::size_t input_columns,
                  const scheduler_t& scheduler) {
    const std::size_t output_rows = avg_pooling::output_rows(input_rows);
    const std::size_t output_columns =
        avg_pooling::output_columns(input_columns);
    const std::size_t cells = output_rows * output_columns;

    if (scheduler.number_of_workers() == 1) {
      batch_size_ = cells;
      return;
    }

    auto best_duration = std::chrono::high_resolution_clock::duration::max();

    std::size_t best_batch_size = 1;
    auto test_and_maybe_set = [&]() {
      for (std::size_t iteration = 0; iteration < 3; ++iteration) {
        const auto start = std::chrono::high_resolution_clock::now();
        operator()(input, temporary, output, input_rows, input_columns,
                   scheduler);
        const auto current_duration =
            std::chrono::high_resolution_clock::now() - start;
          if (current_duration < best_duration) {
            best_duration = current_duration;
            best_batch_size = batch_size_;
          }
        }
    };
    batch_size_ = 1;
    while (batch_size_ < cells) {
        test_and_maybe_set();
        batch_size_ = batch_size_ * 2;
    }
    batch_size_ = cells;
    test_and_maybe_set();
    batch_size_ = cells / scheduler.number_of_workers() +
                  cells % scheduler.number_of_workers() ? 1 : 0;
    while (batch_size_ > 1) {
        test_and_maybe_set();
        batch_size_ = batch_size_ / 2;
    }
    batch_size_ = best_batch_size;
  }

 private:
  static constexpr std::size_t cell_size =
      components::value * kernel::rows * kernel::columns;
  using alignment_t = alignment<type_t>;
  using matrix_input_t = Eigen::Map<
      Eigen::Matrix<type_t, components::value, kernel::columns * kernel::rows>,
      alignment_t::eigen_alignment>;
  using matrix_output_t = Eigen::Map<
      Eigen::Matrix<type_t, components::value, 1>>;
  std::size_t batch_size_;
};
}
}
}

#endif // POLIMIDL_LAYERS_INTERNAL_AVG_POOLING_HPP


/**
 * @file testLinearizedFactor
 * @author Alex Cunningham
 */

#include <boost/assign/std/vector.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/Values.h>

#include <gtsam_unstable/nonlinear/LinearizedFactor.h>

using namespace std;
using namespace boost::assign;
using namespace gtsam;

static const double tol = 1e-5;

/* ************************************************************************* */
TEST( testLinearizedFactor, creation ) {
	// Create a set of local keys (No robot label)
	Key	l1 = 11, l2 = 12,
			l3 = 13, l4 = 14,
			l5 = 15, l6 = 16,
			l7 = 17, l8 = 18;

	// creating an ordering to decode the linearized factor
	Ordering ordering;
	ordering += l1,l2,l3,l4,l5,l6,l7,l8;

	// create a decoder for only the relevant variables
	LinearizedFactor::KeyLookup decoder;
	decoder[2] = l3;
	decoder[4] = l5;

	// create a linear factor
	SharedDiagonal model = noiseModel::Unit::Create(2);
	JacobianFactor::shared_ptr linear_factor(new JacobianFactor(
			ordering[l3], eye(2,2), ordering[l5], 2.0 * eye(2,2), zero(2), model));

	// create a set of values - build with full set of values
	gtsam::Values full_values, exp_values;
	full_values.insert(l3, Point2(1.0, 2.0));
	full_values.insert(l5, Point2(4.0, 3.0));
	exp_values = full_values;
	full_values.insert(l1, Point2(3.0, 7.0));

	// create the test LinearizedFactor
	// This is called in the constructor of DDFSlot for each linear factor
	LinearizedFactor actual1(linear_factor, decoder, full_values);
	LinearizedFactor actual2(linear_factor, ordering, full_values);

	EXPECT(assert_equal(actual1, actual2, tol));

	// Verify the keys
	vector<gtsam::Key> expKeys;
	expKeys += l3, l5;
	EXPECT(assert_container_equality(expKeys, actual1.keys()));
	EXPECT(assert_container_equality(expKeys, actual2.keys()));

	// Verify subset of linearization points
	EXPECT(assert_equal(exp_values, actual1.linearizationPoint(), tol));
	EXPECT(assert_equal(exp_values, actual2.linearizationPoint(), tol));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


#include <iostream>
#include <iomanip>
#include <sstream>
#include <string>
#include <vector>
#include <ccmc/Kameleon.h>
#include <ccmc/Tracer.h>
#include <ccmc/Fieldline.h>
#include <ccmc/StringConstants.h>
#include <ccmc/Interpolator.h>
#include <ccmc/Point3f.h>
#include <ccmc/Vector.h>
#include <boost/lexical_cast.hpp>
//#include <boost/program_options.hpp>
//#include <google/profiler.h>
#include <ctime>
#define LENGTH 500;

using namespace std;
using namespace ccmc;

//namespace po = boost::program_options;

int main (int argc, char * argv[])
{
	std::string filename;
	std::string variable;
	float c0;
	float c1;
	float c2;
	int iterations = 10;

	if (argc != 6)
	{
		cout << "integrator <filename> <variable> c0 c1 c2" << endl;
		exit(1);
	}

	filename = argv[1];
	variable = argv[2];
	c0 = boost::lexical_cast<float>(argv[3]);
	c1 = boost::lexical_cast<float>(argv[4]);
	c2 = boost::lexical_cast<float>(argv[5]);

	cout << "This program will compute the integral for a fieldline of the specified variable" << std::endl;
	Kameleon kameleon; //creates a kameleon object capable of working with any of the ccmc-supported models
	kameleon.open(filename);
	kameleon.loadVectorVariable("b"); //see the namespace ccmc::strings::variables for possible inputs
	kameleon.loadVectorVariable("e");

	Tracer tracer(&kameleon); // Sets the interpolator (based on the kameleon object) to be used for tracing
	tracer.setMaxIterations(20000); //Maximum number of points in the fieldline
	tracer.setInnerBoundary(2.5f); //radius where fieldlines should stop (usually larger than the model's inner boundary)
	tracer.setDn(.2f); //step size relative to the cell size

	clock_t start, finish;

	cout << "Initializing field line\n";
	Fieldline fieldline;

	/*
	 * Supported tracers: input position is cartesian (x,y,z) unless the model is Enlil (r,theta,phi)
	 * 	bidirectionalTrace(string variable, float p0, float p1, float p2)
	 * 	bidirectionalTraceWithDipole(string variable, float p0, float p1, float p2)
	 * 	unidirectionalTrace(string variable, float p0, float p1, float p2, Direction) where Direction must be keyword FORWARD or REVERSE
	 * 	unidirectionalTraceWithDipole(string variable, float p0, float p1, float p2, Direction)
	 */
	cout << "Bidirectional trace beginning at ("<< c0 << ","<<c1<<","<<c2<<")"<<endl;
	start = clock();
	fieldline = tracer.bidirectionalTrace("b",c0,c1,c2);
	finish = clock();
	float elapsed_time = ((double) finish - (double) start) / CLOCKS_PER_SEC;
	cout << "Fieldline trace took "<< elapsed_time << " seconds\n"<<endl;
	cout << "After bidrectional trace\n";
	cout << "Points in fieldline:"<<fieldline.size()<<endl;

	//You can retrieve the kameleon object's interpolator from the tracer, thusly:
	Interpolator * interpolator = tracer.interpolator; //interpolator will be deleted via the Tracer's destructor
	//Or you can make a new one, which is useful for doing interpolations in parallel:
	//Interpolator * interpolator = kameleon.createNewInterpolator(); //but make sure you delete it when done

	Fieldline fieldlineWithVariable;
	Point3f p, eField;
	float datum, pEx, pEy, pEz;

	for (int i = 0; i < fieldline.size(); i++)
	{
		p = fieldline.getPosition(i);
		datum = interpolator->interpolate(variable,p.component1,p.component2,p.component3);
		fieldlineWithVariable.insertPointData(p,datum);
		eField.component1 = interpolator->interpolate("ex",p.component1,p.component2,p.component3);
		eField.component2 = interpolator->interpolate("ey",p.component1,p.component2,p.component3);
		eField.component3 = interpolator->interpolate("ez",p.component1,p.component2,p.component3);
		fieldlineWithVariable.insertVectorData(eField); // eField in [mV/m]
	}

	vector<float> segmentLengths = fieldlineWithVariable.getDs(); //computes line segments of fieldline [Re]
	vector<float> integral = fieldlineWithVariable.integrate(); //sum of individual segmentLengths * variable
	vector<float> fieldlinePotential = fieldlineWithVariable.integrateVector(); // integral(E dot dl)
	std::cout<<"integral result:"<< integral.back()<< "[" << variable << "* Re]"<<endl;
	std::cout<<"Fieldline potential:"<< fieldlinePotential.back()*(6.3781e3)<<"[V]\n"; //[V] = [Re*mV/m]*[1V/1000mV]*[6.3781e6 m/Re]

	kameleon.close();

	//delete interpolator; //required if you used kameleon.createNewInterpolator above

	std::cout << "finished" << std::endl;

	return 0;
}