/*!
 * @file
 * This file defines unit tests for `boost::mpl::adjacent_vertices_of`.
 */

#include <boost/mpl/reachable_set.hpp>
#include <boost/mpl/set.hpp>
#include <boost/mpl/set_equal.hpp>
#include <boost/mpl/vector.hpp>

using namespace boost::mpl;

template <typename ...Neighbors>
struct vertex {
    using open_neighborhood = vector<Neighbors...>;
};

template <typename ...Vertices>
struct graph {
    using vertices = vector<Vertices...>;
};

struct dummy_graph;

struct v0 : vertex<> { };
struct v1 : vertex<> { };
struct g0 : graph<v0, v1> { };

struct v2 : vertex<g0> { };
struct v3 : vertex<g0, v0> { };
struct v4 : vertex<v1, v2> { };
struct g1 : graph<v2, v3, v4> { };

static_assert(set_equal<
    adjacent_vertices_of<dummy_graph, v0>::type, set<>
>::value, "");
static_assert(set_equal<
    adjacent_vertices_of<dummy_graph, v1>::type, set<>
>::value, "");
static_assert(set_equal<
    adjacent_vertices_of<dummy_graph, v2>::type, set<v0, v1>
>::value, "");
static_assert(set_equal<
    adjacent_vertices_of<dummy_graph, v3>::type, set<v0, v1>
>::value, "");
static_assert(set_equal<
    adjacent_vertices_of<dummy_graph, v4>::type, set<v1, v2>
>::value, "");

struct vertex_and_graph : vertex<v3, graph<v2, v1, v0>> { };
static_assert(set_equal<
    adjacent_vertices_of<dummy_graph, vertex_and_graph>::type,
    set<v3, v2, v1, v0>
>::value, "");

int main() { }


// ROS includes
#include "ros/ros.h"
#include "ros/assert.h"
#include "dynamic_reconfigure/server.h"
#include "create_driver/vicon_driver.h"
#include "geometry_msgs/Twist.h"
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Vector3.h>

// Library includes
#include <string>
#include <vector>
#include <map>
#include <Eigen/Core>
#include <Eigen/LU>
#include <algorithm>

#include <stdio.h>
//#include <tchar.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <cmath>

#include "jingfu/control_formation.h"
#include <Eigen/Core>
#include <Eigen/Dense>
using Eigen::MatrixXd;
using namespace Eigen;

using namespace std;
using std::setw;
using std::setprecision;

double joy_x,joy_y,joy_z;
int new_msg=0;
sensor_msgs::Joy joy_msg_in;
geometry_msgs::Vector3 v3_msg;

//int l1 = 1, l2 = 0,l3 = 0;
//int act_leader = 1;
void joy_callback(const sensor_msgs::Joy joy_msg_in)
{
	//Take in joystick
	joy_x=joy_msg_in.axes[4];
	joy_y=joy_msg_in.axes[3];
	joy_z=joy_msg_in.axes[1];

	//l1=joy_msg_in.buttons[0];
	//l2=joy_msg_in.buttons[1];
	//l3=joy_msg_in.buttons[2];

	printf("hello joy\n");
	printf("hello joy\n");
	//Take in time
	//msg_time=(double)ros::Time::now().toNSec();
    	if (joy_x !=0||joy_y !=0)
	{
		new_msg=1;
	}
	else if (joy_z !=0)
	{
		new_msg=0;
	}
	else
	{
		//new_msg=0;
		ROS_INFO("Waiting for joystick message");
	}

/*
	if (l1!=0)
	{
		act_leader = 1;
	}
	else if(l2!=0)
	{
		act_leader = 2;
	}
	else if(l3!=0)
	{
		act_leader = 3;
	}
	else
	{
		act_leader = 1;
	}
*/
}

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

	ros::NodeHandle node;
	ros::NodeHandle private_node("~");

	ros::Publisher pub_v3;
	ros::Subscriber joy_sub;
	pub_v3 = node.advertise<geometry_msgs::Vector3>("joy_vel", 1); //send velocity for graphing on /joy_vel topic
	joy_sub = node.subscribe("/joy", 1, joy_callback); //suscribe to the joystick message

	ROS_INFO("Waiting for joystick message");
	ros::Rate rate(100.0);
	ROS_INFO("Starting Joy --> cmd_vel Node");

	// ROS Parameters

	// List of robot names
	vector<string> robot_names;
	XmlRpc::XmlRpcValue robot_list;
	private_node.getParam("robot_list", robot_list);
	ROS_ASSERT(robot_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < robot_list.size(); i++)
	{
		ROS_ASSERT(robot_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
		robot_names.push_back(static_cast<string>(robot_list[i]));
	}

	// Number of robots
	const int num_robots = robot_names.size();

	// Robot name
	string robot_name;
	private_node.param<std::string>("robotname", robot_name, "defaultname");

	int index = find(robot_names.begin(), robot_names.end(), robot_name) - robot_names.begin();

	// ROS Subscribers
	map<string, create_driver::ViconStream> vicon;
	vector<string>::iterator name_it;
	vector<ros::Subscriber> sub;
	for (name_it = robot_names.begin(); name_it != robot_names.end(); name_it++)
	{
		vicon.insert(pair<string, create_driver::ViconStream>(*name_it, create_driver::ViconStream()));
		sub.push_back(node.subscribe("/" + *name_it + "/tf", 10, &create_driver::ViconStream::callback, &vicon[*name_it]));
	}

	// ROS Publishers
	ros::Publisher vel = node.advertise<geometry_msgs::Twist>("cmd_vel", 10);

	// ROS loop
	ros::Rate loop_rate(250); // 250 Hz

	ros::spinOnce();
	/// initial poses from Vicon///
	pose2D q10; q10.x = vicon[robot_names[0]].x();q10.y = vicon[robot_names[0]].y();q10.h = vicon[robot_names[0]].theta();
	pose2D q20; q20.x = vicon[robot_names[1]].x();q20.y = vicon[robot_names[1]].y();q20.h = vicon[robot_names[1]].theta();
	pose2D q30; q30.x = vicon[robot_names[2]].x();q30.y = vicon[robot_names[2]].y();q30.h = vicon[robot_names[2]].theta();
	pose2D q40; q40.x = vicon[robot_names[3]].x();q40.y = vicon[robot_names[3]].y();q40.h = vicon[robot_names[3]].theta();
	pose2D quad; quad.x = vicon[robot_names[4]].x();quad.y = vicon[robot_names[4]].y();quad.h = vicon[robot_names[4]].theta();

	/// // Desired Formation shape or initlal offests ///
	double k = 0.8; // radius
	int mm = 3;
	int shape = 1;  // triangle = 1; line = 2
	pose2D q1off; pose2D q2off; pose2D q3off; pose2D q4off;
	switch (shape)
	{
  		case 1:
			q1off.x = k*cos(1*PI/4); q1off.y = k*sin(1*PI/4); q1off.h = 0;
			q2off.x = k*cos(3*PI/4); q2off.y = k*sin(3*PI/4); q2off.h = 0;
			q3off.x = k*cos(5*PI/4); q3off.y = k*sin(5*PI/4); q3off.h = 0;
			q4off.x = k*cos(7*PI/4); q4off.y = k*sin(7*PI/4); q4off.h = 0;
	    		break;
  		case 2:
	    		q1off.x = 1; q1off.y = 0; q1off.h = 0;
			q2off.x = 2; q2off.y = 0; q2off.h = 0;
			q3off.x = 3; q3off.y = 0; q3off.h = 0;
			q4off.x = 4; q4off.y = 0; q4off.h = 0;
	    		break;
  	}

	/// end of initial offsets ///

	double df12 = sqrt((q1off.x - q2off.x)*(q1off.x - q2off.x) + (q1off.y - q2off.y)*(q1off.y - q2off.y));
	double df13 = sqrt((q1off.x - q3off.x)*(q1off.x - q3off.x) + (q1off.y - q3off.y)*(q1off.y - q3off.y));
	double df14 = sqrt((q1off.x - q4off.x)*(q1off.x - q4off.x) + (q1off.y - q4off.y)*(q1off.y - q4off.y));

	double df23 = sqrt((q2off.x - q3off.x)*(q2off.x - q3off.x) + (q2off.y - q3off.y)*(q2off.y - q3off.y));
	double df24 = sqrt((q2off.x - q4off.x)*(q2off.x - q4off.x) + (q2off.y - q4off.y)*(q2off.y - q4off.y));

	double df34 = sqrt((q3off.x - q4off.x)*(q3off.x - q4off.x) + (q3off.y - q4off.y)*(q3off.y - q4off.y));

	pose2D q10_v; pose2D q20_v; pose2D q30_v; pose2D q40_v;
	Vector2d p12; Vector2d p13; Vector2d p14;
	Vector2d p23; Vector2d p24; Vector2d p34;
	double d12=0; double d13=0; double d14; double d23=0; double d24; double d34;

	Wheel vd;
	vd.l_v = 0.8;
	vd.a_v = 0;

	/// initial control inputs ///
	Wheel MyVel[4];
	MyVel[0].l_v = 0;
	MyVel[0].a_v = 0;

	MyVel[1].l_v = 0;
	MyVel[1].a_v = 0;

	MyVel[2].l_v = 0;
	MyVel[2].a_v = 0;

	MyVel[3].l_v = 0;
	MyVel[3].a_v = 0;

	/////////////////////////////
	control_formation r1;
	control_formation r2;
	control_formation r3;
	control_formation r4;
	////////////////////////////

	MatrixXd A(4,4); // adjacency matrix
	A <<0,1,1,1,
	    1,0,1,1,
	    1,1,0,1,
	    1,1,1,0;
	MatrixXd D(4,4); // Degree matrix
	D <<3,0,0,0,
	    0,3,0,0,
	    0,0,3,0,
	    0,0,0,3;	;

	pose2D virtualGoal_1; pose2D virtualGoal_2; pose2D virtualGoal_3; pose2D virtualGoal_4; // virtual goal
	point2D po1; point2D po2; point2D po3; point2D po4;// position of obstacle

	ofstream myfile_1("/home/heckert/utd-lars/src/jingfu/data/robot_1.txt",ios::out | ios::binary);
	ofstream myfile_2("/home/heckert/utd-lars/src/jingfu/data/robot_2.txt",ios::out | ios::binary);
	ofstream myfile_3("/home/heckert/utd-lars/src/jingfu/data/robot_3.txt",ios::out | ios::binary);
	ofstream myfile_4("/home/heckert/utd-lars/src/jingfu/data/robot_4.txt",ios::out | ios::binary);
	ofstream myfile_5("/home/heckert/utd-lars/src/jingfu/data/quad.txt",ios::out | ios::binary);

	while (ros::ok())
	{
		// Retrieve Vicon Data
		ros::spinOnce();

		/* An instance of this code is run on each robot in the formation.
		 *
		 * You can access each robot's position and oreintation through the following statements:
		 * vicon[robot_names[i]].x()
		 * vicon[robot_names[i]].y()
		 * vicon[robot_names[i]].theta()
		 *
		 * The index of the robot running the instance of the code is contain in the "index"
		 * variable. For example, if you wanted to get the distance of this robot from all of its
		 * neighbors you could run the following:
		 * double dist[num_robots];
		 * for (int i = 0; i < num_robots; i++) {
		 * 		dist[i] = sqrt(pow(vicon[robot_names[i]].x() - vicon[robot_names[index]].x(),2)
		 * 				     + pow(vicon[robot_names[i]].y() - vicon[robot_names[index]].y(),2));
		 * }
		 */

		/// virtual poses ///
		q10_v.x = q10.x + q1off.x; q10_v.y = q10.y + q1off.y; q10_v.h = q10.h + q1off.h;
		q20_v.x = q20.x + q2off.x; q20_v.y = q20.y + q2off.y; q20_v.h = q20.h + q2off.h;
		q30_v.x = q30.x + q3off.x; q30_v.y = q30.y + q3off.y; q30_v.h = q30.h + q3off.h;
		q40_v.x = q40.x + q4off.x; q40_v.y = q40.y + q4off.y; q40_v.h = q40.h + q4off.h;

		int kk = 2;
		/// virtual goals ///

		MatrixXd virtualGoal(3,4);
		virtualGoal(0,0) = q10_v.x; virtualGoal(0,1) = q20_v.x; virtualGoal(0,2) = q30_v.x; virtualGoal(0,3) = q40_v.x;
		virtualGoal(1,0) = q10_v.y; virtualGoal(1,1) = q20_v.y; virtualGoal(1,2) = q30_v.y; virtualGoal(1,3) = q40_v.y;
		virtualGoal(2,0) = q10_v.h; virtualGoal(2,1) = q20_v.h; virtualGoal(2,2) = q30_v.h; virtualGoal(2,3) = q40_v.h;

		MatrixXd virtualGoal_temp(3,4);
		virtualGoal_temp = virtualGoal*A*D.inverse(); /// calculation virtual goal position ///

		virtualGoal_1.x = virtualGoal_temp(0,0);
		virtualGoal_1.y = virtualGoal_temp(1,0);
		virtualGoal_1.h = virtualGoal_temp(2,0);

		virtualGoal_2.x = virtualGoal_temp(0,1);
		virtualGoal_2.y = virtualGoal_temp(1,1);
		virtualGoal_2.h = virtualGoal_temp(2,1);

		virtualGoal_3.x = virtualGoal_temp(0,2);
		virtualGoal_3.y = virtualGoal_temp(1,2);
		virtualGoal_3.h = virtualGoal_temp(2,2);

		virtualGoal_4.x = virtualGoal_temp(0,3);
		virtualGoal_4.y = virtualGoal_temp(1,3);
		virtualGoal_4.h = virtualGoal_temp(2,3);

		//////////////////////////////////
		p12(0) = q10.x - q20.x; p12(1) = q10.y - q20.y;
		p13(0) = q10.x - q30.x; p13(1) = q10.y - q30.y;
		p14(0) = q10.x - q40.x; p14(1) = q10.y - q40.y;

		p23(0) = q20.x - q30.x; p23(1) = q20.y - q30.y;
		p24(0) = q20.x - q40.x; p24(1) = q20.y - q40.y;

		p34(0) = q30.x - q40.x; p34(1) = q30.y - q40.y;

		/// Distance between robots ///
		d12 = p12.norm();
		d13 = p13.norm();
		d14 = p14.norm();

		d23 = p23.norm();
		d24 = p24.norm();

		d34 = p34.norm();
		//////////////////////////////

		double d1[3] = {d12,d13,d14};
 		int nneighbor = std::min_element(d1,d1+3) - d1;

 		switch (nneighbor) {
 			case 0:
 				po1.x = q20.x;
				po1.y = q20.y;
 				break;
 			case 1:
 				po1.x = q30.x;
				po1.y = q30.y;
 				break;
 			case 2:
 				po1.x = q40.x;
				po1.y = q40.y;
 				break;

 		}
 		double d2[3] = {d12,d23,d24};
 		nneighbor = std::min_element(d2,d2+3) - d2;

 		switch (nneighbor) {
 			case 0:
 				po2.x = q10.x;
				po2.y = q10.y;
 				break;
 			case 1:
 				po2.x = q30.x;
				po2.y = q30.y;
 				break;
 			case 2:
 				po2.x = q40.x;
				po2.y = q40.y;
 				break;
 		}

 		double d3[3] = {d13,d23,d34};
 		nneighbor = std::min_element(d3,d3+3) - d3;

 		switch (nneighbor) {
 			case 0:
 				po3.x = q10.x;
				po3.y = q10.y;
 				break;
 			case 1:
 				po3.x = q20.x;
				po3.y = q20.y;
 				break;
 			case 2:
 				po3.x = q40.x;
				po3.y = q40.y;
 				break;
 		}

 		double d4[3] = {d14,d24,d34};
 		nneighbor = std::min_element(d4,d4+3) - d4;

 		switch (nneighbor) {
 			case 0:
 				po4.x = q10.x;
				po4.y = q10.y;
 				break;
 			case 1:
 				po4.x = q20.x;
				po4.y = q20.y;
 				break;
 			case 2:
 				po4.x = q30.x;
				po4.y = q30.y;
 				break;

 		}

		if (new_msg==0)
		{
			MyVel[0] = r1.nonswitching_w_obstacle(q10_v, virtualGoal_1, q10, po1, vd); // robot 1
			MyVel[1] = r2.nonswitching_w_obstacle(q20_v, virtualGoal_2, q20, po2, vd); // robot 2
			MyVel[2] = r3.nonswitching_w_obstacle(q30_v, virtualGoal_3, q30, po3, vd); // robot 3
			MyVel[3] = r4.nonswitching_w_obstacle(q40_v, virtualGoal_4, q40, po4, vd); // robot 3
		}
		else
		{

			//switch(act_leader)
			//{
			//	case 1:

				double l;

				l = (quad.y-q10.y)/(quad.x-q10.x);

				pose2D temp_quad;
				temp_quad.x = (q10.x+quad.x)/2;
				temp_quad.y = l*temp_quad.x;
				virtualGoal_1 = quad;
				virtualGoal_2.x = q10.x + q1off.x - q2off.x;
				virtualGoal_2.y = q10.y + q1off.y - q2off.y;
				virtualGoal_2.h = q10.h + q1off.h - q2off.h;

				virtualGoal_3.x = q10.x + q1off.x - q3off.x;
				virtualGoal_3.y = q10.y + q1off.y - q3off.y;
				virtualGoal_3.h = q10.h + q1off.h - q3off.h;

				virtualGoal_4.x = q10.x + q1off.x - q4off.x;
				virtualGoal_4.y = q10.y + q1off.y - q4off.y;
				virtualGoal_4.h = q10.h + q1off.h - q4off.h;
			//	break;

			//	case 2:

			//	virtualGoal_2 = quad;
			//	virtualGoal_1.x = q20.x + q2off.x - q1off.x;
			//	virtualGoal_1.y = q20.y + q2off.y - q1off.y;
			//	virtualGoal_1.h = q20.h + q2off.h - q1off.h;

			//	virtualGoal_3.x = q20.x + q2off.x - q3off.x;
			//	virtualGoal_3.y = q20.y + q2off.y - q3off.y;
			//	virtualGoal_3.h = q20.h + q2off.h - q3off.h;
			//	break;

			//	case 3:

			//	virtualGoal_3 = quad;
			//	virtualGoal_1.x = q30.x + q3off.x - q1off.x;
			//	virtualGoal_1.y = q30.y + q3off.y - q1off.y;
			//	virtualGoal_1.h = q30.h + q3off.h - q1off.h;

			//	virtualGoal_2.x = q30.x + q3off.x - q2off.x;
			//	virtualGoal_2.y = q30.y + q3off.y - q2off.y;
			//	virtualGoal_2.h = q30.h + q3off.h - q2off.h;
			//	break;
			//}

			MyVel[0] = r1.singleRobot(virtualGoal_1, q10); // robot 1
			MyVel[1] = r2.singleRobot(virtualGoal_2, q20); // robot 2
			int gain=3;
			MyVel[1].l_v = gain*MyVel[1].l_v;
			MyVel[1].a_v = gain*MyVel[1].a_v;
			MyVel[2] = r3.singleRobot(virtualGoal_3, q30); // robot 3
			MyVel[2].l_v = gain*MyVel[2].l_v;
			MyVel[2].a_v = gain*MyVel[2].a_v;

			MyVel[3] = r4.singleRobot(virtualGoal_4, q40); // robot 4
			MyVel[3].l_v = gain*MyVel[3].l_v;
			MyVel[3].a_v = gain*MyVel[3].a_v;

/*
			MyVel[0] = r1.single_w_obstacle(virtualGoal_1, q10,po1); // robot 1
			MyVel[1] = r2.single_w_obstacle(virtualGoal_2, q20,po2); // robot 2
			int gain=3;
			MyVel[1].l_v = gain*MyVel[1].l_v;
			MyVel[1].a_v = gain*MyVel[1].a_v;
			MyVel[2] = r3.single_w_obstacle(virtualGoal_3, q30,po3); // robot 3
			MyVel[2].l_v = gain*MyVel[2].l_v;
			MyVel[2].a_v = gain*MyVel[2].a_v;
*/

		}

		q10.x = vicon[robot_names[0]].x();
		q10.y = vicon[robot_names[0]].y();
		q10.h = vicon[robot_names[0]].theta();
		q10.h = r1.between2PI(q10.h);

		q20.x = vicon[robot_names[1]].x();
		q20.y = vicon[robot_names[1]].y();
		q20.h = vicon[robot_names[1]].theta();
		q20.h = r2.between2PI(q20.h);

		q30.x = vicon[robot_names[2]].x();
		q30.y = vicon[robot_names[2]].y();
		q30.h = vicon[robot_names[2]].theta();
		q30.h = r3.between2PI(q30.h);

		q40.x = vicon[robot_names[3]].x();
		q40.y = vicon[robot_names[3]].y();
		q40.h = vicon[robot_names[3]].theta();
		q40.h = r3.between2PI(q40.h);

		// virtual leader or waypoint //
		quad.x = vicon[robot_names[4]].x();
		quad.y = vicon[robot_names[4]].y();
		quad.h = vicon[robot_names[4]].theta();

		if (abs(d12-df12)<=0.05 && abs(d13-df13)<=0.05 && abs(d23-df23)<=0.05)
		//if ((MyVel[0].l_v-MyVel[1].l_v)<=0.008 && (MyVel[0].l_v-MyVel[2].l_v)<=0.008)
		{
			vd.l_v = vd.l_v - 0.01;
		}

		if (vd.l_v<=0)
		{
			vd.l_v = 0;
		}

	myfile_1 <<q10.x<<setw(20)<<q10.y<<setw(20)<<q10.h<<setw(20)<<MyVel[0].l_v<<setw(20)<<MyVel[0].a_v<<setw(20)<<r1.zeta<<endl;
	myfile_2 <<q20.x<<setw(20)<<q20.y<<setw(20)<<q20.h<<setw(20)<<MyVel[1].l_v<<setw(20)<<MyVel[1].a_v<<setw(20)<<r2.zeta<<endl;
	myfile_3 <<q30.x<<setw(20)<<q30.y<<setw(20)<<q30.h<<setw(20)<<MyVel[2].l_v<<setw(20)<<MyVel[2].a_v<<setw(20)<<r3.zeta<<endl;
	myfile_3 <<q40.x<<setw(20)<<q40.y<<setw(20)<<q40.h<<setw(20)<<MyVel[3].l_v<<setw(20)<<MyVel[3].a_v<<setw(20)<<r4.zeta<<endl;
	myfile_4 <<quad.x<<setw(20)<<quad.y<<setw(20)<<quad.h<<endl;

		double v = MyVel[index].l_v*200; // linear velocity output
		double w = MyVel[index].a_v; // angular velocity output

		// Send wheel velocities to driver
		geometry_msgs::Twist msg;
		msg.linear.x = v;
		msg.linear.y = 0;
		msg.linear.z = 0;
		msg.angular.x = 0;
		msg.angular.y = 0;
		msg.angular.z = w;
		vel.publish(msg);
		loop_rate.sleep();
	}
	myfile_1.close();
	myfile_2.close();
	myfile_3.close();
	myfile_4.close();
	myfile_5.close();
	return 0;
}


