#include <ros/ros.h>
#include "nav_msgs/Odometry.h"
#include <Eigen/Dense>
#include <std_msgs/Float64.h>
//TF
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>

#include <math.h>
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Point32.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/Odometry.h"
#include "visualization_msgs/Marker.h"

//custom message types
#include "road_detection/LineMatch.h"
#include "road_detection/Line.h"
#include "road_detection/LineArray.h"
#include "road_detection/Road.h"

#include <tf2/LinearMath/Quaternion.h>

#include <dynamic_reconfigure/server.h>
#include <arlo_navigation/posefinderConfig.h>
//Services
#include "arlo_navigation/clearleftlane.h"
#include "arlo_navigation/goalnotfound.h"
#include "simple_layers/reset.h"

using Eigen::MatrixXd;
using Eigen::Vector2d;

//Structs
struct APoint
{
	double x;	//average x
	double y;	//average y
	int N;		//amount of points included in the average
};
struct CIRCLE
	{
	double x;
	double y;
	double r;
	ros::Time Stamp;
	};
struct LINE
	{
	double m;
	double b;
	ros::Time Stamp;
	};
struct map_inf {

    double size_x;
    double size_y;
    double scale;
    double origin_x;
    double origin_y;
};

//definitions to help with map coordinates and indices
#define MAP_INDEX(map, i, j) ((i) + (j) * map.size_x)
//get coordinates relative to /map frame from cell coordinates
#define MAP_WXGX(map, i) (map.origin_x + (i - map.size_x / 2) * map.scale)
#define MAP_WYGY(map, j) (map.origin_y + (j - map.size_y / 2) * map.scale)
//get cell coordinates from coordinates relative to /map
#define MAP_CX(map,i) (i-round(map.origin_x/map.scale)+floor(map.size_x/2))
#define MAP_CY(map,j) (j-round(map.origin_y/map.scale)+floor(map.size_y/2))

ros::Publisher pub;
std::vector<geometry_msgs::PointStamped> scan;
geometry_msgs::PoseStamped robotorigin;
//storing the latest road data for data loss cases
road_detection::Line LeftLine;
road_detection::Line RightLine;
road_detection::Line MiddleLine;
road_detection::Line LeftLane;
road_detection::Line RightLane;
nav_msgs::OccupancyGrid Map;
std::string roadframe;

double CRadThresh=8;//The Threshold radius of the approximated circles if the circles get larger line approx will be used
double GoalDist=4;//The Distance, at which a goal should be found
double GoalAngle=M_PI/3;//The angle, we think we can trust the circle approximation
double reductionradius=1.5;
double MapCostThresh=50;//probability of occupancy in percent

bool valid=true;

double searchradius=5;
double searchangle=60*M_PI/180;
double costthreshold=100;
bool Rightside;

bool removeblockage=false;
double roadangle;
double robot_diameter=0.4;
double rlwidht;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

//should be reference???
double polynomial(double x, road_detection::Line::_polynomial_type::_a_type a) {
	double xp = 1;
	double result = 0;

	for ( int i = 0; i < a.size(); i++ ) {
		result += xp * a[i];
		xp *= x;
	}
	return result;
}

void checkroadblockage(void)
	{
		ros::Duration Timedif;
		geometry_msgs::TransformStamped transformStamped;
		tf2_ros::Buffer tfBuffer;
		tf2_ros::TransformListener tfListener(tfBuffer);

		bool obstacleright=false;
		bool obstacleleft=false;
		//furthestpoint=sqrt(pow(LeftLine.points.back().x,2)+pow(LeftLine.points.back().y,2));

		if(scan.size()>0)
		{
			Timedif=ros::Time::now()-scan.at(0).header.stamp;
		}
		else {
			//ROS_INFO("Road Free");
			removeblockage=false;
			Timedif=ros::Duration(1);
		}
		//check if the scanned point is infront of the robot and if it is in a reasonable timeframe (not older thaen 500ms)
		if(Timedif.toSec()<=0.5){
			for(int i=0; i<scan.size();i++){
				if(scan.at(0).point.x>0){
					geometry_msgs::PointStamped Test;
					Test.header.frame_id=scan.at(i).header.frame_id;
					Test.header.stamp=scan.at(i).header.stamp;
					Test.point.x=scan.at(i).point.x;
					Test.point.y=scan.at(i).point.y;
					Test.point.z=scan.at(i).point.z;
					try{
						//transform scanned point into the frame of the road detection then compare it with the road
						transformStamped = tfBuffer.lookupTransform( roadframe,Test.header.frame_id,Test.header.stamp,ros::Duration(1));

						tf2::doTransform(Test,Test, transformStamped);
						//pub.publish(Test);
						//check whether the point is in the lane by projecting it on to the 3 roadlines and comparing their y values
						//exact calculation not necessary since road makes wide curves
						if(polynomial(Test.point.x, LeftLine.polynomial.a)>Test.point.y && polynomial(Test.point.x, MiddleLine.polynomial.a)<Test.point.y)
						{
							obstacleleft=true;
						}
						else if(polynomial(Test.point.x, RightLine.polynomial.a)<Test.point.y && polynomial(Test.point.x, MiddleLine.polynomial.a)>Test.point.y)
						{
							obstacleright=true;
						}
					}
					catch(tf2::TransformException &ex)
					{
						ROS_WARN("%s", ex.what());
					}
				}
			}
			if(obstacleleft&&!obstacleright)
			{
				removeblockage=false;
			}
			else if(!obstacleleft&&obstacleright)
			{
				removeblockage=true;
			}
			else if(!obstacleleft&&!obstacleright)
			{
				removeblockage=false;
			}
		}
	}

void callback(arlo_navigation::posefinderConfig &config, uint32_t level) {
	CRadThresh=config.CRadThresh;
	GoalDist=config.GoalDist;
	GoalAngle=config.GoalAngle*M_PI;
	reductionradius=config.reductionradius;
	MapCostThresh=config.MapCostThresh;
}

class Drawer
	{
	//class for the draw functions so the publishers don't need to be defined all the time but only for the debugging
	//keeps things more tidy
	private:
		ros::NodeHandle n;
		ros::Publisher line_pub = n.advertise<visualization_msgs::Marker>("Lines", 1);
		ros::Publisher mappoint_pub = n.advertise<geometry_msgs::PointStamped>("mappoints", 1);
		ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloudpoints", 1);
		ros::Publisher circle_pub1 = n.advertise<visualization_msgs::Marker>("Circle1", 1);
		ros::Publisher circle_pub2 = n.advertise<visualization_msgs::Marker>("Circle2", 1);

	////For debugging only
	public:
		void cloud(sensor_msgs::PointCloud cloud)
		{
			cloud_pub.publish(cloud);
		}
		void mappoints(geometry_msgs::PointStamped p)
		{
			mappoint_pub.publish(p);
		}
		void circles(struct CIRCLE c1,struct CIRCLE c2)
			{
			//function to visualize predicted circles
				visualization_msgs::Marker marker;
				marker.header.frame_id = "base_footprint";
				marker.header.stamp = c1.Stamp;
				marker.ns = "basic_shapes";
				marker.id = 0;
				marker.type = visualization_msgs::Marker::CYLINDER;
				marker.action = visualization_msgs::Marker::ADD;
				marker.scale.x=c1.r*2;
				marker.scale.y=c1.r*2;
				marker.scale.z=1;
				marker.pose.position.x=c1.x;
				marker.pose.position.y=c1.y;
				marker.pose.position.z = 0;
				marker.pose.orientation.x = 0.0;
				marker.pose.orientation.y = 0.0;
				marker.pose.orientation.z = 0.0;
				marker.pose.orientation.w = 1.0;
				marker.color.r = 0.0f;
				marker.color.g = 1.0f;
				marker.color.b = 0.0f;
				marker.color.a = 0.5;
				marker.lifetime = ros::Duration();
				circle_pub1.publish(marker);
				marker.header.stamp = c2.Stamp;
				marker.scale.x=c2.r*2;
				marker.scale.y=c2.r*2;
				marker.pose.position.x=c2.x;
				marker.pose.position.y=c2.y;
				circle_pub2.publish(marker);
		}
		void lines(struct LINE l1,struct LINE l2)
		{
		//function to visualize predicted lines
			geometry_msgs::Point p;
			visualization_msgs::Marker marker;
			marker.header.frame_id = "base_footprint";
			marker.header.stamp = l1.Stamp;
			marker.ns = "basic_shapes";
			marker.id = 0;
			marker.type = visualization_msgs::Marker::LINE_LIST;
			marker.action = visualization_msgs::Marker::ADD;
			marker.scale.x=0.1;
			marker.color.r = 0.0f;
			marker.color.g = 1.0f;
			marker.color.b = 0.0f;
			marker.color.a = 0.5;
			marker.lifetime = ros::Duration();
			//ROS_INFO("x: %f, y: %f",x(0),x(1));
			///////

			marker.points.clear();
			p.x=0;
			p.y=l1.b;
			marker.points.push_back(p);
			p.x=4;
			p.y=l1.m*4+l1.b;
			marker.points.push_back(p);
			p.x=0;
			p.y=l2.b;
			marker.points.push_back(p);
			p.x=4;
			p.y=l2.m*4+l2.b;
			marker.points.push_back(p);

			line_pub.publish(marker);
		};

	};
class Listener
	{
	public:
		void roadCallback(const road_detection::RoadConstPtr& road)
		{

			//DataSync so each Line has always the newest data
			if(road->laneLeft.points.size()>0)
			{
				LeftLane=road->laneLeft;

			}
			if(road->laneRight.points.size()>0)
			{
				RightLane=road->laneRight;
			}

			if(road->lineLeft.points.size()>0)
			{
				LeftLine=road->lineLeft;
			}
			if(road->lineRight.points.size()>0)
			{
				RightLine=road->lineRight;
			}
			if(road->lineMiddle.points.size()>0)
			{
				MiddleLine=road->lineMiddle;
			}
			if(road->laneWidthRight>0)rlwidht=road->laneWidthRight;
			searchradius=road->laneWidthLeft+ 0.6*road->laneWidthRight;
			//roadangle=aproxroadangle();
		};
		void mapCallback(const nav_msgs::OccupancyGridConstPtr &map)
		{

			Map.data=map->data;//update global Map variable
			Map.header=map->header;
			Map.info=map->info;
		}
		void odomCallback(nav_msgs::Odometry odom)
		{
			//update global pose variable
			robotorigin.header=odom.header;
			robotorigin.pose=odom.pose.pose;

		}
		void scanCallback(const sensor_msgs::LaserScanConstPtr& Scan)
		{
			geometry_msgs::PointStamped scanpoint;
			scan.clear();
			double range,angle,minangle,maxangle;
			minangle=M_PI-(searchangle/2)+roadangle;
			maxangle=M_PI+(searchangle/2)+roadangle;
			for(int i=0;i<Scan->ranges.size();i++)
				{
					range=Scan->ranges.at(i);
					angle=(i*Scan->angle_increment)+Scan->angle_min;
					if((range<searchradius)&&(angle>minangle)&&(angle<maxangle)){

						scanpoint.header.frame_id=Scan->header.frame_id.c_str();
						scanpoint.header.stamp=Scan->header.stamp;
						scanpoint.point.x=range*cos(angle);
						scanpoint.point.y=range*sin(angle);
						scanpoint.point.z=0.0;

						scan.push_back(scanpoint);
					}
				}
    };
	};
