#include <iostream>
#include <boost/bind.hpp>
#include <boost/algorithm/string.hpp>
#include "../include/grasp_sampler/RRTFunction.h"
#include <string>
#include <cstring>

using namespace std;
using namespace Eigen;

#define REACHED 0
#define ADVANCED 1
#define TRAPPED -1

bool RRT::solveRRT(Robotmodel& model, std::ostream& sout) {

	_model = model;

	nt_start._nodes.clear(); // NodeTree
	nt_goal._nodes.clear(); // NodeTree

	initConfig.clear();// std::vector<double>
	DOF_weights.clear();// std::vector<double>
	int ActiveDoFs = dofSize;
	step_size = 0.3 / M_PI *180.0;
	std::string start_string[dofSize];

	for (int i = 0; i < ActiveDoFs; i++) {
		initConfig.push_back(qinit(i) * 180.0 / M_PI);
		DOF_weights.push_back(1.0);
	}

	auto start_node = std::make_shared<RRTNode>(initConfig);

	nt_start.addNode(start_node);
	nt_pointer = &nt_start;

	// mulitple seed
	for (int i = 0; i < qgoals.size(); i++)
	{
		std::vector<double> goalConfig;
		for (int j = 0; j < ActiveDoFs; j++)
		{
			goalConfig.push_back(qgoals[i][j] * 180.0 / M_PI);
		}

		auto goal_node = std::make_shared<RRTNode>(goalConfig);

		nt_goal.addNode(goal_node);
	}

	bool finished = false;
	int iter = 1;

    std::vector<double> q_rand;
    RRTNodePtr q_a_near;
    std::vector<double> q_a_reached ;
    RRTNodePtr q_b_near;
    std::vector<double> q_b_reached ;
	int root_tree_index, goal_tree_index;

	while (iter < 50) {

		nt_pointer = &nt_start;								  //		// start tree
		q_rand = generateRandomConfig();
		q_a_near = nt_pointer->getNearest(q_rand, DOF_weights);

		q_a_reached = this->extendNode(q_rand, q_a_near);

		nt_pointer = &nt_goal;
		q_b_near = nt_pointer->getNearest(q_a_reached, DOF_weights); // random config�� ���� ����� c_tree�� ��� ã��
		q_b_reached = this->extendNode(q_a_reached, q_b_near); // near��� ���� random config�� Extend

		if (getEuclideanNorm(q_a_reached, q_b_reached, dofSize) < step_size)
		{
			nt_pointer = &nt_start; //		// start tree
			root_tree_index = nt_pointer->getNearestIndex(q_a_reached, DOF_weights);
			nt_pointer = &nt_goal;
			goal_tree_index = nt_pointer->getNearestIndex(q_b_reached, DOF_weights);

			// q_a_reached가 tree내에 있는 노드가 아니여서 불연속이 생김..
			//cout << "t_turn \t" << t_turn << endl;
			//cout << q_a_reached[0] << "\t" << q_a_reached[1] << "\t" << q_a_reached[2] << "\t" << q_a_reached[3] << "\t" << q_a_reached[4] << "\t" << q_a_reached[5] << "\t" << q_a_reached[6] << endl;
			//cout << q_b_reached[0] << "\t" << q_b_reached[1] << "\t" << q_b_reached[2] << "\t" << q_b_reached[3] << "\t" << q_b_reached[4] << "\t" << q_b_reached[5] << "\t" << q_b_reached[6] << endl;

			//cout << getEuclideanNorm(q_a_reached, q_b_reached, dofSize) << endl;
			//cout << "reached" << endl;
			finished = true;

			break;
		}
		iter++;
	}
	if (finished) {

		path = nt_start.getPathWithIndex(root_tree_index);
		vector<vector<double>> p2;
		p2 = nt_goal.getPathWithIndex(goal_tree_index);

		reverse(p2.begin(), p2.end());
		path.insert(path.end(), p2.begin(), p2.end());

		for (int i = 0; i < path.size(); i++) {
			std::vector<double> node = path[i];
			for (int j = 0; j<node.size() - 1; j++)
				sout << node[j] << ",";

			sout << node[node.size() - 1] << "\n";
		}
	}
	else {
		return false;
	}

	return true;
}
bool RRT::solveCRRT(Robotmodel& model, std::ostream& sout) {
	_model = model;

	nt_start._nodes.clear();
	nt_goal._nodes.clear();

	initConfig.clear();
	DOF_weights.clear();

	int ActiveDoFs = dofSize;
	step_size = 0.10/M_PI*180.0;

	std::string start_string[dofSize];

	for (int i = 0; i < ActiveDoFs; i++) {
		initConfig.push_back(qinit(i) * 180.0 / M_PI);
		DOF_weights.push_back(1.0);
	}

	auto start_node = std::make_shared<RRTNode>(initConfig);

	nt_start.addNode(start_node);
	nt_pointer = &nt_start;
	t_turn = 1;

	// mulitple seed
	for (int i = 0; i < qgoals.size(); i++)
	{
		std::vector<double> goalConfig;
		for (int j = 0; j < ActiveDoFs; j++)
		{
			goalConfig.push_back(qgoals[i][j] * 180.0 / M_PI);
		}
		auto goal_node = std::make_shared<RRTNode>(goalConfig);

		nt_goal.addNode(goal_node);
	}
	/// single seed
	// RRTNode g(goal, 0); // ���� RRT
	// gTree.addNode(g);

	dx_from_init_to_goal.resize(6);
	Vector3d pos_init = CalcBodyToBaseCoordinates(_model.model_, qinit, _model.body_id_vec.back(), _model.body_com_pos.back());
	Matrix3d Rot_init = CalcBodyWorldOrientation(_model.model_, qinit, _model.body_id_vec.back(), true).transpose();
	dx_from_init_to_goal.head(3) = refer_pos - pos_init;
	dx_from_init_to_goal.tail(3) = getPhi(Rot_init, refer_rot);

	dx_from_extend_to_goal.resize(6);

	bool finished = false;
	int iter = 1;
    int a = 1;
    std::vector<double> q_rand;
    RRTNodePtr q_a_near;
    std::vector<double> q_a_reached ;
    RRTNodePtr q_b_near;
    std::vector<double> q_b_reached ;
	int root_tree_index, goal_tree_index;
	while (iter < 50000)
	{
		if (t_turn == 1)
		{
			nt_pointer = &nt_start;								  //		// start tree
			q_rand = generateRandomConfig(); //ConstrainedRandomConfig(_model);					  // last node of goal tree(g2) or random config

			//cout << "start tree -> goal tree" << "\t" <<t_turn << endl;
			q_a_near = nt_pointer->getNearest(q_rand, DOF_weights); // random config�� ���� ����� c_tree�� ��� ã��
			q_a_reached = this->extendNodeWithConstraints(q_rand, q_a_near);   // near��� ���� random config�� Extend

			nt_pointer = &nt_goal;
			q_b_near = nt_pointer->getNearest(q_a_reached, DOF_weights); // random config�� ���� ����� c_tree�� ��� ã��

			//cout << q_b_near->getConfiguration()[0] << "\t" <<  q_b_near->getConfiguration()[1] << endl;
			//cout << "goal tree -> start tree" << endl;
			q_b_reached = this->extendNodeWithConstraints(q_a_reached, q_b_near); // near��� ���� random config�� Extend

			if (getEuclideanNorm(q_a_reached, q_b_reached, dofSize) < step_size)
			{
				nt_pointer = &nt_start;								  //		// start tree
				root_tree_index = nt_pointer->getNearestIndex(q_a_reached, DOF_weights);
				nt_pointer = &nt_goal;
				goal_tree_index = nt_pointer->getNearestIndex(q_b_reached, DOF_weights);

				// q_a_reached가 tree내에 있는 노드가 아니여서 불연속이 생김..
				cout << "t_turn \t" << t_turn << endl;
				cout << q_a_reached[0] << "\t" << q_a_reached[1] << "\t" << q_a_reached[2] << "\t" << q_a_reached[3] << "\t" << q_a_reached[4] << "\t" << q_a_reached[5] << "\t" << q_a_reached[6] << endl;
				cout << q_b_reached[0] << "\t" << q_b_reached[1] << "\t" << q_b_reached[2] << "\t" << q_b_reached[3] << "\t" << q_b_reached[4] << "\t" << q_b_reached[5] << "\t" << q_b_reached[6] << endl;

				cout << getEuclideanNorm(q_a_reached, q_b_reached, dofSize) << endl;
				cout << "reached" << endl;
				a = REACHED;
				finished = true;

				break;
			}
			//t_turn = 2;
		}
		// if (t_turn == 2)
		// {
		// 	c_tree = &gTree;	// goal tree
		// 	q_rand = RandomConfig();// ConstrainedRandomConfig(_model);					  // last node of goal tree(g2) or random config

		// 	//cout << "goal tree -> start _tree " << "\t" <<t_turn << endl;
		// 	q_a_near = c_tree->getNearest(q_rand, DOF_weights); // random config�� ���� ����� c_tree�� ��� ã��

		// 	q_a_reached = this->ConstrainedExtend(q_rand, q_a_near); // near��� ���� random config�� Extend

		// 	c_tree = &tree;
		// 	q_b_near = c_tree->getNearest(q_a_reached, DOF_weights); // random config�� ���� ����� c_tree�� ��� ã��
		// 	//cout << "start tree -> goal tree" << endl;

		// 	q_b_reached = this->ConstrainedExtend(q_a_reached, q_b_near); // near��� ���� random config�� Extend
		// 	if (getEuclideanNorm(q_a_reached, q_b_reached, dofSize) < step_size)
		// 	{
		// 		c_tree = &gTree;								  //		// start tree
		// 		root_tree_index = c_tree->getNearestIndex(q_a_reached, DOF_weights);
		// 		c_tree = &tree;
		// 		goal_tree_index = c_tree->getNearestIndex(q_b_reached, DOF_weights);

		// 		// q_a_reached가 tree내에 있는 노드가 아니여서 불연속이 생김..
		// 		cout << "t_turn \t" << t_turn << endl;
		// 		cout << q_a_reached[0] << "\t" << q_a_reached[1] << "\t" << q_a_reached[2] << "\t" << q_a_reached[3] << "\t" << q_a_reached[4] << "\t" << q_a_reached[5] << "\t" << q_a_reached[6] << endl;
		// 		cout << q_b_reached[0] << "\t" << q_b_reached[1] << "\t" << q_b_reached[2] << "\t" << q_b_reached[3] << "\t" << q_b_reached[4] << "\t" << q_b_reached[5] << "\t" << q_b_reached[6] << endl;

		// 		cout << getEuclideanNorm(q_a_reached, q_b_reached, dofSize) << endl;
		// 		cout << "reached" << endl;
		// 		a = REACHED;
		// 		finished = true;
		// 	}
		// 	t_turn = 1;

		// }
		iter++;
	}

	if (finished) {

		path = nt_start.getPathWithIndex(root_tree_index);
		vector<vector<double>> p2;
		p2 = nt_goal.getPathWithIndex(goal_tree_index);

		reverse(p2.begin(), p2.end());
		path.insert(path.end(), p2.begin(), p2.end());

		std::vector<double> end;
		end = path[path.size()-1];

		for (int i = 0; i < path.size(); i++) {
			std::vector<double> node = path[i];
			for (int j = 0; j < node.size() - 1; j++){
				sout << node[j] << ",";
			}
			sout << node[node.size() - 1] << "\n";

		}//error

	}
	else {

		return false;
	}

	return true;
}
std::vector<double> RRT::generateRandomConfig() {

 	std::vector<double> R;
	do {
		for (int i = 0; i < initConfig.size(); i++) {
			double jointrange = upper_limit(i) - lower_limit(i); // angle
			double r = ((double)rand() / (double)RAND_MAX)*jointrange;
			R.push_back(lower_limit(i) + r);
		}
	} while (R.size() != initConfig.size());

	return R;
}
// std::vector<double> RRT::ConstrainedRandomConfig(Robotmodel model){
// 	std::vector<double> R;
// 	std::vector<double> R_project;