#include <boost/hana/unfold_left.hpp>


// Copyright (C) 2016 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * 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.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Dense>

#include <ceres/rotation.h>
#include <math.h>
#include "gtest/gtest.h"

#include "theia/alignment/alignment.h"
#include "theia/sfm/camera/fov_camera_model.h"
#include "theia/test/test_utils.h"
#include "theia/util/random.h"

namespace theia {

using Eigen::AngleAxisd;
using Eigen::Matrix3d;
using Eigen::Matrix;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

TEST(FOVCameraModel, InternalParameterGettersAndSetters) {
  FOVCameraModel camera;
  static const double kDefaultOmega = 0.75;

  EXPECT_EQ(camera.Type(), CameraIntrinsicsModelType::FOV);

  // Check that default values are set
  EXPECT_EQ(camera.FocalLength(), 1.0);
  EXPECT_EQ(camera.AspectRatio(), 1.0);
  EXPECT_EQ(camera.PrincipalPointX(), 0.0);
  EXPECT_EQ(camera.PrincipalPointY(), 0.0);
  EXPECT_EQ(camera.RadialDistortion1(), kDefaultOmega);

  // Set parameters to different values.
  camera.SetFocalLength(600.0);
  camera.SetAspectRatio(0.9);
  camera.SetPrincipalPoint(300.0, 400.0);
  camera.SetRadialDistortion(0.01);

  // Check that the values were updated.
  EXPECT_EQ(camera.FocalLength(), 600.0);
  EXPECT_EQ(camera.AspectRatio(), 0.9);
  EXPECT_EQ(camera.PrincipalPointX(), 300.0);
  EXPECT_EQ(camera.PrincipalPointY(), 400.0);
  EXPECT_EQ(camera.RadialDistortion1(), 0.01);
}

// Test to ensure that the camera intrinsics are being set appropriately.
void TestSetFromCameraintrinsicsPrior(const CameraIntrinsicsPrior& prior) {
  const FOVCameraModel default_camera;
  FOVCameraModel camera;
  camera.SetFromCameraIntrinsicsPriors(prior);

  if (prior.focal_length.is_set) {
    EXPECT_EQ(camera.FocalLength(), prior.focal_length.value[0]);
  } else {
    EXPECT_EQ(camera.FocalLength(), default_camera.FocalLength());
  }

  if (prior.principal_point.is_set) {
    EXPECT_EQ(camera.PrincipalPointX(), prior.principal_point.value[0]);
    EXPECT_EQ(camera.PrincipalPointY(), prior.principal_point.value[1]);
  } else {
    EXPECT_EQ(camera.PrincipalPointX(), default_camera.PrincipalPointX());
    EXPECT_EQ(camera.PrincipalPointY(), default_camera.PrincipalPointY());
  }

  if (prior.aspect_ratio.is_set) {
    EXPECT_EQ(camera.AspectRatio(), prior.aspect_ratio.value[0]);
  } else {
    EXPECT_EQ(camera.AspectRatio(), default_camera.AspectRatio());
  }

  if (prior.radial_distortion.is_set) {
    EXPECT_EQ(camera.RadialDistortion1(), prior.radial_distortion.value[0]);
  } else {
    EXPECT_EQ(camera.RadialDistortion1(), default_camera.RadialDistortion1());
  }
}

// Gradually add one prior at a time and ensure that the method still works. We
// test before and after setting the "is_set" member variable to true to ensure
// that setting the value of priors when is_set=false is handled properly.
TEST(FOVCameraModel, SetFromCameraIntrinsicsPriors) {
  CameraIntrinsicsPrior prior;
  prior.focal_length.value[0] = 1000.0;
  prior.principal_point.value[0] = 400.0;
  prior.principal_point.value[1] = 300.0;
  prior.aspect_ratio.value[0] = 1.01;
  prior.radial_distortion.value[0] = 0.01;

  TestSetFromCameraintrinsicsPrior(prior);

  prior.focal_length.is_set = true;
  TestSetFromCameraintrinsicsPrior(prior);

  prior.principal_point.is_set = true;
  TestSetFromCameraintrinsicsPrior(prior);

  prior.aspect_ratio.is_set = true;
  TestSetFromCameraintrinsicsPrior(prior);

  prior.radial_distortion.is_set = true;
  TestSetFromCameraintrinsicsPrior(prior);
}

TEST(FOVCameraModel, GetSubsetFromOptimizeIntrinsicsType) {
  FOVCameraModel camera;
  std::vector<int> constant_subset;

  constant_subset =
      camera.GetSubsetFromOptimizeIntrinsicsType(OptimizeIntrinsicsType::NONE);
  EXPECT_EQ(constant_subset.size(), camera.NumParameters());

  // Test that optimizing for focal length works correctly.
  constant_subset = camera.GetSubsetFromOptimizeIntrinsicsType(
      OptimizeIntrinsicsType::FOCAL_LENGTH);
  EXPECT_EQ(constant_subset.size(), camera.NumParameters() - 1);
  for (int i = 0; i < constant_subset.size(); i++) {
    EXPECT_NE(constant_subset[i], FOVCameraModel::FOCAL_LENGTH);
  }

  // Test that optimizing for principal_points works correctly.
  constant_subset = camera.GetSubsetFromOptimizeIntrinsicsType(
      OptimizeIntrinsicsType::PRINCIPAL_POINTS);
  EXPECT_EQ(constant_subset.size(), camera.NumParameters() - 2);
  for (int i = 0; i < constant_subset.size(); i++) {
    EXPECT_NE(constant_subset[i], FOVCameraModel::PRINCIPAL_POINT_X);
    EXPECT_NE(constant_subset[i], FOVCameraModel::PRINCIPAL_POINT_Y);
  }

  // Test that optimizing for aspect ratio works correctly.
  constant_subset = camera.GetSubsetFromOptimizeIntrinsicsType(
      OptimizeIntrinsicsType::ASPECT_RATIO);
  EXPECT_EQ(constant_subset.size(), camera.NumParameters() - 1);
  for (int i = 0; i < constant_subset.size(); i++) {
    EXPECT_NE(constant_subset[i], FOVCameraModel::ASPECT_RATIO);
  }

  // Test that optimizing for radial distortion works correctly.
  constant_subset = camera.GetSubsetFromOptimizeIntrinsicsType(
      OptimizeIntrinsicsType::RADIAL_DISTORTION);
  EXPECT_EQ(constant_subset.size(), camera.NumParameters() - 1);
  for (int i = 0; i < constant_subset.size(); i++) {
    EXPECT_NE(constant_subset[i], FOVCameraModel::RADIAL_DISTORTION_1);
  }

  // Test that optimizing for skew and tangential distortion does not optimize
  // any parameters.
  constant_subset =
      camera.GetSubsetFromOptimizeIntrinsicsType(OptimizeIntrinsicsType::SKEW);
  EXPECT_EQ(constant_subset.size(), camera.NumParameters());
  constant_subset = camera.GetSubsetFromOptimizeIntrinsicsType(
      OptimizeIntrinsicsType::TANGENTIAL_DISTORTION);
  EXPECT_EQ(constant_subset.size(), camera.NumParameters());
}

void ReprojectionTest(const FOVCameraModel& camera) {
  static const double kTolerance = 1e-5;
  const double kNormalizedTolerance = kTolerance / camera.FocalLength();
  static const int kImageWidth = 1200;
  static const int kImageHeight = 980;
  static const double kMinDepth = 2.0;
  static const double kMaxDepth = 25.0;

  // Ensure the image -> camera -> image transformation works.
  for (double x = 0.0; x < kImageWidth; x += 10.0) {
    for (double y = 0.0; y < kImageHeight; y += 10.0) {
      const Eigen::Vector2d pixel(x, y);
      // Get the normalized ray of that pixel.
      const Vector3d normalized_ray = camera.ImageToCameraCoordinates(pixel);

      // Test the reprojection at several depths.
      for (double depth = kMinDepth; depth < kMaxDepth; depth += 1.0) {
        // Convert it to a full 3D point in the camera coordinate system.
        const Vector3d point = normalized_ray * depth;
        const Vector2d reprojected_pixel =
            camera.CameraToImageCoordinates(point);

      // Expect the reprojection to be close.
      EXPECT_LT((pixel - reprojected_pixel).norm(), kTolerance)
          << "gt pixel: " << pixel.transpose()
          << "\nreprojected pixel: " << reprojected_pixel.transpose();
      }
    }
  }

  // Ensure the camera -> image -> camera transformation works.
  for (double x = -0.8; x < 0.8; x += 0.1) {
    for (double y = -0.8; y < 0.8; y += 0.1) {
      for (double depth = kMinDepth; depth < kMaxDepth; depth += 1.0) {
        const Eigen::Vector3d point(x, y, depth);
        const Vector2d pixel = camera.CameraToImageCoordinates(point);

        // Get the normalized ray of that pixel.
        const Vector3d normalized_ray = camera.ImageToCameraCoordinates(pixel);

        // Convert it to a full 3D point in the camera coordinate system.
        const Vector3d reprojected_point = normalized_ray * depth;

        // Expect the reprojection to be close.
        EXPECT_LT((point - reprojected_point).norm(), kNormalizedTolerance)
            << "gt pixel: " << point.transpose()
            << "\nreprojected pixel: " << reprojected_point.transpose();
      }
    }
  }
}

TEST(FOVCameraModel, ReprojectionNoDistortion) {
  static const double kPrincipalPoint[2] = {600.0, 400.0};
  static const double kFocalLength = 1200;
  FOVCameraModel camera;
  camera.SetFocalLength(kFocalLength);
  camera.SetPrincipalPoint(kPrincipalPoint[0], kPrincipalPoint[1]);
  camera.SetRadialDistortion(0);
  ReprojectionTest(camera);
}

TEST(FOVCameraModel, ReprojectionOneDistortionSmall) {
  static const double kPrincipalPoint[2] = {600.0, 400.0};
  static const double kFocalLength = 1200;
  FOVCameraModel camera;
  camera.SetFocalLength(kFocalLength);
  camera.SetPrincipalPoint(kPrincipalPoint[0], kPrincipalPoint[1]);

  camera.SetRadialDistortion(0.0001);
  ReprojectionTest(camera);
}

TEST(FOVCameraModel, ReprojectionOneDistortionMedium) {
  static const double kPrincipalPoint[2] = {600.0, 400.0};
  static const double kFocalLength = 1200;
  FOVCameraModel camera;
  camera.SetFocalLength(kFocalLength);
  camera.SetPrincipalPoint(kPrincipalPoint[0], kPrincipalPoint[1]);

  camera.SetRadialDistortion(0.001);
  ReprojectionTest(camera);
}

TEST(FOVCameraModel, ReprojectionOneDistortionLarge) {
  static const double kPrincipalPoint[2] = {600.0, 400.0};
  static const double kFocalLength = 1200;
  FOVCameraModel camera;
  camera.SetFocalLength(kFocalLength);
  camera.SetPrincipalPoint(kPrincipalPoint[0], kPrincipalPoint[1]);

  camera.SetRadialDistortion(0.1);
  ReprojectionTest(camera);
}

}  // namespace theia