class GoalFinder
	{

private:

	int Finder=0;//start with searching a goal on a circle
	bool goaltrigger=true;//might be neccessary to handle error scenarios
	sensor_msgs::PointCloud cloud;
	std::vector<geometry_msgs::PointStamped> goalpoints;
	ros::NodeHandle n;
	ros::ServiceClient client = n.serviceClient<simple_layers::reset>("/move_base/global_costmap/my/MyLayer/reset");

	ros::Duration stampdif1,stampdif2;
	MoveBaseClient ac;
	ros::Time left,right;
	//functions to approximate the future road based on least squared error calculations
	struct LINE leastsquareline(road_detection::Line Line)
		{
			//least square line aproximation https://www.mathsisfun.com/data/least-squares-regression.html
			struct LINE line;
			double Sx,Sy,Sxx,Sxy,Syy2,N;
			N=Line.points.size();
			for(int i=0;i<N;i++)
			{
				Sx+=Line.points.at(i).x;
				Sy+=Line.points.at(i).y;
				Sxy+=Line.points.at(i).x*Line.points.at(i).y;
				Sxx+=Line.points.at(i).x*Line.points.at(i).x;

			}
			try{
				line.m=((N*Sxy)-(Sx*Sy))/((N*Sxx)-pow(Sx,2));
				line.b=(Sy-line.m*Sx)/N;
				line.Stamp = Line.header.stamp;

				for(int i=0;i<Line.points.size();i++)
				{
					//calculating the squared error
					Syy2+=pow(Line.points.at(i).y-(line.m*Line.points.at(i).x+line.b),2);
				}
				return line;
			}
			catch(...)
			{
				//Division by zero occured caused by all points having the same x
				//or b the Input-Line not containing any point
				// In this case we can't return anything, but this shouldn't
				//ever occur in this application

				//C++ doesn't offer a division by zero exception thats why we are catching all of them
			}
		}
	struct CIRCLE leastsquarecircle(road_detection::Line Line)

		{
			//least square circle approximation as discribed by Randy Bullock in his Least-Squares Circle Fit Paper
			//takes a Point32 array and calculates the best fitting circle in x,y dimensions, returns the circle parameters xc,yc,r
			//used to predict the upcoming road that cannot be seen by the camera
			struct CIRCLE circle;
			MatrixXd m(2,2);
			Vector2d b;
			Vector2d x;
			//define size so we don't have to do this all the time
			int size=Line.points.size();
			//defining sums
			double Suuu,Svvv,Suvv,Svuu,Suv,Suu,Svv,Syy2=0;

			//defining averages and parameters u,v that are the point coordinates relative to the average
			double u,v,xa,ya=0;
			//first the averages will be build
			if(size>0){
				for (int i=0;i<size;i++)
				{
					xa+=Line.points.at(i).x;
					ya+=Line.points.at(i).y;
				}
				xa=xa/size;
				ya=ya/size;

				//new the Sums need to be calculated
				for (int i=0;i<size;i++)
				{
					u=Line.points.at(i).x-xa;
					v=Line.points.at(i).y-ya;

					Suu+=u*u;
					Svv+=v*v;
					Suv+=u*v;
					Suvv+=u*v*v;
					Svuu+=v*u*u;
					Suuu+=u*u*u;
					Svvv+=v*v*v;
				}
				m(0,0)=Suu;
				m(0,1)=Suv;
				m(1,0)=Suv;
				m(1,1)=Svv;

				m=m.inverse();

				b(0)=0.5*(Suuu+Suvv);
				b(1)=0.5*(Svvv+Svuu);

				//multiplying b with the inverse of m to solve the linear equation system since m*x=b so x=m⁻¹*b
				x=m*b;

				//calculating radius
				circle.r=sqrt(pow(x(0),2)+pow(x(1),2)+(Suu+Svv)/size);

				//translating into original coordinates
				circle.x=x(0)+xa;
				circle.y=x(1)+ya;

			}
			circle.Stamp=Line.header.stamp;

			//Calculating the squared error
			for(int i=0;i<size;i++)
			{
				u=Line.points.at(i).x-xa;
				v=Line.points.at(i).y-ya;
				Syy2+=pow(pow((u-circle.x),2)+pow(v-circle.y,2)-pow(circle.r,2),2);
			}
			return circle;

		}
	//void construcctor for costmap

	void ConstructPointCloud(double searchradius)
	{
		//check for not zero stamp so we only look for positions if we have received a map
		if(Map.header.stamp.isZero()==false){
			//this function renders a circle with sub-pixel accuracy around the circle at a given radius
			map_inf map_info;
			//collect all cost data in a circle of radius searchradius
			map_info.size_x = Map.info.width;
			map_info.size_y = Map.info.height;
			map_info.scale = Map.info.resolution;
			map_info.origin_x = Map.info.origin.position.x + (map_info.size_x / 2) * map_info.scale;
			map_info.origin_y = Map.info.origin.position.y + (map_info.size_y / 2) * map_info.scale;

			double gt_x = Map.info.origin.position.x;
			double gt_y = Map.info.origin.position.y;

			tf2_ros::Buffer tf_buffer;
			tf2_ros::TransformListener tf2_listener(tf_buffer);
			geometry_msgs::TransformStamped base_footprint_to_map;
			geometry_msgs::TransformStamped map_to_base_footprint;
			//tranform robot pose into map frame
			geometry_msgs::PoseStamped pt;
//			robotorigin.header.stamp =  Map.header.stamp;

//			try{
//				base_footprint_to_map=tf_buffer.lookupTransform("map",robotorigin.header.frame_id, ros::Time(0), ros::Duration(1.0) );
//
//				tf2::doTransform(robotorigin, robotorigin, base_footprint_to_map);
//
//
//			}
//			catch (tf2::TransformException &ex){
//				ROS_ERROR("%s",ex.what());
//			}

			std::vector<geometry_msgs::Point32> Points;
			geometry_msgs::Point32 Point;
			double pt_x=robotorigin.pose.position.x;
			double pt_y=robotorigin.pose.position.y;

			double pt_th;// = tf::getYaw(pt.pose.orientation);

			double minx=round((pt_x-searchradius)/map_info.scale);
			double maxx=round((pt_x+searchradius)/map_info.scale);
			double miny=round((pt_y-searchradius)/map_info.scale);
			double maxy=round((pt_y+searchradius)/map_info.scale);

			//since circle wont ever be point symetrical we need to iterate through all quadrants
			//this is caused since we wont shift the circlecenter to the middle of the nearest field otherwise symmetry could be used
			Points.clear();
			//first quadrant
			int i,j;
			j=round(pt_y/map_info.scale);
			for (i=maxx; i >round(pt_x/map_info.scale); i--) {
				while(checkcellincircle(i,j,pt_x,pt_y,map_info,Points))
				{

					j++;
				}
				j--;
			}

			//second quadrant
			i=round(pt_x/map_info.scale);
			for (j=maxy; j >round(pt_y/map_info.scale); j--) {
				while(checkcellincircle(i,j,pt_x,pt_y,map_info,Points))
				{
					i--;
				}
				i++;
			}
			//third quadrant
			j=round(pt_y/map_info.scale);
			for (i=minx; i<round(pt_x/map_info.scale); i++) {
				while(checkcellincircle(i,j,pt_x,pt_y,map_info,Points))
				{
					j--;
				}
				j++;
			}
			//fourth quadrant
			i=round(pt_x/map_info.scale);
			for (j=miny; j <round(pt_y/map_info.scale); j++) {
				while(checkcellincircle(i,j,pt_x,pt_y,map_info,Points))
				{
					i++;
				}
				i--;
			}

			cloud.points.clear();
			cloud.header.stamp=robotorigin.header.stamp;
			cloud.header.frame_id="map";
			int cellx,celly;
			//now we have a vector containing all cells intersected by the circle ordered by their angle starting on the x axis
			//we need to calculate their position relative to the map frame
			for(i=0; i<Points.size();i++)
			{
				cellx=MAP_CX(map_info,Points[i].x);
				celly=MAP_CY(map_info,Points[i].y);

				try{
					//check bounds of map and don't add them to the cloud
					if((cellx>=0&&cellx<map_info.size_x)&&(celly>=0&&celly<map_info.size_y))
						{
						geometry_msgs::Point32 test;
						test.x=Points[i].x*map_info.scale;
						test.y=Points[i].y*map_info.scale;
						if(Map.data.at(MAP_INDEX(map_info,MAP_CX(map_info,Points[i].x),MAP_CY(map_info,Points[i].y)))>MapCostThresh){

							test.z=0;
							cloud.points.push_back(test);

						}
					}
				}
				catch(...){}
			}

			PointCloudReducer(reductionradius);//reducing the pointcloud to a minimum
			draw.cloud(cloud);

			//after the reduction we should have 4 or more points since both road borders will intersect the circle twice
			//now we need to delete the points behind the robot easiest is to transform them into the base_footrpint frame
			//this will make the evaluation a lot easier

			geometry_msgs::PointStamped p;
			goalpoints.clear();

			for(int i=0;i<cloud.points.size();i++)
			{
				p.header.frame_id="map";
				p.header.stamp=ros::Time::now();
				p.point.x=cloud.points.at(i).x;
				p.point.y=cloud.points.at(i).y;
				p.point.z=cloud.points.at(i).z;
				try{
					map_to_base_footprint=tf_buffer.lookupTransform("base_footprint","map", ros::Time(0), ros::Duration(1.0) );
					//trying to transform the points back into the base_footprint frame
					tf2::doTransform(p,p, map_to_base_footprint);
				}
				catch (tf2::TransformException &ex){
					ROS_ERROR("%s",ex.what());
				}
				//adding all points infront of the robot to the vector

				//removing points outside of certain angle (+- pi/3 might be appropriate)
				if(p.point.x>0&&atan2(p.point.y,p.point.x)<M_PI/3)
				{
					draw.mappoints(p);
					goalpoints.push_back(p);
				}
			}
		//now we should have a cleaned up vector consisting solely of points infront of the robot on the borders
		}
	}
	void PointCloudReducer(double distthresh)
	{
		//this function takes a point cloud and reduces it by averaging all points in a given radius

		geometry_msgs::Point32 p;

		std::vector<struct APoint> BorderPoints;
		struct APoint POINT;
		bool trigger;

		for(int i=0; i<cloud.points.size();i++)
		{
			POINT.x=cloud.points.at(i).x;
			POINT.y=cloud.points.at(i).y;
			POINT.N=1;

			//check if the next point is close to one of the chunks and calculate it into the average
			//else a new chunk will be generated

			trigger=true;
			for (int j=0;j<BorderPoints.size();j++)
			{
				if(sqrt(pow(BorderPoints.at(j).x-POINT.x,2)+pow(BorderPoints.at(j).y-POINT.y,2))<distthresh)
				{
					BorderPoints.at(j).x=(BorderPoints.at(j).x*BorderPoints.at(j).N+POINT.x)/(BorderPoints.at(j).N+1);
					BorderPoints.at(j).y=(BorderPoints.at(j).y*BorderPoints.at(j).N+POINT.y)/(BorderPoints.at(j).N+1);
					BorderPoints.at(j).N+=1;
					j=BorderPoints.size()+1;
					trigger=false;
				}
			}
			if(trigger)
			{
				BorderPoints.push_back(POINT);
			}
		}
		cloud.points.clear();

		for (int i=0;i<BorderPoints.size();i++)
		{
			p.x=BorderPoints.at(i).x;
			p.y=BorderPoints.at(i).y;
			p.z=0;
			cloud.points.push_back(p);
		}
	}
	bool checkcellincircle(int &i, int &j,double &pt_x,double &pt_y, map_inf &map,std::vector<geometry_msgs::Point32> &Points)
		{
			//this function checks the shortest distance from pt_x/pt_y to the given cell
			//if the distance is <= the searchradius the point gets added to the Pointsvector and the function returns true
			//else the function returns false
			double cminx,cminy,dx,dy;
			geometry_msgs::Point32 Point;

			//transform from origin of cell to center of cell
			if(i*map.scale>pt_x)
				dx=abs(pt_x-i*map.scale-map.scale/2);
			else if(i*map.scale<pt_x)
				dx=abs(pt_x-i*map.scale+map.scale/2);
			else dx=0;
			if(j*map.scale>pt_y)
				dy=abs(pt_y-j*map.scale-map.scale/2);
			else if(j*map.scale<pt_y)
				dy=abs(pt_y-j*map.scale+map.scale/2);
			else dy=0;

			//getting coordinates of closest point on cell edge by using intercept theorem so we don't need to use cos/sin/tan
			if((abs(dx)>=abs(dy))||dy==0)
			{
				cminx=map.scale;
				cminy=(abs(dy)/abs(dx))*map.scale;
			}

			else if((abs(dx)<abs(dy))||dx==0)
			{
				cminy=map.scale;
				cminx=(abs(dx)/abs(dy))*map.scale;
			}
			if(sqrt(pow(dx-cminx,2)+pow(dy-cminy,2))<=GoalDist)
			{
				Point.x=i;
				Point.y=j;
				Points.push_back(Point);
				return true;

			}

			else
				{
				return false;
				}
		}
	void constructgoalbymap(void)
	{

		//since we cant be sure that the points infront of the robot are only from the road
		//we can only take the average instead of the weighted average
		tf2::Quaternion myQuaternion;

		double x,y,m1,m2,m;
		double xmax,xmin,ymax,ymin;
		for(int i=0; i<goalpoints.size();i++)
		{
			x+=goalpoints.at(i).point.x;
			y+=goalpoints.at(i).point.y;

		}
		//if circles are large the slope is calculated as a straight line from the robot to the goal
		if(leftcircle.r>CRadThresh||rightcircle.r>CRadThresh)
		{
			m=atan2(y,x);
		}
		//else we estimate the slope the same way we do at the circle approximation
		else
		{
			m1 =atan2(y-leftcircle.y,x-leftcircle.x);
			m2 =atan2(y-rightcircle.y,x-rightcircle.x);
			if(leftcircle.y>0) m1+=M_PI/2;
			else m1-=M_PI/2;

			if(rightcircle.y>0)m2+=M_PI/2;
			else m2-=M_PI/2;
			m=(m1+m2)/2;

		}

		x/=goalpoints.size();
		y/=goalpoints.size();

		goal.target_pose.header.frame_id=goalpoints.at(0).header.frame_id;
		goal.target_pose.header.stamp = goalpoints.at(0).header.stamp;
		goal.target_pose.pose.position.x=x;
		goal.target_pose.pose.position.y=y;
		goal.target_pose.pose.position.z=0;
		myQuaternion.setRPY( 0, 0, atan(m) );
		myQuaternion=myQuaternion.normalized();
		goal.target_pose.pose.orientation.x=myQuaternion.getX();
		goal.target_pose.pose.orientation.y=myQuaternion.getY();
		goal.target_pose.pose.orientation.z=myQuaternion.getZ();
		goal.target_pose.pose.orientation.w=myQuaternion.getW();

	}

	//goal constructors for different road approximations
	void constructgoalbycircles(struct CIRCLE c1,struct CIRCLE c2, double dist, double angle)
		{
			//angle is in radiant
			tf2::Quaternion myQuaternion;

			//polar angles from circle origin to position of robot
			//since we are in the base_footprint frame robot is at 0,0
			double a1=atan2(-c1.y,-c1.x);
			double a2=atan2(-c2.y,-c2.x);
			double x,y,z,c1x,c2x,c1y,c2y,d,m,m1,m2=0;

			//if the approximated circles are very large
			//the covered distance is bigger than the distance we want to plan ahead
			//so we need to check that and adjust the angle accordingly
			if(((c1.r+3*c2.r)/4*angle)>dist)angle=dist*4/(c1.r+3*c2.r);

			//calc polar angle of new point for both circles
			//calc orientation of goal
			if(c1.y>0)
			{
				a1+=angle;
				m1=a1+M_PI/2;
			}
			else
			{
			a1-=angle;
			m1=a1-M_PI/2;
			}
			if(c2.y>0)
			{
				a2+=angle;
				m2=a2+M_PI/2;
			}
			else
			{
			a2-=angle;
			m2=a2-M_PI/2;
			}

			//calc cartesian coordinates
			c1x=c1.x+c1.r*cos(a1);
			c1y=c1.y+c1.r*sin(a1);
			c2x=c2.x+c2.r*cos(a2);
			c2y=c2.y+c2.r*sin(a2);

			//find the point at three quaters so its in the middle of the right lane
			x=(c1x+c2x)/2;
			y=(c1y+c2y)/2;
			m=(m1+m2)/2;

			goaltrigger=true;
			goal.target_pose.header.frame_id="base_footprint";
			goal.target_pose.header.stamp = c1.Stamp;
			goal.target_pose.pose.position.x=x;
			goal.target_pose.pose.position.y=y;
			goal.target_pose.pose.position.z=0;
			myQuaternion.setRPY( 0, 0, m );
			myQuaternion=myQuaternion.normalized();
			goal.target_pose.pose.orientation.x=myQuaternion.getX();
			goal.target_pose.pose.orientation.y=myQuaternion.getY();
			goal.target_pose.pose.orientation.z=myQuaternion.getZ();
			goal.target_pose.pose.orientation.w=myQuaternion.getW();

		}
	void constructgoalbylines(struct LINE l1,struct LINE l2, double dist)
		{
			tf2::Quaternion myQuaternion;

			double l1x,l2x,l1y,l2y;
			double a1,a2,off1,off2;
			double x,y,m;

			//geometric calculations to find point on crooked line in the right distance
			//if robot is turned on the road roadline will become so slanted that calulating with x only
			//will be inaccurate
			a1=atan(l1.m);
			a2=atan(l2.m);
			off1=cos(M_PI/2-a1)*l1.b;
			off2=cos(M_PI/2-a2)*l2.b;
			if(l1.m<0)l1x=x=cos(a1)*(dist+off1);
			else l1x=x=cos(a1)*(dist-off1);
			if(l2.m<0)l2x=x=cos(a2)*(dist+off2);
			else l2x=x=cos(a2)*(dist-off2);
			l1y=l1x*l1.m+l1.b;
			l2y=l2x*l2.m+l2.b;

			//find goal at ratio between both points
			x=(l1x+l2x)/2;
			y=(l1y+l2y)/2;
			m=(l1.m+l2.m)/2;

			goal.target_pose.header.frame_id="base_footprint";
			goal.target_pose.header.stamp = l1.Stamp;
			goal.target_pose.pose.position.x=x;
			goal.target_pose.pose.position.y=y;
			goal.target_pose.pose.position.z=0;
			myQuaternion.setRPY( 0, 0, atan(m) );
			myQuaternion=myQuaternion.normalized();
			goal.target_pose.pose.orientation.x=myQuaternion.getX();
			goal.target_pose.pose.orientation.y=myQuaternion.getY();
			goal.target_pose.pose.orientation.z=myQuaternion.getZ();
			goal.target_pose.pose.orientation.w=myQuaternion.getW();

		}

public:
	//Tuneable parameters TODO dynamic recon

	//move base action client
	Drawer draw;

	GoalFinder() : ac ("move_base",true)
	{
		ac.waitForServer();
	};
	move_base_msgs::MoveBaseGoal goal;

	struct CIRCLE leftcircle,rightcircle;
	struct LINE leftline, rightline;
	//goal manager used to determine which goal needs to be build
	void constructgoal()
			{
				//we dont want to use old data twice
				if(left>=LeftLine.header.stamp||right>=LeftLine.header.stamp) return;
				left=LeftLine.header.stamp;
				right=LeftLine.header.stamp;
				leftcircle=leastsquarecircle(LeftLine);
				rightcircle=leastsquarecircle(RightLine);

				//ConstructPointCloud(GoalDist);
				switch (Finder)
				{
					case 0:
						//if(goalpoints.size()>=1)Finder=2;
						//Circle approximation
						if(LeftLine.points.size()>0&&LeftLine.points.size()>0){
							//switching to line approx since straight is coming up
							if(leftcircle.r>CRadThresh||rightcircle.r>CRadThresh)
							{
								Finder=1;
								break;
							}
							else
							{

									constructgoalbycircles(leftcircle, rightcircle, GoalDist, GoalAngle);
									sendgoal();
									draw.circles(leftcircle,rightcircle);

							}
						}
						break;

					case 1:
						//if(goalpoints.size()>=1)Finder=2;
						//Line approximation
						if(LeftLine.points.size()>0&&LeftLine.points.size()>0){
							leftline=leastsquareline(LeftLine);
							rightline=leastsquareline(RightLine);
							//switching to circle approx since a corner is comming up
							if(leftcircle.r<CRadThresh&&rightcircle.r<CRadThresh)
							{
								Finder=0;
								break;
							}
							else
							{

									constructgoalbylines(leftline, rightline, GoalDist);
									sendgoal();
									draw.lines(leftline,rightline);
							}
						}
						break;

					case 2:
						if(goalpoints.size()>=1)
						{
							constructgoalbymap();
							sendgoal();
						}
						else
						{
							Finder=0;
						}

						break;
				}
			}
	void sendgoal()
	{
		//transforming the goal into the odom frame since the costmaps are fixed to that
		geometry_msgs::TransformStamped transformStamped;
		tf2_ros::Buffer tfBuffer;
		tf2_ros::TransformListener tf2_listener(tfBuffer);
		transformStamped=tfBuffer.lookupTransform("base_footprint",goal.target_pose.header.frame_id, ros::Time(0), ros::Duration(10.0) );
		try
		{
			tf2::doTransform(goal.target_pose,goal.target_pose, transformStamped);
		}
		catch(...){}
		//sending goal to move_base via actionserver
		//syntax for a potential feedback function
		ac.sendGoal(goal,
				MoveBaseClient::SimpleDoneCallback(),
				MoveBaseClient::SimpleActiveCallback(),
				boost::bind(&GoalFinder::fb_callback, this, _1));

		actionlib::SimpleClientGoalState state=ac.getState();

	}
	void fb_callback(const move_base_msgs::MoveBaseFeedback::ConstPtr& feedback)
	{

		//Doesnt return wanted results
		//ROS_INFO("%f",feedback->base_position.pose.position.x);
	}

	bool goalnotfound(arlo_navigation::goalnotfoundRequest &request,arlo_navigation::goalnotfoundResponse &response)
	{
		//this service call is meant to help the robot when it doesnt find a path to the goal
		//everytime this service gets called the Distance shrinks to 90% and a new goal will be planned
		simple_layers::reset::Request req;
		simple_layers::reset::Response res;
		req.trigger=true;
		client.call(req,res);
		//ConstructPointCloud(GoalDist*0.5);
		//constructgoalbymap();
		sendgoal();
		response.result=GoalDist*0.5;
		return true;
	}
	};

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

	ros::init(argc, argv, "posefinder");

	ros::NodeHandle n;
	Listener listener;
	GoalFinder finder;

	//pub=n.advertise<geometry_msgs::PointStamped>("TestPoint", 1000);

	//subscribers
	//ros::Subscriber map = n.subscribe("/map", 1000, &Listener::mapCallback, &listener);
	ros::Subscriber road = n.subscribe("/roadDetection/road", 1000, &Listener::roadCallback, &listener);
	ros::Subscriber scan = n.subscribe("/scan_filtered", 1000, &Listener::scanCallback, &listener);
	ros::Subscriber odom = n.subscribe("/odom", 1000, &Listener::odomCallback, &listener);

	//dynmic reconfigure
	dynamic_reconfigure::Server<arlo_navigation::posefinderConfig> server;
	dynamic_reconfigure::Server<arlo_navigation::posefinderConfig>::CallbackType f;
	f = boost::bind(&callback, _1, _2);
	server.setCallback(f);
	tf2_ros::Buffer tf_buffer;
	tf2_ros::TransformListener tf2_listener(tf_buffer);
	geometry_msgs::TransformStamped wheel_base;
	//service
	ros::ServiceClient client = n.serviceClient<arlo_navigation::clearleftlane>("clearblockage");
	ros::ServiceServer goalserver = n.advertiseService("posefinder", &GoalFinder::goalnotfound, &finder);
	arlo_navigation::clearleftlaneRequest req;
	arlo_navigation::clearleftlaneResponse res;

	//wait untill the actionserverclient in the goalfinder finds the movebase server

	ros::Publisher tester=n.advertise<std_msgs::Float64>("/diff",1);
	std_msgs::Float64 diff;
	ros::Rate rate=2;
	while(n.ok()){
		wheel_base=tf_buffer.lookupTransform("base_footprint","camera_link", ros::Time(0), ros::Duration(1.0) );
		//enabling or disabling the blockage of the left lane
		//checkroadblockage();
		//switching left lane on and off
		if(removeblockage)
		{
			req.trigger=false;
			client.call(req,res);
		}
		else
		{
			req.trigger=true;
			client.call(req,res);
		}
		diff.data=(polynomial(0,RightLane.polynomial.a)+rlwidht/2)*100/rlwidht;

		tester.publish(diff);
		//requesting the goalfinder to construct a new goal
		if(valid) finder.constructgoal();

		ros::spinOnce();
		rate.sleep();
	}
	return 0;
}