// 	while (R_project.empty())
// 	{
// 		do
// 		{
// 			for (int i = 0; i < start.size(); i++)
// 			{
// 				double jointrange = upper_limit(i) - lower_limit(i); // angle
// 				double r = ((double)rand() / (double)RAND_MAX) * jointrange;
// 				R.push_back(lower_limit(i) + r);
// 			}
// 		} while (R.size() != start.size());

// 		R_project = ProjectConfigForRandomConfig(model, R);
// 	}

// 	return R_project;
//}

std::vector<double> RRT::extendNode(std::vector<double> &node, RRTNodePtr &near){
	std::vector<double> qs, qs_old;
	qs.clear();
	qs_old.clear();
	for (int i = 0; i < dofSize; i++)
	{
		qs.push_back(near->getConfiguration()[i]);
		qs_old.push_back(near->getConfiguration()[i]);
	}

	while(true){
		if (getEuclideanNorm(node, qs, dofSize) < step_size)
		{
			return qs;
		}
		else if (getEuclideanNorm(node, qs, dofSize) - getEuclideanNorm(qs_old, node, dofSize) > 0.0)
		{
			return qs_old; // qs_old is close to node
		}

		for (int i = 0; i < initConfig.size(); i++)
		{
			qs[i] = (qs[i] + min(step_size, getEuclideanNorm(node, qs, dofSize)) * (node[i] - qs[i]) / getEuclideanNorm(node, qs, dofSize));
		}

		if (!checkSelfCollision(_model, qs))
		{
			RRTNodePtr old = near;
			near = std::make_shared<RRTNode>(qs, old);
			nt_pointer->addNode(near);
			qs_old = qs;
		}
		else
		{
			cout << "collision config \t" << qs[0] << "\t" <<qs[1] << "\t" <<qs[2] << "\t" <<qs[3] << "\t" <<qs[4] << "\t" <<qs[5] << "\t" <<qs[6] << "\t" << endl;
			return qs_old;
		}

	}
}

std::vector<double> RRT::extendNodeWithConstraints(std::vector<double> &node, RRTNodePtr &near)
{
	// node = q_rand, near : q_near
	std::vector<double> qs, qs_old;
	qs.clear();
	qs_old.clear();
	bool project_flag = false;
	int iteration_ = 0;

	for (int i = 0; i < dofSize; i++)
	{
		qs.push_back(near->getConfiguration()[i]);
		qs_old.push_back(near->getConfiguration()[i]);
	}
	//cout << "input" << node[0] << "\t" << node[1] << "\t" << node[2] << "\t" << node[3] << "\t" << node[4] << "\t" << node[5] << "\t" << node[6] << endl;
	//cout << "q_near" << qs[0] << "\t"<< qs[1] << "\t"<< qs[2] << "\t"<< qs[3] << "\t"<< qs[4] << "\t"<< qs[5] << "\t"<< qs[6] <<endl;
	while (true)
	{
		if (getEuclideanNorm(node, qs, dofSize) < step_size)
		{
			return qs;
		}
		else if (getEuclideanNorm(node, qs, dofSize) - getEuclideanNorm(qs_old, node, dofSize) > 0.0)
		{
			return qs_old; // qs_old is close to node
		}

		for (int i = 0; i < initConfig.size(); i++) // Move from 'qs' to 'node'
			qs[i] = (qs[i] + min(step_size, getEuclideanNorm(node, qs, dofSize)) * (node[i] - qs[i]) / getEuclideanNorm(node, qs, dofSize));

		//cout << "desired_dir" <<qs[0] << "\t" << qs[1] << "\t" << qs[2] << "\t" << qs[3] << "\t" << qs[4] << "\t" << qs[5] << "\t" << qs[6] << endl;

		// qs_old : near / qs : extend
		qs = projectConfig(_model, qs_old, qs); // project qs onto constraint manifold

		// if (!qs.empty() && (std_norm(qs, qs_old, dofSize) > 3.0*step_size)){
		// 	cout << "output1" <<qs_old[0] << "\t" << qs_old[1] << "\t" << qs_old[2] << "\t" << qs_old[3] << "\t" << qs_old[4] << "\t" << qs_old[5] << "\t" << qs_old[6] << endl;
		// 	cout << "output2" <<qs[0] << "\t" << qs[1] << "\t" << qs[2] << "\t" << qs[3] << "\t" << qs[4] << "\t" << qs[5] << "\t" << qs[6] << endl;

		// 	return qs_old;
		// }

		if (!qs.empty() && !checkExternalCollision(_model, qs))
		{
			//			VectorXd q_project_(7);
			//cout << "extend!" << endl;
			RRTNodePtr old = near;		   //qs_old
			near = std::make_shared<RRTNode>(qs, old) ; //(new RRTNode(qs, old)); // qs : configuration, old : parent node
			nt_pointer->addNode(near);

			if((getEuclideanNorm(qs, qs_old, dofSize) < step_size / 2.0)){
				return qs;
			}
			qs_old = qs;

		}
		else
		{
			//cout << "qs_old" << qs_old[0] << "\t"<< qs_old[1] << "\t"<< qs_old[2] << "\t"<< qs_old[3] << "\t"<< qs_old[4] << "\t"<< qs_old[5] << "\t"<< qs_old[6] <<endl;
			//cout << "return NULL" << endl;
			//cout << "collision config \t" << qs[0] << "\t" <<qs[1] << "\t" <<qs[2] << "\t" <<qs[3] << "\t" <<qs[4] << "\t" <<qs[5] << "\t" <<qs[6] << "\t" << endl;

			return qs_old;
		}
	}
}
std::vector<double> RRT::projectConfig(Robotmodel model, std::vector<double> qold, std::vector<double> &qs)
{
	model.q.resize(dofSize);

	// Tc
	Matrix4d T0_c, T0_obj, Tc_obj;
	T0_c.setIdentity();
	//T0_c.topLeftCorner(3,3) = refer_rot;
	T0_c.topRightCorner(3, 1) = local_rot.transpose()*(refer_pos - local_pos); // global to local pos

	MatrixXd J_temp(6, dofSize), J(6, dofSize);
	VectorXd d_c(6), dx(6), q_error(dofSize), q_old(dofSize);
	dx.setZero();
	Vector3d phi;
	Vector3d s[3], v[3], w[3];

	for (int i = 0; i < dofSize; i++)
	{ // deg -> rad
		qs[i] = qs[i] * M_PI / 180.0;
		qold[i] = qold[i] * M_PI / 180.0;

		q_old(i) = qold[i] * M_PI / 180.0;
	}

	while (true)
	{
		for (int i = 0; i < dofSize; i++)
			model.q(i) = qs[i];

		Matrix3d Rot_temp = CalcBodyWorldOrientation(model.model_, model.q, model.body_id_vec.back(), true).transpose();
		Vector3d pos_temp = CalcBodyToBaseCoordinates(model.model_, model.q, model.body_id_vec.back(), model.body_com_pos.back()); // get position and rotation in EE frame;

		T0_obj.setIdentity();
		T0_obj.topLeftCorner(3, 3) = Rot_temp;
		T0_obj.topRightCorner(3, 1) = pos_temp;
		Tc_obj = T0_c.inverse() * T0_obj;

		d_c.head(3) = Tc_obj.topRightCorner(3, 1);

		Matrix3d Tc_obj_rot = refer_rot.transpose()*Rot_temp;
		d_c(3) = atan2(Tc_obj_rot(2, 1), Tc_obj_rot(2, 2));
		d_c(4) = -asin(Tc_obj_rot(2, 0));
		d_c(5) = atan2(Tc_obj_rot(1, 0), Tc_obj_rot(0, 0));

		// 위치가 베이스 프레임 기준이 아니게 됨.
		for (int i = 0; i < 6; i++)
		{
			if (d_c(i) > C(i, 0)) // max
				dx(i) = d_c(i) - C(i, 0);
			else if (d_c(i) < C(i, 1)) // min
				dx(i) = d_c(i) - C(i, 1) ;
			else
				dx(i) = 0.0;
		}

		// Algorithm 4 - line 3
		if (dx.norm() < 0.01)
		{
			Vector3d q_old_pos = CalcBodyToBaseCoordinates(model.model_, q_old, model.body_id_vec.back(), model.body_com_pos.back());
			double dot_product;

			dx_from_extend_to_goal.head(3) = refer_pos - pos_temp;
			dx_from_extend_to_goal.tail(3) = getPhi(Rot_temp, refer_rot);

			dot_product = (dx_from_init_to_goal.head(3)/dx_from_init_to_goal.head(3).norm()).dot((pos_temp - q_old_pos) / (pos_temp - q_old_pos) .norm());

			// TODO : focal point - init position, final positon.  projected point is inside the ellipsoid
			if ((dx_from_init_to_goal.head(3).norm() < dx_from_extend_to_goal.head(3).norm()) || (dot_product < 0.0))
			{
				qs.clear();
				return qs;
			}

			for (int i = 0; i < dofSize; i++)
			{
				qs[i] = qs[i] / M_PI * 180.0;
			}
				return qs; // convert rad to angle
		}

		// Algorithm 4 - line 4
		CalcPointJacobian6D(model.model_, model.q, model.body_id_vec.back(), model.body_com_pos.back(), J_temp, true);
		J.topLeftCorner(3, dofSize) = J_temp.bottomLeftCorner(3, dofSize);

		rpy(0) = atan2(Rot_temp(2, 1), Rot_temp(2, 2));
		rpy(1) = -asin(Rot_temp(2, 0));
		rpy(2) = atan2(Rot_temp(1, 0), Rot_temp(0, 0));

		E_rpy(0,0) = cos(rpy(2))/cos(rpy(1));
		E_rpy(0,1) = sin(rpy(2))/cos(rpy(1));
		E_rpy(0,2) = 0.0;

		E_rpy(1,0) = sin(rpy(2));
		E_rpy(1,1) = cos(rpy(2));
		E_rpy(1,2) = 0.0;

		E_rpy(2,0) = -cos(rpy(2))*sin(rpy(1))/cos(rpy(1));
		E_rpy(2,1) = sin(rpy(2))*sin(rpy(1))/cos(rpy(1));
		E_rpy(2,2) = 1.0;
		J.bottomLeftCorner(3, dofSize) = E_rpy*J_temp.topLeftCorner(3, dofSize);

		// Algorithm 4 - line 5
		q_error = J.transpose() * (J * J.transpose()).inverse() * dx * 0.3;

		// Algorithm 4 -  line 6
		for (int i = 0; i < dofSize; i++)
			qs[i] -= q_error(i);

		// Algorithm 4 - line 7 : stuck here
		if (!checkJointLimit(qs) || (getEuclideanNorm(qs, qold, dofSize) > 3.0*step_size/180.0*M_PI))
		{
			//cout << "joint limit" << "\t" << qs[0]/M_PI*180.0<< "\t" << qs[1]/M_PI*180.0<< "\t" << qs[2]/M_PI*180.0<< "\t" << qs[3]/M_PI*180.0<< "\t" << qs[4]/M_PI*180.0<< "\t" << qs[5]/M_PI*180.0<< "\t" << qs[6]/M_PI*180.0 << endl;
			qs.clear();
			return qs;
			// flag = false;
			// break;
		}
	}
}