#define BOOST_SPIRIT_USE_PHOENIX_V3
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/support_multi_pass.hpp>
#include <boost/spirit/include/classic_position_iterator.hpp>
#include <boost/phoenix/function/adapt_function.hpp>

namespace qi      = boost::spirit::qi;
namespace phx     = boost::phoenix;
namespace classic = boost::spirit::classic;
namespace ascii   = boost::spirit::ascii;

typedef std::vector<double> data_t;

///////// USING A FREE FUNCTION
//
template <typename Grammar, typename Range>
    double doStuff_(Grammar &grammar, Range pos_range)
{
    // for efficiency, cache adhoc grammar:
    static const qi::rule   <typename Range::iterator, double()> r_double = qi::double_;
    static const qi::grammar<typename Range::iterator, double()> g_double(r_double); // caching just the rule may be enough, actually

    double value = 0;
    qi::parse(pos_range.begin(), pos_range.end(), g_double, value);

    std::cout << "debug ('" << grammar.name() << "') at "
       << pos_range.begin().get_position().file   << ":"
       << pos_range.begin().get_position().line   << ":["
       << pos_range.begin().get_position().column << ".."
       << pos_range.end  ().get_position().column << "]\t"
       << "'" << std::string(pos_range.begin(),pos_range.end()) << "'\t = "
       << value
       << '\n';

    return value;
}

BOOST_PHOENIX_ADAPT_FUNCTION(double, doStuff, doStuff_, 2)

template <typename Iterator, typename Skipper>
struct parse_grammar : qi::grammar<Iterator, data_t(), Skipper>
{
    parse_grammar()
        : parse_grammar::base_type(start_p, "start_p")
    {
        using qi::raw;
        using qi::double_;
        using qi::_1;
        using qi::_val;
        using qi::eoi;
        using phx::push_back;

        value_p = raw [ double_ ] [ _val = doStuff(phx::ref(*this), _1) ];
        start_p = value_p % ',' > eoi;

        // // To use without the semantic action (more efficient):
        // start_p = double_ % ',' >> eoi;
    }

    qi::rule<Iterator, data_t::value_type(), Skipper> value_p;
    qi::rule<Iterator, data_t(), Skipper> start_p;
};

// implementation
data_t parse(std::istream& input, const std::string& filename)
{
    // iterate over stream input
    typedef std::istreambuf_iterator<char> base_iterator_type;
    base_iterator_type in_begin(input);

    // convert input iterator to forward iterator, usable by spirit parser
    typedef boost::spirit::multi_pass<base_iterator_type> forward_iterator_type;
    forward_iterator_type fwd_begin = boost::spirit::make_default_multi_pass(in_begin);
    forward_iterator_type fwd_end;

    // wrap forward iterator with position iterator, to record the position
    typedef classic::position_iterator2<forward_iterator_type> pos_iterator_type;
    pos_iterator_type position_begin(fwd_begin, fwd_end, filename);
    pos_iterator_type position_end;

    parse_grammar<pos_iterator_type, ascii::space_type> gram;

    data_t output;
    // parse
    try
    {
        if (!qi::phrase_parse(
                position_begin, position_end,  // iterators over input
                gram,                          // recognize list of doubles
                ascii::space,                  // comment skipper
                output)                        // <-- attribute reference
           )
        {
            std::cerr << "Parse failed at "
               << position_begin.get_position().file   << ":"
               << position_begin.get_position().line   << ":"
               << position_begin.get_position().column << "\n";
        }
    }
    catch(const qi::expectation_failure<pos_iterator_type>& e)
    {
        const classic::file_position_base<std::string>& pos = e.first.get_position();
        std::stringstream msg;
        msg << "parse error at file " << pos.file
            << " line "               << pos.line
            << " column "             << pos.column
            << "\n\t'"                << e.first.get_currentline()
            << "'\n\t "               << std::string(pos.column, ' ') << "^-- here";

        throw std::runtime_error(msg.str());
    }

    return output;
}

int main()
{
    std::istringstream iss(
            "1, -3.4 ,3.1415926\n"
            ",+inF,-NaN  ,\n"
            "2,-.4,4.14e7\n");

    data_t parsed = parse(iss, "<inline-test>");

    std::cout << "Done, parsed " << parsed.size() << " values ("
        << "min: " << *std::min_element(parsed.begin(), parsed.end()) << ", "
        << "max: " << *std::max_element(parsed.begin(), parsed.end()) << ")\n";
}


#include <iostream>
#include <esl/data/filesystem.hpp>

