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

#include <ros/ros.h>
//#include <actionlib/server/simple_action_server.h>
//#include <avoidance_planning_action_server/AvoidancePlanningAction.h>
//#include <data_bridge/DTB_ObjReport.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>

//#include "stdio.h"

//using namespace cv;
using namespace std;

volatile int needPosition = 1;
volatile int needOldPath = 1;
volatile int needMap = 1;
volatile int needInfo = 1;
volatile int startPathPlanning = 0;

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

ros::Subscriber sub;
ros::Subscriber sub1;
ros::Subscriber sub2;
ros::Subscriber sub3;

ros::Publisher pub;
ros::Publisher pub2;
ros::Publisher chatter_pub;

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

const int maxit = 1000;
//const int maxinputX = Velikost_mistnostiX/Rozliseni;
//const int maxinputY = Velikost_mistnostiY/Rozliseni;
int maxinputX; //= /*456;//*//*3;//*/793;
int maxinputY; //= /*233;//*//*24;//*/487;
//const int maxX = maxinputX;
//const int maxY = maxinputY;
//int maxinput = ;

/*
typedef struct {
	int x;
	int y;
//void Point() { x = 0; y = 0;};
}Point;*/

int poc = 0;
//double u [maxit][maxinputX][maxinputY];  
//double w /*[maxit]*/[maxinputX][maxinputY];
//double InputArray [][];
double** InputArray;
//procedure TForm1.Button1Click(Sender: TObject);

 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 arr()
{
        Mat InputArray = Mat::zeros( height, width, CV_32FC1 );
        InputArray.at<float>(width,height) = 0.0f;
        imshow("ia", InputArray );
        waitKey(0);
        imwrite( "inputarray.jpg", InputArray );
} */
 
void occupancyMapRenderObjects( nav_msgs::OccupancyGrid& og, /*vector< vector< geometry_msgs::Point > >*/ 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][0].x, c*objects[i][0].y);
		ppts[1] = cv::Point( c*objects[i][1].x, c*objects[i][1].y);
		ppts[2] = cv::Point( c*objects[i][2].x, c*objects[i][2].y);
		ppts[3] = cv::Point( c*objects[i][3].x, c*objects[i][3].y);*/
		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) );
	}

}


/*
void test ()
{
	Mat mog = imread( "/home/beranv/map.pgm", -1 );
	FileStorage fs( "/home/beranv/occupancyMap.xml", FileStorage::READ );
	fs["og"] >> mog;
	fs.release();

	//cout << "original" << endl; imshow( "original", mog );

	nav_msgs::OccupancyGrid occupancyMap;
	occupancyMap.info.height = mog.rows;
	occupancyMap.info.width = mog.cols;
	occupancyMap.info.resolution = 0.05f;
	occupancyMap.data = Mat_<signed char>(mog.reshape(1,1));
	mog = Mat( occupancyMap.info.height, occupancyMap.info.width, CV_8SC1, &occupancyMap.data[0], occupancyMap.info.width );

	//geometry_msgs::Point [4]
	vector< vector < geometry_msgs::Point > > objects(2);
	objects[0].resize(4);
	objects[1].resize(4);
	objects[0][0].x = 10; objects[0][0].y = 20;
	objects[0][1].x = 13; objects[0][1].y = 20;
	objects[0][2].x = 13; objects[0][2].y = 21;
	objects[0][3].x = 10; objects[0][3].y = 21;
	objects[1][0].x = 20; objects[1][0].y = 20;
	objects[1][1].x = 23; objects[1][1].y = 21;
	objects[1][2].x = 22; objects[1][2].y = 22;
	objects[1][3].x = 20; objects[1][3].y = 22;

	occupancyMapRenderObjects( occupancyMap, objects);

	cout << "objects" << endl; imshow( "objects", mog );
	occupancyMapDilate( occupancyMap, 0.5 );

	cout << "dilated" << endl; imshow( "dilated", mog );
	waitKey();
}*/

// 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;
	}
ROS_INFO("Jsme zde2");
	//imshow( "resigned", mog );

	int sesize = cvCeil( 0.5f * safe_size / og.info.resolution );
	ROS_INFO("Jsme zde3 %d", 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 );