// std::vector<double> RRT::ProjectConfigForRandomConfig(Robotmodel model, std::vector<double> &qs)
// {
// 	model.q.resize(dofSize);

// 	// Tc
// 	Matrix4d T0_c, T0_obj, Tc_obj;
// 	T0_c.setIdentity();
// 	//T0_c.topLeftCorner(3,3) = refer_rot;
// 	T0_c.topRightCorner(3, 1) = local_rot*(refer_pos - local_pos); // global to local pos

// 	MatrixXd J_temp(6, dofSize), J(6, dofSize);
// 	VectorXd d_c(6), dx(6), q_error(dofSize);
// 	dx.setZero();
// 	Vector3d phi;
// 	Vector3d s[3], v[3], w[3];

// 	for (int i = 0; i < dofSize; i++)
// 	{ // deg -> rad
// 		qs[i] = qs[i] * M_PI / 180.0;
// 	}

// 	while (true)
// 	{
// 		for (int i = 0; i < dofSize; i++)
// 			model.q(i) = qs[i];

// 		Matrix3d Rot_temp = CalcBodyWorldOrientation(model.model_, model.q, model.body_id_vec.back(), true).transpose();
// 		Vector3d pos_temp = CalcBodyToBaseCoordinates(model.model_, model.q, model.body_id_vec.back(), model.body_com_pos.back()); // get position and rotation in EE frame;

// 		T0_obj.setIdentity();
// 		T0_obj.topLeftCorner(3, 3) = Rot_temp;
// 		T0_obj.topRightCorner(3, 1) = pos_temp;
// 		Tc_obj = T0_c.inverse() * T0_obj;

// 		d_c.head(3) = Tc_obj.topRightCorner(3, 1);

// 		Matrix3d Tc_obj_rot = refer_rot.transpose()*Rot_temp;
// 		d_c(3) = atan2(Tc_obj_rot(2, 1), Tc_obj_rot(2, 2));
// 		d_c(4) = -asin(Tc_obj_rot(2, 0));
// 		d_c(5) = atan2(Tc_obj_rot(1, 0), Tc_obj_rot(0, 0));

// 		for (int i = 0; i < 6; i++)
// 		{
// 			if (d_c(i) > C(i, 0)) // max
// 				dx(i) = d_c(i) - C(i, 0);
// 			else if (d_c(i) < C(i, 1)) // min
// 				dx(i) = d_c(i) - C(i, 1) ;
// 			else
// 				dx(i) = 0.0;
// 		}

// 		// Algorithm 4 - line 3
// 		if (dx.norm() < 0.01)
// 		{
// 			dx_from_extend_to_goal.head(3) = refer_pos - pos_temp;
// 			dx_from_extend_to_goal.tail(3) = getPhi(Rot_temp, refer_rot);

// 			if (dx_from_init_to_goal.norm() < dx_from_extend_to_goal.norm()){
// 				qs.clear();
// 				return qs;
// 			}

// 			for (int i = 0; i < dofSize; i++)
// 			{
// 				qs[i] = qs[i] / M_PI * 180.0;
// 			}
// 				return qs; // convert rad to angle
// 		}

// 		// Algorithm 4 - line 4
// 		CalcPointJacobian6D(model.model_, model.q, model.body_id_vec.back(), model.body_com_pos.back(), J_temp, true);
// 		J.topLeftCorner(3, dofSize) = J_temp.bottomLeftCorner(3, dofSize);

// 		rpy(0) = atan2(Rot_temp(2, 1), Rot_temp(2, 2));
// 		rpy(1) = -asin(Rot_temp(2, 0));
// 		rpy(2) = atan2(Rot_temp(1, 0), Rot_temp(0, 0));

// 		E_rpy(0,0) = cos(rpy(2))/cos(rpy(1));
// 		E_rpy(0,1) = sin(rpy(2))/cos(rpy(1));
// 		E_rpy(0,2) = 0.0;

// 		E_rpy(1,0) = sin(rpy(2));
// 		E_rpy(1,1) = cos(rpy(2));
// 		E_rpy(1,2) = 0.0;

// 		E_rpy(2,0) = -cos(rpy(2))*sin(rpy(1))/cos(rpy(1));
// 		E_rpy(2,1) = sin(rpy(2))*sin(rpy(1))/cos(rpy(1));
// 		E_rpy(2,2) = 1.0;
// 		J.bottomLeftCorner(3, dofSize) = E_rpy*J_temp.topLeftCorner(3, dofSize);

// 		// Algorithm 4 - line 5
// 		q_error = J.transpose() * (J * J.transpose()).inverse() * dx * 0.3;

// 		// Algorithm 4 -  line 6
// 		for (int i = 0; i < dofSize; i++)
// 			qs[i] -= q_error(i);

// 		// Algorithm 4 - line 7 : stuck here
// 		if (!OutsideJointLimit(qs))
// 		{
// 			//cout << "joint limit" << "\t" << qs[0]/M_PI*180.0<< "\t" << qs[1]/M_PI*180.0<< "\t" << qs[2]/M_PI*180.0<< "\t" << qs[3]/M_PI*180.0<< "\t" << qs[4]/M_PI*180.0<< "\t" << qs[5]/M_PI*180.0<< "\t" << qs[6]/M_PI*180.0 << endl;
// 			qs.clear();
// 			return qs;
// 			// flag = false;
// 			// break;
// 		}
// 	}
// }

double RRT::getDistance(std::vector<double> &a, std::vector<double> &b) {

	//Gets squared Euclidean Distance on C-Space between node and a given
	double accumulated = 0.0f;
	std::vector<double> diff = b;

	for (int i = 0; i< diff.size(); i++) {
		diff[i] -= a[i];
		//double dif = _configuration[i] - (config)[i];
		accumulated += (diff[i] * diff[i] * DOF_weights[i] * DOF_weights[i]);
	}
	return sqrt(accumulated);
}
bool RRT::smoothPath(std::ostream& sout, std::istream& sinput)
{
	std::string fullinput;
	//Parse input
	while (sinput) {
		std::string input;
		sinput >> input;
		fullinput += input;
	}
	int nSmooth = 5; // atoi(fullinput.c_str());

	while (nSmooth) { // while(x) -> while( x != 0 )
		shortcutSmoothing();
		nSmooth--;
	}

	for (int i = 0; i< path.size(); i++) {
		std::vector<double> node = path[i];
		for (int j = 0; j<node.size() - 1; j++) {
			sout << node[j] << ",";
		}
		sout << node[node.size() - 1] << "\n";
	}

	return true;
}
bool RRT::shortcutSmoothing() {
	if (path.size() <= 2) return false;
	int i = ((double)rand() / RAND_MAX)*path.size();
	int j = ((double)rand() / RAND_MAX)*path.size();
	while (abs(j - i)<2) {
		i = ((double)rand() / RAND_MAX)*path.size();
		j = ((double)rand() / RAND_MAX)*path.size();
	}
	if (j < i) { //make sure i is the smaller number
		int a = j;
		j = i;
		i = a;
	}

	if (checkTraj(path[i], path[j])) {
		path.erase(path.begin() + i + 1, path.begin() + j);
		return true;
	}
	return false;
}

