/********************************************************************************

Copyright (c) 2017 Bob Perry / asw204k_AT_yahoo.com
All rights reserved.

Please see license in the project root directory fro more details

*/

#include "oxapp.h"
#include "constants.h"
#include "conversion.h"
#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include "devices/KOBO.h"
#include "algo/checksum.h"
#include "trivial_log.h"
#include <string>
#include <sstream>

using namespace std;

void KOBO::rw_device() {

  if( (OxApp::get_time_ms() - last_run) < interval ) return;
  last_run = OxApp::get_time_ms();

  stringstream PGRMZ;
  stringstream PITV5;
  stringstream PITV3;
  stringstream PITV4;

  PGRMZ << "PGRMZ," << OxApp::algo_press->get_val(PRESSURE_ALTITUDE) << "," <<
    "m" <<
    "," << "2";
  Checksum PGRMZck(PGRMZ);

  PITV3 << "PITV3," <<
    Conv::rad2deg(OxApp::algo_mad_euler->get_val( ROLL )) << "," <<
    Conv::rad2deg(OxApp::algo_mad_euler->get_val( PITCH )) << "," <<
    Conv::rad2deg(OxApp::algo_mad_euler->get_val( YAW )) << "," <<
    OxApp::algo_press->get_val(AIRSPEED) << "," <<
    OxApp::algo_misc_rate->get_val(LOAD_FACTOR);
  Checksum PITV3ck(PITV3);

  PITV5.precision(3);

  PITV5 << "PITV5," <<
    fixed << OxApp::algo_wind->get_val( WIND_SPEED ) << "," <<
    fixed << Conv::rad2deg(OxApp::algo_wind->get_val( WIND_DIR )) << "," <<
    fixed << sqrt( OxApp::algo_press->get_val(LOCAL_RHO) / SI_RHO ) << "," <<
    fixed << 0.0 << "," <<
    0 << "," <<
    fixed << OxApp::manual_double_vals->get_val(MCREADY);
  Checksum PITV5ck(PITV5);

  PITV4 << "PITV4," << OxApp::algo_press_rate->get_val(PRESSURE_TE_ALTITUDE) <<
    "," << OxApp::algo_press_rate->get_val(PRESSURE_TE_ALTITUDE) -
    polar.sink_rate( OxApp::algo_press->get_val(AIRSPEED)) << "," <<
    OxApp::algo_press_rate->get_val(PRESSURE_TE_ALTITUDE) -
    polar.sink_rate( OxApp::algo_press->get_val(AIRSPEED)) << "," <<
    OxApp::algo_press->get_val(PRESSURE_ALTITUDE) << "," <<
    OxApp::algo_press->get_val(PRESSURE_TE_ALTITUDE) << "," <<
    OxApp::algo_press->get_val(PRESSURE_TE_ALTITUDE) ;
  Checksum PITV4ck(PITV4);

  string GPRMC( OxApp::GPRMC->get_str() );
  string GPGGA( OxApp::GPGGA->get_str() );

  if( bus->is_open() ) {
    bus->write( GPRMC );
    bus->write( PGRMZck.get_sentence() );
    bus->write( PITV3ck.get_sentence() );
    bus->write( GPGGA );
    bus->write( PITV4ck.get_sentence() );
    bus->write( PITV5ck.get_sentence() );

  }

  BOOST_LOG_TRIVIAL(debug) << GPRMC;
  BOOST_LOG_TRIVIAL(debug) << GPGGA;
  BOOST_LOG_TRIVIAL(debug) << PGRMZck.get_sentence();
  BOOST_LOG_TRIVIAL(debug) << PITV3ck.get_sentence();
  BOOST_LOG_TRIVIAL(debug) << PITV4ck.get_sentence();
  BOOST_LOG_TRIVIAL(debug) << PITV5ck.get_sentence();
}


#include <tagloc/tracking.h>
#include <bearing_measurement/GetMeasurement.h>
#include <tagloc/algorithms.h>
#include <iostream>
#include <Eigen/Dense>
#include <networking/protocol.h>
#include <control/WaypointNavigation.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ros/ros.h>
#include <sstream>
#include <std_msgs/String.h>
#include <helpers/geometry_msgs.h>
#include <map>
#include <unistd.h>
#include <fstream>

using std::vector;
using std::map;
using std::cout;
using std::cerr;
using std::endl;
using std::string;
using std::stringstream;
using Eigen::MatrixXd;
using ros::ServiceClient;
using ros::Subscriber;
using ros::Publisher;

const string msg_start = SimpleProtocol::msg_start_str;
const string msg_stop = SimpleProtocol::msg_stop_str;
const string kv_delim = SimpleProtocol::key_value_delim_str;
const string p_delim = SimpleProtocol::pair_delim_str;

bool simulate = true;
map<string,string> data;

ServiceClient wpclient;
ServiceClient bearingclient;
Subscriber stateIn;
Subscriber hbin;
Publisher hbout;
Publisher status_out;

vector<double> px;
vector<double> py;
vector<double> R;
vector<double> Z;

vector<double> opx;
vector<double> opy;
vector<double> oR;
vector<double> oZ;

vector<double> apx;
vector<double> apy;
vector<double> aR;
vector<double> aZ;

int DESIRED_ZC=0;
int HAVE_ZC=0;
int FREQ;
int their_count=-1;
long last_time=-1;
long returned_time=999999;
double sensor_var=pow((M_PI/16),2.0);;
double cx,cy;