ROS_INFO("Jsme zde4");
	//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< vector < geometry_msgs::Point > > objects(2);
	objects[0].resize(4);
	objects[1].resize(4);
	objects[0][0].x = 10; objects[0][0].y = 20;
	objects[0][1].x = 13; objects[0][1].y = 20;
	objects[0][2].x = 13; objects[0][2].y = 21;
	objects[0][3].x = 10; objects[0][3].y = 21;
	objects[1][0].x = 20; objects[1][0].y = 20;
	objects[1][1].x = 23; objects[1][1].y = 21;
	objects[1][2].x = 22; objects[1][2].y = 22;
	objects[1][3].x = 20; objects[1][3].y = 22;*/
	
	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));
}


/*std::vector<Point>**/data_bridge::DTB_Trajectory cesta(int i, int j)
//var {k,l,i_n,j_n}:integer;
{
    int iter = 0;
	int maxITER = 500000;
	/*std::vector<Point>*/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]); 
	
	/*Point*/geometry_msgs::Pose2D bod;
	while (InputArray[i][j] < 1.0)
	{
		//double minimum = 100;
		double maximum = 0.0;
		for(int k = i-1; k <= i+1; k++)
		{	
			for(int l = j-1; l <= j+1; l++)
			{
				//if (k == i && j == l) continue;
				//if ((k <= maxinputX) && (l <= maxinputY) && (k >= 0) && (l >= 0))
					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 (/*minimum >*/maximum < InputArray[k][l])
					{
						/*minimum*/maximum = InputArray[k][l];
						bod.x = (float)k;
						bod.y = (float)l;
						
						iter++;
						//if (iter > maxITER)
						//	return path;
					}
			}
		}
		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;
		//geometry_msgs::Path cesticka;
		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/*[0]*/[i][j] = InputArray[i][j];
	
	
	ROS_INFO("zde01");

	//int mit = 1000;
/*
	for(int k = 0; k < maxit; k++)
	 for(int i = 1; i < maxX-1; i++)
	  for(int j = 1; j < maxY-1; j++)
	  {
	   if (w[k][i][j] == 0)
		   w[k+1][i][j] = 0;
	   if (w[k][i][j] == 1) 
		   w[k+1][i][j] = 1;

	   if (w[k][i][j] != 0.0 && w[k][i][j] != 1.0)
	   {
		   w[k+1][i][j] = (//w[k][i-1][j-1]+
					 w[k+1][i-1][j] 
					//+ w[k][i-1][j+1] 
					+ w[k][i][j+1]
					+ w[k+1][i][j-1]
					//+ w[k][i+1][j-1] 
					+ w[k][i+1][j] 
					//+ w[k][i+1][j+1]
					) * 0.25;//0.125;
	   }
	  }*/

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;
   }
  }
  */
  ROS_INFO("zde02");
  
  for(int i = 0;i < maxinputX; i++)
	for(int j = 0; j < maxinputY; j++)
		InputArray[i][j] = w/*[maxit-2]*/[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::NodeHandle nh_*/)
{
	ros::Rate rate(10);
	
	ROS_INFO("PathPlanning subscribers are running");
	
	

	//sub1 = nh_.subscribe<data_bridge::DTB_AbsPosition>(EXPAND(ABS_POSITION_TOPIC_NAME), 1000, getRobotPosition);
	while (needPosition == 1)	
	{
		ROS_INFO("Waiting for info about robot position");
		rate.sleep();
		ros::spinOnce();
	}

	//sub = nh_.subscribe<data_bridge::DTB_ObjReport>(EXPAND(OBJDET_TOPIC_NAME), 1000, getObstacles);
	while (needInfo == 1)	
	{

		ROS_INFO("Waiting for info about obstacles");
		rate.sleep();
		ros::spinOnce();
	}
	
	while (needMap == 1)	
	{
		ROS_INFO("Waiting for info about occupancy map");
		rate.sleep();
		ros::spinOnce();
	}
		//sub3 = nh_.subscribe("map", 1000, getMap);

/*********Zacatek test **********/
/*	ROS_INFO("TADY!!!");
	int i = 0;
	data_bridge::DTB_Object pokus;
	detectedObstacles.objects.push_back(pokus);
	//detectedObstacles.objects[i].m_bb[0].x = 10;
	//ROS_INFO("TADY2!!! %f", detectedObstacles.objects[i].m_bb[0].x);
	detectedObstacles.objects[i].m_bb[0].x = 10; detectedObstacles.objects[i].m_bb[0].y = 20;
	detectedObstacles.objects[i].m_bb[1].x = 13; detectedObstacles.objects[i].m_bb[1].y = 20;
	detectedObstacles.objects[i].m_bb[2].x = 13; detectedObstacles.objects[i].m_bb[2].y = 21;
	detectedObstacles.objects[i].m_bb[3].x = 10; detectedObstacles.objects[i].m_bb[3].y = 21;
	ROS_INFO("TADY2!!!");*/
/****KONEC TEST *******/
	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);
		}
	}



	InputArray = new double*[height];
	for (int i = 0; i < height; ++i)
    		InputArray[i] = new double[width];
	
	
	
	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();
	//poc = 0;