bool RRT::checkTraj(std::vector<double> &a, std::vector<double> &b) {
	std::vector<double> u = getUnitVector(a, b); // a->b
	std::vector<double> p = a;
	while (p != b) {
		if (checkExternalCollision(_model, p)) { return false;
		break;
		}
		if (getDistance(p, b) < step_size / 2) {
			p = b;
		}
		else {
			for (int i = 0; i < u.size(); i++) {
				p[i] += u[i] * step_size / 2;
			}
		}
	}
	return true;
}
std::vector<double> RRT::getUnitVector(std::vector<double> &a, std::vector<double> &b) {

	//Gets squared Euclidean Distance on C-Space between node and a given
	double accumulated = 0.0f;
	std::vector<double> diff = b;

	for (int i = 0; i< diff.size(); i++) {
		diff[i] -= a[i];//double dif = _configuration[i] - (config)[i];
		accumulated += (diff[i] * diff[i] * DOF_weights[i] * DOF_weights[i]);
	}
	accumulated = sqrt(accumulated);
	for (int i = 0; i < diff.size(); i++) {
		diff[i] /= accumulated;
	}
	return diff;
}
bool RRT::checkExternalCollision(Robotmodel model, std::vector<double> &config) {
	model.q.resize(dofSize);
	for (int i = 0; i < dofSize; i++)
		model.q(i) = config[i] * M_PI / 180.0;

	// Update Box modeling for link
	for (int i = 0; i < box_num_link; i++)
	{
		Matrix3d Rot_temp3 = Box_link[i].vRot * CalcBodyWorldOrientation(model.model_, model.q, model.body_id_vec[i], true).transpose();
		Box_link[i].vAxis[0] = Rot_temp3.col(0);
		Box_link[i].vAxis[1] = Rot_temp3.col(1);
		Box_link[i].vAxis[2] = Rot_temp3.col(2);
		Box_link[i].vPos = CalcBodyToBaseCoordinates(model.model_, model.q, model.body_id_vec[i], (Box_link[i].vCenter + model.body_com_pos[i]), true);
	}

	bool chk;
	if (box_num_obs == 0)
	{
		chk = false;
		return chk;
	}
	for (int i = 0; i < box_num_obs; i++)
	{
		for (int j = 0; j < box_num_link; j++)
		{
			chk = checkOBBCollision(&Box_obs[i], &Box_link[j]);
			if (chk)
			{
				// cout << "----------------Collision \n"
				// 	 << i << "\t" << j << "\t" << chk << endl;
				// cout << Box_obs[i].vPos.transpose() << endl;
				// cout << Box_link[j].vPos.transpose() << endl;
				// //cout << CalcBodyToBaseCoordinates(model.model_, model.q, model.body_id_vec[j], Vector3d::Zero(), true).transpose() << endl;
				// cout << model.q(0) * 180.0 / M_PI << "\t" << model.q(1) * 180.0 / M_PI << "\t" << model.q(2) * 180.0 / M_PI << "\t" << model.q(3) * 180.0 / M_PI << "\t" << model.q(4) * 180.0 / M_PI << "\t" << model.q(5) * 180.0 / M_PI << "\t" << model.q(6) * 180.0 / M_PI << "\t" << endl;

				//cout << i << "\t" << j << "\t" << chk << endl;

				return chk;
			}
			//cout << "collision" << i << "\t" << j << endl;
		}
	}
	return chk;
}

bool RRT::checkSelfCollision(Robotmodel model, std::vector<double> &config) {
	model.q.resize(dofSize);
	for (int i = 0; i < dofSize; i++)
		model.q(i) = config[i] * M_PI / 180.0;

	// Update Box modeling for link
	for (int i = 0; i < box_num_link; i++)
	{
		Matrix3d Rot_temp3 = Box_link[i].vRot * CalcBodyWorldOrientation(model.model_, model.q, model.body_id_vec[i], true).transpose();
		Box_link[i].vAxis[0] = Rot_temp3.col(0);
		Box_link[i].vAxis[1] = Rot_temp3.col(1);
		Box_link[i].vAxis[2] = Rot_temp3.col(2);
		Box_link[i].vPos = CalcBodyToBaseCoordinates(model.model_, model.q, model.body_id_vec[i], (Box_link[i].vCenter + model.body_com_pos[i]), true);
	}

	bool chk;
	if (box_num_link == 0)
	{
		chk = false;
		return chk;
	}
	for (int i = 0; i < box_num_link; i++)
	{
		for (int j = i + 2; j < box_num_link; j++)
		{
			chk = checkOBBCollision(&Box_link[i], &Box_link[j]);
			if (chk)
			{
				// cout <<  "----------------Collision \n" << i << "\t" << j << "\t" << chk << endl;
				// cout << Box_link[i].vPos.transpose() << endl;
				// cout << Box_link[j].vPos.transpose() << endl;
				// //cout << CalcBodyToBaseCoordinates(model.model_, model.q, model.body_id_vec[j], Vector3d::Zero(), true).transpose() << endl;
				// cout << model.q(0) * 180.0 / M_PI << "\t" << model.q(1) * 180.0 / M_PI << "\t" << model.q(2) * 180.0 / M_PI << "\t" << model.q(3) * 180.0 / M_PI << "\t" << model.q(4) * 180.0 / M_PI << "\t" << model.q(5) * 180.0 / M_PI << "\t" << model.q(6) * 180.0 / M_PI << "\t" << endl;
				return chk;
			}
			// cout << "No collision \n" << i << "\t" << j << "\t" << chk << endl;
			// cout << Box_link[i].vPos.transpose() << endl;
			// cout << Box_link[j].vPos.transpose() << endl;
			// 	cout << CalcBodyToBaseCoordinates(model.model_, model.q, model.body_id_vec[j], Vector3d::Zero(), true).transpose() << endl;
			//cout << model.q(0) * 180.0 / M_PI << "\t" << model.q(1) * 180.0 / M_PI << "\t" << model.q(2) * 180.0 / M_PI << "\t" << model.q(3) * 180.0 / M_PI << "\t" << model.q(4) * 180.0 / M_PI << "\t" << model.q(5) * 180.0 / M_PI << "\t" << model.q(6) * 180.0 / M_PI << "\t" << endl;
		}
	}
	return chk;
}

bool RRT::checkJointLimit(std::vector<double> q) { // radian
	for (int i = 0;i < q.size();i++) {
		if (q[i] > upper_limit(i)/180.0*M_PI) {
			//cout << q[i] << "\t" << upper_limit(i)/180.0*M_PI << "\t" << i << endl;
			return false;
		}
		else if (q[i] < lower_limit(i)/180.0*M_PI) {
			//cout << q[i] << "\t" << lower_limit(i)/180.0*M_PI  << "\t" << i << endl;

			return false;
		}
	}
	return true;

}