double get_radius(MatrixXd P){
	assert(P.rows()==P.cols());
	Eigen::JacobiSVD<MatrixXd> _svdA;
	_svdA.compute(P);
	return sqrt(_svdA.singularValues()(0));
}
void sync_robots_at(double x, double y);
void move(double x, double y, bool block=true,bool print = true);
void publish_z(double x, double y, double bearing, double sensor_variance);
void publish_all_z(vector<double> px, vector<double> y, vector<double> bearing, vector<double> sensor_variance);
bool measure(int f,double &bearing);
void setupRos(ros::NodeHandle nh);
void nwin(const std_msgs::String::ConstPtr &msg);
void mapstate(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
void get_all_meausrements(vector<double>&  px,vector<double>&  py,vector<double>&  Z,vector<double>&  R,vector<double>& opx,vector<double>& opy,vector<double>& oZ,vector<double>& oR,vector<double>& apx,vector<double>& apy,vector<double>& aZ,vector<double>& aR);
void get_last_measurement(vector<double>& opx,vector<double>& opy,vector<double>& oZ,vector<double>& oR);

////////////////////////////////////////////////////////////////////////////////
void mapstate(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg){
	stringstream ss;
	ss<<msg->pose.pose.position.x;
	data["_x"]=ss.str();
	ss.str("");
	ss<<msg->pose.pose.position.y;
	data["_y"]=ss.str();
	ss.str("");
	ss<<RSN::getYaw(msg->pose.pose.orientation);
	data["_th"]=ss.str();
	ss.str("");
	cx = atof(data["_x"].c_str());
	cy = atof(data["_y"].c_str());
}

void publish_z(double x, double y, double bearing, double sensor_variance){
	stringstream ss;
	std_msgs::String omsg;
	ss<<"Xn="<<x<<",";
	ss<<"Yn="<<y<<",";
	ss<<"Zn="<<bearing<<",";
	ss<<"Rn="<<sensor_variance<<",";
	ss<<"Zc="<<Z.size();
	omsg.data = ss.str();
	ROS_INFO("Publishing %Zu = %s",Z.size(),omsg.data.c_str());
	hbout.publish(omsg);
}

void publish_all_z(vector<double> xs, vector<double> ys, vector<double> zs, vector<double> rs){
	stringstream ss;
	std_msgs::String omsg;
	ss<<"Zc="<<Z.size();
	for (unsigned int i=0;i<zs.size();i++){
		ss<<",X"<<i<<"="<<xs[i];
		ss<<",Y"<<i<<"="<<ys[i];
		ss<<",Z"<<i<<"="<<zs[i];
		ss<<",R"<<i<<"="<<rs[i];
	}
	omsg.data = ss.str();
	ROS_INFO("Publishing %s",omsg.data.c_str());
	hbout.publish(omsg);
}

/** Please don't laugh, I'm in a hurry -jdvh **/
void get_all_meausrements(vector<double>&  px,vector<double>&  py,vector<double>&  Z,vector<double>&  R,vector<double>& opx,vector<double>& opy,vector<double>& oZ,vector<double>& oR,vector<double>& apx,vector<double>& apy,vector<double>& aZ,vector<double>& aR){
		int theirs = atoi(data["Zc"].c_str());

		opx.clear();
		opy.clear();
		oZ.clear();
		oR.clear();

		apx.clear();
		apy.clear();
		aZ.clear();
		aR.clear();

		for (int i=0;i<theirs;i++){
			stringstream ss;
			ss<<"X"<<i;
			opx.push_back(atof(data[ss.str()].c_str()));
		}
		for (int i=0;i<theirs;i++){
			stringstream ss;
			ss<<"Y"<<i;
			opy.push_back(atof(data[ss.str()].c_str()));
		}
		for (int i=0;i<theirs;i++){
			stringstream ss;
			ss<<"Z"<<i;
			oZ.push_back(atof(data[ss.str()].c_str()));
		}
		for (int i=0;i<theirs;i++){
			stringstream ss;
			ss<<"R"<<i;
			oR.push_back(atof(data[ss.str()].c_str()));
		}
		apx.insert(apx.end(),px.begin(),px.end());
		apx.insert(apx.end(),opx.begin(),opx.end());
		apy.insert(apy.end(),py.begin(),py.end());
		apy.insert(apy.end(),opy.begin(),opy.end());
		aZ.insert(aZ.end(),Z.begin(),Z.end());
		aZ.insert(aZ.end(),oZ.begin(),oZ.end());
		aR.insert(aR.end(),R.begin(),R.end());
		aR.insert(aR.end(),oR.begin(),oR.end());
}
void get_last_measurement(vector<double>& opx,vector<double>& opy,vector<double>& oZ,vector<double>& oR){
	opx.push_back(atof(data["Xn"].c_str()));
	opy.push_back(atof(data["Yn"].c_str()));
	oZ.push_back(atof(data["Zn"].c_str()));
	oR.push_back(atof(data["Rn"].c_str()));
}
bool measure(int f,double &bearing){
	if (simulate){
		ROS_WARN("SIMULATING MEASUREMENT!");
		return true;
	}
	ROS_INFO("Taking Bearing Measurement");
	bearing_measurement::GetMeasurement::Request breq;
	bearing_measurement::GetMeasurement::Response bres;
	breq.freq = f;
	if (bearingclient.exists()){
		bearingclient.call(breq,bres);
	} else {
		ROS_WARN("Unable to measure.");
	}
	bearing = ((bres.bearing)/180.0)*M_PI;
	ROS_INFO("Got: %0.3f", bres.bearing);

	return bres.valid.data;
}
void nwin(const std_msgs::String::ConstPtr &msg){

	char *state,*inner_state,*token;

	//ROS_INFO("NWIN: %s",msg->data.c_str());
	string s = msg->data;
	int msgstart = s.find(msg_start)+1;
	int msgend = s.find(msg_stop);
	char *cs_s = (char*)calloc(s.size()+1,sizeof(char));
	strcpy(cs_s,s.c_str());

	string sdata = s.substr(msgstart,msgend-msgstart);
	char *cs_data = (char*)calloc(sdata.size()+1,sizeof(char));
	strcpy(cs_data,sdata.c_str());

	//first get all meta-data from the string
	token = strtok_r(cs_s,p_delim.c_str(),&state);
	while(token!=NULL){
		string key;
		string val;
		stringstream ss;
		ss<<strtok_r(token,kv_delim.c_str(),&inner_state);
		ss>>key;ss.clear();
		ss<<strtok_r(NULL,kv_delim.c_str(),&inner_state);
		ss>>val;ss.clear();
		data[key]=val;
		token = strtok_r(NULL,p_delim.c_str(),&state);
	}
	//now get all msg data from the string
	token = strtok_r(cs_data,p_delim.c_str(),&state);
	while(token!=NULL){
		string key;
		string val;
		stringstream ss;
		ss<<strtok_r(token,kv_delim.c_str(),&inner_state);
		ss>>key;ss.clear();
		ss<<strtok_r(NULL,kv_delim.c_str(),&inner_state);
		ss>>val;ss.clear();
		data[key]=val;
		token = strtok_r(NULL,p_delim.c_str(),&state);
	}
	if(data["Zc"].size()>0){
		their_count = atoi(data["Zc"].c_str());
	}
	if (data["T"].size()>0){
		last_time = atol(data["T"].c_str());
	}
	if (data["Dt"].size()>0){
		returned_time = atol(data["Dt"].c_str());
	}
	if (data["HaveC"].size()>0){
		HAVE_ZC= atoi(data["HaveC"].c_str());
	}
	std_msgs::String omsg;
	stringstream ss;
	ss<<"Dt="<<last_time<<",";
	ss<<"RendC="<<DESIRED_ZC<<",";
	ss<<"HaveC="<<atoi(data["RendC"].c_str());
	omsg.data = ss.str();
	hbout.publish(omsg);
	free(cs_s);
	free(cs_data);
}

void stop(){
	control::WaypointNavigation::Response wpres;
	control::WaypointNavigation::Request wpreq;
	ROS_INFO("Stopping navigation, if any");
	wpreq.desiredState=2;
	wpreq.blockState=wpreq.BLOCKING;
	if (wpclient.exists()){
		wpclient.call(wpreq,wpres);
	} else {
		ROS_WARN("Skipping waypoint service: Doesn't exist");
	}
}
void move(double x, double y, bool block, bool print){
	control::WaypointNavigation::Response wpres;
	control::WaypointNavigation::Request wpreq;
	if (print)
		ROS_INFO("moving to:%0.3f %0.3f",x,y);

	wpreq.desiredState=0;
	wpreq.destination.x=x;
	wpreq.destination.y=y;

	if (block){
		wpreq.blockState=wpreq.BLOCKING;
	}	else {
		wpreq.blockState=wpreq.NONBLOCKING;
	}

	if (wpclient.exists()){
		wpclient.call(wpreq,wpres);
	} else {
		ROS_WARN("Skipping waypoint service: Doesn't exist");
	}
	if (print)
		ROS_INFO("Move Complete");
}
void sync_robots_at(double x, double y){
	DESIRED_ZC++;
	ROS_INFO("Waiting for joint state at %f, %f",x,y);

	returned_time=0;
	ros::Duration(5).sleep();

	time_t tnow = time(NULL);
	long delta_t=tnow-last_time;

	int their_dc=-1;
	ROS_INFO("It's been %ld since I heard from them",delta_t);

	while( (their_dc!=DESIRED_ZC || HAVE_ZC!=DESIRED_ZC) /*|| delta_t>5 || t_delta_t > 5)*/ && ros::ok()) {
		ros::Duration(.1).sleep();
		move(x,y,false,false);
		their_dc=atoi(data["RendC"].c_str());
	}

	ROS_INFO("Seems we sync'd. Waiting a bit...");
	stop();
	ros::Duration(3).sleep();
	move(x,y,false);
	//assume we have both our state and other guy's state,
	//as well as any extra data he sent along

	if (!ros::ok()){
		cerr<<"ROS down. Exiting"<<endl;
		return;
	}

	ROS_INFO("Got joint state.");
}

void setupRos(ros::NodeHandle nh){
  wpclient=nh.serviceClient<control::WaypointNavigation>("/control/command");
	bearingclient=nh.serviceClient<bearing_measurement::GetMeasurement>("/bearing_measurement/get_measurement");
	stateIn = nh.subscribe("/robot/state",10,mapstate);
	hbin = nh.subscribe("/network/received",30,nwin);
	hbout = nh.advertise<std_msgs::String>("/network/out",0);
	status_out = nh.advertise<std_msgs::String>("/loc/out",0);
}

int main(int argc, char** argv){

	cx = 0.0;
	cy = 0.0;
	bool test_sync=false;
	bool userend = true;
	bool assume_stationkeep = true;
	simulate=false;

	MatrixXd firstrend(2,1);
	firstrend<<-30,-20;
	MatrixXd r1(2,1);
	MatrixXd r2(2,1);
	FREQ = 49691;

	string fname="";
	ros::init(argc,argv,"tl");
	ros::NodeHandle nh("tagloc");

	setupRos(nh);
	ros::Duration(5).sleep();
	ros::AsyncSpinner spinner(1);

	FILE* log_file = fopen("tagloc_log","a");
	if (log_file==NULL){
		ROS_ERROR("Cannot open log file!");
		return -1;
	}

	double Dei=100.0;

	nh.getParam("test_sync",test_sync);
	ROS_INFO("test_sync=%s ... true will only go, sync, and return.",test_sync?"true":"false");

	nh.getParam("userend",userend);
	ROS_INFO("userend=%s ... true implies we assume we're at the measurement location.",userend?"true":"false");

	nh.getParam("sensor_var",sensor_var);
	ROS_INFO("sensor_var=%f ",sensor_var);

	nh.getParam("dei",Dei);
	ROS_INFO("Dei: %0.3f ... desired information = 1/sigma^2 ",Dei);

	nh.getParam("simulate",simulate);
	ROS_INFO("simulate: %s",simulate?"true":"false");

	nh.getParam("assume_stationkeep",assume_stationkeep);
	ROS_INFO("simulate: %s",simulate?"true":"false");

	int max_measurement = 3;
	nh.getParam("max_measurement",max_measurement);
	ROS_INFO("max_measurement: %d ... max times to call two-step",max_measurement);

	int max_local_measurement= 1;
	nh.getParam("max_local_measurement",max_local_measurement);
	ROS_INFO("max_local_measurement: %d ... max number of measurements at each loc",max_local_measurement);
	if (max_local_measurement>1){
		ROS_WARN("This is a problem: Not ready for many local measurements");
		//return -1;
	}

	if (simulate){
		ROS_WARN("IGNORING FAILURES. SIMULATING");
	}

	char HOSTNAME[HOST_NAME_MAX];
	gethostname(HOSTNAME,HOST_NAME_MAX);
	ROS_INFO("Multi-step. I am %s",HOSTNAME);
#ifdef MAKE_RECIP
	ROS_INFO("I am the second robot!");
	fprintf(log_file,"%s\n","* robot 2");
#else
	ROS_INFO("I am the first robot!");
	fprintf(log_file,"%s\n","* robot 1");
#endif

	if (test_sync && ! simulate){
		ROS_WARN("TESTING SYNC");
		sync_robots_at(20,-80);
		exit(0);
	}

	/*************Main**********/

	spinner.start();

	MatrixXd t(2,1);
	MatrixXd ti(2,1);
	MatrixXd t0(2,1);

	MatrixXd P(2,2);P = MatrixXd::Identity(2,2);
	MatrixXd Pi(2,2); Pi = MatrixXd::Identity(2,2);
	MatrixXd P0(2,2); P0 = MatrixXd::Identity(2,2);

	MatrixXd y(2,1);
	MatrixXd rend(2,1);

	t0<<-10,-150;
	P0<<2500,0,0,2500;
	cout<<t0<<endl;
	cout<<P0<<endl;

	fprintf(log_file,"P: %4.4f %4.4f %4.4f %4.4f %4.4f %4.4f\n",t0(0),t0(1),P0(0),P0(1),P0(2),P0(3));

	ROS_INFO("STARTING RAD:%f",get_radius(P0));
	move(firstrend(0),firstrend(1));

	r1 = firstrend;
	r2 = firstrend;

	P = P0;
	t = t0;

	for (int measurement=0;measurement<max_measurement;measurement++){
		ROS_INFO("**Measurement %d",measurement);
		if (userend){
			r1 = rend;
			r2 = rend;
		}
		RSN::onestep_circle(r1,r2,t,get_radius(P),sensor_var,Dei);
		rend = (r1+r2)/2;
#ifdef MAKE_RECIP
		y = r2;
		r2 = r1;
		r1 = y;
#else
#endif

		ROS_INFO("Got destination: %0.3f,%0.3f. Other guy:%0.3f,%0.3f",r1(0),r1(1),r2(0),r2(1));

		for (int local_measurement = 0;local_measurement<max_local_measurement;local_measurement++){

			fprintf(log_file,"M: %4.4f %4.4f %4.4f %4.4f\n",r1(0),r1(1),r2(0),r2(1));
			move(r1(0),r1(1),true);

			double bearing;
			bool zok = false;
			if (simulate){
				stringstream ss;
				ss<<r1(0);
				data["_x"]=ss.str();ss.str("");
				ss<<r1(1);
				data["_y"]=ss.str();ss.str("");
				bearing=atan2(t0(1)-r1(1),t0(0)-r1(0));
				zok = true;
			} else{
				zok = measure(FREQ,bearing);
			}

			if (zok){
				//r1<<atof(data["_x"].c_str()),atof(data["_y"].c_str());
				Z.push_back(bearing);
				R.push_back(sqrt(sensor_var));
				if (assume_stationkeep){
					px.push_back(r1(0));
					py.push_back(r1(1));
					//publish_z(r1(0),r1(1),bearing,sqrt(sensor_var));
				}	else{
					px.push_back(cx);
					py.push_back(cy);
					//publish_z(cx,cy,bearing,sqrt(sensor_var));
				}
				fprintf(log_file,"Z: %4.4f %4.4f %4.4f %4.4f\n",r1(0),r1(1),bearing,sensor_var);
				fprintf(log_file,"Y: %4.4f %4.4f %4.4f %4.4f\n",cx,cy,bearing,sensor_var);
			} else {
				ROS_INFO("Bad measurement");
			}
		}

		ROS_INFO("Publishing Measurement value over network");
		publish_all_z(px,py,Z,R);
		ROS_INFO("Going to rendesvous point");

		move(rend(0),rend(1),true);
		fprintf(log_file,"M: %4.4f %4.4f \n",rend(0),rend(1));
		if (!simulate || test_sync){
			sync_robots_at(rend(0),rend(1));
		}
		if (!ros::ok()){
			return EXIT_FAILURE;
		}

		ROS_INFO("Examining their data");
		get_all_meausrements(px,py,Z,R,opx,opy,oZ,oR,apx,apy,aZ,aR);

		//int theirs = atoi(data["Zc"].c_str());
		/*
		if (max_local_measurement>1){
			//send ours to be joined with theirs into all
			// this is safe to do if they failed
			get_all_meausrements(px,py,Z,R,opx,opy,oZ,oR,apx,apy,aZ,aR);
		}else{
			//insert our most recent
			apx.insert(apx.end(),px[px.size()-1]);
			apy.insert(apy.end(),py[py.size()-1]);
			aZ.insert(aZ.end(),Z[Z.size()-1]);
			aR.insert(aR.end(),R[R.size()-1]);
			//record their most recent
			get_last_measurement(opx,opy,oZ,oR);
			//insert their most recent
			apx.insert(apx.end(),opx[opx.size()-1]);
			apy.insert(apy.end(),opy[opy.size()-1]);
			aZ.insert(aZ.end(),oZ[oZ.size()-1]);
			aR.insert(aR.end(),oR[oR.size()-1]);
		}
		if ((size_t)theirs == 0){
			ROS_INFO("They have no measurements");
		} else if ((size_t)theirs >= Z.size()){
			ROS_INFO("%Zu (mine) %d (theirs)",Z.size(),theirs);
			ROS_INFO("Looks good.");
		} else if ((size_t) theirs < Z.size()){
			ROS_INFO("They missed a measurement!");
		}*/

		ROS_INFO("Got Joint State. Updating hypothesis with %Zu total measurements",aZ.size());
		ROS_INFO("Measurement Sequence is:");
		for (unsigned int i=0;i<aZ.size();i++){
			cout<<apx[i]<<" "<<apy[i]<<" "<<aZ[i]<<" "<<aR[i]<<endl;
		}

		double tz[2];
		double c[4];

		RSN::line_intersection(aZ.size(),apx,apy,aZ,tz[0],tz[1]);
		RSN::BOT::IWLS_2D(aZ.size(),apx,apy,aZ,aR,tz,c,true);

		ti<<tz[0],tz[1];
		Pi<<c[0],c[1],c[2],c[3];

		ROS_INFO("Z gives:");
		cout<<ti<<endl;
		cout<<Pi<<endl;

		P = (Pi.inverse()+P0.inverse()).inverse();
		t =  P * (Pi.inverse() * ti + P0.inverse() * t0);

		cout<<"With Prior:"<<endl;
		cout<<t<<endl;
		cout<<P<<endl;

	}

	fprintf(log_file,"* \n");
	fclose(log_file);
	move(0,-20,true);
	spinner.stop();
	ros::Duration(3).sleep();
	return 0;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// 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)

#ifndef BOOST_UNITS_SI_PREFIXES_HPP
#define BOOST_UNITS_SI_PREFIXES_HPP

#include <boost/units/static_constant.hpp>

#include <boost/units/make_scaled_unit.hpp>
#include <boost/units/systems/si/dimensionless.hpp>

namespace boost {

namespace units {

namespace si {

#define BOOST_UNITS_METRIC_PREFIX(exponent, name)                                                       \
    typedef make_scaled_unit<dimensionless, scale<10, static_rational<exponent> > >::type name ## _type;\
    BOOST_UNITS_STATIC_CONSTANT(name, name ## _type)

BOOST_UNITS_METRIC_PREFIX(-24, yocto);
BOOST_UNITS_METRIC_PREFIX(-21, zepto);
BOOST_UNITS_METRIC_PREFIX(-18, atto);
BOOST_UNITS_METRIC_PREFIX(-15, femto);
BOOST_UNITS_METRIC_PREFIX(-12, pico);
BOOST_UNITS_METRIC_PREFIX(-9, nano);
BOOST_UNITS_METRIC_PREFIX(-6, micro);
BOOST_UNITS_METRIC_PREFIX(-3, milli);
BOOST_UNITS_METRIC_PREFIX(-2, centi);
BOOST_UNITS_METRIC_PREFIX(-1, deci);
BOOST_UNITS_METRIC_PREFIX(1, deka);
BOOST_UNITS_METRIC_PREFIX(2, hecto);
BOOST_UNITS_METRIC_PREFIX(3, kilo);
BOOST_UNITS_METRIC_PREFIX(6, mega);
BOOST_UNITS_METRIC_PREFIX(9, giga);
BOOST_UNITS_METRIC_PREFIX(12, tera);
BOOST_UNITS_METRIC_PREFIX(15, peta);
BOOST_UNITS_METRIC_PREFIX(18, exa);
BOOST_UNITS_METRIC_PREFIX(21, zetta);
BOOST_UNITS_METRIC_PREFIX(24, yotta);

/*BOOST_UNITS_STATIC_CONSTANT(yocto,long double) = (1e-24);       ///< metric prefix for 1.0e-24
BOOST_UNITS_STATIC_CONSTANT(zepto,long double) = (1e-21);       ///< metric prefix for 1.0e-21
BOOST_UNITS_STATIC_CONSTANT(atto,long double) = (1e-18);        ///< metric prefix for 1.0e-18
BOOST_UNITS_STATIC_CONSTANT(femto,long double) = (1e-15);       ///< metric prefix for 1.0e-15
BOOST_UNITS_STATIC_CONSTANT(pico,long double) = (1e-12);        ///< metric prefix for 1.0e-12
BOOST_UNITS_STATIC_CONSTANT(nano,long double) = (1e-9);         ///< metric prefix for 1.0e-9
BOOST_UNITS_STATIC_CONSTANT(micro,long double) = (1e-6);        ///< metric prefix for 1.0e-6
BOOST_UNITS_STATIC_CONSTANT(milli,long double) = (1e-3);        ///< metric prefix for 1.0e-3
BOOST_UNITS_STATIC_CONSTANT(centi,long double) = (1e-2);        ///< metric prefix for 1.0e-2
BOOST_UNITS_STATIC_CONSTANT(deci,long double) = (1e-1);         ///< metric prefix for 1.0e-1
BOOST_UNITS_STATIC_CONSTANT(deka,long double) = (1e1);          ///< metric prefix for 1.0e+1
BOOST_UNITS_STATIC_CONSTANT(hecto,long double) = (1e2);         ///< metric prefix for 1.0e+2
BOOST_UNITS_STATIC_CONSTANT(kilo,long double) = (1e3);          ///< metric prefix for 1.0e+3
BOOST_UNITS_STATIC_CONSTANT(mega,long double) = (1e6);          ///< metric prefix for 1.0e+6
BOOST_UNITS_STATIC_CONSTANT(giga,long double) = (1e9);          ///< metric prefix for 1.0e+9
BOOST_UNITS_STATIC_CONSTANT(tera,long double) = (1e12);         ///< metric prefix for 1.0e+12
BOOST_UNITS_STATIC_CONSTANT(peta,long double) = (1e15);         ///< metric prefix for 1.0e+15
BOOST_UNITS_STATIC_CONSTANT(exa,long double) = (1e18);          ///< metric prefix for 1.0e+18
BOOST_UNITS_STATIC_CONSTANT(zetta,long double) = (1e21);        ///< metric prefix for 1.0e+21
BOOST_UNITS_STATIC_CONSTANT(yotta,long double) = (1e24);        ///< metric prefix for 1.0e+24 */

} // namespace si

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_SI_PREFIXES_HPP


/**
 ** Copyright (c) 2014 Illumina, Inc.
 **
 ** This file is part of Illumina's Enhanced Artificial Genome Engine (EAGLE),
 ** covered by the "BSD 2-Clause License" (see accompanying LICENSE file)
 **
 ** \description Generation of random/uniform intervals with precise boundaries
 ** Based on:
 **       [1] Jon Louis Bentley and James B. Saxe. 1980. Generating Sorted Lists of Random Numbers.
 **           ACM Trans. Math. Softw. 6, 3 (September 1980), 359-364.
 **           DOI=10.1145/355900.355907 http://doi.acm.org/10.1145/355900.355907
 **
 ** \author Lilian Janin
 **/

#include <numeric>

#include <boost/foreach.hpp>
#include <boost/format.hpp>

#include "common/Exceptions.hh"
#include "model/IntervalGenerator.hh"

using namespace std;

namespace eagle
{
namespace model
{

// Class RandomSequenceGenerator

unsigned long RandomSequenceGenerator::getNext()
{
    return static_cast<unsigned long>( floor(getNextAsDouble()) );
}

bool RandomSequenceGenerator::hasFinished()
{
    return (remainingSampleCount_ <= 0);
}

double RandomSequenceGenerator::getNextAsDouble()
{
    if (remainingSampleCount_ <= 0)
    {
        BOOST_THROW_EXCEPTION( eagle::common::EagleException( 0, "RandomSequenceGenerator: all samples have already been generated") );
    }
    double rand0to1 = (1.0+rand())/(1.0+RAND_MAX);
    curMax_ *= exp(log(rand0to1)/remainingSampleCount_);
    remainingSampleCount_--;
    return (1-curMax_)*intervalSize_;
}

// Class RandomIntervalGenerator

pair< unsigned long, unsigned int > RandomIntervalGenerator::getNext( const signed long testValue )
{
    unsigned long intervalNum = (testValue<0)?randomSeq_.getNext():testValue;

    // Convert intervalNum to interval
    while ( intervalNum > currentContigIntervalInfo_->lastInterval || currentContigIntervalInfo_->lastInterval == 0xFFFFFFFFFFFFFFFF )
    {
        ++currentContigIntervalInfo_;
        if (verbose_)
        {
            clog << "Generating fragments for chromosome starting at global pos " << currentContigIntervalInfo_->firstIntervalGlobalPos << endl;
        }
    }
    assert( intervalNum >= currentContigIntervalInfo_->firstInterval );

    pair< unsigned long, unsigned int > p;
    if ( intervalNum < currentContigIntervalInfo_->firstSmallerInterval )
    {
        unsigned long localIntervalNum = intervalNum - currentContigIntervalInfo_->firstInterval;
        p.first  = currentContigIntervalInfo_->firstIntervalGlobalPos + localIntervalNum / intervalsPerNormalPosition_;
        p.second = minFragmentLength_ + localIntervalNum % intervalsPerNormalPosition_;
    }
    else
    {
//        static unsigned long lastSmallerIntervalNum = -1;
//        if ( currentContigIntervalInfo_->firstSmallerInterval != lastSmallerIntervalNum )
        {
            unsigned long localIntervalNum = intervalNum - currentContigIntervalInfo_->firstSmallerInterval;
            unsigned int intervalCountForThisPos = maxFragmentLength_ - minFragmentLength_;
            unsigned long currentPos = currentContigIntervalInfo_->firstSmallerIntervalGlobalPos;
            while (intervalCountForThisPos > 0)
            {
                if ( localIntervalNum < intervalCountForThisPos )
                {
                    p.first  = currentPos;
                    p.second = localIntervalNum + minFragmentLength_;
                    break;
                }
                localIntervalNum -= intervalCountForThisPos;
                --intervalCountForThisPos;
                ++currentPos;
            }
            assert( intervalCountForThisPos>0 && "should never happen" );
        }
    }
    return p;
}

unsigned long RandomIntervalGenerator::getIntervalCount()
{
    unsigned long intervalCount = 0;
    unsigned long globalPos = 0;
    BOOST_FOREACH( const unsigned long l, contigLengths_ )
    {
        struct ContigIntervalInfo intervalInfo;
        intervalInfo.firstInterval = intervalCount;
        intervalInfo.firstIntervalGlobalPos = globalPos;

        if ( l+1 >= maxFragmentLength_ )
        {
            unsigned long normalPositionCount = l - maxFragmentLength_ + 1;
            intervalCount += intervalsPerNormalPosition_ * normalPositionCount;
            intervalInfo.firstSmallerInterval = intervalCount;
            intervalCount += intervalsPerNormalPosition_ * (intervalsPerNormalPosition_-1) / 2;

            intervalInfo.firstSmallerIntervalGlobalPos = globalPos + normalPositionCount;
        }
        else
        {
            intervalInfo.firstSmallerIntervalGlobalPos = 0;
            EAGLE_WARNING( "Chromosome shorter than insert length" );
        }
        intervalInfo.lastInterval = intervalCount - 1;

        if (verbose_)
        {
            clog << "Chromosome at global pos " << globalPos << " has " << (intervalInfo.lastInterval-intervalInfo.firstInterval+1) << " intervals" << endl;
        }
        globalPos += l;
        contigIntervalInfo_.push_back( intervalInfo );
    }
    return intervalCount;
}

// Class RandomIntervalGeneratorUsingIntervalLengthDistribution

pair< unsigned long, unsigned int > RandomIntervalGeneratorUsingIntervalLengthDistribution::getNext( const signed long testValue )
{
    assert( testValue < 0 ); // positive test case not implemented

    if (randomSeq_.hasFinished())
    {
        return make_pair(0,0);
    }
    double randomPos = randomSeq_.getNextAsDouble();

    // randomPos => currentPos
    double probaOfCurrentPos = getIntervalsProbaAtPos( currentPos_ );
    while (randomPos >= accumulatedProbasUntilCurrentPos_ + probaOfCurrentPos)
    {
        accumulatedProbasUntilCurrentPos_ += probaOfCurrentPos;
        ++currentPos_;
        probaOfCurrentPos = getIntervalsProbaAtPos( currentPos_ );
    }

    // randomPos(-currentPos) => fragmentLength
    unsigned int fragmentLength = fragmentLengthDist_.min();
    double probaDiff = randomPos - accumulatedProbasUntilCurrentPos_;
    while (probaDiff > fragmentLengthDist_[fragmentLength])
    {
        probaDiff -= fragmentLengthDist_[fragmentLength++];
    }

    return make_pair( currentPos_, fragmentLength );
}

double RandomIntervalGeneratorUsingIntervalLengthDistribution::getIntervalsProbaAtPos( unsigned long globalPos )
{
    assert( contigLengths_.size() == 1 ); // We restrict the current implementation to one contig at a time
    const unsigned long contigLength = contigLengths_[0];

    if (globalPos + fragmentLengthDist_.max() < contigLength)
    {
        return 1.0;
    }
    else
    {
        unsigned int distanceToContigEnd = contigLength - globalPos;
        unsigned int index = fragmentLengthDist_.max() - distanceToContigEnd;
        if (index >= probas_.size())
        {
            // We expect to build the index 1 entry at a time( index == probas_.size() most of the time), but we need to skip some initial entries for chromosomes smaller than the max template length
            if (index > probas_.size())
            {
                probas_.resize( index, 0 );
            }

            double p = 0;
            for (unsigned int i=fragmentLengthDist_.min(); i<=distanceToContigEnd; ++i)
            {
                p += fragmentLengthDist_[i];
            }
            probas_.push_back( p );
//            clog << (boost::format("Adding proba %f at index %d (for globalPos %d)") % p % index % globalPos).str() << endl;
        }
        return probas_[index];
    }
}

double RandomIntervalGeneratorUsingIntervalLengthDistribution::getTotalIntervalsProba()
{
    assert( contigLengths_.size() == 1 ); // We restrict the current implementation to one contig at a time
    const unsigned long contigLength = contigLengths_[0];

    double intervalCount = 0;
    for (unsigned long globalPos=0; globalPos<contigLength; ++globalPos)
    {
        intervalCount += getIntervalsProbaAtPos( globalPos );
    }
    if (verbose_)
    {
        EAGLE_DEBUG( 0, "Contig's total intervals probability: " << intervalCount );
    }
    return intervalCount;
}

// Class UniformIntervalGenerator

UniformIntervalGenerator::UniformIntervalGenerator( const vector<unsigned long>& contigLengths, const unsigned int medianFragmentLength, const double step, unsigned long& readCount, bool verbose)
    : IntervalGenerator( contigLengths, readCount, medianFragmentLength, medianFragmentLength, medianFragmentLength, verbose)
    , step_( step )
{
    unsigned long intervalCount = 0;
    unsigned long globalPos = 0;
    BOOST_FOREACH( const unsigned long l, contigLengths_ )
    {
        struct ContigIntervalInfo intervalInfo;
        intervalInfo.firstGlobalPos = globalPos;

        unsigned long contigValidPosCount = l - medianFragmentLength + 1;
        intervalCount += contigValidPosCount;
        intervalInfo.lastGlobalPos = globalPos + contigValidPosCount - 1;

        globalPos += l;
        contigIntervalInfo_.push_back( intervalInfo );
    }

    currentContigIntervalInfo_ = contigIntervalInfo_.begin();
    currentGlobalPos_ = 0 - step_;

    unsigned long newReadCount = static_cast<unsigned long>( (double)intervalCount / step_ );
    if (verbose_)
    {
        clog << (boost::format("Uniform coverage attempts to achieve the specified coverage depth for all the chromosome positions, which is impossible to achieve at the chromosome extremities => the average coverage depth will be lower than specified. Changing read count from %d to %d, for a step of %d") % readCount % newReadCount % step_).str() << endl;
    }
    readCount = newReadCount;
}

pair< unsigned long, unsigned int > UniformIntervalGenerator::getNext( const signed long testValue )
{
    currentGlobalPos_ += step_;
    // Convert intervalNum to interval
    while ( floor(currentGlobalPos_) > currentContigIntervalInfo_->lastGlobalPos )
    {
        if ( currentContigIntervalInfo_+1 != contigIntervalInfo_.end() )
        {
            ++currentContigIntervalInfo_;
            currentGlobalPos_ = currentContigIntervalInfo_->firstGlobalPos;
            if (verbose_)
            {
                clog << "Generating fragments for chromosome starting at global pos " << currentContigIntervalInfo_->firstGlobalPos << endl;
            }
        }
        else
        {
            return make_pair( 0, 0 );
            //EAGLE_WARNING( "Too many reads generated at the end!" );
        }
    }
    assert( floor(currentGlobalPos_) >= currentContigIntervalInfo_->firstGlobalPos );

    return std::make_pair( (unsigned long)floor(currentGlobalPos_), medianFragmentLength_ );
}

pair< unsigned long, unsigned int > RandomIntervalGeneratorFromProbabilityMatrix::getNext( const signed long testValue )
{
    if (remainingSampleCount_ > 0)
    {
        double rand0to1 = (1.0+rand())/(1.0+RAND_MAX);
        curMax_ *= exp(log(rand0to1)/remainingSampleCount_);
        remainingSampleCount_--;
        double choice = (1-curMax_) * intervalSize_;

        lastChoiceRemainder += choice - lastChoice;
        while (lastChoiceRemainder >= fragmentLengthProbabilityMatrix_.getProbabilities()[ lastChoiceIndex ])
        {
            lastChoiceRemainder -= fragmentLengthProbabilityMatrix_.getProbabilities()[ lastChoiceIndex ];
            if (lastChoiceIndex < fragmentLengthProbabilityMatrix_.getProbabilities().size())
            {
                ++lastChoiceIndex;
            }
            else
            {
                // Avoid going past the end of the array because of rounding errors
                break;
            }
        }
        lastChoice = choice;
        unsigned long pickedGlobalPos = lastChoiceIndex / fragmentLengthDist_size;
        unsigned long pickedFragmentLength = lastChoiceIndex % fragmentLengthDist_size + fragmentLengthDist_min;
        cout << (boost::format("remainingSampleCount_=%d, choice=%f, choiceIndex=%d, pickedGlobalPos=%d, pickedFragmentLength=%d") % remainingSampleCount_ % choice % lastChoiceIndex % pickedGlobalPos % pickedFragmentLength).str() << endl;
        return make_pair( pickedGlobalPos, pickedFragmentLength );
    }
    else
    {
        EAGLE_WARNING( "Too many reads generated at the end!" );
        return make_pair( 0, 0 );
    }
}

} // namespace model
} // namespace eagle


// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_LOCALOPERATOR_INTEGRAL_HH
#define DUNE_GDT_LOCALOPERATOR_INTEGRAL_HH

#include <vector>
#include <utility>
#include <type_traits>
#include <limits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/densematrix.hh>

#include <dune/geometry/quadraturerules.hh>

#include <dune/stuff/functions/interfaces.hh>

#include "../localevaluation/interface.hh"
#include "interface.hh"

namespace Dune {
namespace GDT {
namespace LocalOperator {

// forward, to be used in the traits
template< class BinaryEvaluationImp >
class Codim0Integral;

namespace internal {

template< class BinaryEvaluationImp >
class Codim0IntegralTraits
{
  static_assert(std::is_base_of< LocalEvaluation::Codim0Interface< typename BinaryEvaluationImp::Traits, 2 >,
                                 BinaryEvaluationImp >::value,
                "BinaryEvaluationImp has to be derived from LocalEvaluation::Codim0Interface< ..., 2 >!");
public:
  typedef Codim0Integral< BinaryEvaluationImp > derived_type;
};

} // namespace internal

template< class BinaryEvaluationImp >
class Codim0Integral
  : public LocalOperator::Codim0Interface< internal::Codim0IntegralTraits< BinaryEvaluationImp > >
{
public:
  typedef internal::Codim0IntegralTraits< BinaryEvaluationImp > Traits;
  typedef BinaryEvaluationImp                                   BinaryEvaluationType;

private:
  static const size_t numTmpObjectsRequired_ = 1;

public:
  template< class... Args >
  explicit Codim0Integral(Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(0)
  {}

  template< class... Args >
  explicit Codim0Integral(const int over_integrate, Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(boost::numeric_cast< size_t >(over_integrate))
  {}

  template< class... Args >
  explicit Codim0Integral(const size_t over_integrate, Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(over_integrate)
  {}

  size_t numTmpObjectsRequired() const
  {
    return numTmpObjectsRequired_;
  }

  template< class E, class D, size_t d, class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  void apply(const Stuff::LocalfunctionSetInterface< E, D, d, R, rT, rCT >& testBase,
             const Stuff::LocalfunctionSetInterface< E, D, d, R, rA, rCA >& ansatzBase,
             Dune::DynamicMatrix< R >& ret,
             std::vector< Dune::DynamicMatrix< R > >& tmpLocalMatrices) const
  {
    const auto& entity = ansatzBase.entity();
    const auto localFunctions = evaluation_.localFunctions(entity);
    // quadrature
    typedef Dune::QuadratureRules< D, d > VolumeQuadratureRules;
    typedef Dune::QuadratureRule< D, d > VolumeQuadratureType;
    const size_t integrand_order = evaluation_.order(localFunctions, ansatzBase, testBase) + over_integrate_;
    const VolumeQuadratureType& volumeQuadrature = VolumeQuadratureRules::rule(entity.type(),
                                                                               boost::numeric_cast< int >(integrand_order));
    // check matrix and tmp storage
    const size_t rows = testBase.size();
    const size_t cols = ansatzBase.size();
    ret *= 0.0;
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    assert(tmpLocalMatrices.size() >= numTmpObjectsRequired_);
    auto& evaluationResult = tmpLocalMatrices[0];
    // loop over all quadrature points
    const auto quadPointEndIt = volumeQuadrature.end();
    for (auto quadPointIt = volumeQuadrature.begin(); quadPointIt != quadPointEndIt; ++quadPointIt) {
      const Dune::FieldVector< D, d > x = quadPointIt->position();
      // integration factors
      const double integrationFactor = entity.geometry().integrationElement(x);
      const double quadratureWeight = quadPointIt->weight();
      // evaluate the local operation
      evaluation_.evaluate(localFunctions, ansatzBase, testBase, x, evaluationResult);
      // compute integral
      for (size_t ii = 0; ii < rows; ++ii) {
        auto& retRow = ret[ii];
        const auto& evaluationResultRow = evaluationResult[ii];
        for (size_t jj = 0; jj < cols; ++jj)
          retRow[jj] += evaluationResultRow[jj] * integrationFactor * quadratureWeight;
      } // compute integral
    } // loop over all quadrature points
  } // ... apply(...)

private:
  const BinaryEvaluationType evaluation_;
  const size_t over_integrate_;
}; // class Codim0Integral

} // namespace LocalOperator
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_LOCALOPERATOR_INTEGRAL_HH


#include <uwb_msgs/uwb.h>
#include <nav_msgs/Odometry.h>
#include <can_msgs/Frame.h>
#include <string>
#include <ros/ros.h>
#include <math.h>
#include <Eigen/Eigen>

using namespace Eigen;

#define MAX_STATION_NUM     6
#define EPSILON             std::numeric_limits<double>::epsilon()

ros::Publisher uwb_publisher;
ros::Publisher uwb_pub_odom;
bool uwb_start = false;
int uwb_index = 0;
double temp_pos_x = 0;
double temp_pos_y = 0;
double temp_pos_theta = 0;
double temp_distance[MAX_STATION_NUM];

uint16_t temp_error = 0;
const Eigen::MatrixXd P = (Eigen::MatrixXd(4, 3)
        <<
        0.0, 0.0, 2.0,
        5.0, 0.0, 2.0,
        0.0, 8.0, 2.03,
        5.0, 8.0, 2.03).finished();

void msgCallback(const can_msgs::Frame &f) {
    if (uwb_start) {
        if (f.dlc == 8) {
            uwb_index++;
            if (uwb_index == 1) {
                temp_pos_x =
                        ((int16_t)((uint16_t) f.data[1] << 8 | (uint16_t) f.data[0])) / 100.0; // in meter
                temp_pos_y =
                        ((int16_t)((uint16_t) f.data[3] << 8 | (uint16_t) f.data[2])) / 100.0; // in meter
                temp_pos_theta =
                        (uint16_t)((uint16_t) f.data[5] << 8 | (uint16_t) f.data[4]) * (2 * M_PI) / (36000); // in rad
                temp_distance[0] = ((int16_t)((uint16_t) f.data[7] << 8 | (uint16_t) f.data[6])) / 100.0; // in meter
            } else if (uwb_index == 2) {
                temp_distance[1] = ((int16_t)((uint16_t) f.data[1] << 8 | (uint16_t) f.data[0])) / 100.0; // in meter
                temp_distance[2] = ((int16_t)((uint16_t) f.data[3] << 8 | (uint16_t) f.data[2])) / 100.0; // in meter
                temp_distance[3] = ((int16_t)((uint16_t) f.data[5] << 8 | (uint16_t) f.data[4])) / 100.0; // in meter
                temp_distance[4] = ((int16_t)((uint16_t) f.data[7] << 8 | (uint16_t) f.data[6])) / 100.0; // in meter
            }
        } else if (f.dlc == 6) {
            uwb_index = 0;

            temp_distance[5] = ((int16_t)((uint16_t) f.data[1] << 8 | (uint16_t) f.data[0])) / 100.0; // in meter
            temp_error = (uint16_t)((uint16_t) f.data[2] << 8 | (uint16_t) f.data[1]);

            uwb_msgs::uwb uwb_msg;
            uwb_msg.header.stamp = f.header.stamp;
            uwb_msg.header.frame_id = "world";
            uwb_msg.pos_x = temp_pos_x;
            uwb_msg.pos_y = temp_pos_y;
            uwb_msg.pos_theta = temp_pos_theta;
            uwb_msg.num = 0;

            for (int i = 0; i < MAX_STATION_NUM; i++) {
                uwb_msg.distance[i] = temp_distance[i];
                if (temp_distance[i] > EPSILON)
                    uwb_msg.num++;
            }
            uwb_msg.check_failed = ((temp_error >> 3) & 1);
            uwb_msg.need_calibrated = ((temp_error >> 2) & 1);
            uwb_msg.level = (uint8_t)(temp_error >> 14);

            Vector4d d(temp_distance[0], temp_distance[1], temp_distance[2], temp_distance[3]);
            Vector3d x = ((P.transpose() * P).inverse()) * P.transpose() * d;

            uwb_msg.pos_x_self = x(0);
            uwb_msg.pos_y_self = x(1);
            uwb_msg.pos_z_self = x(2);
            uwb_msg.pos_theta_self = 0;

            uwb_publisher.publish(uwb_msg);

            // For debugging purpose
            nav_msgs::Odometry odom;
            odom.header.stamp = f.header.stamp;
            odom.header.frame_id = "world";
            odom.pose.pose.position.x = temp_pos_x;
            odom.pose.pose.position.y = temp_pos_y;
            odom.pose.pose.orientation.z = temp_pos_theta;
            uwb_pub_odom.publish(odom);
        }
    }
    if (f.dlc == 6 && !uwb_start) {
        uwb_start = true;
    }
}

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "uwb_driver_node");
    ros::NodeHandle nh("~"), nh_param("~");

    std::string can_device;
    nh_param.param<std::string>("can_device", can_device, "can0");

    uwb_publisher = nh.advertise<uwb_msgs::uwb>("info", 100);
    uwb_pub_odom = nh.advertise<nav_msgs::Odometry>("odom", 100);
    ros::Subscriber can_subscriber = nh.subscribe("/" + can_device + "_raw", 100, msgCallback);

    ros::spin();
}


//  Copyright (c)        2012 Zach Byerly
//  Copyright (c) 2011 - 2012 Bryce Adelstein-Lelbach
//  Copyright (c) 2012    Jonathan Parziale
//  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)

//  This is a 1 dimensional hydrodynamics code, using a simple first-order
//  upwind advection scheme (to keep dependencies simple).  1st order in time.
//  It employs a predictive timestep method in order to eliminate global
//  barriers after every timestep. No gravity. A dual-energy formalism is also
//  used to allow for heating in the shocks.

// INCLUDES
#include <hpx/hpx_init.hpp>
#include <hpx/runtime/actions/plain_action.hpp>
#include <hpx/runtime/components/plain_component_factory.hpp>
#include <hpx/include/async.hpp>
#include <hpx/include/iostreams.hpp>
#include <hpx/lcos/future_wait.hpp>
#include <hpx/lcos/wait_each.hpp>

#include <boost/format.hpp>
#include <boost/math/constants/constants.hpp>

using hpx::naming::id_type;
using hpx::naming::invalid_id;

using hpx::lcos::future;
using hpx::lcos::wait;
using hpx::lcos::wait_each;
using hpx::async;

using hpx::util::high_resolution_timer;

using boost::program_options::variables_map;
using boost::program_options::options_description;
using boost::program_options::value;

using hpx::init;
using hpx::finalize;
using hpx::find_here;

using hpx::cout;
using hpx::flush;

// USING STATEMENTS

////////////////////////////////////////
// globals

// initialized in hpx_main
id_type here = invalid_id;
boost::uint64_t nt = 0;
boost::uint64_t nx = 0;
boost::uint64_t n_predict = 0;
double fluid_gamma = 0.0;
double x_min = 0.0;
double x_max = 0.0;
double dx = 0.0;
double cfl_factor = 0.0;
double cfl_predict_factor = 0.0;
double ptime;

// this is the fundimental element of the hydrodynamics code, the
// individual cell.  It stores all of the variables that the code
// tracks their changes in time.
struct cell{
  // default constructor
  cell()
    : mtx()
    , rho(0.0)    // mass density
    , mom(0.0)    // momentum density
    , etot(0.0)   // total energy density
    , tau(0.0)   // internal energy density (etot - kinetic energy)
    , computed(false)
  {}

  // copy constructor
  cell(
       cell const& other
       )
    : mtx()
    , rho(other.rho)
    , mom(other.mom)
    , etot(other.etot)
    , tau(other.tau)
    , computed(other.computed)
  {}

  // assignment operator:
  //
  //   cell c1, c2;
  //   c1 = c2; // invoked by this syntax
  //  Bryce:  I think i'd like to change this so the "calculated" is not copied in the assignment op
  cell& operator=(
       cell const& other
       )
  {
    // first, we lock both the mutex of this cell, and the mutex of the other
    // cell
    //    hpx::lcos::local::mutex::scoped_lock this_lock(mtx), other_lock(other.mtx);

    rho = other.rho;
    mom = other.mom;
    etot = other.etot;
    tau = other.tau;
    computed = other.computed;

    // return a reference to ourselves
    return *this;
  }

  // dummy serialization functionality
  template <typename Archive>
  void serialize(Archive &, unsigned) {}

  mutable hpx::lcos::local::mutex mtx;
  double rho;
  double mom;
  double etot;
  double tau;
  bool computed;
};
// this is a single element of a "time array" that will be 1D array
// containing information about timestep size. the index will be
// the integer timestep number, so time_array[timestep].dt will be
// the timestep size at that timestep
struct time_element{
  // default constructor
  time_element()
    : dt(0.0)
    , elapsed_time(0.0)
    , computed(false),fluid_future(0),fluid(0)
  {}

  time_element(boost::uint64_t number_of_cells)
      :fluid_future(number_of_cells)
      ,fluid(number_of_cells)
      {}
  // copy constructor
  time_element(
       time_element const& other
       )
    : dt(other.dt)                     // timestep size
    , elapsed_time(other.elapsed_time) // elapsed time
    , computed(other.computed)
    ,fluid_future(other.fluid_future)
    ,fluid(other.fluid)
    {}

  time_element& operator=(time_element const& rhs)
  {
    if (this != &rhs)
    {
      dt = rhs.dt;
      elapsed_time = rhs.elapsed_time;
      computed = rhs.computed;
      fluid=rhs.fluid;
      fluid_future=rhs.fluid_future;

    }
    return *this;
  }

  hpx::lcos::local::mutex mtx;
  double dt;
  double elapsed_time;
  bool computed;
  double physics_time;
  std::vector<hpx::lcos::shared_future<cell> > fluid_future;//future for each cell
  std::vector<cell> fluid;
};
// declaring time_array

// Object to store the Fluid seperated in cell and computed by a Time Zone of tie Steps
// Will store the 2d grid created by the division of the fluid into cells and the computation over time
// Will be able to Retrieve,remove,add a timestep to the grid

class One_Dimension_Grid
{
public:
    One_Dimension_Grid():time_array(0)
    {}
    // ~One_Dimension_Grid();
    void remove_bottom_time_step();//takes the timesteps position in the vector
    void addNewTimeStep();

public:
   std::vector<time_element> time_array;//pointer to the Grid we will create whden the user starts a simulation

};
void One_Dimension_Grid::remove_bottom_time_step()
{
    time_array.pop_back();
}
void One_Dimension_Grid::addNewTimeStep()
{
    time_array.insert(time_array.begin(),time_array.at(nx-1));
}
/*One_Dimension_Grid::~One_Dimension_Grid()
{
    for(boost::uint64_t i=0;i<nt;i++)
    {
        time_array.pop_back();
    }
    delete &time_array;
}*/

// declaring grid of all cells for all timesteps
One_Dimension_Grid grid;
// forward declaration of the compute function
cell compute(boost::uint64_t timestep, boost::uint64_t location);
double timestep_size(boost::uint64_t timestep);
cell initial_sod(boost::uint64_t location);
double get_pressure(cell input);

// Wrapping in plain_action
HPX_PLAIN_ACTION(compute);

typedef hpx::lcos::future<cell> compute_future;

// this will return the timestep size.  The timestep index will refer to the
// timestep where it will be USED rather than the timestep where it was
// calculated.
double timestep_size(boost::uint64_t timestep)
{
  // locking

  hpx::lcos::local::mutex::scoped_lock l(grid.time_array.at(timestep).mtx);

  // if it has already been calculated, then just return the value
  if (grid.time_array.at(timestep).computed)
  {return grid.time_array.at(timestep).dt;}
  //  cout << (boost::format("calculating timestep, ts=%1% \n") % timestep) << flush;

  // if the current timestep is less than n_predict, then we manually
  // decide the timestep
  if (timestep < n_predict)
    {
      grid.time_array.at(timestep).computed = true;
      grid.time_array.at(timestep).dt = dx*0.033;// this should be fine unless
        // the initial conditions are changed
        if(timestep>0&&grid.time_array.at(timestep-1).computed)
            grid.time_array[timestep].physics_time = (grid.time_array.at(timestep-1).physics_time+grid.time_array.at(timestep).dt);
  //    time_array[timestep].dt = cfl_predict_factor*dt_cfl;
        else if(timestep==0)
        {
            grid.time_array.at(timestep).physics_time=grid.time_array.at(timestep).dt;
        }
      return grid.time_array.at(timestep).dt;
    }

  // send back the compute futures for the whole grid
  // n_predict timesteps previous to the one we want to decide
  // the timestep for
  //  cout << (boost::format("pushing back futures for ts calc, ts=%1% \n") % timestep) << flush;
  if(timestep>=n_predict)
  {
  for (boost::uint64_t i=0;i<nx;i++)
      grid.time_array.at(timestep).fluid_future.push_back(async<compute_action>(here,timestep-n_predict,i));
  }

  double dt_cfl = 1000.0;

   wait_each(
      hpx::util::unwrapped([&](cell const& this_cell)
      {
      // look at all of the cells at a timestep, then pick the smallest
      // dt_cfl = cfl_factor*dx/(soundspeed+absolute_velocity)
      double abs_velocity = this_cell.mom/this_cell.rho;
      double pressure = get_pressure(this_cell);
      double soundspeed = sqrt(fluid_gamma*pressure/this_cell.rho);
      double dt_cfl_here = cfl_factor*dx/(soundspeed+abs_velocity);
      if (dt_cfl_here <=  0.0)
        {
          cout << (boost::format("error: CFL value can't be zero")) << flush;
          //error, quit everything
        }
      if (dt_cfl_here < dt_cfl)
        dt_cfl = dt_cfl_here;
     }),
     grid.time_array.at(timestep).fluid_future);

  // initialize dt_cfl to some arbitrary high value

  // wait for an array of futures
  /*wait_each(grid.time_array,
    hpx::util::unwrapped([&](cell const& this_cell)
    {
      // look at all of the cells at a timestep, then pick the smallest
      // dt_cfl = cfl_factor*dx/(soundspeed+absolute_velocity)
      double abs_velocity = this_cell.mom/this_cell.rho;
      double pressure = get_pressure(this_cell);
      double soundspeed = sqrt(fluid_gamma*pressure/this_cell.rho);
      double dt_cfl_here = cfl_factor*dx/(soundspeed+abs_velocity);
      if (dt_cfl_here <=  0.0)
        {
          cout << (boost::format("error: CFL value can't be zero")) << flush;
          //error, quit everything
        }
      if (dt_cfl_here < dt_cfl)
        dt_cfl = dt_cfl_here;
    }));
*/

  if(dt_cfl > 999.0)
    {
      cout << (boost::format("error: CFL value too high")) << flush;
      // error, quit everything
    }

  // we don't want to let the timestep increase too quickly, so
  // we only let it increase by 25% each timestep
  grid.time_array.at(timestep).computed = true;
  grid.time_array.at(timestep).dt = (std::min)(
                                     cfl_predict_factor*dt_cfl
                                     ,
                                     1.25*grid.time_array.at(timestep-1).dt);

  //  cout << (boost::format("timestep = %1%, dt = %2%\n") % timestep % time_array[timestep].dt) << flush;
  return grid.time_array[timestep].dt;
}

cell compute(boost::uint64_t timestep, boost::uint64_t location)
{
    hpx::lcos::local::mutex::scoped_lock l(grid.time_array.at(timestep).fluid.at(location).mtx);

  // if it is already computed then just return the value
    if (grid.time_array.at(timestep).fluid.at(location).computed == true)
        return grid.time_array.at(timestep).fluid.at(location);

  //  cout << (boost::format("computing new value, loc = %1%,ts=%2% \n") % location % timestep) << flush;

  //initial values
  if (timestep == 0)
    {
      //  cout << (boost::format("calling initial_sod, loc = %1%,ts=%2% \n") % location % timestep) << flush;
        grid.time_array.at(timestep).fluid.at(location) = initial_sod(location);
      //  cout << (boost::format("returning value, loc = %1%,ts=%2% \n") % location % timestep) << flush;
        grid.time_array.at(timestep).fluid.at(location).computed = true;
        return grid.time_array.at(timestep).fluid.at(location);
    }

   //boundary conditions (using sod shock tube boundaries)
  if ( (location == 0) || (location == nx-1) )
    {
      grid.time_array.at(timestep).fluid.at(location) = initial_sod(location);
      grid.time_array.at(timestep).fluid.at(location).computed = true;
      return grid.time_array.at(timestep).fluid.at(location);
    }

  //now we have to actually compute some values.

  //these are the dependencies, or "stencil"
  //if(timestep<0) // unsigned comparision always false
  //      return grid.time_array.at(timestep).fluid.at(location);

  compute_future nleft = async<compute_action>(here,timestep-1,location-1);
  compute_future nmiddle = async<compute_action>(here,timestep-1,location);
  compute_future nright = async<compute_action>(here,timestep-1,location+1);

  // OR is this the correct way to do it?
  //future<cell> left;
  //left = async<compute_action>(here,timestep-1,location-1);
  //future<cell> middle;
  //middle = async<compute_action>(here,timestep-1,location);
  //future<cell> right;
  //right = async<compute_action>(here,timestep-1,location+1);

  cell now;

  cell left   = nleft.get();
  cell middle = nmiddle.get();
  cell right  = nright.get();

  // calling this function may or may not make futures
  double dt = timestep_size(timestep);

  now.rho = middle.rho;
  now.mom = middle.mom;
  now.etot = middle.etot;
  now.tau = middle.tau;

  // now that we have all of the information we need, we can proceed with
  // the physics part of the update

  double right_pressure = get_pressure(right);
  double left_pressure = get_pressure(left);
  double middle_pressure = get_pressure(middle);

  // first we will calculate the advection of all of the variables
  // through the left face of the cell.
  // start by calculating the velocity on the left face
  // if this velocity is positive, then fluid flows from the cell to the left.
  // if it is negative, fluid flows out of the middle cell.
  double velocity_left = (left.mom+middle.mom)/(left.rho+middle.rho);
  if (velocity_left > 0.0)
    {
      now.rho +=  left.rho*velocity_left*dt/dx;
      now.mom +=  left.mom*velocity_left*dt/dx;
      now.etot += (left.etot+left_pressure)*velocity_left*dt/dx;
      now.tau +=  left.tau*velocity_left*dt/dx;
    }
  else
    {
      now.rho +=  middle.rho*velocity_left*dt/dx;
      now.mom +=  middle.mom*velocity_left*dt/dx;
      now.etot += (middle.etot+middle_pressure)*velocity_left*dt/dx;
      now.tau +=  middle.tau*velocity_left*dt/dx;
    }

  // now repeat the process for the right side
  double velocity_right = (right.mom+middle.mom)/(right.rho+middle.rho);
  if (velocity_right < 0.0)
    {
      now.rho -=  right.rho*velocity_right*dt/dx;
      now.mom -=  right.mom*velocity_right*dt/dx;
      now.etot -= (right.etot+right_pressure)*velocity_right*dt/dx;
      now.tau -=  right.tau*velocity_right*dt/dx;
    }
  else
    {
      now.rho -=  middle.rho*velocity_right*dt/dx;
      now.mom -=  middle.mom*velocity_right*dt/dx;
      now.etot -= (middle.etot+middle_pressure)*velocity_right*dt/dx;
      now.tau -=  middle.tau*velocity_right*dt/dx;
    }

  // source terms
  now.mom += 0.5*dt*(left_pressure - right_pressure)/(dx);

  // check for CFL (courant friedrichs levy) violation (makes code unstable)
  double soundspeed = std::sqrt(fluid_gamma*middle_pressure/middle.rho);
  double abs_velocity = (std::max)(velocity_right,velocity_right);
  double dt_cfl_here = cfl_factor*dx/(soundspeed+abs_velocity);
  if (dt_cfl_here > timestep)
    {
      cout << (boost::format("error! cfl violation!\n")) << flush;
      cout << (boost::format("loc=%1% ts=%2%\n") % location % timestep) << flush;
      cout << (boost::format("dt_cfl_here=%1% dt=%2%\n") % dt_cfl_here % dt ) << flush;

      // Bryce: I should add some real error handling. can you help me with this?
      // error, quit everything
    }

  double e_kinetic = 0.5*middle.mom*middle.mom/middle.rho;
  double e_internal = middle.etot - e_kinetic;
  //dual energy formalism
  if ( std::abs(e_internal) > 0.1*middle.etot)
    {
      //cout << (boost::format("gas is shocking!\n")) << flush;
      now.tau = pow(e_internal,(1.0/fluid_gamma));
    }
  // cout << (boost::format("computing new value, loc = %1%, ts= %2%\n") % location % timestep) << flush;
  // cout << (boost::format("loc = %1%, rho = %2%\n") % location % left.rho) << flush;
  // cout << (boost::format("loc = %1%, mom = %2%\n") % location % left.mom) << flush;
  // cout << (boost::format("loc = %1%, etot = %2%\n") % location % left.etot) << flush;
  // cout << (boost::format("loc = %1%, vel left = %2%\n") % location % velocity_left) << flush;

  //  if (location == 1)
  //    cout << (boost::format("calculating timestep = %1%\n") % timestep) << flush;

  grid.time_array.at(timestep).fluid.at(location) = now;
  grid.time_array.at(timestep).fluid.at(location).computed = true;
  bool time_step_complete= false;
  for(boost::uint64_t i=0;i<nx;i++)
  {
      if(grid.time_array[0].fluid.at(i).computed&&grid.time_array[1].fluid.at(i).computed)
        time_step_complete=true;
      else
          time_step_complete=false;
  }
  if(time_step_complete&&!(grid.time_array.at(nt-1).physics_time>=ptime))
  {
      grid.remove_bottom_time_step();
      grid.addNewTimeStep();
  }

  return grid.time_array.at(timestep).fluid.at(location);
}

double get_pressure(cell input)
{
  double pressure = 0.0;
  double e_kinetic = 0.5*input.mom*input.mom/input.rho;
  double e_internal = input.etot - e_kinetic;

  // dual energy
  if ( std::abs(e_internal) > 0.001*input.etot )
    {
      pressure = (fluid_gamma-1.0)*e_internal;
    }
  else
    {
      pressure = (fluid_gamma-1.0)*pow(input.tau,fluid_gamma);
    }

  return pressure;
}

cell initial_sod(boost::uint64_t location)
{

  //  cout << (boost::format("initial_sod, loc = %1%\n") % location) << flush;

  // calculate what the x coordinate is here
  double x_here = (location-0.5)*dx+x_min;

  cell cell_here;
  double e_internal = 0.0;

  // This is the Sod Shock Tube problem, which has a known analytical
  // solution.  A mass and energy contact discontinuity is placed on the grid.
  // A shockwave then forms and propogates through the grid.
  if (x_here < -0.1)
    {
      cell_here.rho = 1.0;
      e_internal = 2.5;
    }
  else
    {
      cell_here.rho = 0.125;
      e_internal = 0.25;
    }

  cell_here.mom = 0.0;
  cell_here.tau = pow(e_internal,(1.0/fluid_gamma));
  cell_here.etot = e_internal;  // ONLY true when mom=0, not in general!

  //  cout << (boost::format("returning from initial_sod, loc = %1%\n") % location) << flush;
  return cell_here;
}

cell get_analytic(double x_here, double time)
{
  cell output;

  // values for analytic solution come from Patrick Motl's dissertation

  //  cout << (boost::format("calculating analytic... x=%1% t=%2%\n") % x_here % time) << flush;

  double x_0 = -0.1;

  double c_1 = 1.183;
  double x_head = x_0 - c_1*time;

  double w_3 = 0.9274;
  double c_3 = 0.9978;

  double x_tail = x_0 + (w_3 - c_3)*time;

  double w_4 = 0.9274;

  double x_contact = x_0 + w_4*time;

  double c_5 = 1.058;
  double p_4 = 0.3031;
  double p_5 = 0.1;

  double W = c_5*std::pow( (1.0+(fluid_gamma+1.0)*(p_4 - p_5)/(2.0*fluid_gamma*p_5)), 0.5  );

  double x_shock = x_0 + W*time;

  if (x_here < x_head)  // region 1
    {
      output.rho = 1.0;
      output.tau = 1.924;
      output.mom = 0.0;
    }
  else if (x_here < x_tail) // region 2
    {
      double w_2 = 2.0*(c_1+(x_here-x_0)/time)/(fluid_gamma+1.0);
      double exponent = 2.0/(fluid_gamma-1.0);
      output.rho = std::pow( (1.0-(fluid_gamma-1.0)*w_2/(2.0*c_1)), exponent);
      output.mom = output.rho*w_2;
      output.tau = pow( (1.0-0.5*(fluid_gamma-1)*(w_2/c_1)), exponent)/pow(fluid_gamma-1.0,1.0/fluid_gamma);
    }
  else if (x_here < x_contact) // region 3
    {
      output.rho = 0.4263;
      output.mom = 0.9274*output.rho;
      output.tau = 0.8203;
    }
  else if (x_here < x_shock) // region 4
    {
      output.rho = 0.2656;
      output.mom = 0.9274*output.rho;
      output.tau = 0.8203;
    }
  else // region 5
    {
      output.rho = 0.125;
      output.mom = 0.0;
      output.tau = 0.3715;
    }

  return output;
}

///////////////////////////////////////////////////////////////////////////////
int hpx_main(
    variables_map& vm
    )
{
    {
  here = find_here();

  // some physics parameters
  nx = vm["nx-value"].as<boost::uint64_t>();
  nt = vm["nt-value"].as<boost::uint64_t>();
  n_predict = vm["npredict-value"].as<boost::uint64_t>();
  ptime=vm["ptime-value"].as<double>();
  fluid_gamma = 1.4;

  x_min = -0.5;
  x_max = 0.5;
  dx = (x_max - x_min)/(nx-2);

  cfl_factor = 0.5;
  cfl_predict_factor = 0.8;

  cout << (boost::format("nt = %1%\n") % nt) << flush;
  cout << (boost::format("nx = %1%\n") % nx) << flush;
  cout << (boost::format("n_predict = %1%\n") % n_predict) << flush;

  // allocating the time array
  grid.time_array = std::vector<time_element>(nt);

  // allocating the grid 2d array of all of the cells for all timesteps
  for(boost::uint64_t i=0;i<nt;i++)
  {
      grid.time_array[i].fluid=std::vector<cell>(nx);
  }
    //HPX stuff goes here

    // Keep track of the time required to execute.
    high_resolution_timer t;

    timestep_size(0);
    for (boost::uint64_t i=0;i<nx;i++)
           grid.time_array[0].fluid_future.push_back(async<compute_action>(here,nt-1,i));
    // open file for output
    std::ofstream outfile;
    outfile.open ("output.dat");

    wait(grid.time_array[0].fluid_future, [&](std::size_t i, cell n)
         { double x_here = (i-0.5)*dx+x_min;
           double pressure_here = get_pressure(n);
           //double tauoverrho = n.tau/n.rho;
           double velocity_here = n.mom/n.rho;

           double e_kinetic = 0.5*n.mom*n.mom/n.rho;
           double e_internal = n.etot - e_kinetic;
           double tauoverrho =  pow(e_internal,(1.0/fluid_gamma))/n.rho;
           //           double e_internal2 = pow(n.tau,fluid_gamma);

           outfile << (boost::format("%1% %2% %3% %4% %5%\n") % x_here % n.rho % pressure_here % tauoverrho % velocity_here) << flush; });
           //           outfile << (boost::format("%1% %2% %3% %4% %5%\n") % x_here % n.rho % pressure_here % e_internal % e_internal2) << flush; });

    outfile.close();

    std::ofstream outfile2;
    outfile2.open ("time.dat");

    boost::uint64_t i;
    // writing the "time array" to a file
    grid.time_array[0].elapsed_time = grid.time_array[0].dt;
    for (i=1;i<nt;i++)
      grid.time_array[i].elapsed_time = grid.time_array[i-1].elapsed_time + grid.time_array[i].dt;

    for (i =0;i<nt;i++)
      {
        outfile2 << (boost::format("%1% %2% %3%\n") % i % grid.time_array[i].dt % grid.time_array[i].elapsed_time) << flush;
      }
    outfile2.close();

    // writing the analytic solution for the final time to a file
    std::ofstream analytic_file;
    analytic_file.open ("analytic.dat");
    double total_mass = 0.0;
    for (i =0;i<nx;i++)
      {
        double x_here = (i-0.5)*dx+x_min;
        cell analytic = get_analytic(x_here,grid.time_array[nt-1].elapsed_time);
        double velocity_here = analytic.mom/analytic.rho;
        double tauoverrho = analytic.tau/analytic.rho;
        double pressure_here = get_pressure(analytic);
        analytic_file << (boost::format("%1% %2% %3% %4% %5%\n") % x_here % analytic.rho % pressure_here % tauoverrho % velocity_here) << flush;
        total_mass += grid.time_array[nt-1].fluid[i].rho*dx;
      }
    analytic_file.close();

    cout << (boost::format("total mass = %1%\n") % total_mass ) << flush;
    char const* fmt = "wall elapsed time: %1% [s]\n";
    std::cout << (boost::format(fmt) % t.elapsed());
    char const* fmt0 = "code elapsed time: %1%\n";
    double t_code_time= grid.time_array[0].elapsed_time;

        t_code_time+=grid.time_array[nt-1].elapsed_time;

      std::cout << (boost::format(fmt0) %  t_code_time);

  }

  finalize();
  return 0;
}

///////////////////////////////////////////////////////////////////////////////
int main(
    int argc
  , char* argv[]
    )
{
  // Configure application-specific options.
  options_description cmdline("usage: " HPX_APPLICATION_STRING " [options]");

  cmdline.add_options()
    ( "nx-value"
      , value<boost::uint64_t>()->default_value(5)
      , "nx parameter of the wave equation")

    ( "nt-value"
      , value<boost::uint64_t>()->default_value(10)
      , "nt parameter of the wave equation")

    ( "npredict-value"
      , value<boost::uint64_t>()->default_value(10)
      , "prediction parameter of the wave equation")

      ( "ptime-value"
      , value<double>()->default_value(2.50)
      , "Physics time to run the simulation to")
    ;

  // Initialize and run HPX.
  return init(cmdline, argc, argv);
}


#include <boost/program_options.hpp>
#include <iomanip>
#include <iostream>

#include "terrain.h"

namespace po = boost::program_options;

auto get_arguments(int argc, char* argv[]) {
    size_t n {20};
    double distance {30.0};
    double delta_mean {0.02};
    double delta_stddev {0.05};
    double flat_fraction {0.2};
    Seed_t seed {1234};

    po::options_description desc("Create random terrain");
    desc.add_options()
        ("help", "show help message")
        ("n", po::value<size_t>(), "number of points in terrain")
        ("distance", po::value<double>(), "distance of the terrain")
        ("delta_mean", po::value<double>(), "mean altitude change per point")
        ("delta_stddev", po::value<double>(), "standard deviation altitude change per point")
        ("flat_fraction", po::value<double>(), "fraction of the terrain that is flat")
        ("seed", po::value<Seed_t>(), "seed for the random number generator")
    ;
    try {
        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);
        po::notify(vm);

        if (vm.count("help")) {
            std::cout << desc << std::endl;
            std::exit(0);
        }
        if (vm.count("n"))
            n = vm["n"].as<size_t>();
        if (vm.count("distance"))
            distance = vm["distance"].as<double>();
        if (vm.count("delta_mean"))
            delta_mean = vm["delta_mean"].as<double>();
        if (vm.count("delta_stddev"))
            delta_stddev = vm["delta_stddev"].as<double>();
        if (vm.count("flat_fraction"))
            flat_fraction = vm["flat_fraction"].as<double>();
        if (vm.count("seed"))
            seed = vm["seed"].as<Seed_t>();
    } catch(std::exception& e) {
        std::cerr << "error: " << e.what() << std::endl;
        std::exit(1);
    }

    return std::make_tuple(n, distance, delta_mean, delta_stddev,
                           flat_fraction, seed);
}

int main(int argc, char* argv[]) {
    auto [n, distance, delta_mean, delta_stddev,
          flat_fraction, seed] = get_arguments(argc, argv);
    Terrain terrain(n, distance, delta_mean, delta_stddev, flat_fraction, seed);
    std::cout << terrain;
    return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/QR>

template<typename LineType> void parametrizedline(const LineType& _line)
{
  /* this test covers the following files:
     ParametrizedLine.h
  */
  using std::abs;
  typedef typename LineType::Index Index;
  const Index dim = _line.dim();
  typedef typename LineType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, LineType::AmbientDimAtCompileTime, 1> VectorType;
  typedef Hyperplane<Scalar,LineType::AmbientDimAtCompileTime> HyperplaneType;
  typedef Matrix<Scalar, HyperplaneType::AmbientDimAtCompileTime,
                         HyperplaneType::AmbientDimAtCompileTime> MatrixType;

  VectorType p0 = VectorType::Random(dim);
  VectorType p1 = VectorType::Random(dim);

  VectorType d0 = VectorType::Random(dim).normalized();

  LineType l0(p0, d0);

  Scalar s0 = internal::random<Scalar>();
  Scalar s1 = abs(internal::random<Scalar>());

  VERIFY_IS_MUCH_SMALLER_THAN( l0.distance(p0), RealScalar(1) );
  VERIFY_IS_MUCH_SMALLER_THAN( l0.distance(p0+s0*d0), RealScalar(1) );
  VERIFY_IS_APPROX( (l0.projection(p1)-p1).norm(), l0.distance(p1) );
  VERIFY_IS_MUCH_SMALLER_THAN( l0.distance(l0.projection(p1)), RealScalar(1) );
  VERIFY_IS_APPROX( Scalar(l0.distance((p0+s0*d0) + d0.unitOrthogonal() * s1)), s1 );

  // casting
  const int Dim = LineType::AmbientDimAtCompileTime;
  typedef typename GetDifferentType<Scalar>::type OtherScalar;
  ParametrizedLine<OtherScalar,Dim> hp1f = l0.template cast<OtherScalar>();
  VERIFY_IS_APPROX(hp1f.template cast<Scalar>(),l0);
  ParametrizedLine<Scalar,Dim> hp1d = l0.template cast<Scalar>();
  VERIFY_IS_APPROX(hp1d.template cast<Scalar>(),l0);

  // intersections
  VectorType p2 = VectorType::Random(dim);
  VectorType n2 = VectorType::Random(dim).normalized();
  HyperplaneType hp(p2,n2);
  Scalar t = l0.intersectionParameter(hp);
  VectorType pi = l0.pointAt(t);
  VERIFY_IS_MUCH_SMALLER_THAN(hp.signedDistance(pi), RealScalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN(l0.distance(pi), RealScalar(1));
  VERIFY_IS_APPROX(l0.intersectionPoint(hp), pi);

  // transform
  if (!NumTraits<Scalar>::IsComplex)
  {
    MatrixType rot = MatrixType::Random(dim,dim).householderQr().householderQ();
    DiagonalMatrix<Scalar,LineType::AmbientDimAtCompileTime> scaling(VectorType::Random());
    Translation<Scalar,LineType::AmbientDimAtCompileTime> translation(VectorType::Random());

    while(scaling.diagonal().cwiseAbs().minCoeff()<RealScalar(1e-4)) scaling.diagonal() = VectorType::Random();

    LineType l1 = l0;
    VectorType p3 = l0.pointAt(Scalar(1));
    VERIFY_IS_MUCH_SMALLER_THAN( l1.transform(rot).distance(rot * p3), Scalar(1) );
    l1 = l0;
    VERIFY_IS_MUCH_SMALLER_THAN( l1.transform(rot,Isometry).distance(rot * p3), Scalar(1) );
    l1 = l0;
    VERIFY_IS_MUCH_SMALLER_THAN( l1.transform(rot*scaling).distance((rot*scaling) * p3), Scalar(1) );
    l1 = l0;
    VERIFY_IS_MUCH_SMALLER_THAN( l1.transform(rot*scaling*translation)
                                   .distance((rot*scaling*translation) * p3), Scalar(1) );
    l1 = l0;
    VERIFY_IS_MUCH_SMALLER_THAN( l1.transform(rot*translation,Isometry)
                                   .distance((rot*translation) * p3), Scalar(1) );
  }

}

template<typename Scalar> void parametrizedline_alignment()
{
  typedef ParametrizedLine<Scalar,4,AutoAlign> Line4a;
  typedef ParametrizedLine<Scalar,4,DontAlign> Line4u;

  EIGEN_ALIGN_MAX Scalar array1[16];
  EIGEN_ALIGN_MAX Scalar array2[16];
  EIGEN_ALIGN_MAX Scalar array3[16+1];
  Scalar* array3u = array3+1;

  Line4a *p1 = ::new(reinterpret_cast<void*>(array1)) Line4a;
  Line4u *p2 = ::new(reinterpret_cast<void*>(array2)) Line4u;
  Line4u *p3 = ::new(reinterpret_cast<void*>(array3u)) Line4u;

  p1->origin().setRandom();
  p1->direction().setRandom();
  *p2 = *p1;
  *p3 = *p1;

  VERIFY_IS_APPROX(p1->origin(), p2->origin());
  VERIFY_IS_APPROX(p1->origin(), p3->origin());
  VERIFY_IS_APPROX(p1->direction(), p2->direction());
  VERIFY_IS_APPROX(p1->direction(), p3->direction());

  #if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES>0
  if(internal::packet_traits<Scalar>::Vectorizable && internal::packet_traits<Scalar>::size<=4)
    VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(array3u)) Line4a));
  #endif
}

void test_geo_parametrizedline()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( parametrizedline(ParametrizedLine<float,2>()) );
    CALL_SUBTEST_2( parametrizedline(ParametrizedLine<float,3>()) );
    CALL_SUBTEST_2( parametrizedline_alignment<float>() );
    CALL_SUBTEST_3( parametrizedline(ParametrizedLine<double,4>()) );
    CALL_SUBTEST_3( parametrizedline_alignment<double>() );
    CALL_SUBTEST_4( parametrizedline(ParametrizedLine<std::complex<double>,5>()) );
  }
}


#include <vector>
#include <numeric>
#include <iostream>

#include <nt2/sdk/simd/io.hpp>
#include <nt2/sdk/simd/range.hpp>

#include <nt2/sdk/memory/allocate.hpp>
#include <nt2/sdk/memory/allocator.hpp>

#include <boost/range/numeric.hpp>

using namespace std;

int main()
{
  vector<char> tab(48);
  nt2::simd::pack<char> r;

  for(int i=0;i<48;++i) tab[i] = 1;
  r = boost::accumulate(nt2::simd::range(tab), nt2::simd::pack<char>());
  cout << r << "\n";
  cout << boost::accumulate(r,0) << "\n";
}


#include <boost/spirit/home/x3/support/numeric_utils/sign.hpp>


#ifndef INVKIN_H_INCLUDED
#define INVKIN_H_INCLUDED

#include "pinocchio/math/rpy.hpp"
#include "pinocchio/spatial/explog.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>

class InvKin
{
public:
    InvKin();
    InvKin(double dt_in);

    Eigen::Matrix<double, 1, 3> cross3(Eigen::Matrix<double, 1, 3> left, Eigen::Matrix<double, 1, 3> right);

    Eigen::MatrixXd refreshAndCompute(const Eigen::MatrixXd& x_cmd, const Eigen::MatrixXd& contacts,
                                      const Eigen::MatrixXd& goals, const Eigen::MatrixXd& vgoals, const Eigen::MatrixXd& agoals,
                                      const Eigen::MatrixXd& posf, const Eigen::MatrixXd& vf, const Eigen::MatrixXd& wf, const Eigen::MatrixXd& af,
                                      const Eigen::MatrixXd& Jf, const Eigen::MatrixXd& posb, const Eigen::MatrixXd& rotb, const Eigen::MatrixXd& vb,
                                      const Eigen::MatrixXd& ab, const Eigen::MatrixXd& Jb);
    Eigen::MatrixXd computeInvKin(const Eigen::MatrixXd& posf, const Eigen::MatrixXd& vf, const Eigen::MatrixXd& wf, const Eigen::MatrixXd& af,
                                  const Eigen::MatrixXd& Jf, const Eigen::MatrixXd& posb, const Eigen::MatrixXd& rotb, const Eigen::MatrixXd& vb, const Eigen::MatrixXd& ab,
                                  const Eigen::MatrixXd& Jb);
    Eigen::MatrixXd get_q_step();
    Eigen::MatrixXd get_dq_cmd();

private:
    // Inputs of the constructor
    double dt;  // Time step of the contact sequence (time step of the MPC)

    // Matrices initialisation
    Eigen::Matrix<double, 4, 3> feet_position_ref = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 4, 3> feet_velocity_ref = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 4, 3> feet_acceleration_ref = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 1, 4> flag_in_contact = Eigen::Matrix<double, 1, 4>::Zero();
    Eigen::Matrix<double, 3, 3> base_orientation_ref = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 1, 3> base_angularvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 1, 3> base_angularacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 1, 3> base_position_ref = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 1, 3> base_linearvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 1, 3> base_linearacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 6, 1> x_ref = Eigen::Matrix<double, 6, 1>::Zero();
    Eigen::Matrix<double, 6, 1> x = Eigen::Matrix<double, 6, 1>::Zero();
    Eigen::Matrix<double, 6, 1> dx_ref = Eigen::Matrix<double, 6, 1>::Zero();
    Eigen::Matrix<double, 6, 1> dx = Eigen::Matrix<double, 6, 1>::Zero();
    Eigen::Matrix<double, 18, 18> J = Eigen::Matrix<double, 18, 18>::Zero();
    Eigen::Matrix<double, 18, 18> invJ = Eigen::Matrix<double, 18, 18>::Zero();
    Eigen::Matrix<double, 1, 18> acc = Eigen::Matrix<double, 1, 18>::Zero();
    Eigen::Matrix<double, 1, 18> x_err = Eigen::Matrix<double, 1, 18>::Zero();
    Eigen::Matrix<double, 1, 18> dx_r = Eigen::Matrix<double, 1, 18>::Zero();

    Eigen::Matrix<double, 4, 3> pfeet_err = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 4, 3> vfeet_ref = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 4, 3> afeet = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 1, 3> e_basispos = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 1, 3> abasis = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 1, 3> e_basisrot = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 1, 3> awbasis = Eigen::Matrix<double, 1, 3>::Zero();

    Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(18, 1);
    Eigen::MatrixXd q_step = Eigen::MatrixXd::Zero(18, 1);
    Eigen::MatrixXd dq_cmd = Eigen::MatrixXd::Zero(18, 1);

    // Gains
    double Kp_base_orientation = 100.0;
    double Kd_base_orientation = 2.0 * std::sqrt(Kp_base_orientation);

    double Kp_base_position = 100.0;
    double Kd_base_position = 2.0 * std::sqrt(Kp_base_position);

    double Kp_flyingfeet = 1000.0;
    double Kd_flyingfeet = 5.0 * std::sqrt(Kp_flyingfeet);
};

template <typename _Matrix_Type_>
_Matrix_Type_ pseudoInverse(const _Matrix_Type_& a, double epsilon = std::numeric_limits<double>::epsilon())
{
    Eigen::JacobiSVD<_Matrix_Type_> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
    double tolerance = epsilon * static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
    return svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
}
#endif  // INVKIN_H_INCLUDED


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::test_doc_code unit test
#include <libs/icl/test/disable_test_warnings.hpp>

#include <limits>
#include <complex>

#include <string>
#include <vector>
#include <boost/mpl/list.hpp>
#include "../unit_test_unwarned.hpp"
#include <boost/test/test_case_template.hpp>

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"
#include "../test_laws.hpp"

#include <boost/type_traits/is_same.hpp>

#define BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS
#define BOOST_ICL_DISCRETE_STATIC_INTERVAL_DEFAULT right_open_interval
#define BOOST_ICL_DISCRETE_STATIC_INTERVAL_BORDERS 2 //0=() 1=(] 2=[) 3=[]

#include <boost/icl/rational.hpp>

#include <boost/icl/detail/interval_morphism.hpp>
#include <boost/icl/interval_map.hpp>

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

BOOST_AUTO_TEST_CASE(test_intervals_4_changed_lib_defaults)
{
    typedef int T;
    typedef int U;
    typedef interval_map<T,U, total_absorber> IntervalMapT;
    typedef interval_set<T>                   IntervalSetT;
    typedef IntervalMapT::interval_type       IntervalT;

    typedef icl::map<int,int> MapII;

    //const bool xx = is_same< typename icl::map<int,int>::codomain_type,
    //    typename codomain_type_of<icl::map<int,int> >::type >::value;

#if defined(BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS) && !defined(BOOST_ICL_DISCRETE_STATIC_INTERVAL_DEFAULT)
    BOOST_CHECK( (boost::is_same<icl::interval<int   >::type, right_open_interval<int   > >::value) );
    BOOST_CHECK( (boost::is_same<icl::interval<double>::type, right_open_interval<double> >::value) );

    BOOST_CHECK_EQUAL( icl::interval<int>::open(0,2),       icl::construct<right_open_interval<int> >(1,2) );
    BOOST_CHECK_EQUAL( icl::interval<int>::left_open(0,1),  icl::construct<right_open_interval<int> >(1,2) );
    BOOST_CHECK_EQUAL( icl::interval<int>::right_open(1,2), icl::construct<right_open_interval<int> >(1,2) );
    BOOST_CHECK_EQUAL( icl::interval<int>::closed(1,1),     icl::construct<right_open_interval<int> >(1,2) );

    BOOST_CHECK_EQUAL( icl::interval<float>::right_open(1.0,2.0), icl::construct<right_open_interval<float> >(1.0,2.0) );
    //The next yields compiletime error: STATIC_ASSERTION_FAILURE
    //BOOST_CHECK_EQUAL( icl::interval<float>::left_open(1.0,2.0), icl::construct<right_open_interval<float> >(1.0,2.0) );
#endif

#if defined(BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS) && defined(BOOST_ICL_DISCRETE_STATIC_INTERVAL_DEFAULT)
#   if defined(BOOST_ICL_DISCRETE_STATIC_INTERVAL_BORDERS) && (BOOST_ICL_DISCRETE_STATIC_INTERVAL_BORDERS == 0)
    cout << "discrete_interval == open_interval\n";
    BOOST_CHECK( (boost::is_same<icl::interval<int>::type, open_interval<int> >::value) );
    BOOST_CHECK_EQUAL( icl::interval<int>::open(0,2),       icl::construct<open_interval<int> >(0,2) );
    BOOST_CHECK_EQUAL( icl::interval<int>::left_open(0,1),  icl::construct<open_interval<int> >(0,2) );
    BOOST_CHECK_EQUAL( icl::interval<int>::right_open(1,2), icl::construct<open_interval<int> >(0,2) );
    BOOST_CHECK_EQUAL( icl::interval<int>::closed(1,1),     icl::construct<open_interval<int> >(0,2) );

#   elif defined(BOOST_ICL_DISCRETE_STATIC_INTERVAL_BORDERS) && (BOOST_ICL_DISCRETE_STATIC_INTERVAL_BORDERS == 1)
    cout << "discrete_interval == left_open_interval\n";
    BOOST_CHECK( (boost::is_same<icl::interval<int>::type, left_open_interval<int> >::value) );
    BOOST_CHECK_EQUAL( icl::interval<int>::open(0,2),       icl::construct<left_open_interval<int> >(0,1) );
    BOOST_CHECK_EQUAL( icl::interval<int>::left_open(0,1),  icl::construct<left_open_interval<int> >(0,1) );
    BOOST_CHECK_EQUAL( icl::interval<int>::right_open(1,2), icl::construct<left_open_interval<int> >(0,1) );
    BOOST_CHECK_EQUAL( icl::interval<int>::closed(1,1),     icl::construct<left_open_interval<int> >(0,1) );

#   elif defined(BOOST_ICL_DISCRETE_STATIC_INTERVAL_BORDERS) && (BOOST_ICL_DISCRETE_STATIC_INTERVAL_BORDERS == 2)
    cout << "discrete_interval == right_open_interval\n";
    BOOST_CHECK( (boost::is_same<icl::interval<int>::type, right_open_interval<int> >::value) );
    BOOST_CHECK_EQUAL( icl::interval<int>::open(0,2),       icl::construct<right_open_interval<int> >(1,2) );
    BOOST_CHECK_EQUAL( icl::interval<int>::left_open(0,1),  icl::construct<right_open_interval<int> >(1,2) );
    BOOST_CHECK_EQUAL( icl::interval<int>::right_open(1,2), icl::construct<right_open_interval<int> >(1,2) );
    BOOST_CHECK_EQUAL( icl::interval<int>::closed(1,1),     icl::construct<right_open_interval<int> >(1,2) );

#   elif defined(BOOST_ICL_DISCRETE_STATIC_INTERVAL_BORDERS) && (BOOST_ICL_DISCRETE_STATIC_INTERVAL_BORDERS == 3)
    cout << "discrete_interval == closed_interval\n";
    BOOST_CHECK( (boost::is_same<icl::interval<int>::type, closed_interval<int> >::value) );
    BOOST_CHECK_EQUAL( icl::interval<int>::open(0,2),       icl::construct<closed_interval<int> >(1,1) );
    BOOST_CHECK_EQUAL( icl::interval<int>::left_open(0,1),  icl::construct<closed_interval<int> >(1,1) );
    BOOST_CHECK_EQUAL( icl::interval<int>::right_open(1,2), icl::construct<closed_interval<int> >(1,1) );
    BOOST_CHECK_EQUAL( icl::interval<int>::closed(1,1),     icl::construct<closed_interval<int> >(1,1) );

#   else
    cout << "#else part, INTERVAL_BORDERS not in {0,1,2,3}\n";
#endif //defined(BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS) && defined(BOOST_ICL_DISCRETE_STATIC_INTERVAL_DEFAULT)

#else
    BOOST_CHECK( (boost::is_same<icl::interval<int   >::type,   discrete_interval<int   > >::value) );
    BOOST_CHECK( (boost::is_same<icl::interval<double>::type, continuous_interval<double> >::value) );
#endif

}


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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)

#ifndef FCPPT_MPL_NUMERIC_MAX_HPP_INCLUDED
#define FCPPT_MPL_NUMERIC_MAX_HPP_INCLUDED

#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/mpl/integral_c.hpp>
#include <limits>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace mpl
{

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

/**
\brief Metafunction to compute the maximum value of a type

\ingroup fcpptmpl
*/
template<
	typename Type
>
struct numeric_max
:
boost::mpl::integral_c<
	Type,
	std::numeric_limits<
		Type
	>::max()
>
{
};

FCPPT_PP_POP_WARNING

}
}

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_POSMIN_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_POSMIN_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(posmin_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE  std::size_t operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using stype = bd::scalar_of_t<A0>;
         std::size_t p = 0;
        stype m = a0[0];
        // TODO UNROLL
        for(size_t i=1; i < bs::cardinal_of<A0>::value; i++)
        {
          if (m > a0[i])
          {
            m = a0[i];
            p = i;
          }
        }
        return p;
      }
   };

} } }

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_PREDICATES_FUNCTIONS_SCALAR_IS_ODD_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_PREDICATES_FUNCTIONS_SCALAR_IS_ODD_HPP_INCLUDED
#include <boost/simd/toolbox/predicates/functions/is_odd.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/functions/scalar/minusone.hpp>
#include <boost/simd/include/functions/scalar/is_even.hpp>
#include <boost/simd/sdk/simd/logical.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_odd_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return result_type(b_and(a0,One<A0>()) != 0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_odd_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return is_even(minusone(a0));
    }
  };
} } }

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2012 Bruno Lalande, Paris, France.
// Copyright (c) 2012 Mateusz Loskot, London, UK.

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