////////////////////////////////////////////////////////////////////////////////
#include <esl/computation/environment.hpp>
#include <esl/simulation/model.hpp>
#include <esl/economics/markets/quote.hpp>
#include <esl/law/jurisdictions.hpp>
#include <esl/economics/markets/walras/tatonnement.hpp>
#include <esl/economics/markets/walras/price_setter.hpp>
#include <esl/economics/markets/walras/quote_message.hpp>
#include <esl/economics/finance/bond.hpp>
#include <esl/data/file.hpp>
#include <esl/computation/thread_pool.hpp>

using namespace esl;
using namespace esl::computation;
using namespace esl::economics;
using namespace esl::economics::markets::walras;
using namespace esl::economics::finance;
using namespace esl::law;
using namespace esl::economics::currencies;
using namespace esl::simulation;
using namespace esl::simulation::parameter;

#include "traded_company.hpp"

////////////////////////////////////////////////////////////////////////////////

using std::string;
using std::to_string;
using std::pair;
using std::vector;
using std::map;
using std::shared_ptr;
using std::make_shared;
using std::tuple;
using std::get;
using std::make_tuple;
using std::dynamic_pointer_cast;
////////////////////////////////////////////////////////////////////////////////

#include "strategies/constant_demand.hpp"
#include "strategies/fundamental_value/dividend_discount.hpp"
#include "strategies/fundamental_value/mean_reverting_noise.hpp"
#include "strategies/technical/trend_follower.hpp"
#include <experiment/experiment_1_population_fluctuations.hpp>
#include <experiment/experiment_2_statistics.hpp>
#include <experiment/experiment_3_simplex.hpp>
#include <experiment/experiment_4_trajectories.hpp>
#include <experiment/experiment_5_flows.hpp>
#include <experiment/experiment_6_kelly.hpp>

////////////////////////////////////////////////////////////////////////////////

#include <boost/program_options.hpp>
using namespace boost::program_options;

////////////////////////////////////////////////////////////////////////////////

#if defined(WIN32) || defined(_WIN32) || defined(__WIN32) && !defined(__CYGWIN__)
#include <windows.h>
#endif

///
/// \param argument_count
/// \param arguments
/// \return
int main(int argument_count, char *arguments[])
{
    // on windows hosts, prefer to run simulations with below normal priority
    // because otherwise the host may become difficult to interact with
#if defined(WIN32) || defined(_WIN32) || (defined(__WIN32) && !defined(__CYGWIN__))
    SetPriorityClass(GetCurrentProcess(), BELOW_NORMAL_PRIORITY_CLASS);
#endif
    // set preferred floating point format
    std::cout.setf(std::ios::fixed, std::ios::floatfield);
    std::cout.setf(std::ios::showpoint);

    for(auto a = 0; a < argument_count; ++a) {
        LOG(notice) << arguments[a] << std::endl;
    }

    options_description description_("Allowed options");
    description_.add_options()
        ("help", "produce help message")
        ("experiment", value<std::string>()->default_value("experiment_1"), "choose experiment")
        ("reinvestment", value<double>()->default_value(3.), "reinvestment rate")
        ("ntv", value<double>()->default_value(0.2), "noise trader volatility")
        ("dpv", value<double>()->default_value(0.10), "dividend process volatility")
        ("stocks", value<unsigned int>()->default_value(1), "set the number of stocks")
        ;

    variables_map arguments_;
    store(parse_command_line(argument_count, arguments, description_), arguments_);

    notify(arguments_);

    if (arguments_.count("help")) {
        std::cout << description_ << std::endl;
        return 0;
    }

    unsigned int stocks_count_;
    if (arguments_.count("stocks")) {
        stocks_count_ = arguments_["stocks"].as<unsigned int>();
    }

    LOG(notice) << "stocks_count_ " << stocks_count_ << std::endl;

    std::string experiment_ = arguments_["experiment"].as<std::string>();
    std::transform(experiment_.begin(), experiment_.end(), experiment_.begin(),
                   [](unsigned char c){ return std::tolower(c); });

    if("experiment_1" == experiment_) {
        volatility_illustration();
    }else if("experiment_2" == experiment_) {
        experiment_2();
    } else if("experiment_3" == experiment_) {
        experiment_3(16);
    }else if("experiment_3_best" == experiment_){
        experiment_3_best(32);
    }else if("experiment_4" == experiment_){
        experiment_4();
    }else if("experiment_5" == experiment_){
        double reinvestment_rate = arguments_["reinvestment"].as<double>();
        experiment_5(arguments_["ntv"].as<double>(), arguments_["dpv"].as<double>(), reinvestment_rate);

    }else if("experiment_6" == experiment_){
        experiment_6_best(arguments_["ntv"].as<double>(), arguments_["dpv"].as<double>());

    }else if("volatility_illustration" == experiment_){
        volatility_illustration(1);
    }

    return 0;
}


// This file is part of CRAAM, a C++ library for solving plain
// and robust Markov decision processes.
//
// MIT License
//
// 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.

#pragma once

#include "craam/ImMDP.hpp"
#include "craam/Samples.hpp"
#include "craam/algorithms/values.hpp"
#include "craam/modeltools.hpp"
#include "craam/simulation.hpp"
#include "simulation_tests.hpp"

#include <rm/range.hpp>

#include <boost/functional/hash.hpp>
#include <cmath>
#include <iostream>
#include <iterator>

using namespace std;
using namespace craam;
using namespace craam::algorithms;
using namespace craam::impl;
using namespace util::lang;

/**
Creates a simple chain problem.
Actions:   0 - left
           1 - right
Optimal solution: Action 1, with value function:
    [1.1 gamma^2/(1-gamma), 1.1 gamma/(1-gamma), 1.1/(1-gamma)]

*/
MDP make_chain1() {
  MDP rmdp(3);

  add_transition(rmdp, 0, 1, 1, 1, 0);
  add_transition(rmdp, 1, 1, 2, 1, 0);
  add_transition(rmdp, 2, 1, 2, 1, 1.1);

  add_transition(rmdp, 0, 0, 0, 1, 0);
  add_transition(rmdp, 1, 0, 0, 1, 1);
  add_transition(rmdp, 2, 0, 1, 1, 1);

  return rmdp;
}

BOOST_AUTO_TEST_CASE(simple_construct_mdpi) {

  auto mdp = make_shared<MDP>();
  vector<long> observations({0, 0});
  Transition initial(vector<long>{0, 1}, vector<prec_t>{0.5, 0.5},
                     vector<prec_t>{0, 0});

  add_transition(*mdp, 0, 0, 1, 1.0, 1.0);
  add_transition(*mdp, 1, 0, 0, 1.0, 1.0);
  BOOST_CHECK_EQUAL(mdp->state_count(), 2);

  MDPI im(const_pointer_cast<const MDP>(mdp), observations, initial);

  MDPI im2(*mdp, observations, initial);

  // check that we really have a copy
  add_transition(*mdp, 1, 0, 2, 1.0, 1.0);

  BOOST_CHECK_EQUAL(mdp->state_count(), 3);
  BOOST_CHECK_EQUAL(im.get_mdp()->state_count(), 3);
  BOOST_CHECK_EQUAL(im2.get_mdp()->state_count(), 2);
}

BOOST_AUTO_TEST_CASE(simple_construct_mdpi_r) {

  auto mdp = make_shared<MDP>();
  vector<long> observations({0, 0});
  Transition initial(vector<long>{0, 1}, vector<prec_t>{0.5, 0.5},
                     vector<prec_t>{0, 0});

  add_transition(*mdp, 0, 0, 1, 1.0, 1.0);
  add_transition(*mdp, 1, 0, 0, 1.0, 2.0);

  MDPI_R imr(const_pointer_cast<const MDP>(mdp), observations, initial);

  // COPY ! so we can change the threshold
  auto rmdp = imr.get_robust_mdp();

  BOOST_CHECK_EQUAL(rmdp.state_count(), 1);
  BOOST_CHECK_EQUAL(rmdp[0].size(), 1);
  BOOST_CHECK_EQUAL(rmdp[0][0].outcome_count(), 2);

  vector<prec_t> iv(rmdp.state_count(), 0.0);

  auto so = rsolve_mpi(rmdp, 0.9, nats::optimistic_unbounded(), iv, indvec(0),
                       100, 0.0, 10, 0.0);
  BOOST_CHECK_CLOSE(so.valuefunction[0], 20, 1e-3);

  auto sr = rsolve_mpi(rmdp, 0.9, nats::robust_unbounded(), iv, indvec(0), 100,
                       0.0, 10, 0.0);
  BOOST_CHECK_CLOSE(sr.valuefunction[0], 10, 1e-3);
}

BOOST_AUTO_TEST_CASE(small_construct_mdpi_r) {

  auto mdp = make_shared<MDP>();
  vector<long> observations{0, 0, 1};

  Transition initial(vector<long>{0, 1, 2},
                     vector<prec_t>{1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0},
                     vector<prec_t>{0, 0, 0});

  // action 0
  add_transition(*mdp, 0, 0, 0, 0.5, 1.0);
  add_transition(*mdp, 0, 0, 1, 0.5, 1.0);

  add_transition(*mdp, 1, 0, 0, 0.5, 2.0);
  add_transition(*mdp, 1, 0, 1, 0.5, 2.0);

  add_transition(*mdp, 2, 0, 2, 1.0, 1.2);

  // action 1
  add_transition(*mdp, 0, 1, 2, 1.0, 1.2);
  add_transition(*mdp, 1, 1, 2, 1.0, 1.2);

  BOOST_TEST_CHECKPOINT("Constructing MDPI_R.");
  MDPI_R imr(const_pointer_cast<const MDP>(mdp), observations, initial);

  // Copy to change threshold
  auto rmdp = imr.get_robust_mdp();

  BOOST_TEST_CHECKPOINT("Checking MDP properties.");
  BOOST_CHECK_EQUAL(rmdp.state_count(), 2);
  BOOST_CHECK_EQUAL(rmdp[0].size(), 2);
  BOOST_CHECK_EQUAL(rmdp[1].size(), 1);
  BOOST_CHECK_EQUAL(rmdp[0][0].outcome_count(), 2);
  BOOST_CHECK_EQUAL(rmdp[0][1].outcome_count(), 2);
  BOOST_CHECK_EQUAL(rmdp[1][0].outcome_count(), 1);

  vector<prec_t> iv(rmdp.state_count(), 0.0);

  vector<prec_t> target_v_opt{20.0, 12.0};
  vector<prec_t> target_v_rob{12.0, 12.0};

  BOOST_TEST_CHECKPOINT("Solving MDPO");
  auto so = rsolve_mpi(rmdp, 0.9, nats::optimistic_unbounded(), iv, indvec(0),
                       100, 0.0, 10, 0.0);
  CHECK_CLOSE_COLLECTION(so.valuefunction, target_v_opt, 1e-3);

  auto sr = rsolve_mpi(rmdp, 0.9, nats::robust_unbounded(), iv, indvec(0), 100,
                       0.0, 10, 0.0);
  CHECK_CLOSE_COLLECTION(sr.valuefunction, target_v_rob, 1e-3);
}

BOOST_AUTO_TEST_CASE(small_reweighted_solution) {

  auto mdp = make_shared<MDP>();
  vector<long> observations({0, 0, 1});
  Transition initial(vector<long>{0, 1, 2},
                     vector<prec_t>{1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0},
                     vector<prec_t>{0, 0, 0});

  // action 0
  add_transition(*mdp, 0, 0, 0, 0.5, 1.0);
  add_transition(*mdp, 0, 0, 1, 0.5, 1.0);

  add_transition(*mdp, 1, 0, 0, 0.5, 2.0);
  add_transition(*mdp, 1, 0, 1, 0.5, 2.0);

  add_transition(*mdp, 2, 0, 2, 1.0, 1.2);

  // action 1
  add_transition(*mdp, 0, 1, 2, 1.0, 1.2);
  add_transition(*mdp, 1, 1, 2, 1.0, 1.2);

  BOOST_TEST_CHECKPOINT("Constructing MDPI_R.");
  MDPI_R imr(const_pointer_cast<const MDP>(mdp), observations, initial);

  BOOST_TEST_CHECKPOINT("Solving MDPI_R.");
  auto pol = imr.solve_reweighted(10, 0.9);

  indvec polvec{0, 0};
  BOOST_CHECK_EQUAL_COLLECTIONS(pol.begin(), pol.end(), polvec.begin(),
                                polvec.end());

  auto pol2 = imr.solve_robust(10, 0.0, 0.9);
  BOOST_CHECK_EQUAL_COLLECTIONS(pol2.begin(), pol2.end(), polvec.begin(),
                                polvec.end());

  // auto retval = imr.total_return(pol, 0.99);
  // cout << "Return: " << retval << endl;

  // ostream_iterator<prec_t> output(cout, ", ");
  // copy(pol.begin(), pol.end(), output);
}