bool RRT::checkOBBCollision(ST_OBB* box0, ST_OBB* box1) // Collision-free : False
{
	// compute difference of box centers,D=C1-C0
	Vector3d D = Vector3d(box1->vPos(0) - box0->vPos(0), box1->vPos(1) - box0->vPos(1), box1->vPos(2) - box0->vPos(2));

	float C[3][3];    //matrix C=A^T B,c_{ij}=Dot(A_i,B_j)
	float absC[3][3]; //|c_{ij}|
	float AD[3];      //Dot(A_i,D)
	float R0, R1, R;    //interval radii and distance between centers
	float R01;        //=R0+R1

					  //A0
	C[0][0] = FDotProduct(box0->vAxis[0], box1->vAxis[0]);// vAxis : direction // 3D Dot product
	C[0][1] = FDotProduct(box0->vAxis[0], box1->vAxis[1]);
	C[0][2] = FDotProduct(box0->vAxis[0], box1->vAxis[2]);
	AD[0] = FDotProduct(box0->vAxis[0], D);
	absC[0][0] = (float)fabsf(C[0][0]);
	absC[0][1] = (float)fabsf(C[0][1]);
	absC[0][2] = (float)fabsf(C[0][2]);
	R = (float)fabsf(AD[0]);
	R1 = box1->fAxis(0) * absC[0][0] + box1->fAxis(1) * absC[0][1] + box1->fAxis(2) * absC[0][2];
	R01 = box0->fAxis(0) + R1;
	if (R > R01)
		return 0;

	//A1
	C[1][0] = FDotProduct(box0->vAxis[1], box1->vAxis[0]);
	C[1][1] = FDotProduct(box0->vAxis[1], box1->vAxis[1]);
	C[1][2] = FDotProduct(box0->vAxis[1], box1->vAxis[2]);
	AD[1] = FDotProduct(box0->vAxis[1], D);
	absC[1][0] = (float)fabsf(C[1][0]);
	absC[1][1] = (float)fabsf(C[1][1]);
	absC[1][2] = (float)fabsf(C[1][2]);
	R = (float)fabsf(AD[1]);
	R1 = box1->fAxis(0) * absC[1][0] + box1->fAxis(1) * absC[1][1] + box1->fAxis(2) * absC[1][2];
	R01 = box0->fAxis(1) + R1;
	if (R > R01)
		return 0;

	//A2
	C[2][0] = FDotProduct(box0->vAxis[2], box1->vAxis[0]);
	C[2][1] = FDotProduct(box0->vAxis[2], box1->vAxis[1]);
	C[2][2] = FDotProduct(box0->vAxis[2], box1->vAxis[2]);
	AD[2] = FDotProduct(box0->vAxis[2], D);
	absC[2][0] = (float)fabsf(C[2][0]);
	absC[2][1] = (float)fabsf(C[2][1]);
	absC[2][2] = (float)fabsf(C[2][2]);
	R = (float)fabsf(AD[2]);
	R1 = box1->fAxis(0) * absC[2][0] + box1->fAxis(1) * absC[2][1] + box1->fAxis(2) * absC[2][2];
	R01 = box0->fAxis(2) + R1;
	if (R > R01)
		return 0;

	//B0
	R = (float)fabsf(FDotProduct(box1->vAxis[0], D));
	R0 = box0->fAxis(0) * absC[0][0] + box0->fAxis(1) * absC[1][0] + box0->fAxis(2) * absC[2][0];
	R01 = R0 + box1->fAxis(0);
	if (R > R01)
		return 0;

	//B1
	R = (float)fabsf(FDotProduct(box1->vAxis[1], D));
	R0 = box0->fAxis(0) * absC[0][1] + box0->fAxis(1) * absC[1][1] + box0->fAxis(2) * absC[2][1];
	R01 = R0 + box1->fAxis(1);
	if (R > R01)
		return 0;

	//B2
	R = (float)fabsf(FDotProduct(box1->vAxis[2], D));
	R0 = box0->fAxis(0) * absC[0][2] + box0->fAxis(1) * absC[1][2] + box0->fAxis(2) * absC[2][2];
	R01 = R0 + box1->fAxis(2);
	if (R > R01)
		return 0;

	//A0xB0
	R = (float)fabsf(AD[2] * C[1][0] - AD[1] * C[2][0]);
	R0 = box0->fAxis(1) * absC[2][0] + box0->fAxis(2) * absC[1][0];
	R1 = box1->fAxis(1) * absC[0][2] + box1->fAxis(2) * absC[0][1];
	R01 = R0 + R1;
	if (R > R01)
		return 0;

	//A0xB1
	R = (float)fabsf(AD[2] * C[1][1] - AD[1] * C[2][1]);
	R0 = box0->fAxis(1) * absC[2][1] + box0->fAxis(2) * absC[1][1];
	R1 = box1->fAxis(0) * absC[0][2] + box1->fAxis(2) * absC[0][0];
	R01 = R0 + R1;
	if (R > R01)
		return 0;

	//A0xB2
	R = (float)fabsf(AD[2] * C[1][2] - AD[1] * C[2][2]);
	R0 = box0->fAxis(1) * absC[2][2] + box0->fAxis(2) * absC[1][2];
	R1 = box1->fAxis(0) * absC[0][1] + box1->fAxis(1) * absC[0][0];
	R01 = R0 + R1;
	if (R > R01)
		return 0;

	//A1xB0
	R = (float)fabsf(AD[0] * C[2][0] - AD[2] * C[0][0]);
	R0 = box0->fAxis(0) * absC[2][0] + box0->fAxis(2) * absC[0][0];
	R1 = box1->fAxis(1) * absC[1][2] + box1->fAxis(2) * absC[1][1];
	R01 = R0 + R1;
	if (R > R01)
		return 0;

	//A1xB1
	R = (float)fabsf(AD[0] * C[2][1] - AD[2] * C[0][1]);
	R0 = box0->fAxis(0) * absC[2][1] + box0->fAxis(2) * absC[0][1];
	R1 = box1->fAxis(0) * absC[1][2] + box1->fAxis(2) * absC[1][0];
	R01 = R0 + R1;
	if (R > R01)
		return 0;

	//A1xB2
	R = (float)fabsf(AD[0] * C[2][2] - AD[2] * C[0][2]);
	R0 = box0->fAxis(0) * absC[2][2] + box0->fAxis(2) * absC[0][2];
	R1 = box1->fAxis(0) * absC[1][1] + box1->fAxis(1) * absC[1][0];
	R01 = R0 + R1;
	if (R > R01)
		return 0;

	//A2xB0
	R = (float)fabsf(AD[1] * C[0][0] - AD[0] * C[1][0]);
	R0 = box0->fAxis(0) * absC[1][0] + box0->fAxis(1) * absC[0][0];
	R1 = box1->fAxis(1) * absC[2][2] + box1->fAxis(2) * absC[2][1];
	R01 = R0 + R1;
	if (R > R01)
		return 0;

	//A2xB1
	R = (float)fabsf(AD[1] * C[0][1] - AD[0] * C[1][1]);
	R0 = box0->fAxis(0) * absC[1][1] + box0->fAxis(1) * absC[0][1];
	R1 = box1->fAxis(0) * absC[2][2] + box1->fAxis(2) * absC[2][0];
	R01 = R0 + R1;
	if (R > R01)
		return 0;

	//A2xB2
	R = (float)fabsf(AD[1] * C[0][2] - AD[0] * C[1][2]);
	R0 = box0->fAxis(0) * absC[1][2] + box0->fAxis(1) * absC[0][2];
	R1 = box1->fAxis(0) * absC[2][1] + box1->fAxis(1) * absC[2][0];
	R01 = R0 + R1;
	if (R > R01)
		return 0;

	return 1;
}
MatrixXd RRT::MergeRRTResults(MatrixXd joint_target1, MatrixXd joint_target2, int case_){
	// case 0 : RRT / RRT
	// case 1 : RRT / CBiRRT
	// case 2 : CBiRRT / RRT
	// case 3 : CBiRRT / CBiRRT
	MatrixXd merged_joint_target;
	int row[2];
	int reminder[2];
	switch(case_ )
	{
		case 0 :
			row[0] = joint_target1.rows();
			row[1] = joint_target2.rows();
			merged_joint_target.resize(row[0] + row[1] -1, dofSize);
			merged_joint_target.topRows(row[0]) = joint_target1.topRows(row[0]);
			merged_joint_target.bottomRows(row[1]-1) = joint_target2.bottomRows(row[1]-1);
		break;
		case 1 :
			row[0] = joint_target1.rows();
			row[1] = joint_target2.rows() / 20;
			reminder[1] = joint_target2.rows() % 20;
			if (reminder[1] == 0)
			{
				merged_joint_target.resize(row[0] + row[1], dofSize);
				merged_joint_target.topRows(row[0]) = joint_target1.topRows(row[0]);
				for (int i = 0; i < row[1]; i++)
					merged_joint_target.row(row[0] + i) = joint_target2.row(20 * (i + 1) - 1);
			}
			else
			{
				merged_joint_target.resize(row[0] + row[1] + 1,dofSize);
				merged_joint_target.topRows(row[0]) = joint_target1.topRows(row[0]);
				for (int i = 0; i < row[1]; i++)
					merged_joint_target.row(row[0] + i) = joint_target2.row(20 * (i + 1)- 1);

				merged_joint_target.bottomRows(1) = joint_target2.bottomRows(1);
			}
			break;
		case 2 :
			row[0] = joint_target1.rows() /20;
			row[1] = joint_target2.rows() ;
			reminder[0] = joint_target1.rows() % 20;
			if (reminder[0] == 0)
			{
				merged_joint_target.resize(row[0] + row[1],dofSize);
				for (int i = 0; i < row[0]; i++)
					merged_joint_target.row(i) = joint_target1.row(20 * (i));

				merged_joint_target.bottomRows(row[1]) = joint_target2.bottomRows(row[1]);
			}
			else{
				merged_joint_target.resize(row[0] + row[1] + 1,dofSize);
				for (int i = 0; i < row[0] + 1; i++)
					merged_joint_target.row(i) = joint_target1.row(20 * (i));

				merged_joint_target.bottomRows(row[1]) = joint_target2.bottomRows(row[1]);
			}
		break;
		case 3 :
			row[0] = joint_target1.rows() /20;
			row[1] = joint_target2.rows() /20;
			reminder[0] = joint_target1.rows() % 20;
			reminder[1] = joint_target2.rows() % 20;
			if (reminder[0] == 0){
				if (reminder[1] == 0){
					merged_joint_target.resize(row[0] + row[1] + 1,dofSize);
					for (int i = 0; i < row[0] + 1; i++)
						merged_joint_target.row(i) = joint_target1.row(20 * (i));

					for (int i = 0; i < row[1]; i++)
						merged_joint_target.row(row[0] + 1 + i) = joint_target2.row(20 * (i+1));
				}
				else{
					merged_joint_target.resize(row[0] + row[1] + 2,dofSize);
					for (int i = 0; i < row[0] + 1; i++)
						merged_joint_target.row(i) = joint_target1.row(20 * (i));

					for (int i = 0; i < row[1]; i++)
						merged_joint_target.row(row[0] + 1 + i) = joint_target2.row(20 * (i+1));

					merged_joint_target.bottomRows(1) = joint_target2.bottomRows(1);
				}
			}
			else{
				if (reminder[1] == 0){
					merged_joint_target.resize(row[0] + row[1] + 2,dofSize);
					for (int i = 0; i < row[0] + 1; i++)
						merged_joint_target.row(i) = joint_target1.row(20 * (i));

					merged_joint_target.row(row[0]+1) = joint_target1.bottomRows(1);

					for (int i = 0; i < row[1]; i++)
						merged_joint_target.row(row[0] + 2 + i) = joint_target2.row(20 * (i+1));
				}
				else{
					merged_joint_target.resize(row[0] + row[1] + 3,dofSize);
					for (int i = 0; i < row[0] + 1; i++)
						merged_joint_target.row(i) = joint_target1.row(20 * (i));

					merged_joint_target.row(row[0]+1) = joint_target1.bottomRows(1);

					for (int i = 0; i < row[1]; i++)
						merged_joint_target.row(row[0] + 2 + i) = joint_target2.row(20 * (i+1));

					merged_joint_target.bottomRows(1) = joint_target2.bottomRows(1);
				}
			}
		break;
	}

	return merged_joint_target;
}


/*
 * MIT License
 *
 * Copyright (c) 2020 Jenna Reher (jreher@caltech.edu)
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
*/

#include <cassie_description/cassie_model.hpp>
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/algorithm/contact-dynamics.hpp"
#include "pinocchio/container/aligned-vector.hpp"

#include <ros/ros.h>
#include <ros/package.h>
#include <rbdl/addons/urdfreader/urdfreader.h>
#include <Eigen/Geometry>
#include <fstream>

using namespace RigidBodyDynamics;
using namespace Eigen;