#include <geometry_test_common.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/util/calculation_type.hpp>

template <typename G1, typename G2>
inline std::string helper()
{
    std::string result;
    result += typeid(typename bg::coordinate_type<G1>::type).name();
    result += "/";
    result += typeid(typename bg::coordinate_type<G2>::type).name();
    return result;
}

template <typename G1, typename G2, typename G3>
inline std::string helper3()
{
    std::string result;
    result += typeid(typename bg::coordinate_type<G1>::type).name();
    result += "/";
    result += typeid(typename bg::coordinate_type<G2>::type).name();
    result += "/";
    result += typeid(typename bg::coordinate_type<G3>::type).name();
    return result;
}

template
<
    typename G1,
    typename G2,
    typename DefaultFP,
    typename DefaultInt,
    typename ExpectedType
>
void test()
{
    typedef typename bg::util::calculation_type::geometric::binary
        <
            G1,
            G2,
            void,
            DefaultFP,
            DefaultInt
        >::type type;

    std::string const caption = helper<G1, G2>();

    BOOST_CHECK_MESSAGE((boost::is_same<type, ExpectedType>::type::value),
        "Failure, types do not agree;"
            << " input: " << caption
            << " defaults: " << typeid(DefaultFP).name()
                << "/" << typeid(DefaultInt).name()
            << " expected: " << typeid(ExpectedType).name()
            << " detected: " << typeid(type).name()
        );
}