BOOST_AUTO_TEST_CASE(simple_mdpo_save_load_save_load) {
  MDP rmdp1 = make_chain1();

  Transition initial(indvec{0, 1, 2}, numvec{1.0 / 3.0, 1.0 / 3.0, 1 / 3.0});
  indvec state2obs{0, 0, 1};

  MDPI mdpi1(rmdp1, state2obs, initial);

  stringstream store1, store2, store3;

  mdpi1.to_csv(store1, store2, store3);
  store1.seekg(0);
  store2.seekg(0);
  store3.seekg(0);

  auto string11 = store1.str();
  auto string12 = store2.str();
  auto string13 = store3.str();

  auto mdpi2 = MDPI::from_csv(store1, store2, store3);

  stringstream store21, store22, store23;

  mdpi2->to_csv(store21, store22, store23);

  auto string21 = store21.str();
  auto string22 = store22.str();
  auto string23 = store23.str();

  BOOST_CHECK_EQUAL(string11, string21);
  BOOST_CHECK_EQUAL(string12, string22);
  BOOST_CHECK_EQUAL(string13, string23);
}

BOOST_AUTO_TEST_CASE(simple_mdpor_save_load_save_load) {
  MDP rmdp1 = make_chain1();

  Transition initial(indvec{0, 1, 2}, numvec{1.0 / 3.0, 1.0 / 3.0, 1 / 3.0});
  indvec state2obs{0, 0, 1};

  MDPI_R mdpi1(rmdp1, state2obs, initial);

  stringstream store1, store2, store3;

  mdpi1.to_csv(store1, store2, store3);
  store1.seekg(0);
  store2.seekg(0);
  store3.seekg(0);

  auto string11 = store1.str();
  auto string12 = store2.str();
  auto string13 = store3.str();

  auto mdpi2 = MDPI_R::from_csv(store1, store2, store3);

  stringstream store21, store22, store23;

  mdpi2->to_csv(store21, store22, store23);

  auto string21 = store21.str();
  auto string22 = store22.str();
  auto string23 = store23.str();

  BOOST_CHECK_EQUAL(string11, string21);
  BOOST_CHECK_EQUAL(string12, string22);
  BOOST_CHECK_EQUAL(string13, string23);
}

using namespace craam::msen;

/*
template<class T>
void print_vector(vector<T> vec){
    for(auto p : vec){
        cout << p << " ";
    }
}*/

#if __cplusplus >= 201703L

BOOST_AUTO_TEST_CASE(implementable_from_samples) {
  const int terminal_state = 10;

  CounterTerminal sim(0.9, 0, terminal_state, 1);
  RandomPolicy<CounterTerminal> random_pol(sim, 1);

  auto samples = make_samples<CounterTerminal>();
  simulate(sim, samples, random_pol, 50, 50);
  simulate(sim, samples, [](int) { return 1; }, 10, 20);
  simulate(sim, samples, [](int) { return -1; }, 10, 20);

  SampleDiscretizerSI<typename CounterTerminal::State,
                      typename CounterTerminal::Action>
      sd;
  // initialize action values
  sd.add_action(-1);
  sd.add_action(+1);
  // initialize state values
  for (auto i : range(-terminal_state, terminal_state))
    sd.add_state(i);

  sd.add_samples(samples);

  BOOST_CHECK_EQUAL(samples.get_initial().size(),
                    sd.get_discrete()->get_initial().size());
  BOOST_CHECK_EQUAL(samples.size(), sd.get_discrete()->size());

  SampledMDP smdp;
  smdp.add_samples(*sd.get_discrete());
  auto mdp = smdp.get_mdp();
  auto initial = smdp.get_initial();

  auto sol = solve_mpi(*mdp, 0.9);

  // cout << "Optimal policy: " << endl; print_vector(sol.policy); cout << endl;

  BOOST_CHECK_CLOSE(sol.total_return(initial), 51.313973553, 1e-3);

  // define observations
  indvec observations(mdp->state_count(), -1);
  size_t last_obs(0), inobs(0);
  for (auto i : range(size_t(0), mdp->state_count())) {
    // check if this is a terminal state
    if ((*mdp)[i].empty() || inobs >= 2) {
      if (inobs > 0) {
        inobs = 0;
        last_obs++;
      }
      observations[i] = last_obs++;
    } else {
      observations[i] = last_obs;
      inobs++;
    }
    // cout << " " << observations[i] ;
  }
  // cout << endl;

  MDPI_R mdpi(mdp, observations, initial);
  auto randompolicy = mdpi.random_policy(25);

  auto isol = mdpi.solve_reweighted(0, 0.9, randompolicy);
  BOOST_CHECK_EQUAL_COLLECTIONS(randompolicy.begin(), randompolicy.end(),
                                isol.begin(), isol.end());
  isol = mdpi.solve_robust(0, 0.0, 0.9, randompolicy);
  BOOST_CHECK_EQUAL_COLLECTIONS(randompolicy.begin(), randompolicy.end(),
                                isol.begin(), isol.end());

  isol = mdpi.solve_reweighted(1, 0.9, randompolicy);

  auto sol_impl = solve_mpi(*mdp, 0.9, numvec(0), mdpi.obspol2statepol(isol));

  BOOST_CHECK_CLOSE(sol_impl.total_return(initial), 51.3135, 0.1);
  BOOST_CHECK_CLOSE(mdpi.total_return(0.9), 51.3135, 0.1);

  isol = mdpi.solve_robust(1, 0.0, 0.9, randompolicy);
  sol_impl = solve_mpi(*mdp, 0.9, numvec(0), mdpi.obspol2statepol(isol));

  BOOST_CHECK_CLOSE(sol_impl.total_return(initial), 51.3135, 0.1);
  BOOST_CHECK_CLOSE(mdpi.total_return(0.9), 51.3135, 0.1);
}

#endif //  __cplusplus >= 201703L

BOOST_AUTO_TEST_CASE(test_return_of_implementable) {
  // test return with different initial states

  const prec_t gamma = 0.99;

  MDP mdp = make_chain1();
  indvec observations = {0, 0, 0};
  Transition initial1(numvec({1.0, 0.0, 0.0})),
      initial2(numvec({0.0, 1.0, 0.0})), initial3(numvec({0.0, 0.0, 1.0}));

  MDPI mdpi1(mdp, observations, initial1);
  BOOST_CHECK_CLOSE(mdpi1.total_return(gamma, 1e-5),
                    1.1 * pow(gamma, 2) / (1 - gamma), 1e-3);
  MDPI mdpi2(mdp, observations, initial2);
  BOOST_CHECK_CLOSE(mdpi2.total_return(gamma, 1e-5),
                    1.1 * pow(gamma, 1) / (1 - gamma), 1e-3);
  MDPI mdpi3(mdp, observations, initial3);
  BOOST_CHECK_CLOSE(mdpi3.total_return(gamma, 1e-5),
                    1.1 * pow(gamma, 0) / (1 - gamma), 1e-3);
}

// TODO: make sure there is a test that checks that the return of the
// implementable policy with the true weights has the same return as the true
// MDP.


#include <ancse/cfl_condition.hpp>

#include <Eigen/Dense>

/// make CFL condition for FVM
std::shared_ptr<CFLCondition>
make_cfl_condition(const Grid &grid,
                   const std::shared_ptr<Model> &model,
                   double cfl_number) {
    // implement this 'factory' for your CFL condition.
    return nullptr;
}

/// make CFL condition for DG
std::shared_ptr<CFLCondition>
make_cfl_condition(const Grid &grid,
                   const std::shared_ptr<Model> &model,
                   const DGHandler &dg_handler,
                   double cfl_number) {
    // implement this 'factory' for your CFL condition.
    return nullptr;
}


/*
 * incidence_list_helpers.h
 * Author: Aven Bross
 *
 * Simple helper functions for working with incidence lists
 */

#ifndef __INCIDENCE_LIST_HELPERS_HPP
#define __INCIDENCE_LIST_HELPERS_HPP

// Basic graph headers
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>

template<
        typename graph_t, typename vertex_t
            = typename boost::graph_traits<graph_t>::vertex_descriptor,
        typename edge_t = typename boost::graph_traits<graph_t>::edge_descriptor
    >
inline vertex_t get_incident_vertex(
        vertex_t vertex, edge_t edge, const graph_t & graph
    )
{
    vertex_t neighbor = boost::source(edge, graph);
    if(neighbor == vertex)
        neighbor = boost::target(edge, graph);
    return neighbor;
}

template<
        typename graph_t, typename planar_embedding_t, typename vertex_t
            = typename boost::graph_traits<graph_t>::vertex_descriptor,
        typename edge_iterator_t = typename boost
            ::property_traits<planar_embedding_t>::value_type::const_iterator
    >
inline edge_iterator_t find_edge_iterator(
        vertex_t vertex, vertex_t target,
        const planar_embedding_t & planar_embedding, const graph_t & graph
    )
{
    for(edge_iterator_t edge_iter = planar_embedding[vertex].begin();
            edge_iter != planar_embedding[vertex].end(); ++edge_iter
        )
    {
        vertex_t neighbor = get_incident_vertex(vertex, *edge_iter, graph);
        if(neighbor == target) {
            return edge_iter;
        }
    }
    return planar_embedding[vertex].end();
}

template<
        typename graph_t, typename planar_embedding_t, typename vertex_t
            = typename boost::graph_traits<graph_t>::vertex_descriptor,
        typename edge_iterator_t = typename boost
            ::property_traits<planar_embedding_t>::value_type::const_iterator
    >
inline edge_iterator_t find_edge_iterator_restricted(
        vertex_t vertex, vertex_t target, edge_iterator_t begin,
        edge_iterator_t end, const planar_embedding_t & planar_embedding,
        const graph_t & graph
    )
{
    for(edge_iterator_t edge_iter = begin; edge_iter != end; ++edge_iter) {
        if(edge_iter == planar_embedding[vertex].end())
            edge_iter = planar_embedding[vertex].begin();

        vertex_t neighbor = get_incident_vertex(vertex, *edge_iter, graph);

        if(neighbor == target) {
            return edge_iter;
        }
    }
    return planar_embedding[vertex].end();
}

template<
        typename graph_t, typename augmented_embedding_t, typename vertex_t
            = typename boost::graph_traits<graph_t>::vertex_descriptor,
        typename neighbor_iterator_t = typename boost
            ::property_traits<augmented_embedding_t>::value_type::const_iterator
    >
inline neighbor_iterator_t find_neighbor_iterator(
        vertex_t v, vertex_t u,
        const augmented_embedding_t & augmented_embedding, const graph_t & graph
    )
{
    for(auto neighbor_iter = augmented_embedding[v].begin();
            neighbor_iter != augmented_embedding[v].end(); ++neighbor_iter
        )
    {
        vertex_t n = neighbor_iter -> vertex;
        if(n == u) {
            return neighbor_iter;
        }
    }
    return augmented_embedding[v].end();
}

template<
        typename graph_t, typename augmented_embedding_t, typename vertex_t
            = typename boost::graph_traits<graph_t>::vertex_descriptor,
        typename neighbor_iterator_t = typename boost
            ::property_traits<augmented_embedding_t>::value_type::const_iterator
    >
inline neighbor_iterator_t find_neighbor_iterator_restricted(
        vertex_t v, vertex_t u, neighbor_iterator_t begin,
        neighbor_iterator_t end,
        const augmented_embedding_t & augmented_embedding, const graph_t & graph
    )
{
    for(neighbor_iterator_t neighbor_iter = begin; neighbor_iter != end;
            ++neighbor_iter
        )
    {
        if(neighbor_iter == augmented_embedding[v].end())
            neighbor_iter = augmented_embedding[v].begin();

        vertex_t n = neighbor_iter -> vertex;
        if(n == u) {
            return neighbor_iter;
        }
    }
    return augmented_embedding[v].end();
}

template<
        typename vertex_t, typename neighbor_range_map_t,
        typename embedding_t
    >
inline void remove_first_neighbor(
        vertex_t v, neighbor_range_map_t & neighbor_range_map,
        const embedding_t & embedding
    )
{
    if(++neighbor_range_map[v].first == embedding[v].end()) {
        neighbor_range_map[v].first = embedding[v].begin();
    }
}

template<
        typename vertex_t, typename neighbor_range_map_t,
        typename embedding_t
    >
inline void remove_last_neighbor(
        vertex_t v, neighbor_range_map_t & neighbor_range_map,
        const embedding_t & embedding
    )
{
    if(neighbor_range_map[v].second == embedding[v].begin()) {
        neighbor_range_map[v].second = embedding[v].end();
    }
    --neighbor_range_map[v].second;
}

template<
        typename vertex_t, typename embedding_t,
        typename neighbor_iterator_t
            = typename boost::property_traits<embedding_t>::value_type
                ::const_iterator,
        typename neighbor_range_map_t, typename neighbor_range_t
            = typename boost::property_traits<neighbor_range_map_t>::value_type
    >
inline std::pair<neighbor_range_t, neighbor_range_t> split_neighbor_range(
        vertex_t v, neighbor_iterator_t mid_iter,
        neighbor_range_map_t & neighbor_range_map,
        embedding_t & embedding
    )
{
    neighbor_range_t range_0, range_1;
    range_0.first = neighbor_range_map[v].first;
    range_0.second = mid_iter;

    if(++mid_iter == embedding[v].end())
        mid_iter = embedding[v].begin();

    range_1.first = mid_iter;
    range_1.second = neighbor_range_map[v].second;

    return std::pair<neighbor_range_t, neighbor_range_t>(range_0, range_1);
}

