// PotentialFields.cpp : Defines the entry point for the console application.
//

#include <ros/ros.h>
#include <data_bridge/DTB_Trajectory.h>
#include <data_bridge/DTB_AbsPosition.h>
#include <data_bridge/DTB_ObjReport.h>
#include <data_bridge/definitions.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/Path.h>
#include "std_msgs/String.h"
#include <geometry_msgs/Pose2D.h>
#include <opencv2/opencv.hpp>
#include <vector>


using namespace std;



class PathPlanner
{
private:

ros::NodeHandle nh_;

volatile int needPosition;
volatile int needOldPath;
volatile int needMap;
volatile int needInfo;
volatile int startPathPlanning;

data_bridge::DTB_AbsPosition robotPosition;
data_bridge::DTB_Trajectory oldPath;
nav_msgs::OccupancyGrid occupancyMap;  
data_bridge::DTB_Trajectory safePath;
data_bridge::DTB_ObjReport detectedObstacles;

//for path plnanning we need some info...
//info about obstacles
ros::Subscriber sub;
//info about current robot position
ros::Subscriber sub1;
//info about robot path
ros::Subscriber sub2;
//info about robot surroundings
ros::Subscriber sub3;

//publisher for LGV
ros::Publisher pub;
//publisher for rviz
ros::Publisher pub2;

typedef struct {
	int x1;
	int y1;
	int x2;
	int y2;
	int x3;
	int y3;
	int x4;
	int y4;
}Rectangle;

vector< Rectangle > objects2;

int width;
int height;
float resolution;

nav_msgs::Path cestaRviz;

//const int Velikost_mistnostiX = 2000; // cm
//const int Velikost_mistnostiY = 1500; // cm
//const int Rozliseni = 5; // cm/bunku

int maxit;

int maxinputX;
int maxinputY; 


//int poc = 0;

double** InputArray;

public:

PathPlanner(ros::NodeHandle &nh)
  {
	nh_ = nh;
    
	maxit = 1000;
	
	needPosition = 1;
	needOldPath = 1;
	needMap = 1;
	needInfo = 1;
	startPathPlanning = 0;
    
    //set up the publisher for the cmd_vel topic
	sub = nh_.subscribe<data_bridge::DTB_ObjReport>(EXPAND(OBJDET_TOPIC_NAME), 1000,  &PathPlanner::getObstacles, this);
	sub1 = nh_.subscribe<data_bridge::DTB_AbsPosition>(EXPAND(ABS_POSITION_TOPIC_NAME), 1000,  &PathPlanner::getRobotPosition, this);
	sub2 = nh_.subscribe<data_bridge::DTB_Trajectory>(EXPAND(TRAJECTORY_IN_TOPIC_NAME), 1000,  &PathPlanner::getOldPath, this);
	sub3 = nh_.subscribe<nav_msgs::OccupancyGrid>("map", 1000, &PathPlanner::getMap, this);
	
	pub = nh_.advertise<data_bridge::DTB_Trajectory>(EXPAND(TRAJECTORY_OUT_TOPIC_NAME), 1000);
	pub2 = nh_.advertise<nav_msgs::Path>("SafePathToShow", 1000);
  }


 void getRobotPosition(const data_bridge::DTB_AbsPosition::ConstPtr& msg)
 {
 /*	ROS_INFO("Callback getRobotPosition entered");
 	ROS_INFO("needPosition %d", needPosition);
	ROS_INFO("startPathPlanning %d", startPathPlanning);*/
	if ((needPosition == 1) && (startPathPlanning == 1))
	{    
		needPosition = 0;
		ROS_INFO("We are getting robot position");
		robotPosition = *msg;
	}
	ros::spinOnce();
}