template
<
    typename G1,
    typename G2,
    typename CalculationType,
    typename ExpectedType
>
void test_with_calculation_type()
{
    typedef typename bg::util::calculation_type::geometric::binary
        <
            G1,
            G2,
            CalculationType,
            double,
            int
        >::type type;

    std::string const caption = helper<G1, G2>();

    BOOST_CHECK_MESSAGE((boost::is_same<type, ExpectedType>::type::value),
        "Failure, types do not agree;"
            << " input: " << caption
            << " calculation type: " << typeid(CalculationType).name()
            << " expected: " << typeid(ExpectedType).name()
            << " detected: " << typeid(type).name()
        );
}

template
<
    typename Geometry,
    typename DefaultFP,
    typename DefaultInt,
    typename ExpectedType
>
void test_unary()
{
    typedef typename bg::util::calculation_type::geometric::unary
        <
            Geometry,
            void,
            DefaultFP,
            DefaultInt
        >::type type;

    BOOST_CHECK_MESSAGE((boost::is_same<type, ExpectedType>::type::value),
        "Failure, types do not agree;"
            << " input: " << typeid(typename bg::coordinate_type<Geometry>::type).name()
            << " defaults: " << typeid(DefaultFP).name()
                << "/" << typeid(DefaultInt).name()
            << " expected: " << typeid(ExpectedType).name()
            << " detected: " << typeid(type).name()
        );
}