template<
        typename vertex_t, typename neighbor_range_map_t,
        typename embedding_t, typename neighbor_iterator_t
            = typename boost::property_traits<embedding_t>::value_type
                ::const_iterator
    >
inline void initialize_neighbor_range(
        vertex_t v, neighbor_iterator_t start_iter,
        neighbor_range_map_t & neighbor_range_map,
        const embedding_t & embedding
    )
{
    neighbor_range_map[v].first = start_iter;
    neighbor_range_map[v].second = start_iter;
    remove_last_neighbor(v, neighbor_range_map, embedding);
}

#endif


/*
ID: septicmk
LANG: C++
TASK:
*/
/*
 * modified by Yuan Lufeng in 20151113
 * function:data format is changed to Collection.
 */

#include "MLalgorithm.h"
#include <vector>
#include <boost/serialization/vector.hpp>

void MLalgorithm::Init(size_t nrow, size_t ncol){
    this->nrow = nrow;
    this->ncol = ncol;
    data.resize(nrow * ncol);
    std::fill(data.begin(), data.end(), 0.0f);
}

void MLalgorithm::add (std::vector<double> record, size_t i){
    for (size_t j = 0; j < record.size(); ++j){
        data[ i*ncol + j ] += record[j];
    }
}

std::vector<double> MLalgorithm::get_value(size_t i){
    std::vector<double> ret;
    for (size_t j = 0; j < ncol; ++j){
        ret.push_back(data[i*ncol + j]);
    }
    return ret;
}


//==============================================================================
//         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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 polynom toolbox - reduce/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of polynom components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 06/03/2011
///
#include <nt2/include/functions/reduce.hpp>
#include <nt2/include/functions/isequal.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/table.hpp>

#include <boost/array.hpp>

NT2_TEST_CASE_TPL ( reduce_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::reduce;
  using nt2::tag::reduce_;
  nt2::table<T> a =  nt2::_(T(1), T(4));
  nt2::table<T> b =  nt2::_(T(0), T(0), T(3));
  nt2::table<T> c(nt2::of_size(1, 0));
  NT2_DISPLAY(a);
  NT2_DISPLAY(reduce(a));
  NT2_DISPLAY(b);
  NT2_DISPLAY(reduce(b));
  NT2_DISPLAY(reduce(a, nt2::_));
  NT2_DISPLAY(reduce(a,nt2::Eps<T>()));
  NT2_TEST(nt2::isequal(a, reduce(a)));
  NT2_TEST(nt2::isequal(a, reduce(a, nt2::Eps<T>())));
  NT2_TEST(nt2::isequal(a, reduce(a, nt2::_)));
  NT2_TEST(nt2::isequal(c, reduce(b)));

} // end of test for floating_


//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//  Adaptation to Boost of the libcxx
//  Copyright 2010 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#include <boost/chrono/chrono.hpp>
#include <boost/type_traits/is_same.hpp>

#if !defined(BOOST_NO_CXX11_STATIC_ASSERT)
#define NOTHING ""
#endif

template <class D1, class D2, class De>
void
test()
{
    typedef typename boost::common_type<D1, D2>::type Dc;
    BOOST_CHRONO_STATIC_ASSERT((boost::is_same<Dc, De>::value), NOTHING, (D1, D2, Dc, De));
}

void testall()
{
    test<boost::chrono::duration<int, boost::ratio<1, 100> >,
         boost::chrono::duration<long, boost::ratio<1, 1000> >,
         boost::chrono::duration<long, boost::ratio<1, 1000> > >();
    test<boost::chrono::duration<long, boost::ratio<1, 100> >,
         boost::chrono::duration<int, boost::ratio<1, 1000> >,
         boost::chrono::duration<long, boost::ratio<1, 1000> > >();
    test<boost::chrono::duration<char, boost::ratio<1, 30> >,
         boost::chrono::duration<short, boost::ratio<1, 1000> >,
         boost::chrono::duration<int, boost::ratio<1, 3000> > >();
    test<boost::chrono::duration<double, boost::ratio<21, 1> >,
         boost::chrono::duration<short, boost::ratio<15, 1> >,
         boost::chrono::duration<double, boost::ratio<3, 1> > >();
}


/* Copyright 2003-2015 Joaquin M Lopez Munoz.
 * 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://www.boost.org/libs/multi_index for library home page.
 */

#ifndef BOOST_MULTI_INDEX_DETAIL_INDEX_MATCHER_HPP
#define BOOST_MULTI_INDEX_DETAIL_INDEX_MATCHER_HPP

#if defined(_MSC_VER)
#pragma once
#endif

#include <algorithm>
#include <boost/config.hpp> /* keep it first to prevent nasty warns in MSVC */
#include <boost/multi_index/detail/auto_space.hpp>
#include <boost/multi_index/detail/raw_ptr.hpp>
#include <boost/noncopyable.hpp>
#include <cstddef>
#include <functional>

namespace boost
{

namespace multi_index
{

namespace detail
{

/* index_matcher compares a sequence of elements against a
 * base sequence, identifying those elements that belong to the
 * longest subsequence which is ordered with respect to the base.
 * For instance, if the base sequence is:
 *
 *   0 1 2 3 4 5 6 7 8 9
 *
 * and the compared sequence (not necesarilly the same length):
 *
 *   1 4 2 3 0 7 8 9
 *
 * the elements of the longest ordered subsequence are:
 *
 *   1 2 3 7 8 9
 *
 * The algorithm for obtaining such a subsequence is called
 * Patience Sorting, described in ch. 1 of:
 *   Aldous, D., Diaconis, P.: "Longest increasing subsequences: from
 *   patience sorting to the Baik-Deift-Johansson Theorem", Bulletin
 *   of the American Mathematical Society, vol. 36, no 4, pp. 413-432,
 *   July 1999.
 *   http://www.ams.org/bull/1999-36-04/S0273-0979-99-00796-X/
 *   S0273-0979-99-00796-X.pdf
 *
 * This implementation is not fully generic since it assumes that
 * the sequences given are pointed to by index iterators (having a
 * get_node() memfun.)
 */

namespace index_matcher
{

/* The algorithm stores the nodes of the base sequence and a number
 * of "piles" that are dynamically updated during the calculation
 * stage. From a logical point of view, nodes form an independent
 * sequence from piles. They are stored together so as to minimize
 * allocated memory.
 */

struct entry
{
    entry(void* node_, std::size_t pos_ = 0) : node(node_), pos(pos_)
    {
    }

    /* node stuff */

    void* node;
    std::size_t pos;
    entry* previous;
    bool ordered;

    struct less_by_node
    {
        bool operator()(const entry& x, const entry& y) const
        {
            return std::less<void*>()(x.node, y.node);
        }
    };

    /* pile stuff */

    std::size_t pile_top;
    entry* pile_top_entry;

    struct less_by_pile_top
    {
        bool operator()(const entry& x, const entry& y) const
        {
            return x.pile_top < y.pile_top;
        }
    };
};

/* common code operating on void *'s */

template <typename Allocator>
class algorithm_base : private noncopyable
{
  protected:
    algorithm_base(const Allocator& al, std::size_t size) :
        spc(al, size), size_(size), n_(0), sorted(false)
    {
    }

    void add(void* node)
    {
        entries()[n_] = entry(node, n_);
        ++n_;
    }

    void begin_algorithm() const
    {
        if (!sorted)
        {
            std::sort(entries(), entries() + size_, entry::less_by_node());
            sorted = true;
        }
        num_piles = 0;
    }

    void add_node_to_algorithm(void* node) const
    {
        entry* ent =
            std::lower_bound(entries(), entries() + size_, entry(node),
                             entry::less_by_node()); /* localize entry */
        ent->ordered = false;
        std::size_t n = ent->pos; /* get its position */

        entry dummy(0);
        dummy.pile_top = n;

        entry* pile_ent =    /* find the first available pile */
            std::lower_bound(/* to stack the entry            */
                             entries(), entries() + num_piles, dummy,
                             entry::less_by_pile_top());

        pile_ent->pile_top = n; /* stack the entry */
        pile_ent->pile_top_entry = ent;

        /* if not the first pile, link entry to top of the preceding pile */
        if (pile_ent > &entries()[0])
        {
            ent->previous = (pile_ent - 1)->pile_top_entry;
        }

        if (pile_ent == &entries()[num_piles])
        { /* new pile? */
            ++num_piles;
        }
    }

    void finish_algorithm() const
    {
        if (num_piles > 0)
        {
            /* Mark those elements which are in their correct position, i.e.
             * those belonging to the longest increasing subsequence. These are
             * those elements linked from the top of the last pile.
             */

            entry* ent = entries()[num_piles - 1].pile_top_entry;
            for (std::size_t n = num_piles; n--;)
            {
                ent->ordered = true;
                ent = ent->previous;
            }
        }
    }

    bool is_ordered(void* node) const
    {
        return std::lower_bound(entries(), entries() + size_, entry(node),
                                entry::less_by_node())
            ->ordered;
    }

  private:
    entry* entries() const
    {
        return raw_ptr<entry*>(spc.data());
    }

    auto_space<entry, Allocator> spc;
    std::size_t size_;
    std::size_t n_;
    mutable bool sorted;
    mutable std::size_t num_piles;
};

/* The algorithm has three phases:
 *   - Initialization, during which the nodes of the base sequence are added.
 *   - Execution.
 *   - Results querying, through the is_ordered memfun.
 */

template <typename Node, typename Allocator>
class algorithm : private algorithm_base<Allocator>
{
    typedef algorithm_base<Allocator> super;

  public:
    algorithm(const Allocator& al, std::size_t size) : super(al, size)
    {
    }

    void add(Node* node)
    {
        super::add(node);
    }

    template <typename IndexIterator>
    void execute(IndexIterator first, IndexIterator last) const
    {
        super::begin_algorithm();

        for (IndexIterator it = first; it != last; ++it)
        {
            add_node_to_algorithm(get_node(it));
        }

        super::finish_algorithm();
    }

    bool is_ordered(Node* node) const
    {
        return super::is_ordered(node);
    }

  private:
    void add_node_to_algorithm(Node* node) const
    {
        super::add_node_to_algorithm(node);
    }

    template <typename IndexIterator>
    static Node* get_node(IndexIterator it)
    {
        return static_cast<Node*>(it.get_node());
    }
};

} // namespace index_matcher

} // namespace detail

} /* namespace multi_index */

} /* namespace boost */

#endif


#pragma once
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include "../DerivedPtrHolder.hh"
#include "Utility.hh"        // assumption: TMesh is derived from TMeshBase and Utility<TMeshBase, TMesh>

namespace kt84 {

template <class TMeshBase, class TMesh>
struct SurfaceTopology : public DerivedPtrHolder<TMesh, SurfaceTopology<TMeshBase, TMesh>> {
    struct Data {
        int num_connected_components;
        int num_boundary_loops;
        Data()
            : num_connected_components()
            , num_boundary_loops()
        {}
        bool is_closed() const { return num_boundary_loops == 0; }
    } surfaceTopology;

    void surfaceTopology_compute() {
        TMesh* mesh = DerivedPtrHolder<TMesh, SurfaceTopology<TMeshBase, TMesh>>::derived_ptr;

        // count connected components
        {
            boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> graph;
            for (auto f : mesh->faces())
                boost::add_vertex(graph);
            for (auto e : mesh->edges()) {
                if (mesh->is_boundary(e)) continue;
                auto h0h1 = mesh->util_edge_to_halfedge_pair(e);
                auto f0 = mesh->face_handle(h0h1.first );
                auto f1 = mesh->face_handle(h0h1.second);
                boost::add_edge(f0.idx(), f1.idx(), graph);
            }
            std::vector<int> f_component_id(mesh->n_faces());
            surfaceTopology.num_connected_components = boost::connected_components(graph, &f_component_id[0]);
        }

        // count boundary loops
        {
            boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> graph;
            for (auto v : mesh->vertices()) {
                if (!mesh->is_boundary(v)) continue;
                auto h0 = mesh->halfedge_handle(v);
                auto h1 = mesh->prev_halfedge_handle(h0);
                auto e0 = mesh->edge_handle(h0);
                auto e1 = mesh->edge_handle(h1);
                boost::add_edge(e0.idx(), e1.idx(), graph);
            }
            std::vector<int> e_component_id(mesh->n_edges());
            surfaceTopology.num_boundary_loops = boost::connected_components(graph, &e_component_id[0]);
        }
    }
};

}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file ored/configuration/yieldcurveconfig.hpp
    \brief Yield curve configuration classes
    \ingroup configuration
*/

#pragma once

#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <boost/shared_ptr.hpp>
#include <map>
#include <ored/configuration/curveconfig.hpp>
#include <ored/utilities/xmlutils.hpp>
#include <ql/patterns/visitor.hpp>
#include <ql/types.hpp>
#include <set>