//	InitGrid();
//	InitGridSuper();
	//pocitej(10,10);
	pocitejZnovu(/*160, 172*/cilInMapX, cilInMapY);//CIL	
//	printToBMP();
	data_bridge::DTB_Trajectory a = cesta(/*270,230*/robotInMapX, robotInMapY);//START	
//	printFinalToBMP(a);
	
	//pub = nh_.advertise<data_bridge::DTB_Trajectory>(EXPAND(TRAJECTORY_OUT_TOPIC_NAME), 1000);
	ros::spinOnce();
	//pub2 = nh_.advertise<nav_msgs::Path>("SafePathToShow", 1000);
	
	//chatter_pub = nh_.advertise<std_msgs::String>("chatter", 1000);
	//std_msgs::String msg;
	//msg.data = "Ahoj";
	
	ros::spinOnce();
	//chatter_pub.publish(msg);

	
	ros::spinOnce();
	ROS_INFO("Length of the safe path is %d:", a.traj_points.size());	
	/*********TEST **************/
	
	geometry_msgs::Pose2D bod;
	bod.x = 154;
	bod.y = 12;
	safePath.traj_points.push_back(bod);
	bod.x = 3;
	bod.y = 11;
	safePath.traj_points.push_back(bod);	
	safePath.proc_mode = 'a';
	safePath.mode = 'a';
	safePath.object_IDs.push_back(1);
	safePath.header.seq = 10245;
	//safePath.header.
	/****************************/
	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;
}

/*
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
  needMap = 0;
}
*/

 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(/*nh_*/);
	}
 }

int main(int argc, char *argv[])
{
	
	/* initialization of the ROS stuff */
  	ros::init(argc, argv, "potentialFields");
  	ros::NodeHandle nh_;
  	
  	ros::Rate rate(10);
  	
  	ROS_INFO("PathPlanning node is running");
  	
  	
  	sub = nh_.subscribe<data_bridge::DTB_ObjReport>(EXPAND(OBJDET_TOPIC_NAME), 1000, getObstacles);
  	sub1 = nh_.subscribe<data_bridge::DTB_AbsPosition>(EXPAND(ABS_POSITION_TOPIC_NAME), 1000, getRobotPosition);
  	sub2 = nh_.subscribe<data_bridge::DTB_Trajectory>(EXPAND(TRAJECTORY_IN_TOPIC_NAME), 1000, getOldPath);
  	sub3 = nh_.subscribe("map", 1000, getMap);

	pub = nh_.advertise<data_bridge::DTB_Trajectory>(EXPAND(TRAJECTORY_OUT_TOPIC_NAME), 1000);
	pub2 = nh_.advertise<nav_msgs::Path>("SafePathToShow", 1000);
	
	//chatter_pub = nh_.advertise<std_msgs::String>("chatter", 1000);
  	
//	sub = nh_.subscribe<data_bridge::DTB_ObjReport>(EXPAND(OBJDET_TOPIC_NAME), 1000, getObstacles);


/*	sub = nh_.subscribe("data_bridge_objdet", 1000, getObstacles);
	
	//sub3 = nh_.subscribe("map", 1000, getMap);//pokud se posilani mapy pusti az po startu nodu, tak to zustane ve whilu, kde se ceka na mapu
	
	while(ros::ok())
	{
		
		ROS_INFO("zde0");
		ROS_INFO("Pocet prekazek je %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))
			{
				startPathPlanning = 1;
				ROS_INFO("zde2");
				
			}
		}
		ROS_INFO("startPathPlanning je %d:", startPathPlanning);
		if (startPathPlanning == 1)
		{
			//startPathPlanning = 0;
			pathPlanning(nh_);
		}
			
		ros::spinOnce();
		rate.sleep();
	}
  	*/
 	//initial_ROS_time = ros::Time::now(); /* initialization of starting time of the node - it is used for timestamp convertions */
	
	
	//pathPlanning(nh_);
	
	//160, 172 cil 240,  230
	ros::spin();
	return 0;
}