#include <tagloc/tracking.h>
#include <bearing_measurement/GetMeasurement.h>
#include <tagloc/algorithms.h>
#include <iostream>
#include <Eigen/Dense>
#include <lavant/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 namespace std;
bool haveboth;
bool update;
map<string,string> data;

ofstream logfile;

ros::ServiceClient wpclient;
ros::ServiceClient bearingclient;
ros::Subscriber stateIn;
ros::Subscriber hbin;
ros::Publisher hbout;
lavant::WaypointNavigation::Response wpres;
lavant::WaypointNavigation::Request wpreq;

Eigen::MatrixXd r1(2,1);
Eigen::MatrixXd r2(2,1);
Eigen::MatrixXd t(2,1);
Eigen::MatrixXd P(2,2);
Eigen::MatrixXd rend(2,2);

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

double sensor_sigma=(M_PI/8)*(M_PI/8);

void mapstate(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg){
		std::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("");
}

string pdelim=",";
string delim="=";

void nwin(const std_msgs::String::ConstPtr &msg){
	ROS_INFO("NWIN");
	if (!update) return;

	std::string s = msg->data;
	if (s.find("=")==string::npos){
		ROS_INFO("No values in %s",s.c_str());
		return;
	}
	std::string key,val;
	int dpos,ppos;
	int msgstart = s.find("[")+1;
	int msgend = s.find("]");
	s = s.substr(msgstart,msgend-msgstart);
	printf("Heard: %s\n",msg->data.c_str());
	while( (dpos=s.find(delim))!=std::string::npos ){
		key=s.substr(0,dpos);
		ppos = s.find(pdelim);
		if (((size_t)ppos)==std::string::npos){
			val = s.substr(dpos+1);
			s="";
		}
		else{
			val = s.substr(dpos+1,ppos-dpos-1);
			s = s.substr(ppos+1);
		}
		std::stringstream ss;
		ss<<key;
		ss>>key;
		ss<<val;
		ss>>val;
		ROS_INFO("Found: %s = %s\n",key.c_str(),val.c_str());
		data[key]=val;
	}
	if (data["x"].length()>0 &&
			data["y"].length()>0 &&
			data["th"].length()>0 &&
			data["_x"].length()>0 &&
			data["_y"].length()>0 &&
			data["_th"].length()>0){
		haveboth=true;
	}
}
void setupRos(ros::NodeHandle nh){
	wpclient=nh.serviceClient<lavant::WaypointNavigation>("/control/command");
	bearingclient=nh.serviceClient<bearing_measurement::GetMeasurement>("bearing_measurement/GetMeasurement");
	stateIn = nh.subscribe("/robot/state",1,mapstate);
	hbin = nh.subscribe("/network/received",1,nwin);
	hbout = nh.advertise<std_msgs::String>("/network/out",1);
}
int main(int argc, char** argv){

	if (argc!=7){
		return EXIT_FAILURE;
	}

	t = Eigen::MatrixXd(2,1);
	P = Eigen::MatrixXd(2,2);

	t<<atof(argv[1]),atof(argv[2]);
	P<<atof(argv[3]),atof(argv[4]),atof(argv[5]),atof(argv[6]);

	cout<<t<<endl;
	cout<<P<<endl;

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

	setupRos(nh);

	char HOSTNAME[HOST_NAME_MAX];
	gethostname(HOSTNAME,HOST_NAME_MAX);
	ROS_INFO("Onestep. I am %s",HOSTNAME);

	ROS_INFO("Stopping waypoint navigation, if any");
	wpreq.desiredState=2;
	wpreq.blockState=wpreq.NONBLOCKING;
	if (wpclient.exists()){
		wpclient.call(wpreq,wpres);
	} else {
		cerr<<"WP Service doesn't exist"<<endl;
		ROS_WARN("Skipping waypoint service");
	}

	ROS_WARN("DEFAULT VALUES");

	ROS_INFO("Waiting for joint state");
	haveboth=false;
	update = true;

	data.clear();
	while(haveboth==false && ros::ok()){
		ros::spinOnce();
		ros::Duration(1).sleep();
	}
	if (!ros::ok()){
		cerr<<"ROS down. Exiting"<<endl;
		return EXIT_FAILURE;
	}
	update = false;

	ROS_INFO("Got joint state, calculating step");

	r1<<atof(data["_x"].c_str()),atof(data["_y"].c_str());
	r2<<atof(data["x"].c_str()),atof(data["y"].c_str());

	cout<<"________________"<<endl;
	cout<<"Robot positions:"<<endl;
	cout<<"Me:"<<r1.transpose()<<endl;
	cout<<"Other guy:"<<r2.transpose()<<endl;
	cout<<"________________"<<endl;

	RSN::onestep(r1,r2,t,P,.1,3);
	rend = (r1+r2)/2;

	ROS_INFO("Got destination: %0.3f,%0.3f",r1(0),r1(1));
	cout<<r1<<endl;

	wpreq.destination.x=r1(0);
	wpreq.destination.y=r1(1);
	wpreq.destination.theta=0;

	if (wpclient.exists()){
		wpclient.call(wpreq,wpres);
	} else {
		cerr<<"wpservice problem"<<endl;
		ROS_WARN("Skipping waypoint service");
	}

	ROS_INFO("Taking Bearing Measurement");
	bearing_measurement::GetMeasurement::Request breq;
	bearing_measurement::GetMeasurement::Response bres;

	if (bearingclient.exists()){
		bearingclient.call(breq,bres);
	} else {
		ROS_WARN("Assuming fake bearing emasurement and spelling");
		bres.valid.data=true;
		bres.bearing=0;
	}

	stringstream ss;
	r1<<atof(data["_x"].c_str()),atof(data["_y"].c_str());
	px.push_back(r1(0));
	py.push_back(r1(1));
	Z.push_back(bres.bearing);
	R.push_back(sensor_sigma);

	ROS_INFO("Publishing Measurement value over network");
	ss<<"Zn="<<bres.bearing<<",";
	ss<<"Xn="<<r1(0)<<",";
	ss<<"Yn="<<r1(1)<<",";
	ss<<"Rn="<<sensor_sigma<<",";
	ss<<"Zc="<<Z.size();

	std_msgs::String omsg;
	omsg.data = ss.str();

	hbout.publish(omsg);

	ROS_INFO("Going to rendesvous point");

	if (wpclient.exists()){
		wpreq.destination.x = rend(0);
		wpreq.destination.y = rend(1);
		wpreq.destination.theta = 0;
		wpreq.desiredState = 0;
		wpreq.blockState=wpreq.BLOCKING;
		wpclient.call(wpreq,wpres);
	}

	ROS_INFO("Arrived. Waiting for joint state");
	haveboth=false;
	update = true;
	data.clear();

	while(haveboth==false && ros::ok()){
		ros::spinOnce();
		ros::Duration(1).sleep();
	}
	if (!ros::ok()){
		cerr<<"ROS down. Exiting"<<endl;
		return EXIT_FAILURE;
	}
	update = false;

	int theirs = atoi(data["Zc"].c_str());
	if ((size_t)theirs != Z.size()){
		ROS_INFO("Mis-match in measurement count: %Zu (mine) %d (theirs)",Z.size(),theirs);
	}

	px.push_back(atof(data["Xn"].c_str()));
	py.push_back(atof(data["Yn"].c_str()));
	Z.push_back(atof(data["Zn"].c_str()));
	R.push_back(atof(data["Rn"].c_str()));

	ROS_INFO("Got Joint State. Updating hypothesis");

	double t[2];
	double c[4];
	RSN::line_intersection(Z.size(),px,py,Z,t[0],t[1]);
	RSN::BOT::IWLS_2D(Z.size(),px,py,Z,R,t,c,true);
	cout<<t[0]<<","<<t[1]<<endl;
	cout<<c[0]<<" "<<c[1]<<endl;
	cout<<c[2]<<" "<<c[3]<<endl;

	return EXIT_SUCCESS;
}