namespace ore {
namespace data {
using std::string;
using std::vector;
using std::set;
using std::pair;
using std::map;
using boost::optional;
using ore::data::XMLNode;
using QuantLib::AcyclicVisitor;
using QuantLib::Real;

//! Base class for yield curve segments.
/*!
  \ingroup configuration
*/
class YieldCurveSegment : public XMLSerializable {
public:
    //! supported segment types
    enum class Type {
        Zero,
        ZeroSpread,
        Discount,
        Deposit,
        FRA,
        Future,
        OIS,
        Swap,
        AverageOIS,
        TenorBasis,
        TenorBasisTwo,
        BMABasis,
        FXForward,
        CrossCcyBasis,
        CrossCcyFixFloat,
        DiscountRatio
    };
    //! Default destructor
    virtual ~YieldCurveSegment() {}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node) = 0;
    virtual XMLNode* toXML(XMLDocument& doc) = 0;
    //@}

    //! \name Inspectors
    //@{
    Type type() const { return type_; }
    // TODO: why typeID?
    const string& typeID() const { return typeID_; }
    const string& conventionsID() const { return conventionsID_; }
    const vector<pair<string, bool>>& quotes() const { return quotes_; }
    //@}

    //! \name Visitability
    //@{
    virtual void accept(AcyclicVisitor&);
    //@}

protected:
    //! \name Constructors
    //@{
    //! Default constructor
    YieldCurveSegment() {}
    //! Detailed constructor - assumes all quotes are mandatory
    YieldCurveSegment(const string& typeID, const string& conventionsID, const vector<string>& quotes);
    //@}

    //! Quote and optional flag pair
    vector<pair<string, bool>> quotes_;

    //! Utility to build a quote, optional flag defaults to false
    pair<string, bool> quote(const string& name, bool opt = false) { return make_pair(name, opt); }

    //! Utility method to read quotes from XML
    void loadQuotesFromXML(XMLNode* node);
    //! Utility method to write quotes to XML
    XMLNode* writeQuotesToXML(XMLDocument& doc);

private:
    // TODO: why type and typeID?
    Type type_;
    string typeID_;
    string conventionsID_;
};

//! Direct yield curve segment
/*!
  A direct yield curve segment is used when the segments is entirely defined by
  a set of quotes that are passed to the constructor.

  \ingroup configuration
*/
class DirectYieldCurveSegment : public YieldCurveSegment {
public:
    //! \name Constructors/Destructors
    //@{
    //! Default constructor
    DirectYieldCurveSegment() {}
    //! Detailed constructor
    DirectYieldCurveSegment(const string& typeID, const string& conventionsID, const vector<string>& quotes);
    //! Default destructor
    virtual ~DirectYieldCurveSegment() {}
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}

    //! \name Visitability
    //@{
    virtual void accept(AcyclicVisitor&);
    //@}
};

//! Simple yield curve segment
/*!
  A simple yield curve segment is used when the curve segment is determined by
  a set of quotes and a projection curve.

  \ingroup configuration
*/
class SimpleYieldCurveSegment : public YieldCurveSegment {
public:
    //! \name Constructors/Destructors
    //@{
    //! Default constructor
    SimpleYieldCurveSegment() {}
    //! Detailed constructor
    SimpleYieldCurveSegment(const string& typeID, const string& conventionsID, const vector<string>& quotes,
                            const string& projectionCurveID = string());
    //! Default destructor
    virtual ~SimpleYieldCurveSegment() {}
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}

    //! \name Inspectors
    //@{
    const string& projectionCurveID() const { return projectionCurveID_; }
    //@}

    //! \name Visitability
    //@{
    virtual void accept(AcyclicVisitor&);
    //@}

private:
    string projectionCurveID_;
};

//! Avergae OIS yield curve segment
/*!
  The average OIS yield curve segment is used e.g. for USD OIS curve building where
  the curve segment is determined by  a set of composite quotes and a projection curve.
  The composite quote is  represented here as a pair of quote strings, a tenor basis spread
  and an interest rate swap quote.

  \ingroup configuration
*/
class AverageOISYieldCurveSegment : public YieldCurveSegment {
public:
    //! \name Constructors/Destructors
    //@{
    //! Default constructor
    AverageOISYieldCurveSegment() {}
    //! Detailec constructor
    AverageOISYieldCurveSegment(const string& typeID, const string& conventionsID, const vector<string>& quotes,
                                const string& projectionCurveID);
    //! Default destructor
    virtual ~AverageOISYieldCurveSegment() {}
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}

    //! \name Inspectors
    //@{
    const string& projectionCurveID() const { return projectionCurveID_; }
    //@}

    //! \name Visitability
    //@{
    virtual void accept(AcyclicVisitor&);
    //@}

private:
    string projectionCurveID_;
};

//! Tenor Basis yield curve segment
/*!
  Yield curve building from tenor basis swap quotes requires a set of tenor
  basis spread quotes and the projection curve for either the shorter or the longer tenor
  which acts as the reference curve.

  \ingroup configuration
*/
class TenorBasisYieldCurveSegment : public YieldCurveSegment {
public:
    //! \name Constructors/Destructors
    //@{
    //! Default constructor
    TenorBasisYieldCurveSegment() {}
    //! Detailed constructor
    TenorBasisYieldCurveSegment(const string& typeID, const string& conventionsID, const vector<string>& quotes,
                                const string& shortProjectionCurveID, const string& longProjectionCurveID);
    //! Default destructor
    virtual ~TenorBasisYieldCurveSegment() {}
    //@}

    //!\name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}

    //! \name Inspectors
    //@{
    const string& shortProjectionCurveID() const { return shortProjectionCurveID_; }
    const string& longProjectionCurveID() const { return longProjectionCurveID_; }
    //@}

    //! \name Visitability
    //@{
    virtual void accept(AcyclicVisitor&);
    //@}

private:
    string shortProjectionCurveID_;
    string longProjectionCurveID_;
};

//! Cross Currency yield curve segment
/*!
  Cross currency basis spread adjusted discount curves for 'domestic' currency cash flows
  are built using this segment type which requires cross currency basis spreads quotes,
  the spot FX quote ID and at least the 'foreign' discount curve ID.
  Projection curves for both currencies can be provided as well for consistency with
  tenor basis in each currency.

  \ingroup configuration
*/
class CrossCcyYieldCurveSegment : public YieldCurveSegment {
public:
    //! \name Constructors/Destructors
    //@{
    //! Default constructor
    CrossCcyYieldCurveSegment() {}
    //! Detailed constructor
    CrossCcyYieldCurveSegment(const string& typeID, const string& conventionsID, const vector<string>& quotes,
                              const string& spotRateID, const string& foreignDiscountCurveID,
                              const string& domesticProjectionCurveID = string(),
                              const string& foreignProjectionCurveID = string());
    //! Default destructor
    virtual ~CrossCcyYieldCurveSegment() {}
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}

    //! \name Inspectors
    //@{
    const string& spotRateID() const { return spotRateID_; }
    const string& foreignDiscountCurveID() const { return foreignDiscountCurveID_; }
    const string& domesticProjectionCurveID() const { return domesticProjectionCurveID_; }
    const string& foreignProjectionCurveID() const { return foreignProjectionCurveID_; }
    //@}

    //! \name Visitability
    //@{
    virtual void accept(AcyclicVisitor&);
    //@}

private:
    string spotRateID_;
    string foreignDiscountCurveID_;
    string domesticProjectionCurveID_;
    string foreignProjectionCurveID_;
};

//! Zero Spreaded yield curve segment
/*!
  A zero spreaded segment is used to build a yield curve from zero spread quotes and
  a reference yield curve.

  \ingroup configuration
*/
class ZeroSpreadedYieldCurveSegment : public YieldCurveSegment {
public:
    //! \name Constructors/Destructors
    //@{
    //! Default constructor
    ZeroSpreadedYieldCurveSegment() {}
    //! Detailed constructor
    ZeroSpreadedYieldCurveSegment(const string& typeID, const string& conventionsID, const vector<string>& quotes,
                                  const string& referenceCurveID);
    //! Default destructor
    virtual ~ZeroSpreadedYieldCurveSegment() {}
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}

    //! \name Inspectors
    //@{
    const string& referenceCurveID() const { return referenceCurveID_; }
    //@}

    //! \name Visitability
    //@{
    virtual void accept(AcyclicVisitor&);
    //@}

private:
    string referenceCurveID_;
};

//! Discount ratio yield curve segment
/*! Used to configure a QuantExt::DiscountRatioModifiedCurve.

    \ingroup configuration
*/
class DiscountRatioYieldCurveSegment : public YieldCurveSegment {
public:
    //! \name Constructors/Destructors
    //@{
    //! Default constructor
    DiscountRatioYieldCurveSegment() {}
    //! Detailed constructor
    DiscountRatioYieldCurveSegment(const std::string& typeId, const std::string& baseCurveId,
                                   const std::string& baseCurveCurrency, const std::string& numeratorCurveId,
                                   const std::string& numeratorCurveCurrency, const std::string& denominatorCurveId,
                                   const std::string& denominatorCurveCurrency);
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}

    //! \name Inspectors
    //@{
    const string& baseCurveId() const { return baseCurveId_; }
    const string& baseCurveCurrency() const { return baseCurveCurrency_; }
    const string& numeratorCurveId() const { return numeratorCurveId_; }
    const string& numeratorCurveCurrency() const { return numeratorCurveCurrency_; }
    const string& denominatorCurveId() const { return denominatorCurveId_; }
    const string& denominatorCurveCurrency() const { return denominatorCurveCurrency_; }
    //@}

    //! \name Visitability
    //@{
    void accept(QuantLib::AcyclicVisitor& v);
    //@}

private:
    std::string baseCurveId_;
    std::string baseCurveCurrency_;
    std::string numeratorCurveId_;
    std::string numeratorCurveCurrency_;
    std::string denominatorCurveId_;
    std::string denominatorCurveCurrency_;
};

//! Yield Curve configuration
/*!
  Wrapper class containing all yield curve segments needed to build a yield curve.

  \ingroup configuration
 */
class YieldCurveConfig : public CurveConfig {
public:
    //! \name Constructors/Destructurs
    //@{
    //! Default constructor
    YieldCurveConfig() {}
    //! Detailed constructor
    YieldCurveConfig(const string& curveID, const string& curveDescription, const string& currency,
                     const string& discountCurveID, const vector<boost::shared_ptr<YieldCurveSegment>>& curveSegments,
                     const string& interpolationVariable = "Discount", const string& interpolationMethod = "LogLinear",
                     const string& zeroDayCounter = "A365", bool extrapolation = true, Real tolerance = 1.0e-12);
    //! Default destructor
    virtual ~YieldCurveConfig() {}
    //@}

    //! \name Serilalisation
    //@{
    virtual void fromXML(XMLNode* node) override;
    virtual XMLNode* toXML(XMLDocument& doc) override;
    //@}

    //! \name Inspectors
    //@{
    const string& currency() const { return currency_; }
    const string& discountCurveID() const { return discountCurveID_; }
    const vector<boost::shared_ptr<YieldCurveSegment>>& curveSegments() const { return curveSegments_; }
    const string& interpolationVariable() const { return interpolationVariable_; }
    const string& interpolationMethod() const { return interpolationMethod_; }
    const string& zeroDayCounter() const { return zeroDayCounter_; }
    bool extrapolation() const { return extrapolation_; }
    Real tolerance() const { return tolerance_; }
    const set<string>& requiredYieldCurveIDs() const { return requiredYieldCurveIDs_; }
    //@}

    //! \name Setters
    //@{
    string& interpolationVariable() { return interpolationVariable_; }
    string& interpolationMethod() { return interpolationMethod_; }
    string& zeroDayCounter() { return zeroDayCounter_; }
    bool& extrapolation() { return extrapolation_; }
    Real& tolerance() { return tolerance_; }
    //@}

    const vector<string>& quotes() override;

private:
    void populateRequiredYieldCurveIDs();

    // Mandatory members
    string currency_;
    string discountCurveID_;
    vector<boost::shared_ptr<YieldCurveSegment>> curveSegments_;
    set<string> requiredYieldCurveIDs_;

    // Optional members
    string interpolationVariable_;
    string interpolationMethod_;
    string zeroDayCounter_;
    bool extrapolation_;
    Real tolerance_;
};

// Map form curveID to YieldCurveConfig
using YieldCurveConfigMap = std::map<string, boost::shared_ptr<YieldCurveConfig>>;
} // namespace data
} // namespace ore


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <iostream>

#include <algorithms/mis.hpp>
#include <algorithms/bfs.hpp>
#include <graphblas/graphblas.hpp>

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE mis_test_suite