template
<
    typename G1,
    typename G2,
    typename G3,
    typename DefaultFP,
    typename DefaultInt,
    typename ExpectedType
>
void test_ternary()
{
    typedef typename bg::util::calculation_type::geometric::ternary
        <
            G1,
            G2,
            G3,
            void,
            DefaultFP,
            DefaultInt
        >::type type;

    std::string const caption = helper3<G1, G2, G3>();

    BOOST_CHECK_MESSAGE((boost::is_same<type, ExpectedType>::type::value),
        "Failure, types do not agree;"
            << " input: " << caption
            << " defaults: " << typeid(DefaultFP).name()
                << "/" << typeid(DefaultInt).name()
            << " expected: " << typeid(ExpectedType).name()
            << " detected: " << typeid(type).name()
        );
}

struct user_defined {};

int test_main(int, char* [])
{
    using namespace boost::geometry;
    typedef model::point<double, 2, cs::cartesian> d;
    typedef model::point<float, 2, cs::cartesian> f;
    typedef model::point<int, 2, cs::cartesian> i;
    typedef model::point<char, 2, cs::cartesian> c;
    typedef model::point<short int, 2, cs::cartesian> s;
    typedef model::point<boost::long_long_type, 2, cs::cartesian> ll;
    typedef model::point<user_defined, 2, cs::cartesian> u;

    // Calculation type "void" so
    test<f, f, double, int, double>();
    test<d, d, double, int, double>();
    test<f, d, double, int, double>();

    // FP/int mixed
    test<i, f, double, int, double>();
    test<s, f, double, short int, double>();

    // integers
    test<s, s, double, short int, short int>();
    test<i, i, double, int, int>();
    test<c, i, double, int, int>();
    test<c, c, double, char, char>();
    test<c, c, double, int, int>();
    test<i, i, double, boost::long_long_type, boost::long_long_type>();

    // Even if we specify "int" as default-calculation-type, it should never go downwards.
    // So it will select "long long"
    test<ll, ll, double, int, boost::long_long_type>();

    // user defined
    test<u, i, double, char, user_defined>();
    test<u, d, double, double, user_defined>();

    test_with_calculation_type<i, i, double, double>();
    test_with_calculation_type<u, u, double, double>();

    test_unary<i, double, int, int>();
    test_unary<u, double, double, user_defined>();
    test_ternary<u, u, u, double, double, user_defined>();

    return 0;
}