namespace cassie_model {

void BodyPoint::initialize(Model *model, std::string point_name, std::string body_name, Eigen::Vector3d &point_position) {
    this->body_name = body_name;
    this->point_name = point_name;
    this->point_position << point_position;
    this->body_id = model->GetBodyId(body_name.c_str());
} // BodyPoint::initialize

Kinematics::Kinematics() {
    this->cache.init();
}

void Kinematics::Cache::init() {
    this->J_positionLeft.resize(3,22);
    this->J_positionRight.resize(3,22);
    this->J_leftLegLength.resize(1,7);
    this->J_rightLegLength.resize(1,7);
    this->J_poseLeft.resize(6,22);
    this->J_poseRight.resize(6,22);
    this->Jdot_poseLeft.resize(6,22);
    this->Jdot_poseRight.resize(6,22);

    this->J_poseLeftConstraint.resize(5,22);
    this->J_poseRightConstraint.resize(5,22);
    this->Jdot_poseLeftConstraint.resize(5,22);
    this->Jdot_poseRightConstraint.resize(5,22);
    this->J_achilles.resize(2,22);
    this->J_rigid.resize(4,22);
    this->Jdot_achilles.resize(2,22);
    this->Jdot_rigid.resize(4,22);
}

void Kinematics::initialize(Model *model) {
    Vector3d left_midfoot_body_frame_position = Vector3d(0,0,0);
    this->LeftMidFoot_Point.initialize(model, "LeftMidFoot", "leftfoot_link", left_midfoot_body_frame_position);

    Vector3d right_midfoot_body_frame_position = Vector3d(0,0,0);
    this->RightMidFoot_Point.initialize(model, "RightMidFoot", "rightfoot_link", right_midfoot_body_frame_position);

    Vector3d left_heelspringend_body_frame_position = Vector3d(0,0,0);
    this->LeftHeel_Point.initialize(model, "LeftSpringEnd", "leftheelspring_link", left_heelspringend_body_frame_position);

    Vector3d rightHeelspringend_body_frame_position = Vector3d(0,0,0);
    this->RightHeel_Point.initialize(model, "RightSpringEnd", "rightheelspring_link", rightHeelspringend_body_frame_position);
} // Kinematics::initialize

void Kinematics::update(Model *model, const VectorXd &q, const VectorXd &dq) {
    //RigidBodyDynamics::UpdateKinematicsCustom(*model, &q, &dq, nullptr);
    this->cache.J_achilles.setZero();
    SymFunction::J_achilles_constraint(this->cache.J_achilles, q);
    this->cache.J_poseLeft.setZero();
    SymFunction::J_leftSole_constraint(this->cache.J_poseLeftConstraint, q);
    this->cache.J_poseRight.setZero();
    SymFunction::J_rightSole_constraint(this->cache.J_poseRightConstraint, q);
    this->cache.Jdot_achilles.setZero();
    SymFunction::Jdot_achilles_constraint(this->cache.Jdot_achilles, q, dq);
    this->cache.Jdot_poseLeft.setZero();
    SymFunction::Jdot_leftSole_constraint(this->cache.Jdot_poseLeftConstraint, q, dq);
    this->cache.Jdot_poseRight.setZero();
    SymFunction::Jdot_rightSole_constraint(this->cache.Jdot_poseRightConstraint, q, dq);

    MatrixXd temp_left(2,22), temp_right(2,22);
    temp_left.setZero(); temp_right.setZero();
    SymFunction::J_left_fixed_constraint(temp_left, q);
    SymFunction::J_right_fixed_constraint(temp_right, q);
    this->cache.J_rigid << temp_left, temp_right;

    temp_left.setZero(); temp_right.setZero();
    SymFunction::Jdot_left_fixed_constraint(temp_left, q, dq);
    SymFunction::Jdot_right_fixed_constraint(temp_right, q, dq);
    this->cache.Jdot_rigid << temp_left, temp_right;
} // Kinematics::update

void Kinematics::computeConstrainedToeJacobian(Eigen::VectorXd &q, Eigen::MatrixXd &Jl, Eigen::MatrixXd &Jr) {
    assert_size_vector(q, 22);
    assert_size_matrix(Jl, 3,7);
    assert_size_matrix(Jr, 3,7);

    Kinematics::Cache *ca = &this->cache;

    SymFunction::J_leftToe(ca->J_positionLeft, q);
    SymFunction::J_rightToe(ca->J_positionRight, q);
    SymFunction::J_achilles_constraint(ca->J_achilles, q);

    MatrixXd Jlf_active(3,7), Jrf_active(3,7), Jach_active_lf(1,7), Jach_active_rf(1,7);

    Jlf_active << ca->J_positionLeft.block(0, CassieStateEnum::LeftHipRoll, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftHipYaw, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftHipPitch, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftKneePitch, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftShinPitch, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftHeelSpring, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftFootPitch, 3, 1);
    Jrf_active << ca->J_positionRight.block(0, CassieStateEnum::RightHipRoll, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightHipYaw, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightHipPitch, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightKneePitch, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightShinPitch, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightHeelSpring, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightFootPitch, 3, 1);

    Jach_active_lf << ca->J_achilles.block(0, CassieStateEnum::LeftHipRoll, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipYaw, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftKneePitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftShinPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHeelSpring, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftFootPitch, 1, 1);
    Jach_active_rf << ca->J_achilles.block(1, CassieStateEnum::RightHipRoll, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipYaw, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightKneePitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightShinPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHeelSpring, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightFootPitch, 1, 1);

    Jl = Jlf_active - ca->J_positionLeft.block(0, CassieStateEnum::LeftTarsusPitch, 3, 1) / ca->J_achilles(0, CassieStateEnum::LeftTarsusPitch) * Jach_active_lf;
    Jr = Jrf_active - ca->J_positionRight.block(0, CassieStateEnum::RightTarsusPitch, 3, 1) / ca->J_achilles(1, CassieStateEnum::RightTarsusPitch) * Jach_active_rf;
} // Kinematics::computeConstrainedToeJacobian

void Kinematics::computeConstrainedFootJacobian(Eigen::VectorXd &q, Eigen::MatrixXd &Jl, Eigen::MatrixXd &Jr) {
    assert_size_vector(q, 22);
    assert_size_matrix(Jl, 3,7);
    assert_size_matrix(Jr, 3,7);

    Kinematics::Cache *ca = &this->cache;

    SymFunction::J_leftFoot(ca->J_poseLeft, q);
    SymFunction::J_rightFoot(ca->J_poseRight, q);
    SymFunction::J_achilles_constraint(ca->J_achilles, q);

    MatrixXd Jlf_active(3,7), Jrf_active(3,7), Jach_active_lf(1,7), Jach_active_rf(1,7);

    Jlf_active << ca->J_poseLeft.block(0, CassieStateEnum::LeftHipRoll, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftHipYaw, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftHipPitch, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftKneePitch, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftShinPitch, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftHeelSpring, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftFootPitch, 3, 1);
    Jrf_active << ca->J_poseRight.block(0, CassieStateEnum::RightHipRoll, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightHipYaw, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightHipPitch, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightKneePitch, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightShinPitch, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightHeelSpring, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightFootPitch, 3, 1);

    Jach_active_lf << ca->J_achilles.block(0, CassieStateEnum::LeftHipRoll, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipYaw, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftKneePitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftShinPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHeelSpring, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftFootPitch, 1, 1);
    Jach_active_rf << ca->J_achilles.block(1, CassieStateEnum::RightHipRoll, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipYaw, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightKneePitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightShinPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHeelSpring, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightFootPitch, 1, 1);

    Jl = Jlf_active - ca->J_poseLeft.block(0, CassieStateEnum::LeftTarsusPitch, 3, 1) / ca->J_achilles(0, CassieStateEnum::LeftTarsusPitch) * Jach_active_lf;
    Jr = Jrf_active - ca->J_poseRight.block(0, CassieStateEnum::RightTarsusPitch, 3, 1) / ca->J_achilles(1, CassieStateEnum::RightTarsusPitch) * Jach_active_rf;
} // Kinematics::computeConstrainedFootJacobian

void Kinematics::computeStanceConstrainedJacobian(Eigen::VectorXd &q, Eigen::MatrixXd &Jl, Eigen::MatrixXd &Jr) {
    assert_size_vector(q, 22);
    assert_size_matrix(Jl, 5,7);
    assert_size_matrix(Jr, 5,7);

    Kinematics::Cache *ca = &this->cache;

    // This assumes that kinematics.update() has been called!

    MatrixXd Jlf_active(5,7), Jrf_active(5,7), Jach_active_lf(1,7), Jach_active_rf(1,7);

    Jlf_active << ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftHipRoll, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftHipYaw, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftHipPitch, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftKneePitch, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftShinPitch, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftHeelSpring, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftFootPitch, 5, 1);
    Jrf_active << ca->J_poseRightConstraint.block(0, CassieStateEnum::RightHipRoll, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightHipYaw, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightHipPitch, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightKneePitch, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightShinPitch, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightHeelSpring, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightFootPitch, 5, 1);

    Jach_active_lf << ca->J_achilles.block(0, CassieStateEnum::LeftHipRoll, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipYaw, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftKneePitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftShinPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHeelSpring, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftFootPitch, 1, 1);
    Jach_active_rf << ca->J_achilles.block(1, CassieStateEnum::RightHipRoll, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipYaw, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightKneePitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightShinPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHeelSpring, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightFootPitch, 1, 1);

    Jl = Jlf_active - ca->J_poseLeft.block(0, CassieStateEnum::LeftTarsusPitch, 5, 1) / ca->J_achilles(0, CassieStateEnum::LeftTarsusPitch) * Jach_active_lf;
    Jr = Jrf_active - ca->J_poseRight.block(0, CassieStateEnum::RightTarsusPitch, 5, 1) / ca->J_achilles(1, CassieStateEnum::RightTarsusPitch) * Jach_active_rf;
} // Kinematics::computeConstrainedFootJacobian

void Dynamics::initialize(Model *model) {
    this->H.resize(model->dof_count,model->dof_count);
    this->C.resize(model->dof_count);
    this->H.setZero();
    this->C.setZero();
} // Dynamics::initialize

void Dynamics::calcHandC(Model *model, VectorXd &Q, VectorXd &QDot) {
    // Evaluate inertia matrix (H) and velocity terms (C)
    VectorXd QDDot(22);
    QDDot.setZero();
    // InverseDynamics(*model, Q, QDot, QDDot, this->C);
    NonlinearEffects(*model,Q,QDot,this->C); // compute Coriolis
    CompositeRigidBodyAlgorithm(*model, Q, this->H, false);

    // Add reflected rotor inertias
    this->H(LeftHipRoll,LeftHipRoll)       += 0.038125;
    this->H(RightHipRoll,RightHipRoll)     += 0.038125;
    this->H(LeftHipYaw,LeftHipYaw)         += 0.038125;
    this->H(RightHipYaw,RightHipYaw)       += 0.038125;
    this->H(LeftHipPitch,LeftHipPitch)     += 0.09344;
    this->H(RightHipPitch,RightHipPitch)   += 0.09344;
    this->H(LeftKneePitch,LeftKneePitch)   += 0.09344;
    this->H(RightKneePitch,RightKneePitch) += 0.09344;
    this->H(LeftFootPitch,LeftFootPitch)   += 0.01225;
    this->H(RightFootPitch,RightFootPitch) += 0.01225;
} // Dynamics::calcHandC

void Linearizations::initialize(pinocchio::Model *pmodel) {
    this->A.resize(44,44);
    this->B.resize(44,10);
    this->C.resize(44);
}