#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
BOOST_AUTO_TEST_CASE(mis_test)
{
    using GBMatrix = grb::Matrix<double>;
    using GBVector = grb::Vector<double>;

    grb::IndexType const NUM_NODES(7);
    grb::IndexArrayType i = {0, 0, 1, 1, 2, 2, 3, 3, 3,
                                   4, 4, 5, 5, 5, 6, 6};
    grb::IndexArrayType j = {1, 2, 0, 3, 0, 5, 1, 4, 6,
                                   3, 5, 2, 4, 6, 3, 5};
    std::vector<double> v(i.size(), 1);
    GBMatrix graph(NUM_NODES, NUM_NODES);
    graph.build(i, j, v);

    std::cout << "*********** MIS1 *********" << std::endl;
    double set_sizes(0);
    double num_samples(0);
    // Run MANY experiments to get different IS's
    for (unsigned int seed = 1000; seed < 1200; ++seed)
    {
        grb::Vector<bool> independent_set(NUM_NODES);
        algorithms::mis(graph, independent_set, float(seed));
        //grb::print_vector(std::cout, independent_set,
        //                        "independent_set (flags)");

        //grb::IndexArrayType result(
        //    algorithms::get_vertex_IDs(independent_set));
        //std::cout << "Seed=" << seed << ": ";
        //for (auto it = result.begin(); it != result.end(); ++it)
        //    std::cerr << *it << " ";
        //std::cerr << std::endl;

        set_sizes += independent_set.nvals();
        num_samples += 1.0;

        // Check the result by performing bfs_level() from the independent_set
        // All levels should be 1 (if in IS) or 2 (if not in IS)
        GBVector levels(NUM_NODES);
        algorithms::bfs_level_masked(graph, independent_set, levels);
        //grb::print_vector(std::cout, levels, "BFS levels from IS");

        for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
        {
            if (levels.hasElement(ix))
            {
                double lvl = levels.extractElement(ix);
                BOOST_CHECK((lvl < 3) && (lvl > 0));
                if (lvl != 1)
                {
                    BOOST_CHECK_EQUAL(independent_set.hasElement(ix), false);
                }
                else
                {
                    BOOST_CHECK_EQUAL(independent_set.hasElement(ix), true);
                    BOOST_CHECK_EQUAL(independent_set.extractElement(ix), true);
                }
            }
        }
    }
    std::cerr << "Avg. maximal independent set size = "
              << set_sizes/num_samples << std::endl;
    BOOST_CHECK(set_sizes/num_samples > 3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(mis_test_isolated_vertex)
{
    // FA Tech Note graph
    //
    //    {-, -, -, 1, -, -, -, -, -},
    //    {-, -, -, 1, -, -, 1, -, -},
    //    {-, -, -, -, 1, 1, 1, -, 1},
    //    {1, 1, -, -, 1, -, 1, -, -},
    //    {-, -, 1, 1, -, -, -, -, 1},
    //    {-, -, 1, -, -, -, -, -, -},
    //    {-, 1, 1, 1, -, -, -, -, -},
    //    {-, -, -, -, -, -, -, -, -},
    //    {-, -, 1, -, 1, -, -, -, -};

    /// @todo change scalar type to unsigned int or grb::IndexType
    using T = grb::IndexType;
    using GBMatrix = grb::Matrix<T, grb::DirectedMatrixTag>;

    grb::IndexType const NUM_NODES = 9;
    grb::IndexArrayType i = {0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3,
                                   4, 4, 4, 5, 6, 6, 6, 8, 8};
    grb::IndexArrayType j = {3, 3, 6, 4, 5, 6, 8, 0, 1, 4, 6,
                                   2, 3, 8, 2, 1, 2, 3, 2, 4};
    std::vector<T> v(i.size(), 1);

    std::cout << "*********** MIS: isolated vertex *********" << std::endl;

    GBMatrix G_tn(NUM_NODES, NUM_NODES);
    G_tn.build(i.begin(), j.begin(), v.begin(), i.size());
    //grb::print_matrix(std::cout, G_tn, "Graph adjacency matrix:");

    double set_sizes(0);
    double num_samples(0);
    for (unsigned int seed = 1000; seed < 1200; ++seed)
    {
        grb::Vector<bool> independent_set(NUM_NODES);
        algorithms::mis(G_tn, independent_set, float(seed));

        //grb::IndexArrayType result(
        //    algorithms::get_vertex_IDs(independent_set));
        //std::cout << "Seed=" << seed << ": ";
        //for (auto it = result.begin(); it != result.end(); ++it)
        //    std::cout << *it << " ";
        //std::cout << std::endl;

        set_sizes += independent_set.nvals();
        num_samples += 1.0;

        // Check the result by performing bfs_level() from the independent_set
        // All levels should be 1 (if in IS) or 2 (if not in IS)
        grb::Vector<grb::IndexType> levels(NUM_NODES);
        algorithms::bfs_level_masked(G_tn, independent_set, levels);
        //grb::print_vector(std::cout, levels, "BFS levels from IS");

        for (grb::IndexType ix = 0; ix < NUM_NODES; ++ix)
        {
            if (levels.hasElement(ix))
            {
                double lvl = levels.extractElement(ix);
                BOOST_CHECK((lvl < 3) && (lvl > 0));
                if (lvl != 1)
                {
                    BOOST_CHECK_EQUAL(independent_set.hasElement(ix), false);
                }
                else
                {
                    BOOST_CHECK_EQUAL(independent_set.hasElement(ix), true);
                    BOOST_CHECK_EQUAL(independent_set.extractElement(ix), true);
                }
            }
        }
    }
    std::cerr << "Avg. maximal independent set size = "
              << set_sizes/num_samples << std::endl;
    BOOST_CHECK(set_sizes/num_samples > 4);
}

BOOST_AUTO_TEST_SUITE_END()


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2019 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
//
// 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
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE sha_test

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <boost/filesystem/path.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <boost/crypto3/hash/algorithm/hash.hpp>
#include <boost/crypto3/hash/adaptor/hashed.hpp>

#include <boost/crypto3/hash/sha.hpp>

using namespace boost::crypto3;
using namespace boost::crypto3::accumulators;

namespace boost {
    namespace test_tools {
        namespace tt_detail {
            template<template<typename, typename> class P, typename K, typename V>
            struct print_log_value<P<K, V>> {
                void operator()(std::ostream &, P<K, V> const &) {
                }
            };
        }    // namespace tt_detail
    }        // namespace test_tools
}    // namespace boost

BOOST_TEST_DONT_PRINT_LOG_VALUE(hashes::sha::digest_type)

class fixture {
public:
    accumulator_set<hashes::sha> acc;
    typedef hashes::sha hash_t;

    virtual ~fixture() {
    }
};

#ifndef CRYPTO3_CI_DATA_DRIVEN_TESTS_DISABLED

const char *construct_file(const char *path) {
    return (boost::filesystem::path(path).parent_path() / "data" / "sha.json").c_str();
}

boost::property_tree::ptree string_data() {
    boost::property_tree::ptree string_data;
    boost::property_tree::read_json(construct_file(BOOST_PP_STRINGIZE(__FILE__)), string_data);

    return string_data;
}

BOOST_AUTO_TEST_SUITE(sha_stream_processor_data_driven_algorithm_test_suite)

BOOST_DATA_TEST_CASE(sha_string_various_range_value_hash, string_data(), array_element) {
    std::string out = hash<hashes::sha>(array_element.first);

    BOOST_CHECK_EQUAL(out, array_element.second.data());
}

BOOST_DATA_TEST_CASE(sha_string_various_itr_value_hash, string_data(), array_element) {
    std::string out = hash<hashes::sha>(array_element.first.begin(), array_element.first.end());

    BOOST_CHECK_EQUAL(out, array_element.second.data());
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(sha_stream_processor_data_driven_adaptor_test_suite)

    BOOST_DATA_TEST_CASE(sha_string_various_range_value_hash, string_data(), array_element) {
        std::string out = array_element.first | adaptors::hashed<hashes::sha>;

        BOOST_CHECK_EQUAL(out, array_element.second.data());
    }

BOOST_AUTO_TEST_SUITE_END()

#endif

BOOST_AUTO_TEST_SUITE(sha_stream_processor_test_suite)

BOOST_AUTO_TEST_CASE(sha_shortmsg_byte) {
    // https://nvlpubs.nist.gov/nistpubs/Legacy/FIPS/NIST.FIPS.180.pdf
    // Appendix A: "abc"
    std::array<char, 3> a = {'\x61', '\x62', '\x63'};
    hashes::sha::digest_type d = hash<hashes::sha>(a);

    BOOST_CHECK_EQUAL("0164b8a914cd2a5e74c4f7ff082c4d97f1edf880", std::to_string(d).data());
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(sha_accumulator_test_suite)

BOOST_FIXTURE_TEST_CASE(sha_accumulator1, fixture) {
    hash_t::construction::type::block_type m = {{}};

    m[0] = 0x61626300;
    acc(m, accumulators::bits = 24);

    hash_t::digest_type s = extract::hash<hash_t>(acc);

#ifdef CRYPTO3_HASH_SHOW_PROGRESS
    std::printf("%s\n", std::to_string(s).data());
#endif

    BOOST_CHECK_EQUAL("0164b8a914cd2a5e74c4f7ff082c4d97f1edf880", std::to_string(s).data());
}

BOOST_FIXTURE_TEST_CASE(sha_accumulator2, fixture) {
    // Appendix B: "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq"
    hash_t::construction::type::block_type m = {{0x61626364, 0x62636465, 0x63646566, 0x64656667, 0x65666768, 0x66676869,
                                                 0x6768696a, 0x68696a6b, 0x696a6b6c, 0x6a6b6c6d, 0x6b6c6d6e, 0x6c6d6e6f,
                                                 0x6d6e6f70, 0x6e6f7071, 0x00000000, 0x00000000}};
    acc(m, accumulators::bits = 512 - 64);

    hash_t::digest_type s = extract::hash<hash_t>(acc);

#ifdef CRYPTO3_HASH_SHOW_PROGRESS
    std::printf("%s\n", std::to_string(s).data());
#endif

    BOOST_CHECK_EQUAL("d2516ee1acfa5baf33dfc1c471e438449ef134c8", std::to_string(s).data());
}

BOOST_AUTO_TEST_CASE(sha_preprocessor1) {
    accumulator_set<hashes::sha> acc;

    acc(0x61000000, accumulators::bits = 8);
    acc(0x62000000, accumulators::bits = 8);
    acc(0x63000000, accumulators::bits = 8);

    hashes::sha::digest_type s = extract::hash<hashes::sha>(acc);

#ifdef CRYPTO3_HASH_SHOW_PROGRESS
    std::printf("%s\n", std::to_string(s).data());
#endif

    BOOST_CHECK_EQUAL("0164b8a914cd2a5e74c4f7ff082c4d97f1edf880", std::to_string(s).data());
}

BOOST_AUTO_TEST_CASE(sha_preprocessor2) {
    // Appendix C: million repetitions of "a"
    accumulator_set<hashes::sha> acc;

    for (unsigned i = 0; i != 1000000; ++i)
        acc(0x61000000, accumulators::bits = 8);

    hashes::sha::digest_type s = extract::hash<hashes::sha>(acc);

#ifdef CRYPTO3_HASH_SHOW_PROGRESS
    std::printf("%s\n", std::to_string(s).data());
#endif

    BOOST_CHECK_EQUAL("3232affa48628a26653b5aaa44541fd90d690603", std::to_string(s).data());
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef W_GEOM_HPP
#define W_GEOM_HPP 1

#include <cassert>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>
#include <armadillo>

#include "constant.hpp"
#include "elements.hpp"
#include "message.hpp"

namespace willow {

inline arma::mat read_geom (const std::string& fname,
			    std::vector<std::string>& at_name)
{

  std::string str_geom;

  const int sys_me = mpi::rank ();

  if (sys_me == 0) {
    std::ifstream ifs_geom (fname);
    assert (ifs_geom.good());

    // Read the entire file into a string that can be broadcasted
    //
    std::ostringstream oss;
    //is_geom >> oss.rdbuf();
    oss << ifs_geom.rdbuf ();
    str_geom = oss.str();
  }

  mpi::barrier ();
  mpi::broadcast_string (str_geom);

  std::istringstream is(str_geom);
  // xyz file

  size_t natom;

  is >> natom;

  arma::mat pos(3,natom);

  std::string rest_of_line;
  std::getline (is, rest_of_line);

  // second line = comment;
  std::string comment;
  std::getline (is, comment);

  for (size_t ia = 0; ia < natom; ++ia) {
    std::string line;
    std::getline (is, line);

    std::istringstream iss (line);
    std::string name;
    double x, y, z;

    iss >> name >> x >> y >> z;

    at_name.push_back (name);
    double *xyz = pos.colptr(ia);
    // length unit is A
    /*xyz[0] = x;
    xyz[1] = y;
    xyz[2] = z;*/

    // Bohr
    xyz[0] = x*ang2bohr;
    xyz[1] = y*ang2bohr;
    xyz[2] = z*ang2bohr;

  }

  return pos;

}

inline arma::ivec atom_to_Z (std::vector<std::string>& at_name)
{

  const size_t natom = at_name.size();
  arma::ivec Z_list(natom);

  for (auto ia = 0; ia < natom; ++ia) {
    const auto symbol = at_name[ia];
    for (const auto& e : willow::element_info) {
      if (symbol == e.symbol) {
	Z_list(ia) = e.Z;
	break;
      }
    }
  }

  return Z_list;

}

} // namespace willow

#endif