/*
 * spatialTest.cpp
 *
 *  Created on: Feb 5, 2014
 *      Author: petr
 */

#include <iostream>
#include <Eigen/Dense>

#include "Geometry.hpp"

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

	Geometry geom = Geometry::fromFile("/home/petr/Dropbox/testGeometries/dragon.stl", 1.0);
	geom.initSearchAccelerator();

	std::cout << Eigen::Vector3d({ 0, 0, 0 }).transpose() << " = " << geom.computeDistance(Eigen::Vector3d({ 0, 0, 0 }), true) << std::endl;
	std::cout << Eigen::Vector3d({ 0, 0, 0 }).transpose() << " = " << geom.computeDistance(Eigen::Vector3d({ 0, 0, 0 }), false) << std::endl;

	return 0;
}


/**
 * Copyright (c) 2011-2017 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <boost/test/unit_test.hpp>

#include <cstddef>
#include <cstdint>
#include <bitcoin/bitcoin.hpp>

using namespace bc;

BOOST_AUTO_TEST_SUITE(limits_tests)

// cast_add
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__cast_add__uint32_to_int64_0__returns_0)
{
    static const int64_t expected = 0;
    BOOST_REQUIRE_EQUAL(cast_add<int64_t>(0u, 0u), expected);
}

BOOST_AUTO_TEST_CASE(limits__cast_add__uint32_to_int64_maximum_plus_0__returns_maximum)
{
    static const int64_t expected = max_uint32;
    BOOST_REQUIRE_EQUAL(cast_add<int64_t>(max_uint32, uint32_t{ 0 }), expected);
}

BOOST_AUTO_TEST_CASE(limits__cast_add__uint32_to_int64_maximum_plus_maximum__returns_twice_maximum)
{
    static const int64_t expected = 2 * static_cast<int64_t>(max_uint32);
    BOOST_REQUIRE_EQUAL(cast_add<int64_t>(max_uint32, max_uint32), expected);
}

// cast_subtract
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__cast_subtract__uint32_to_int64_0__returns_0)
{
    static const int64_t expected = 0;
    BOOST_REQUIRE_EQUAL(cast_subtract<int64_t>(0u, 0u), expected);
}

BOOST_AUTO_TEST_CASE(limits__cast_subtract__uint32_to_int64_0_minus_maximum__returns_negtive_maximum)
{
    static const int64_t expected = -1 * static_cast<int64_t>(max_uint32);
    BOOST_REQUIRE_EQUAL(cast_subtract<int64_t>(uint32_t{ 0 }, max_uint32), expected);
}

BOOST_AUTO_TEST_CASE(limits__cast_subtract__uint32_to_int64_maximum_minus_maximum__returns_0)
{
    static const int64_t expected = 0;
    BOOST_REQUIRE_EQUAL(cast_subtract<int64_t>(max_uint32, max_uint32), expected);
}

static const size_t minimum = 0;
static const size_t maximum = max_size_t;
static const size_t half = maximum / 2;

// ceiling_add
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__ceiling_add__size_t_minimum_plus_minimum__minimum)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(minimum, minimum), minimum);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__size_t_maximum_plus_maximum__maximum)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(maximum, maximum), maximum);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__size_t_minimum_plus_maximum__maximum)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(minimum, maximum), maximum);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__size_t_maximum_plus_minimum__maximum)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(maximum, minimum), maximum);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__size_t_half_plus_maximum__maximum)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(half, maximum), maximum);
}

// floor_subtract
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__floor_subtract__size_t_minimum_minus_minimum__minimum)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(minimum, minimum), minimum);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__size_t_maximum_minus_maximum__minimum)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(maximum, maximum), minimum);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__size_t_maximum_minus_minimum__maximum)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(maximum, minimum), maximum);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__size_t_minimum_minus_maximum__minimum)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(minimum, maximum), minimum);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__size_t_half_minus_maximum__minimum)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(half, maximum), minimum);
}

static const uint32_t min_uint32 = 0;
static const uint32_t half_uint32 = max_uint32 / 2;

// ceiling_add32
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__ceiling_add__min_uint32_plus_minimum__min_uint32)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(min_uint32, min_uint32), min_uint32);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__max_uint32_plus_max_uint32__max_uint32)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(max_uint32, max_uint32), max_uint32);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__min_uint32_plus_max_uint32__max_uint32)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(min_uint32, max_uint32), max_uint32);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__max_uint32_plus_min_uint32__max_uint32)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(max_uint32, min_uint32), max_uint32);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__half_uint32_plus_max_uint32__max_uint32)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(half_uint32, max_uint32), max_uint32);
}

// floor_subtract32
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__floor_subtract__min_uint32_minus_min_uint32__min_uint32)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(min_uint32, min_uint32), min_uint32);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__max_uint32_minus_max_uint32__min_uint32)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(max_uint32, max_uint32), min_uint32);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__max_uint32_minus_min_uint32__max_uint32)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(max_uint32, min_uint32), max_uint32);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__min_uint32_minus_max_uint32__min_uint32)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(min_uint32, max_uint32), min_uint32);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__half_uint32_minus_max_uint32__min_uint32)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(half_uint32, max_uint32), min_uint32);
}

static const uint64_t min_uint64 = 0;
static const uint64_t half_uint64 = max_uint64 / 2;

// ceiling_add64
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__ceiling_add__min_uint64_plus_min_uint64__min_uint64)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(min_uint64, min_uint64), min_uint64);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__max_uint64_plus_max_uint64__max_uint64)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(max_uint64, max_uint64), max_uint64);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__min_uint64_plus_max_uint64__max_uint64)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(min_uint64, max_uint64), max_uint64);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__max_uint64_plus_min_uint64__max_uint64)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(max_uint64, min_uint64), max_uint64);
}

BOOST_AUTO_TEST_CASE(limits__ceiling_add__half_uint64_plus_max_uint64__max_uint64)
{
    BOOST_REQUIRE_EQUAL(ceiling_add(half_uint64, max_uint64), max_uint64);
}

// floor_subtract64
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__floor_subtract__min_uint64_minus_min_uint64__min_uint64)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(min_uint64, min_uint64), min_uint64);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__max_uint64_minus_max_uint64__min_uint64)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(max_uint64, max_uint64), min_uint64);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__max_uint64_minus_min_uint64__max_uint64)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(max_uint64, min_uint64), max_uint64);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__min_uint64_minus_max_uint64__min_uint64)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(min_uint64, max_uint64), min_uint64);
}

BOOST_AUTO_TEST_CASE(limits__floor_subtract__half_uint64_minus_max_uint64__min_uint64)
{
    BOOST_REQUIRE_EQUAL(floor_subtract(half_uint64, max_uint64), min_uint64);
}

// safe_add
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__safe_add__size_t_minimum_plus_minimum__minimum)
{
    BOOST_REQUIRE_EQUAL(safe_add(minimum, minimum), minimum);
}

BOOST_AUTO_TEST_CASE(limits__safe_add__size_t_maximum_plus_maximum__throws_overflow)
{
    BOOST_REQUIRE_THROW(safe_add(maximum, maximum), std::overflow_error);
}

BOOST_AUTO_TEST_CASE(limits__safe_add__size_t_minimum_plus_maximum__maximum)
{
    BOOST_REQUIRE_EQUAL(safe_add(minimum, maximum), maximum);
}

BOOST_AUTO_TEST_CASE(limits__safe_add__size_t_maximum_plus_minimum__maximum)
{
    BOOST_REQUIRE_EQUAL(safe_add(maximum, minimum), maximum);
}

BOOST_AUTO_TEST_CASE(limits__safe_add__size_t_half_plus_maximum__throws_overflow)
{
    BOOST_REQUIRE_THROW(safe_add(half, maximum), std::overflow_error);
}

// safe_subtract
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__safe_subtract__size_t_minimum_minus_minimum__minimum)
{
    BOOST_REQUIRE_EQUAL(safe_subtract(minimum, minimum), minimum);
}

BOOST_AUTO_TEST_CASE(limits__safe_subtract__size_t_maximum_minus_maximum__minimum)
{
    BOOST_REQUIRE_EQUAL(safe_subtract(maximum, maximum), minimum);
}

BOOST_AUTO_TEST_CASE(limits__safe_subtract__size_t_maximum_minus_minimum__maximum)
{
    BOOST_REQUIRE_EQUAL(safe_subtract(maximum, minimum), maximum);
}

BOOST_AUTO_TEST_CASE(limits__safe_subtract__size_t_minimum_minus_maximum__throws_underflow)
{
    BOOST_REQUIRE_THROW(safe_subtract(minimum, maximum), std::underflow_error);
}

BOOST_AUTO_TEST_CASE(limits__safe_subtract__size_t_half_minus_maximum__throws_underflow)
{
    BOOST_REQUIRE_THROW(safe_subtract(half, maximum), std::underflow_error);
}

// safe_increment
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__safe_increment__size_t_minimum__expected)
{
    auto value = minimum;
    static const auto expected = minimum + 1u;
    safe_increment(value);
    BOOST_REQUIRE_EQUAL(value, expected);
}

BOOST_AUTO_TEST_CASE(limits__safe_increment__size_t_half__expected)
{
    auto value = half;
    static const auto expected = half + 1u;
    safe_increment(value);
    BOOST_REQUIRE_EQUAL(value, expected);
}

BOOST_AUTO_TEST_CASE(limits__safe_increment__size_t_maximum__throws_overflow)
{
    auto value = maximum;
    BOOST_REQUIRE_THROW(safe_increment(value), std::overflow_error);
}

// safe_decrement
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__safe_decrement__size_t_maximum__expected)
{
    auto value = maximum;
    static const auto expected = maximum - 1u;
    safe_decrement(value);
    BOOST_REQUIRE_EQUAL(value, expected);
}

BOOST_AUTO_TEST_CASE(limits__safe_decrement__size_t_half__expected)
{
    auto value = half;
    static const auto expected = half - 1u;
    safe_decrement(value);
    BOOST_REQUIRE_EQUAL(value, expected);
}

BOOST_AUTO_TEST_CASE(limits__safe_decrement__size_t_minimum__throws_underflow)
{
    auto value = minimum;
    BOOST_REQUIRE_THROW(safe_decrement(value), std::underflow_error);
}

// safe_signed
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__safe_signed__min_int32_to_int32__min_int32)
{
    BOOST_REQUIRE_EQUAL(safe_signed<int32_t>(min_int32), min_int32);
}

BOOST_AUTO_TEST_CASE(limits__safe_signed__max_int32_to_int32__max_int32)
{
    BOOST_REQUIRE_EQUAL(safe_signed<int32_t>(max_int32), max_int32);
}

BOOST_AUTO_TEST_CASE(limits__safe_signed__min_int64_to_int32__throws_range)
{
    BOOST_REQUIRE_THROW(safe_signed<int32_t>(min_int64), std::range_error);
}

// safe_unsigned
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__safe_unsigned__min_uint32_to_uint32__min_uint32)
{
    BOOST_REQUIRE_EQUAL(safe_unsigned<uint32_t>(min_uint32), min_uint32);
}

BOOST_AUTO_TEST_CASE(limits__safe_unsigned__max_uint32_to_uint32__max_uint32)
{
    BOOST_REQUIRE_EQUAL(safe_unsigned<uint32_t>(max_uint32), max_uint32);
}

BOOST_AUTO_TEST_CASE(limits__safe_unsigned__max_uint64_to_uint32__throws_range)
{
    BOOST_REQUIRE_THROW(safe_unsigned<uint32_t>(max_uint64), std::range_error);
}

// safe_to_signed
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__safe_to_signed__min_uint32_to_int32__min_uint32)
{
    BOOST_REQUIRE_EQUAL(safe_to_signed<int32_t>(min_uint32), (int32_t)min_uint32);
}

BOOST_AUTO_TEST_CASE(limits__safe_to_signed__max_uint32_to_int32__throws_range)
{
    BOOST_REQUIRE_THROW(safe_to_signed<int32_t>(max_uint32), std::range_error);
}

BOOST_AUTO_TEST_CASE(limits__safe_to_signed__min_uint64_to_int32__min_uint64)
{
    BOOST_REQUIRE_EQUAL(safe_to_signed<int32_t>(min_uint64), (int32_t)min_uint64);
}

// safe_to_unsigned
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__safe_to_unsigned__min_int32_to_uint32__throws_range)
{
    BOOST_REQUIRE_THROW(safe_to_unsigned<uint32_t>(min_int32), std::range_error);
}

BOOST_AUTO_TEST_CASE(limits__safe_to_unsigned__max_int32_to_uint32__max_int32)
{
    BOOST_REQUIRE_EQUAL(safe_to_unsigned<uint32_t>(max_int32), (uint32_t)max_int32);
}

BOOST_AUTO_TEST_CASE(limits__safe_to_unsigned__min_int64_to_uint32__throws_range)
{
    BOOST_REQUIRE_THROW(safe_to_unsigned<uint32_t>(min_int64), std::range_error);
}

// range_constrain
//-----------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE(limits__range_constrain__over__max)
{
    const size_t expected = 10;
    const auto result = range_constrain(size_t(42), size_t(1), expected);
    BOOST_REQUIRE_EQUAL(result, expected);
}

BOOST_AUTO_TEST_CASE(limits__range_constrain__under__min)
{
    const size_t expected = 50;
    const auto result = range_constrain(size_t(42), expected, size_t(100));
    BOOST_REQUIRE_EQUAL(result, expected);
}

BOOST_AUTO_TEST_CASE(limits__range_constrain__internal__unchanged)
{
    const size_t expected = 42;
    const auto result = range_constrain(expected, size_t(10), size_t(100));
    BOOST_REQUIRE_EQUAL(result, expected);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * \ file AttackReleaseHysteresisFilter.cpp
 */