void Linearizations::calcLinearizations(pinocchio::Model *pmodel, VectorXd &Q_bar,VectorXd &QDot_bar, VectorXd &U_bar, MatrixXd Be, MatrixXd Jc, MatrixXd dJc) {
    // NonlinearEffects(*model,Q,QDot,this->C); // compute Coriolis
    // CompositeRigidBodyAlgorithm(*model, Q, this->H, false);

    // this->H_inv = H.inverse();

    // SymFunction::Df(this->Df, Q_bar, QDot_bar);
    // this->f = this->H_inv*this->C;

    Quaterniond q;
    q = AngleAxisd(Q_bar(3), Vector3d::UnitX())
        * AngleAxisd(Q_bar(4), Vector3d::UnitY())
        * AngleAxisd(Q_bar(5), Vector3d::UnitZ());

    VectorXd Q_bar_quat(23);
    Q_bar_quat << Q_bar.segment(0,3), q.coeffs(), Q_bar.segment(6,16);

    pinocchio::Data data_nonlinear(*pmodel);
    pinocchio::Data data_jacobian(*pmodel);

    // Allocate result container
    Eigen::MatrixXd djoint_acc_dq = Eigen::MatrixXd::Zero(22,22);
    Eigen::MatrixXd djoint_acc_dv = Eigen::MatrixXd::Zero(22,22);
    Eigen::MatrixXd djoint_acc_dtau = Eigen::MatrixXd::Zero(22,22);

    pinocchio::forwardDynamics(*pmodel, data_nonlinear, Q_bar_quat, QDot_bar, Be*U_bar, Jc, dJc*QDot_bar, 1e-12);

    VectorXd lambda = data_nonlinear.lambda_c;
    VectorXd zero = VectorXd::Zero(6);
    VectorXd left(6);
    VectorXd right(6);
    left << lambda(6),lambda(7),lambda(8),0,lambda(9),lambda(10);
    right << lambda(11),lambda(12),lambda(13),0,lambda(14),lambda(15);

    pinocchio::Force zeroForce(zero);
    pinocchio::Force leftForce(left);
    pinocchio::Force rightForce(right);

    pinocchio::container::aligned_vector<pinocchio::Force> forces;
    for (int i = 0; i < 9; i ++)
        forces.push_back(zeroForce);
    forces.push_back(leftForce);
    for (int i = 10; i < 17; i ++)
        forces.push_back(zeroForce);
    forces.push_back(rightForce);

    // TODO: what is J_rigid??
    computeABADerivatives(*pmodel, data_jacobian, Q_bar_quat, QDot_bar, Be*U_bar, forces, djoint_acc_dq, djoint_acc_dv, djoint_acc_dtau);
    this->A << MatrixXd::Zero(22,22), MatrixXd::Identity(22,22), djoint_acc_dq, djoint_acc_dv;
    this->B << MatrixXd::Zero(22,10), djoint_acc_dtau*Be;
//    aba(*pmodel, data, Q_bar_quat, QDot_bar, Be*U_bar);
    this->C << Eigen::VectorXd::Zero(22), data_nonlinear.ddq - (djoint_acc_dq*Q_bar + djoint_acc_dv*QDot_bar) - djoint_acc_dtau*Be*U_bar;

    //    pinocchio::aba(*(this->robot->pmodel), data, Q_bar_quat, QDot, this->config.Be*this->cache.u);

    //    pinocchio::computeJointJacobians(*(this->robot->pmodel), data, Q_bar_quat);

    //    MatrixXd Jac = data.J.transpose();
    //    VectorXd F = Jac.householderQr().solve(this->config.Be*this->cache.u);

    // Compute the robot constraints

//    this->cache.u << torqueScale.cwiseProduct(this->cache.u);
}

Cassie::Cassie(bool verbose) {
    // Retreive the path to the robot URDF description
    std::string urdf_path = ros::package::getPath("cassie_description");
    urdf_path.append("/urdf/cassie_v4.urdf");

    const char* urdf_path_cstr = urdf_path.c_str();
    const std::string urdf_path_c = "/home/jreher/cassie_ws/src/cassie_description/urdf/cassie_v4.urdf";

    // Construct the RBDL model
    this->model = new RigidBodyDynamics::Model();
    Addons::URDFReadFromFile(urdf_path_cstr, this->model, true, verbose);

    // Construct Pinocchio model
    this->pmodel = new pinocchio::Model();
    pinocchio::urdf::buildModel(urdf_path_c,pinocchio::JointModelFreeFlyer(), *(this->pmodel));

    // Add the leg loop constraints
    // TODO: This is not fully integrated.
    this->constraints.AddLoopConstraint(
        this->model->GetBodyId("lefthippitch"),
        this->model->GetBodyId("leftheelspring"),
        Math::SpatialTransform(Matrix3d::Identity(3,3), Vector3d(0., 0., 0.045)),
        Math::SpatialTransform(Matrix3d::Identity(3,3), Vector3d(0.11877, -0.01, 0.)),
        Math::SpatialVector(0.,0.,0.,1.,1.,1.),
        false,
        0.1,
        "leftachilles");
    this->constraints.AddLoopConstraint(
        this->model->GetBodyId("righthippitch"),
        this->model->GetBodyId("rightheelspring"),
        Math::SpatialTransform(Matrix3d::Identity(3,3), Vector3d(0., 0., -0.045)),
        Math::SpatialTransform(Matrix3d::Identity(3,3), Vector3d(0.11877, -0.01, 0.)),
        Math::SpatialVector(0.,0.,0.,1.,1.,1.),
        false,
        0.1,
        "rightachilles");
    this->constraints.Bind(*this->model);
    this->constraints.clear();

    this->torque.resize(10);
    this->torque.setZero();

    // Create robot statespace
    this->q.resize(22);
    this->dq.resize(22);
    this->ddq.resize(22);

    this->q.setZero();
    this->dq.setZero();
    this->ddq.setZero();
    this->quat_pelvis.setIdentity();

    // Indexing helpers
    this->iRotorMap.resize(10);
    this->iJointMap.resize(4);
    this->iEncoderMap.resize(14);
    this->iRotorMap <<
            CassieStateEnum::LeftHipRoll,
            CassieStateEnum::LeftHipYaw,
            CassieStateEnum::LeftHipPitch,
            CassieStateEnum::LeftKneePitch,
            CassieStateEnum::LeftFootPitch,
            CassieStateEnum::RightHipRoll,
            CassieStateEnum::RightHipYaw,
            CassieStateEnum::RightHipPitch,
            CassieStateEnum::RightKneePitch,
            CassieStateEnum::RightFootPitch;
    this->iJointMap <<
            CassieStateEnum::LeftShinPitch,
            CassieStateEnum::LeftTarsusPitch,
            CassieStateEnum::RightShinPitch,
            CassieStateEnum::RightTarsusPitch;
    this->iEncoderMap <<
            CassieStateEnum::LeftHipRoll,
            CassieStateEnum::LeftHipYaw,
            CassieStateEnum::LeftHipPitch,
            CassieStateEnum::LeftKneePitch,
            CassieStateEnum::LeftShinPitch,
            CassieStateEnum::LeftTarsusPitch,
            CassieStateEnum::LeftFootPitch,
            CassieStateEnum::RightHipRoll,
            CassieStateEnum::RightHipYaw,
            CassieStateEnum::RightHipPitch,
            CassieStateEnum::RightKneePitch,
            CassieStateEnum::RightShinPitch,
            CassieStateEnum::RightTarsusPitch,
            CassieStateEnum::RightFootPitch;

    // Append additional modeling considerations
    this->model->gravity = Vector3d(0., 0., -9.80665);

    // Contact modes
    this->leftContact  = 0.0;
    this->rightContact = 0.0;

    this->GRF.resize(6);
    this->GRF = Eigen::VectorXd::Zero(6);

    // Construct the class members
    this->kinematics.initialize(this->model);
    this->dynamics.initialize(this->model);
    this->linearizations.initialize(this->pmodel);
} // Cassie::Cassie

} // namespace cassie_model


// version 11: sort 100M doubles
#include <boost/noncopyable.hpp>
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>

#include <datetime/Timestamp.h>
#include <thread/ThreadPool.h>
#include <thread/BlockingQueue.h>

#include <algorithm>
#include <vector>

#include <assert.h>
#include <stdio.h>

#include <sys/resource.h>

using muduo::Timestamp;
typedef double ElementType;

struct Source
{
  const std::vector<ElementType>* data;
  int first, last;

  bool read(ElementType* value)
  {
    if (first < last)
    {
      *value = (*data)[first];
      ++first;
      return true;
    }
    else
    {
      return false;
    }
  }

};

struct Record
{
  ElementType value;
  Source* source;

  Record(Source& src)
    : source(&src)
  {
    next();
  }

  bool next()
  {
    return source->read(&value);
  }

  bool operator<(const Record& rhs) const
  {
    return value > rhs.value;
  }
};

class OutputFile : boost::noncopyable
{
 public:
  OutputFile(const char* filename)
    : file_(fopen(filename, "wb"))
  {
    assert(file_);
    setbuffer(file_, buffer_, sizeof buffer_);
  }

  ~OutputFile()
  {
    fflush(file_);
    fdatasync(fileno(file_));
    fclose(file_);
  }

  void writeRecord(ElementType x)
  {
    fwrite_unlocked(&x, 1, sizeof x, file_);
  }

  void sync()
  {
    fdatasync(fileno(file_));
  }

 private:
  FILE* file_;
  char buffer_[4*1024*1024];
};

typedef std::vector<ElementType> Batch;
typedef boost::shared_ptr<Batch> BatchPtr;

void output(muduo::BlockingQueue<BatchPtr>* queueOut,
            muduo::BlockingQueue<BatchPtr>* queueIn)
{
  OutputFile out("output");
  while (BatchPtr batch = queueOut->take())
  {
    Timestamp start = Timestamp::now();
    printf("%s write %zd records\n", start.toString().c_str(), batch->size());

    for (Batch::iterator it = batch->begin();
        it != batch->end();
        ++it)
    {
      out.writeRecord(*it);
    }
    out.sync();

    queueIn->put(batch);
  }
  printf("flushing\n");
}