 void getMap(const nav_msgs::OccupancyGrid/*MapMetaData*/::ConstPtr& msg)
 {
  /*	ROS_INFO("Callback getMap entered");
	ROS_INFO("needMap %d", needMap);
	ROS_INFO("startPathPlanning %d", startPathPlanning);*/
	if ((needMap == 1)  && (startPathPlanning == 1))
	{    
		needMap = 0;
		ROS_INFO("We are getting map");
		occupancyMap = *msg;
		width = occupancyMap.info.width;
		height = occupancyMap.info.height;
		resolution = occupancyMap.info.resolution;
	}
	ros::spinOnce();
 }
 
 void getObstacles(const data_bridge::DTB_ObjReport::ConstPtr& msg)
 { 
 /*	ROS_INFO("Callback getObstacles entered");
 	ROS_INFO("pocet objektu %d", msg->objects.size());
	ROS_INFO("startPathPlanning %d", startPathPlanning);*/
        if ((needInfo == 1) && (msg->objects.size()>0) && (startPathPlanning == 1))
	{
  		needInfo = 0;
  		detectedObstacles = *msg;
		ROS_INFO("We are getting info about obstacle");
	}
	ros::spinOnce();
 }
 
void occupancyMapRenderObjects( nav_msgs::OccupancyGrid& og, vector<Rectangle>& objects )
{
	cv::Mat mog;


	mog = cv::Mat( /*og.info.*/height, /*og.info.*/width, CV_8SC1, &og.data[0], /*og.info.*/width );

	cv::Point ppts[4];
	const double c = 1./og.info.resolution;

	for( size_t i = 0; i < objects.size(); ++i )
	{
		ppts[0] = cv::Point( c*objects[i].x1, c*objects[i].y1);
		ppts[1] = cv::Point( c*objects[i].x2, c*objects[i].y2);
		ppts[2] = cv::Point( c*objects[i].x3, c*objects[i].y3);
		ppts[3] = cv::Point( c*objects[i].x4, c*objects[i].y4);
		//cout << ppts[0] << " " << ppts[1] << ppts[2] << " " << ppts[3] << endl;
		// redner
		fillConvexPoly( mog, &ppts[0], 4, cvScalarAll(100) );
	}

}




// size in meters
void occupancyMapDilate( nav_msgs::OccupancyGrid& og, float safe_size )
{
	cv::Mat mog;

	ROS_INFO("Jsme zde");	
	
	//mog = imread( "/home/beranv/map.pgm", -1 );
	//FileStorage fs( "/home/beranv/occupancyMap.xml", FileStorage::READ );
	//fs["og"] >> mog;
	//fs.release();
	//imshow( "original", mog );

	mog = cv::Mat( /*og.info.height, og.info.width*/height, width, CV_8SC1, /*og.data*/&og.data[0], /*og.info.width*/width );

	// set -1 to 100
	for( int j = 0; j < mog.rows; j++ )
	{
		signed char* mj = mog.ptr<signed char>(j);
		for( int i = 0; i < mog.cols; i++ )
			if( mj[i] < 0 ) mj[i] = 100;
	}

	//imshow( "resigned", mog );

	int sesize = cvCeil( 0.5f * safe_size / og.info.resolution );
	
	//int sesize = cvCeil( 0.5f * safe_size / 0.05f );
	cv::Mat se = cv::getStructuringElement( cv::MORPH_ELLIPSE, cv::Size(2*sesize+1, 2*sesize+1), cv::Point(sesize,sesize) );

	cv::Mat tmp;
	mog.convertTo(tmp, CV_32FC1 );
	morphologyEx( tmp, tmp, cv::MORPH_DILATE, se );
	tmp.convertTo( mog, CV_8SC1 );

	//imshow( "dilated", mog );

	//fs = FileStorage( "/home/beranv/occupancyMap_out.xml", FileStorage::WRITE );
	//fs << "og" << mog;
	//fs.release();

	//waitKey();
}