#include <ATK/Dynamic/AttackReleaseHysteresisFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

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

constexpr gsl::index PROCESSSIZE = 1024*64;

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_attack_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_attack(0.5);
  BOOST_CHECK_EQUAL(filter.get_attack(), 0.5);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_attack_range_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_attack(-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_attack_range2_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_attack(1.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_release_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release(0.5);
  BOOST_CHECK_EQUAL(filter.get_release(), 0.5);
}

BOOST_AUTO_TEST_CASE( AttackRelease_Hysteresisrelease_range_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_release(-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_release_range2_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_release(1.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_attack_hysteresis_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release_hysteresis(0.1);
  filter.set_attack_hysteresis(0.5);
  BOOST_CHECK_EQUAL(filter.get_attack_hysteresis(), 0.5);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_attack_hysteresis_db_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release_hysteresis(0.001);
  filter.set_attack_hysteresis_db(-20);
  BOOST_CHECK_CLOSE(filter.get_attack_hysteresis(), 0.1, 0.1);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_attack_hysteresis_range_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release_hysteresis(.5);
  BOOST_CHECK_THROW(filter.set_attack_hysteresis(.5-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_attack_hysteresis_range2_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_attack_hysteresis(1.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_hysteresis_release_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release_hysteresis(0.5);
  BOOST_CHECK_EQUAL(filter.get_release_hysteresis(), 0.5);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_release_hysteresis_db_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  filter.set_release_hysteresis_db(-20);
  BOOST_CHECK_CLOSE(filter.get_release_hysteresis(), 0.1, 0.1);
}

BOOST_AUTO_TEST_CASE( AttackRelease_Hysteresisrelease_hysteresis_range_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_release_hysteresis(-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresis_release_hysteresis_range2_test )
{
  ATK::AttackReleaseHysteresisFilter<double> filter;
  BOOST_CHECK_THROW(filter.set_release_hysteresis(1.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresisFilter_triangle_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[i] = i / 48000;
  }
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    data[PROCESSSIZE/2 + i] = (PROCESSSIZE/2 - i) / 48000;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::AttackReleaseHysteresisFilter<double> filter(1);
  filter.set_attack(std::exp(-1./(48000 * 1e-3)));
  filter.set_release(std::exp(-1./(48000 * 100e-3)));
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    BOOST_REQUIRE_GE(data[i], outdata[i]);
  }
  for(gsl::index i = 0; i < PROCESSSIZE/2; ++i)
  {
    BOOST_REQUIRE_GE(outdata[PROCESSSIZE/2+i], outdata[PROCESSSIZE/2+i-1]);
  }
}

#define CUSTOMPROCESSSIZE 7

BOOST_AUTO_TEST_CASE( AttackReleaseHysteresisFilter_release_custom_test )
{
  double data[] = {0., 1., .5, .4, .3, .2, .1};
  double target[] = {0., 1., 1., .46, .46, .226, .1126};

  ATK::InPointerFilter<double> generator(data, 1, CUSTOMPROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, CUSTOMPROCESSSIZE> outdata;

  ATK::AttackReleaseHysteresisFilter<double> filter(1);
  filter.set_attack(0);
  filter.set_release(.1);
  filter.set_release_hysteresis(.5);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, CUSTOMPROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(CUSTOMPROCESSSIZE);

  for(gsl::index i = 0; i < CUSTOMPROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(target[i], outdata[i], .001);
  }
}


#ifndef __General_Utilities_hpp_
#define __General_Utilities_hpp_

#include <fstream>
#include <Eigen/Geometry>
#include <string>
#include <vector>

//Input Parameter from Config file
	extern std::string SimID,FormFactor,version,experiment;
	extern double vcut;
	extern double mChi,sigma0,nJ2000,rhoDM;
	extern double Detector_Depth;
	extern unsigned long long int Global_SampleSize_Initial;
	extern unsigned int Global_SampleSize_Velocity;
	extern int Isodetection_Rings;
	extern int SimDate [3];
	extern int SimTime [3];
	extern Eigen::Vector3d vEarth;
	extern int SampleSize;

	//Read config file and define Input Parameters
	 	extern void Read_Config_File(const char *inputfile);
	 	extern void Copy_Config_File(const char *inputfile);
//For the simulator
	//LogFile
		//Global Ofstream for logfile
			extern std::ofstream LogStream;
		//Create and write stuff into the logfile
			extern void LogFile_Initialize(int numprocs, std::ofstream& f=LogStream);
			extern void LogFile_Detector(std::ofstream& f=LogStream);
			extern void LogFile_Finalize(unsigned long long int nParticles,unsigned long long int nScatterings,unsigned long long int nFree,unsigned long long int vcutoff,unsigned long long int nIni,unsigned int nData,unsigned long long int nCrossing0,unsigned long long int nCrossing,double AvDensity,double duration1,double duration2,double duration3,std::ofstream& f=LogStream);

	//DM Density
		extern double IsoDetectionRing_Area(int theta,int rings,double depth=Detector_Depth);
		extern std::vector< std::vector<double>> DM_EnergyDensity(long double w0[],int long long unsigned n0total,long double w[],int unsigned long long ntotal,long double w0sq[],long double wsq[],int rings);
		extern std::vector< std::vector<double>> DM_NumberDensity(double mDM,std::vector<std::vector<double>> density);
		extern double DM_AverageDensity(std::vector<std::vector<double>> density,int rings,double depth=Detector_Depth);

//For the analyzer
	//Create an entry in the log file after successful data analysis
		extern void LogFile_Analysis(double duration,int worldsize);

	//Divide up the isodetection rings between the MPI processes as evenly as possible
		std::vector<int> WorkDivision(int WorldSize,int rings);
#endif


#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <Eigen/StdVector>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <map>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <set>
#include <sstream>
#include "opencv2/imgproc/imgproc.hpp"
using namespace cv;
using namespace std;

#define Pi 3.1415926
#define MAX_DETECTIONS 10

struct CornerPoints {
  double left_x, left_y, right_x, right_y;
  CornerPoints() {}
  CornerPoints(double l_x, double l_y, double r_x, double r_y)
      : left_x(l_x), left_y(l_y), right_x(r_x), right_y(r_y) {}
};

void RecordDetectedObjects(string filename,
                           multimap<int, CornerPoints>& objects_info) {
  ifstream file;
  file.open(filename.data());
  if (!file.is_open()) {
    cout << "Unable to Open the Detection File" << endl;
    exit(1);
  }
  vector<string> vec;
  string temp;
  while (getline(file, temp)) {
    vec.push_back(temp);
  }
  cout << "the Raw Detection Date is" << endl;
  for (vector<string>::iterator it = vec.begin(); it != vec.end(); it++) {
    cout << *it << endl;
    istringstream is(*it);
    string s0, s1, s2, s3, s4, s5, s6;
    is >> s0 >> s1 >> s2 >> s3 >> s4 >> s5 >> s6;
    double left_x = atof(s1.c_str());
    double left_y = atof(s2.c_str());
    double right_x = atof(s3.c_str());
    double right_y = atof(s4.c_str());
    int id = atof(s5.c_str());
    objects_info.insert(pair<int, CornerPoints>(
        id, CornerPoints(left_x, left_y, right_x, right_y)));
  }
  cout << endl;
  cout << "the Detected Objects Information is" << endl;

  for (multimap<int, CornerPoints>::iterator it = objects_info.begin();
       it != objects_info.end(); it++) {
    cout << it->first << " " << it->second.left_x << " " << it->second.left_y
         << " " << it->second.right_x << " " << it->second.right_y << " "
         << objects_info.count(it->first) << endl;
  }
  cout << endl;
}

void DrawBoundingBox() {
  Mat img = imread("/home/csj/Documents/quadric/dataset/dish/1.png",
                   CV_LOAD_IMAGE_COLOR);
  rectangle(img, Point(263, 252), Point(341, 327), Scalar(0, 0, 255), 1, 1, 0);
  rectangle(img, Point(430, 213), Point(535, 278), Scalar(255, 0, 0), 1, 1, 0);
  rectangle(img, Point(60, 236), Point(160, 291), Scalar(0, 255, 0), 1, 1, 0);
  rectangle(img, Point(261, 229), Point(361, 256), Scalar(0, 255, 255), 1, 1,
            0);
  imshow("img", img);
  waitKey(0);
  imwrite("/home/csj/Documents/quadric/dataset/boundingbox/1.png", img);
}

void ComputePointMat(multimap<int, CornerPoints> objects_info,
                     int detected_id,
                     int detected_nums,
                     std::vector<Point2d>& points) {
  int num = 0;
  multimap<int, CornerPoints>::iterator it = objects_info.find(detected_id);
  for (multimap<int, CornerPoints>::iterator p = it;
       it != objects_info.end() && num < detected_nums; p++, num++) {
    Point2d top_left(p->second.left_x, p->second.left_y);
    Point2d botton_right(p->second.right_x, p->second.right_y);
    // cout<<top_left<<" "<<botton_right<<endl;
    points.push_back(top_left);
    points.push_back(botton_right);
  }
}

// void
// ComputeLineMat(std::vector<Point2d>points,std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d>
// >& lines){
//  for(int i=0;i<points.size();i+=2){
//    lines.push_back(Eigen::Vector3d(1,0,-points[i].x));
//    lines.push_back(Eigen::Vector3d(0,1,-points[i].y));
//    lines.push_back(Eigen::Vector3d(1,0,-points[i+1].x));
//    lines.push_back(Eigen::Vector3d(0,1,-points[i+1].y));
//  }
//}
void ComputeLineMat(
    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>
        bboxes,
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>&
        lines) {
  for (int i = 0; i < bboxes.size(); i++) {
    lines.push_back(Eigen::Vector3d(1, 0, -(bboxes[i](0) - bboxes[i](2) / 2)));
    lines.push_back(Eigen::Vector3d(0, 1, -(bboxes[i](1) - bboxes[i](3) / 2)));
    lines.push_back(Eigen::Vector3d(1, 0, -(bboxes[i](0) + bboxes[i](2) / 2)));
    lines.push_back(Eigen::Vector3d(0, 1, -(bboxes[i](1) + bboxes[i](3) / 2)));
  }
}
Eigen::Matrix3d Quater2Rotation(const double& x,
                                const double& y,
                                const double& z,
                                const double& w) {
  Eigen::Quaterniond q;
  q.x() = x;
  q.y() = y;
  q.z() = z;
  q.w() = w;

  Eigen::Matrix3d R = q.normalized().toRotationMatrix();
  return R;
}

void ComputeProjectionMat(
    string filename,
    std::vector<Eigen::Matrix<double, 3, 4>,
                Eigen::aligned_allocator<Eigen::Matrix<double, 3, 4>>>&
        projection_matrix) {
  ifstream file;
  file.open(filename.data());
  if (!file.is_open()) {
    cout << "Unable to Open the Trajectories File" << endl;
    exit(1);
  }
  vector<string> vec;
  string temp;
  while (getline(file, temp)) {
    vec.push_back(temp);
  }

  Eigen::Matrix3d k;
  k << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;

  ofstream f_rotation("camera_rt35.txt");
  // ofstream f_rotation_mat("camera_test.txt");
  for (vector<string>::iterator it = vec.begin(); it != vec.end(); it++) {
    istringstream is(*it);
    string s0, s1, s2, s3, s4, s5, s6;
    is >> s0 >> s1 >> s2 >> s3 >> s4 >> s5 >> s6;
    double tx = atof(s0.c_str());
    double ty = atof(s1.c_str());
    double tz = atof(s2.c_str());
    double qx = atof(s3.c_str());
    double qy = atof(s4.c_str());
    double qz = atof(s5.c_str());
    double qw = atof(s6.c_str());

    Eigen::Matrix3d rotation;
    Eigen::Vector3d translation;

    Eigen::Matrix4d cam_rt;
    cam_rt = Eigen::Matrix4d::Identity();
    rotation = Quater2Rotation(qx, qy, qz, qw);

    cam_rt.block(0, 0, 3, 3) = rotation;
    cam_rt.block(0, 3, 3, 1) = Eigen::Vector3d(tx, ty, tz);
    f_rotation << cam_rt.col(0).transpose() << " " << cam_rt.col(1).transpose()
               << " " << cam_rt.col(2).transpose() << " "
               << cam_rt.col(3).transpose() << endl;

    translation << -tx, -ty, -tz;

    Eigen::Matrix<double, 3, 4> projection, rt;

    rt.block(0, 0, 3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1;

    rt.block(0, 0, 3, 3) = rotation.inverse();
    rt.block(0, 3, 3, 1) = rotation.inverse() * translation;  //?

    projection = k * rt;

    projection_matrix.push_back(projection);
  }
  //  for (int i = 0; i < projection_matrix.size(); i++) {
  //    cout << "The Projection of Frame " << i << " is " << endl
  //         << projection_matrix[i] << endl
  //         << endl;
  //  }
}

void ComputePlanesMat(
    const Eigen::Matrix<double, 3, 3>& calib,
    const vector<Eigen::Matrix<double, 3, 4>,
                 Eigen::aligned_allocator<Eigen::Matrix<double, 3, 4>>>&
        projection_matrix,
    const vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>&
        lines,
    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>&
        planes) {
  for (int i = 0; i < lines.size(); i++) {
    //    cout << lines[i] << endl;
    //    cout << calib * projection_matrix[i / 4] << endl;
    planes.push_back((calib * projection_matrix[i / 4]).transpose() * lines[i]);
  }
}

void ComputePlanesParameters(
    vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>& planes,
    std::vector<Eigen::Matrix<double, 1, 10>,
                Eigen::aligned_allocator<Eigen::Matrix<double, 1, 10>>>&
        planes_parameter) {
  for (vector<Eigen::Vector4d,
              Eigen::aligned_allocator<Eigen::Vector4d>>::iterator it =
           planes.begin();
       it != planes.end(); it++) {
    Eigen::Matrix<double, 1, 10> parm;

    parm << pow((*it)(0, 0), 2), 2 * (*it)(0, 0) * (*it)(1, 0),
        2 * (*it)(0, 0) * (*it)(2, 0), 2 * (*it)(0, 0) * (*it)(3, 0),
        pow((*it)(1, 0), 2), 2 * (*it)(1, 0) * (*it)(2, 0),
        2 * (*it)(1, 0) * (*it)(3, 0), pow((*it)(2, 0), 2),
        2 * (*it)(2, 0) * (*it)(3, 0), pow((*it)(3, 0), 2);
    //    cout << "parm " << parm << endl;
    planes_parameter.push_back(parm);
  }
}

void ComputeDualQuadric(
    const vector<Eigen::Matrix<double, 1, 10>,
                 Eigen::aligned_allocator<Eigen::Matrix<double, 1, 10>>>&
        planes_parameter,
    Eigen::Matrix3d& rotation,
    Eigen::Vector3d& shape,
    Eigen::Vector3d& translation,
    Eigen::Matrix4d& constrained_quadric) {
  //  ofstream ofile("quadric_parameters.txt", ios::app);

  int rows = planes_parameter.size();
  cout << "rows=" << rows << endl;
  Eigen::MatrixXd planes_svd(rows, 10);
  for (int i = 0; i < rows; i++) {
    planes_svd.row(i) = Eigen::VectorXd::Map(&planes_parameter[i][0],
                                             planes_parameter[i].size());
  }

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      planes_svd, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<double, 1, 10> dual_quadric_parm = svd.matrixV().col(9);

  Eigen::Matrix4d dual_quadric, raw_quadric;

  dual_quadric << dual_quadric_parm(0, 0), dual_quadric_parm(0, 1),
      dual_quadric_parm(0, 2), dual_quadric_parm(0, 3), dual_quadric_parm(0, 1),
      dual_quadric_parm(0, 4), dual_quadric_parm(0, 5), dual_quadric_parm(0, 6),
      dual_quadric_parm(0, 2), dual_quadric_parm(0, 5), dual_quadric_parm(0, 7),
      dual_quadric_parm(0, 8), dual_quadric_parm(0, 3), dual_quadric_parm(0, 6),
      dual_quadric_parm(0, 8), dual_quadric_parm(0, 9);

  raw_quadric = dual_quadric.inverse() * cbrt(dual_quadric.determinant());
  //  cout << "The Primary Form of Quadric is " << endl
  //       << raw_quadric << endl
  //       << endl;

  Eigen::Matrix3d quadric_33;
  quadric_33 = raw_quadric.block(0, 0, 3, 3);
  double det = raw_quadric.determinant() / quadric_33.determinant();
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(quadric_33);

  Eigen::Matrix3d eigen_vectors = eigen_solver.eigenvectors();
  cout << "The Rotation Matrix of the Constrained Quadric is " << endl
       << eigen_vectors << endl
       << endl;

  rotation = eigen_vectors;

  Eigen::Vector3d eigen_values;
  eigen_values = eigen_solver.eigenvalues();

  Eigen::Vector3d eigen_values_inverse;
  eigen_values_inverse = eigen_values.array().inverse();

  cout << "eigen_values  " << eigen_values << endl;
  cout << "eigen_values_inverse  " << eigen_values_inverse << endl;

  shape << (((-det) * eigen_values_inverse).array().abs()).array().sqrt();
  cout << "The Shape of the Constrained Quadric is " << endl
       << shape << endl
       << endl;

  translation << dual_quadric_parm(0, 3) / dual_quadric_parm(0, 9),
      dual_quadric_parm(0, 6) / dual_quadric_parm(0, 9),
      dual_quadric_parm(0, 8) / dual_quadric_parm(0, 9);

  cout << "The Translation of the Constrained Quadric is " << endl
       << translation << endl
       << endl;
  Eigen::Matrix4d T = Eigen::Matrix4d::Identity();

  // T.block(0,0,3,1)=eigen_vectors*translation;
  T.block(0, 0, 3, 3) = eigen_vectors;
  T.block(0, 3, 3, 1) = translation;

  //  ofile << shape.transpose() << " " << T.col(0).transpose() << " "
  //        << T.col(1).transpose() << " " << T.col(2).transpose() << " "
  //        << T.col(3).transpose() << endl;
  Eigen::Vector4d shape_parm;
  shape_parm << pow(shape(0, 0), 2), pow(shape(1, 0), 2), pow(shape(2, 0), 2),
      -1;

  Eigen::Matrix4d quadric_shape(shape_parm.asDiagonal());

  constrained_quadric = T * quadric_shape * T.transpose();
  cout << "The Constrained Quadric Matrix is " << endl
       << constrained_quadric << endl
       << endl;
}

void ComputeConicsMat(
    const Eigen::Matrix4d& constrained_quadric,
    const vector<Eigen::Matrix<double, 3, 4>,
                 Eigen::aligned_allocator<Eigen::Matrix<double, 3, 4>>>&
        projection_matrix,
    std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>&
        conics_matrix) {
  Eigen::Matrix3d dual_conic;
  for (int i = 0; i < projection_matrix.size(); i++) {
    dual_conic = projection_matrix[i] * constrained_quadric *
                 projection_matrix[i].transpose();
    conics_matrix.push_back(dual_conic.inverse() *
                            cbrt(dual_conic.determinant()));
  }
  for (int i = 0; i < projection_matrix.size(); i++) {
    cout << "The conic of frame " << i << " is " << endl
         << conics_matrix[i] << endl
         << endl;
  }
}


#include "ObjectND.h"

#include <vector>
#include <QOpenGLVertexArrayObject>
#include <QOpenGLBuffer>
#include <QOpenGLShaderProgram>
#include <QOpenGLFunctions>
#include <QObject>
#include <fstream>

#include <sstream>
#include <QMessageBox>

#include <Eigen/Dense>

#include "NMatrixOperations.h"
#include "RenderingWindow.h"
#include "CameraND.h"
#include "Camera3D.h"

#define print(something) {std::stringstream ss; ss << something; qDebug() << ss.str().c_str(); }

//Each vertex location as a column in the vertices matrix
//The edges between the vertexes as their indexes, as columns in the matrix
ObjectND::ObjectND(unsigned int N,
		Eigen::MatrixXd &vertices,
		Eigen::MatrixXi &edges,
		Eigen::MatrixXi &edgesWithCulling,
		Eigen::MatrixXi &faces,
		Eigen::MatrixXi &facesWithCulling,
		std::vector<CameraND> &camerasND,
		RenderingWindow* parent,
		int renderingMode){

	this->N = N;
	this->originalVertices = vertices;

	this->edgesList = edges;
	this->edgesWithCullingList = edgesWithCulling;
	this->facesList = faces;
	this->facesWithCullingList = facesWithCulling;

	this->parent = parent;
	this->requiresUpdate = false;

	updateVertexMatrix(camerasND, false);

	initializeOpenGL();

	this->useCulling = new bool;
	*this->useCulling = false;

	this->renderingMode = new int;
	*this->renderingMode = -1;
	this->setRenderingMode(renderingMode);
}

ObjectND::~ObjectND(){
	VAO->destroy();
	VBO->destroy();
	EBO->destroy();
	delete useCulling;
	delete renderingMode;
}

void ObjectND::initializeOpenGL() {
	//Create VAO, VBO and EBO
	VAO = new QOpenGLVertexArrayObject(parent);
	VBO = new QOpenGLBuffer(QOpenGLBuffer::VertexBuffer);
	EBO = new QOpenGLBuffer(QOpenGLBuffer::IndexBuffer);

	VAO->create();
	VBO->create();
	EBO->create();

	VAO->bind();

	VBO->bind();
	VBO->setUsagePattern(QOpenGLBuffer::DynamicDraw);
	Eigen::MatrixXf f = this->currentVertices.cast<float>();
	VBO->allocate(f.transpose().data(), this->currentVertices.size() * sizeof(float));

	EBO->bind();
	EBO->setUsagePattern(QOpenGLBuffer::StaticDraw);
	EBO->allocate(this->edgesList.data(), this->edgesList.size() * sizeof(unsigned int));

	parent->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(float), (void*)0);
	parent->glEnableVertexAttribArray(0);

	VAO->release();
}

void ObjectND::setRenderingMode(int newRenderingMode) {
	if (*this->renderingMode == newRenderingMode) return;
	*this->renderingMode = newRenderingMode;

	updateEBO();
}

int ObjectND::getRenderingMode() {
	return *renderingMode;
}

void ObjectND::setUseCulling(bool newUseCulling) {
	if (*useCulling == newUseCulling) return;
	*useCulling = newUseCulling;

	updateEBO();
}

bool ObjectND::getUseCulling() {
	return *useCulling;
}

void ObjectND::updateEBO() {
	EBO->bind();

	switch (*renderingMode) {
	case OBJECTND_RENDERINGMODE_LINES: {
		if (*useCulling) {
			EBO->allocate(this->edgesWithCullingList.data(), this->edgesWithCullingList.size() * sizeof(unsigned int));
		} else {
			EBO->allocate(this->edgesList.data(), this->edgesList.size() * sizeof(unsigned int));
		}
		break;
	}
	case OBJECTND_RENDERINGMODE_FACES: {
		if (*useCulling) {
			EBO->allocate(this->facesWithCullingList.data(), this->facesWithCullingList.size() * sizeof(unsigned int));
		}
		else {
			EBO->allocate(this->facesList.data(), this->facesList.size() * sizeof(unsigned int));
		}
		break;
	}
	default:
		break;
	}

	EBO->release();
}

void ObjectND::render() {
	//qDebug() << "R " << renderingMode;
	switch (*renderingMode) {
	case OBJECTND_RENDERINGMODE_LINES:
		VAO->bind();
		//glDrawArrays(GL_LINES, 0, currentEdges.size());
		if (*useCulling) {
			glDrawElements(GL_LINES, edgesWithCullingList.size(), GL_UNSIGNED_INT, 0);
		} else {
			glDrawElements(GL_LINES, edgesList.size(), GL_UNSIGNED_INT, 0);
		}
		VAO->release();
		break;
	case OBJECTND_RENDERINGMODE_FACES:
		VAO->bind();
		if (*useCulling) {
			glDrawElements(GL_TRIANGLES, facesWithCullingList.size(), GL_UNSIGNED_INT, 0);
		} else {
			glDrawElements(GL_TRIANGLES, facesList.size(), GL_UNSIGNED_INT, 0);
		}
		VAO->release();
	default:
		break;
	}
}

void ObjectND::updateVertexMatrix(std::vector<CameraND> camerasND, bool doUpdateVBO) {
	currentVertices = originalVertices;

	for (CameraND c : camerasND) {
		currentVertices = projectPointsLosingDimension(currentVertices, c.GetViewProjectionModelMatrix(), c.usePerspective);
	}

	if (doUpdateVBO) {
		updateVBO();
	}

	//print(currentVertices);
}

void ObjectND::updateVBO() {
	VBO->bind();
	Eigen::MatrixXf f = this->currentVertices.cast<float>();
	VBO->allocate(f.transpose().data(), currentVertices.size() * sizeof(float));
	VBO->release();
}