void merge(const std::vector<ElementType>& data, const int kChunkSize)
{
  const int nChunks = (data.size() + kChunkSize - 1) / kChunkSize;
  printf("merge %d chunks\n", nChunks);

  std::vector<Source> sources;
  sources.reserve(nChunks);
  std::vector<Record> keys;
  keys.reserve(nChunks);

  size_t first = 0;
  for (first = 0; first + kChunkSize <= data.size(); first += kChunkSize)
  {
    Source src = { &data, first, first + kChunkSize };
    sources.push_back(src);
    Record rec(sources.back());
    keys.push_back(rec);
  }
  if (first < data.size())
  {
    assert(data.size() - first < kChunkSize);
    Source src = { &data, first, data.size()};
    sources.push_back(src);
    Record rec(sources.back());
    keys.push_back(rec);
  }
  assert(sources.size() == nChunks);

  muduo::BlockingQueue<BatchPtr> queueOut, queueIn;
  muduo::ThreadPool threadPool;
  threadPool.start(1);
  threadPool.run(boost::bind(output, &queueOut, &queueIn));
  const int batchSize = 1000 * 1000 * 10;

  {
    BatchPtr batch(new Batch);
    batch->reserve(batchSize);
    queueIn.put(batch);
  }

  {
    BatchPtr batch(new Batch);
    batch->reserve(batchSize);
    queueIn.put(batch);
  }

  {
    BatchPtr batch(new Batch);
    batch->reserve(batchSize);
    queueIn.put(batch);
  }

  BatchPtr batch(new Batch);
  batch->reserve(batchSize);

  std::make_heap(keys.begin(), keys.end());
  ElementType current = keys.front().value;
  while (!keys.empty())
  {
    std::pop_heap(keys.begin(), keys.end());
    assert(current <= keys.back().value);
    if (current > keys.back().value)
    {
      abort();
    }

    batch->push_back(keys.back().value);
    if (batch->size() >= batchSize)
    {
      queueOut.put(batch);
      batch = queueIn.take();
      batch->clear();
    }

    if (keys.back().next())
    {
      std::push_heap(keys.begin(), keys.end());
    }
    else
    {
      keys.pop_back();
    }
  }
  queueOut.put(batch);
  queueOut.put(BatchPtr());
  printf("flushing\n");
}

int main(int argc, char* argv[])
{
  {
    // set max virtual memory to 3GB.
    size_t kOneGB = 1024*1024*1024;
    rlimit rl = { 3.0*kOneGB, 3.0*kOneGB };
    setrlimit(RLIMIT_AS, &rl);
  }

  std::vector<ElementType> data;

  Timestamp start = Timestamp::now();

  {
    data.resize(1000*1000*100);
    for (std::vector<ElementType>::iterator it = data.begin();
        it != data.end();
        ++it)
    {
      *it = drand48();
    }
  }

  Timestamp readDone = Timestamp::now();
  printf("%zd\nread  %f\n", data.size(), timeDifference(readDone, start));

  int kChunkSize = atoi(argv[1]);
  printf("sort %zd records with chunk size %d\n", data.size(), kChunkSize);

  int i = 0;
  for (i = 0; i + kChunkSize <= data.size(); i += kChunkSize)
  {
    std::sort(data.begin() + i, data.begin() + i + kChunkSize);
  }
  printf("%d\n%zd\n", i, data.size());

  std::sort(data.begin() + i, data.end());
  Timestamp sortDone = Timestamp::now();
  printf("sort  %f\n", timeDifference(sortDone, readDone));

  merge(data, kChunkSize);
  Timestamp mergeDone = Timestamp::now();
  printf("merge %f\n", timeDifference(mergeDone, sortDone));

  printf("sort & merge %f\n", timeDifference(mergeDone, readDone));
  // getchar();
}


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Test_SGD1_CompoundScalarHelpers
#include <boost/test/unit_test.hpp>
#include <popart/compoundscalarhelper.hpp>
#include <popart/sessionoptions.hpp>
#include <popart/sgd.hpp>

using namespace popart;

namespace {
void validate(SGD sgd,
              float expected_lr,
              float expected_wd,
              float expected_dp,
              float expected_mm) {
  ScaledLearningRate1Helper lr;
  ScaledWeightDecay1Helper wd;
  DampeningScaleFactor1Helper dp;
  ScaledMomentum1Helper mm;

  BOOST_CHECK_EQUAL(lr.val("", sgd), expected_lr);
  BOOST_CHECK_EQUAL(wd.val("", sgd), expected_wd);
  BOOST_CHECK_EQUAL(dp.val("", sgd), expected_dp);
  BOOST_CHECK_EQUAL(mm.val("", sgd), expected_mm);
}
} // namespace

BOOST_AUTO_TEST_CASE(TestSGD1_Base) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, (1.0f - 0.8f) * 0.01f, 1.0f - 0.8f, 0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_lossScaling) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
              {"lossScaling", {4.0f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, (1.0f - 0.8f) * 0.01f, (1.0f - 0.8f) / 4.0f, 0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_velocityScaling) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
              {"defaultVelocityScaling", {2.0f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  sgd.setFactorsFromOptions(opts);
  validate(sgd,
           0.1f / 2.0f,
           (1.0f - 0.8f) * 0.01f * 2.0f,
           (1.0f - 0.8f) * 2.0f,
           0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_ReplicaSum) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableReplicatedGraphs = true;
  opts.replicatedGraphCount   = 2;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, (1.0f - 0.8f) * 0.01f, (1.0f - 0.8f), 0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_ReplicaMeanPostAndLoss) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::PostAndLoss;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, (1.0f - 0.8f) * 0.01f, (1.0f - 0.8f), 0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_ReplicaMeanPost) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Post;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, (1.0f - 0.8f) * 0.01f, (1.0f - 0.8f) / 2.0f, 0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_ReplicaMeanRunning) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Running;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, (1.0f - 0.8f) * 0.01f, (1.0f - 0.8f) / 2.0f, 0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_AccumSum) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableGradientAccumulation = true;
  opts.accumulationFactor         = 4;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, (1.0f - 0.8f) * 0.01f, (1.0f - 0.8f), 0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_AccumMeanPostAndLoss) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::PostAndLoss;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, (1.0f - 0.8f) * 0.01f, (1.0f - 0.8f) / 4.0f, 0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_AccumMeanPost) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Post;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, (1.0f - 0.8f) * 0.01f, (1.0f - 0.8f) / 4.0f, 0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_AccumMeanRunning) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Running;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, (1.0f - 0.8f) * 0.01f, (1.0f - 0.8f) / 4.0f, 0.9f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_AccumAndReplicaSum) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableGradientAccumulation = true;
  opts.accumulationFactor         = 4;
  opts.enableReplicatedGraphs     = true;
  opts.replicatedGraphCount       = 2;
  sgd.setFactorsFromOptions(opts);
  validate(sgd,
           0.1f / 2.0f,
           (1.0f - 0.8f) * 0.01f,
           (1.0f - 0.8f) * 2.0f,
           0.9f / 2.0f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_AccumAndReplicaMeanPostAndLoss) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::PostAndLoss;
  sgd.setFactorsFromOptions(opts);
  validate(sgd,
           0.1f / 2.0f,
           (1.0f - 0.8f) * 0.01f,
           (1.0f - 0.8f) * 2.0f / 4.0f,
           0.9f / 2.0f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_AccumAndReplicaMeanPost) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Post;
  sgd.setFactorsFromOptions(opts);
  validate(sgd,
           0.1f / 2.0f,
           (1.0f - 0.8f) * 0.01f,
           (1.0f - 0.8f) / 4.0f,
           0.9f / 2.0f);
}

BOOST_AUTO_TEST_CASE(TestSGD1_AccumAndReplicaMeanRunning) {
  SGD sgd{{
              {"defaultLearningRate", {0.1f, true}},
              {"defaultWeightDecay", {0.01f, true}},
              {"defaultDampening", {0.8f, true}},
              {"defaultMomentum", {0.9f, true}},
          },
          {},
          SGDAccumulatorAndMomentum::Combined};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Running;
  sgd.setFactorsFromOptions(opts);
  validate(sgd,
           0.1f / 2.0f,
           (1.0f - 0.8f) * 0.01f,
           (1.0f - 0.8f) / 4.0f,
           0.9f / 2.0f);
}


/*
 * Copyright 2020 Adobe. All rights reserved.
 * This file is licensed to you 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 REPRESENTATIONS
 * OF ANY KIND, either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
 */
#include <Eigen/Core>
#include <lagrange/testing/common.h>

#include <lagrange/Components.h>
#include <lagrange/Connectivity.h>
#include <lagrange/Mesh.h>
#include <lagrange/common.h>
#include <lagrange/create_mesh.h>

TEST_CASE("Components", "[components][simple]")
{
    using namespace lagrange;

    Vertices3D vertices(3, 3);
    vertices << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0;
    Triangles facets(1, 3);
    facets << 0, 1, 2;

    auto mesh = create_mesh(vertices, facets);
    mesh->initialize_components();

    REQUIRE(mesh->get_num_components() == 1);
    const auto& comp_list = mesh->get_components();
    REQUIRE(comp_list[0].size() == 1);
}

TEST_CASE("ComponentsVertexTouch", "[components][simple]")
{
    using namespace lagrange;

    Vertices3D vertices(5, 3);
    vertices << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0;
    Triangles facets(2, 3);
    facets << 0, 1, 2, 0, 3, 4;

    auto mesh = create_mesh(vertices, facets);
    mesh->initialize_components();

    REQUIRE(mesh->get_num_components() == 2);
    const auto& comp_list = mesh->get_components();
    REQUIRE(comp_list[0].size() == 1);
    REQUIRE(comp_list[1].size() == 1);

    const auto& comp_ids = mesh->get_per_facet_component_ids();
    REQUIRE(comp_ids.size() == 2);
}

TEST_CASE("MultiComps", "[components][simple]")
{
    using namespace lagrange;

    Vertices3D vertices(6, 3);
    vertices << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0,
        1.0;
    Triangles facets(2, 3);
    facets << 0, 1, 2, 3, 4, 5;

    auto mesh = create_mesh(vertices, facets);
    mesh->initialize_components();

    REQUIRE(mesh->get_num_components() == 2);
    const auto& comp_list = mesh->get_components();
    REQUIRE(comp_list[0].size() == 1);
    REQUIRE(comp_list[1].size() == 1);

    const auto& comp_ids = mesh->get_per_facet_component_ids();
    REQUIRE(comp_ids.size() == 2);
}