 //z TRAJECTORY_IN_TOPIC_NAME zjistit ID a odnekud jejich polohy, vypocitat teziste (idealne to bude jen jeden objekt), jeho
 //poloha by mela odpovidat poloze prekazky v gmappingu
 //udelat narust prekazek o polovinu velikosti LGV
 //4m LGV + 3m bezpecna vzdalenost + 4m prekazka + 3m bezpecna vzdalenost = 14x14m (velikost zvolit parametrem)
 //prevzorkovat podle parametru (px/cm) (+ dalsi parametr ma samotna mapa), klidne muze byt i 20 - 50cm/pix 
 //kolem okraje udelat ramecek s hodnotou 0.0
 //zjistit prusecik puvodni cesty a ramecku?
 //OPTIONAL - z dtto zjistit puvodni cestu, udelat krivky ve zvolene vzdalenosti 
 void processMap()
 {
	ROS_INFO("Rozliseni mapy je: %f", resolution);
	
	vector < Rectangle > objects(2);
	objects[0].x1 = 10; objects[0].y1 = 20;
	objects[0].x2 = 13; objects[0].y2 = 20;
	objects[0].x3 = 13; objects[0].y3 = 21;
	objects[0].x4 = 10; objects[0].y4 = 21;
	objects[1].x1 = 20; objects[1].y1 = 20;
	objects[1].x2 = 23; objects[1].y2 = 21;
	objects[1].x3 = 22; objects[1].y3 = 22;
	objects[1].x4 = 20; objects[1].y4 = 22;



	occupancyMapRenderObjects( occupancyMap, objects2 );
	occupancyMapDilate(occupancyMap, 1.0 );
	
	cv::Mat m;
	//m = cv::Mat(occupancyMap.info.height, occupancyMap.info.width, cv::CV_8SC1, occupancyMap.data, occupancyMap.info.width);
	m = cv::Mat(height,width,CV_8SC1,&occupancyMap.data[0], width );	
	cv::FileStorage fs("occupancyMap.xml", cv::FileStorage::WRITE );
	fs << "og" << m;
	fs.release();

	ROS_INFO("Sirka mapy je: %d", width);
	ROS_INFO("Vyska mapy je: %d", height);
	ROS_INFO("Rozliseni mapy je: %f", resolution);
	//InputArray[width][height];
	ROS_INFO("neprozkoumano v mape je: %d", occupancyMap.data[10]);
	int pocitadlo1 = 0;
	int pocitadlo2 = 0;
	
	//for (int j = height-1; j >= 0; j--)
	
	
	for (int j =0; j < height; j++)
		for (int i = 0; i < width; i++)
		{
			if ((occupancyMap.data[j*width+i] > 50) || (occupancyMap.data[j*width+i] == -1))
			{
				InputArray[i][height-1 - j] = 0.0;
				pocitadlo1++;
			}
			else
			{
				InputArray[i][height-1 - j] = 0.00001;//*/0.0000000000001;
				pocitadlo2++;
			}
		}
	ROS_INFO("pocitadlo1 je: %d", pocitadlo1);
	ROS_INFO("pocitadlo2 je: %d", pocitadlo2);	
	ROS_INFO("Prekazka v mape je: %d", occupancyMap.data[(height - /*130*/133)*width + 173]);
	ROS_INFO("Start v mape je: %f", InputArray[160][172]);
	ROS_INFO("Cil v mape je: %f", InputArray[240][230]);
	

	
	 cv::Mat MatArray = cv::Mat( height, width, CV_32FC1 );
	 for(int i = 0; i < width; i++)
	 	for(int j = 0; j < height; j++)
		{
			MatArray.at<float>(i,j) = InputArray[i][j];
		}
			
	MatArray.at<float>(160,172) = 0.0;
	MatArray.at<float>(240,230) = 0.0;
        
        cv::imshow("ia", MatArray );
        cv::waitKey(0);
        cv::imwrite( "inputarray.jpg", MatArray );
	//arr();
 }
 
 
void InitGrid()
//var i,j: integer;
{
for(int i = 0; i < maxinputX; i++)
 for(int j = 0; j < maxinputY; j++)
    if ((i == 0) || (i == maxinputX-1) || (j == 0) || (j == maxinputY-1))
      InputArray[i][j] = 1.0;
     else
      InputArray[i][j] = 0.5;//{0.5}0.001;
  //InputArray[i,j]:=sqrt(sqr({maxinput}20-i)+sqr({maxinput}15-j));
}


data_bridge::DTB_Trajectory cesta(int i, int j)
{
    int iter = 0;
	int maxITER = 500000;
	data_bridge::DTB_Trajectory path;
	
	ROS_INFO("zde03");
	
	ROS_INFO("Start X je %d:", i);
	ROS_INFO("Start Y je %d:", j); 
	ROS_INFO("InputArray %f:", InputArray[i][j]); 
	
	geometry_msgs::Pose2D bod;
	while (InputArray[i][j] < 1.0)
	{
		double maximum = 0.0;
		for(int k = i-1; k <= i+1; k++)
		{	
			for(int l = j-1; l <= j+1; l++)
			{
				ROS_INFO("Bod k je %d:", k);
				ROS_INFO("Bod l je %d:", l);
				ROS_INFO("InputArray na [k][l] je %f:", InputArray[k][l]);
				ROS_INFO("maximum je %f:", maximum);
				if (maximum < InputArray[k][l])
				{
					maximum = InputArray[k][l];
					bod.x = (float)k;
					bod.y = (float)l;	
				}
			}
		}
		i = (int)bod.x;
		j = (int)bod.y;
		path.traj_points.push_back(bod);
		//safePath.traj_points.push_back(bod);
		ROS_INFO("Bod je %f:", bod.x);
		ROS_INFO("Bod je %f:", bod.y);
		ROS_INFO("Bod i je %d:", i);
		ROS_INFO("Bod j je %d:", j);
		ROS_INFO("Length of the safe path is so far %d:", path.traj_points.size());
		ROS_INFO("zde04");
	}
	 ROS_INFO("zde05");
	
	
	
	 cv::Mat MatArray3 = cv::Mat( height, width, CV_32FC1 );
	 for(int i = 0; i < width; i++)
	 	for(int j = 0; j < height; j++)
		{
			MatArray3.at<float>(i,j) = 255 * InputArray[i][j];
		}
			
	for(int i = 0; i < path.traj_points.size(); i++)
	{
		MatArray3.at<float>(path.traj_points[i].x,path.traj_points[i].y) = 0.5;
	}
        
        cv::imshow("ia3", MatArray3 );
        cv::waitKey(0);
        cv::imwrite( "inputarray.jpg", MatArray3 );
        
        

	//float originX	= -occupancyMap.info.origin.position.x / resolution;
	//float originY	= height + occupancyMap.info.origin.position.y / resolution - 1;
	
	for(int i = 0; i < path.traj_points.size(); i++)
	{
		//int pathInMapX = originX + path.traj_points[i].x / resolution;
		//int pathInMapY = originY - path.traj_points[i].y / resolution;
		geometry_msgs::Pose2D bod;
		bod.x = path.traj_points[i].x*resolution + occupancyMap.info.origin.position.x;//*/pathInMapX;
		bod.y = (height - path.traj_points[i].y)*resolution + /*height +*/ occupancyMap.info.origin.position.y;//*/pathInMapY;
        	safePath.traj_points.push_back(bod);
	}
	

	//vyrabim cestu pro rviz
	for(int i = 0; i < safePath.traj_points.size(); i++)
	{
		geometry_msgs::PoseStamped poloha;
		poloha.pose.position.x = safePath.traj_points[i].x;
		poloha.pose.position.y = safePath.traj_points[i].y;
		cestaRviz.poses.push_back(poloha);
	}
	cestaRviz.header.frame_id = "/map";
	ROS_INFO("Path for Rviz is %d:", cestaRviz.poses.size());
	
	return safePath;
}

void pocitejZnovu(int x, int y)
{
	int maxinputX = width;
	int maxinputY = height;
	
	double w[maxinputX][maxinputY];
	
	ROS_INFO("zde00");
	ROS_INFO("maxinputX %d", maxinputX);
	ROS_INFO("maxinputY %d", maxinputY);
	
	InputArray[x][y] = 1.0;//goal coordinates
	ROS_INFO("Hodnota cile %f", InputArray[x][y]);
	
	for(int i = 0;i < maxinputX; i++)
	 for(int j = 0; j < maxinputY; j++)
		w[i][j] = InputArray[i][j];
	
	
	ROS_INFO("zde01");



for(int k = 0; k < maxit; k++)
 for(int i = 1; i < maxinputX - 1; i++)
  for(int j = 1; j < maxinputY - 1; j++)
  {
   if (w[i][j] == 0)
	   w[i][j] = 0;
   if (w[i][j] == 1) 
	   w[i][j] = 1;

   if ((w[i][j] != 0.0) && (w[i][j] != 1.0)/* && (w[i][j] != -1.0)*/)
   {
	   w[i][j] = (w[i-1][j-1]
	            + w[i-1][j] 
				+ w[i-1][j+1] 
				+ w[i][j+1]
				+ w[i][j-1]
	            + w[i+1][j-1] 
				+ w[i+1][j] 
				+ w[i+1][j+1])
					* 0.125;
   }
  }
 
//VYCHYTANY VYPOCET PRO ZLEPSENI  
/*
for(int k = 0; k < maxit; k++)
 for(int i = maxinputX - 1; i > 0; i--)
  for(int j = maxinputY - 1; j > 0; j--)
  {
   if (w[i][j] == 0)
	   w[i][j] = 0;
   if (w[i][j] == 1) 
	   w[i][j] = 1;

   if ((w[i][j] != 0.0) && (w[i][j] != 1.0))
   {
	   w[i][j] = (w[i-1][j-1]
	            + w[i-1][j] 
				+ w[i-1][j+1] 
				+ w[i][j+1]
				+ w[i][j-1]
	            + w[i+1][j-1] 
				+ w[i+1][j] 
				+ w[i+1][j+1])
					* 0.125;
   }
  }
  */
  
  for(int i = 0;i < maxinputX; i++)
	for(int j = 0; j < maxinputY; j++)
		InputArray[i][j] = w[i][j];
		
/****************/		
	 cv::Mat MatArray2 = cv::Mat( height, width, CV_32FC1 );
	 for(int i = 0; i < width; i++)
	 	for(int j = 0; j < height; j++)
		{
			MatArray2.at<float>(i,j) = 255 * InputArray[i][j];
		}
			
	//MatArray2.at<float>(160,172) = 0.0;
	//MatArray2.at<float>(240,230) = 0.0;
     /*   
        cv::imshow("ia2", MatArray2 );
        cv::waitKey(0);
        cv::imwrite( "inputarray.jpg", MatArray2 );*/
/***************/
  ROS_INFO("Hodnota cile %f", InputArray[x][y]);
}


void pathPlanning()
{
	ros::Rate rate(10);
	
	ROS_INFO("PathPlanning subscribers are running");
	
	
	//first we get robot position
	while (needPosition == 1)	
	{
		ROS_INFO("Waiting for info about robot position");
		rate.sleep();
		ros::spinOnce();
	}

	//then obstacles
	while (needInfo == 1)	
	{

		ROS_INFO("Waiting for info about obstacles");
		rate.sleep();
		ros::spinOnce();
	}
	
	//nd finally map
	while (needMap == 1)	
	{
		ROS_INFO("Waiting for info about occupancy map");
		rate.sleep();
		ros::spinOnce();
	}
		

	// the positions of bounding boxes of the obstacles are added
	ROS_INFO("Pocet vsech objektu %d", detectedObstacles.objects.size());
	for(int i = 0; i <detectedObstacles.objects.size(); i++)
	{
		ROS_INFO("zde1");
		if ((detectedObstacles.objects[i].m_zone == 2) && (detectedObstacles.objects[i].m_class > 0) && (detectedObstacles.objects[i].m_speed_class == 0))
		{
			Rectangle obj;
			obj.x1 = detectedObstacles.objects[i].m_bb[0].x;
			obj.y1 = detectedObstacles.objects[i].m_bb[0].y;
			obj.x2 = detectedObstacles.objects[i].m_bb[1].x;
			obj.y2 = detectedObstacles.objects[i].m_bb[1].y;
			obj.x3 = detectedObstacles.objects[i].m_bb[2].x;
			obj.y3 = detectedObstacles.objects[i].m_bb[2].y;
			obj.x4 = detectedObstacles.objects[i].m_bb[3].x;
			obj.y4 = detectedObstacles.objects[i].m_bb[3].y;
			objects2.push_back(obj);
			ROS_INFO("Zpracovavany objekt %d", i);
		}
	}


	//in this matrix we compute the potential field
	InputArray = new double*[height];
	for (int i = 0; i < height; ++i)
    		InputArray[i] = new double[width];
	
	
	//transformations to the map
	float originX	= -occupancyMap.info.origin.position.x / resolution;
	float originY	= height + occupancyMap.info.origin.position.y / resolution - 1;
	
	int robotInMapX = originX + /*_X*/robotPosition.abs_position.x / resolution;
	int robotInMapY = originY - /*_Y*/robotPosition.abs_position.y / resolution;
	
	float cilX = oldPath.traj_points[oldPath.traj_points.size() - 1].x;
	float cilY = oldPath.traj_points[oldPath.traj_points.size() - 1].y;
	
	
	int cilInMapX = originX + /*_X*/cilX / resolution;
	int cilInMapY = originY + /*_X*/cilY / resolution;
	
	
	cilInMapX = 160;
	cilInMapY = 172;
	robotInMapX = 270;
	robotInMapY = 230;
	
	processMap();
	pocitejZnovu(cilInMapX, cilInMapY);//CIL	
	data_bridge::DTB_Trajectory a = cesta(robotInMapX, robotInMapY);//START	
	//ros::spinOnce();
	
	
	ros::spinOnce();
	ROS_INFO("Length of the safe path is %d:", a.traj_points.size());	
	
	pub.publish(safePath);	
	
	/*for(int i = 0; i < 10; i++)
	{
		ros::spinOnce();
	}*/

	pub2.publish(cestaRviz);	
	
	//
	needInfo = 1;
	needPosition = 1;
	needOldPath = 1;
	needMap = 1;
	startPathPlanning = 0;


	for (int i = 0; i < height; ++i)
    		delete [] InputArray[i];
	delete [] InputArray;
}


/*
	Callback for getting old robot trajectory, IDs of obstacles and some other info,
	this is called everytime, when some message come into /data_bridge_trajectory_in topic
*/
 void getOldPath(const data_bridge::DTB_Trajectory::ConstPtr& msg)
 {
  	ROS_ERROR("Callback getOldPath entered");
	if (needOldPath == 1)
	{    
		startPathPlanning = 1;
		needOldPath = 0;
		ROS_INFO("We are getting old path");
		oldPath = *msg;
		pathPlanning();//here the path planning is started
	}
 }

};

PathPlanner *pathPlanner;

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

  
	
	/* initialization of the ROS stuff */
  	ros::init(argc, argv, "pathPlanner");
  	ros::NodeHandle nh;
  	
  	pathPlanner = new PathPlanner(nh);
  	
  	//ros::Rate rate(10);
  	
  	ROS_INFO("PathPlanning node is running");
  	

	

	ros::spin();
	return 0;
}


