/*********************************************************************
//  created:    2013/06/19 - 18:40
//  filename:   CameraObstacleGridComponent.cpp
//
//  author:     Danilo Alves de Lima and Students of SY27
//              Copyright Heudiasyc UMR UTC/CNRS 6599
// 
//  version:    $Id: $
//
//  purpose:   Camera obstacle's grid calculation
//			   
//
*********************************************************************/

//#include "GeneralDefinitions.h"

#include "CameraObstacleGridComponent.h"

#include <iostream>
#include <string>
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/core/core.hpp"

#include <qapplication.h>
#include <string>

// Includes, qt.
#include <QMetaType>
#include <QDateTime>

#include "Pacpus/kernel/ComponentFactory.h"
#include "Pacpus/kernel/DbiteException.h"
#include "Pacpus/kernel/DbiteFileTypes.h"
#include "Pacpus/kernel/Log.h"
#include "GeneralDefinitions.h"

using namespace std;
using namespace pacpus;

DECLARE_STATIC_LOGGER("pacpus.base.CameraObstacleGridComponent");

// Construct the factory
static ComponentFactory<CameraObstacleGridComponent> sFactory("CameraObstacleGridComponent");

const int kMaxFilepathLength = 512; // TODO: should be same for all images
#define PI_VALUE 3.1415926535897932

//------------------------------- From stereo data----------------------------------
static const string CameraObstacleGridMemoryName_mask1 = "ObstacleDetection-mask1";
static const string CameraObstacleGridMemoryName_mask2 = "ObstacleDetection-mask2";
static const string CameraObstacleGridMemoryName_disp16 = "ObstacleDetection-disp";

static const string CameraObstacleGridMemoryName_obstgrid = "CameraObstacleGrid-obst";
//----------------------------------------------------------------------------------

//-------------------------------- From mono data ----------------------------------
static const string CameraObstacleGridMemoryName_roadseg = "LineDetection-mask";
static const string CameraObstacleGridMemoryName_roadgrid = "CameraObstacleGrid-road";
//----------------------------------------------------------------------------------

//////////////////////////////////////////////////////////////////////////
/// Constructor.
CameraObstacleGridComponent::CameraObstacleGridComponent(QString name)
	: ComponentBase(name)
{   
	LOG_TRACE(Q_FUNC_INFO);

	recording = 0;

	this->cam_width = 1280;		// Image width
	this->cam_height = 960;		// Image height
	this->showdebug = false;	// Show frame acquired

	// Size of the image data sizeof(char)*width*height*channels
	this->mMaxImageInputSize1 = sizeof(char)*this->cam_width*this->cam_height*3;

	// Input data
	this->shmem_mask1 = 0;		// Shared memory control access to the image data (free space mask)
	this->shmem_mask2 = 0;		// Shared memory control access to the image data (obstacles mask)
	this->shmem_disp16 = 0;		// Shared memory control access to the image data (disparity map 16)

	// Output data
	this->shmem_obst = 0;			// Shared memory control access to the image data

	// Output data via UDP
	//this->udp_con = 0;
}

//////////////////////////////////////////////////////////////////////////
/// Destructor.
CameraObstacleGridComponent::~CameraObstacleGridComponent()
{
	LOG_TRACE(Q_FUNC_INFO);

	if(this->shmem_mask1)
		delete shmem_mask1;

	this->shmem_mask1 = NULL;

	if(this->shmem_mask2)
		delete shmem_mask2;

	this->shmem_mask2 = NULL;

	if(this->shmem_disp16)
		delete shmem_disp16;

	this->shmem_disp16 = NULL;

	if(this->shmem_obst)
		delete shmem_obst;

	this->shmem_obst = NULL;

/*	if(this->udp_con)
		delete this->udp_con;

	this->udp_con = NULL;*/
}

//////////////////////////////////////////////////////////////////////////
/// Called by the ComponentManager to start the component
void CameraObstacleGridComponent::startActivity()
{
	LOG_TRACE(Q_FUNC_INFO);

	if(this->run_stereo)
	{
		this->mMaxImageInputSize1 = sizeof(unsigned char)*this->cam_width*this->cam_height;
		this->mMaxImageInputSize2 = sizeof(unsigned short)*this->cam_width*this->cam_height;

		if(this->data_type == 0)
		{
			this->mMaxOutputSize1 = sizeof(TimestampedCameraOccData);
		}
		else
		{
			this->mMaxOutputSize1 = sizeof(TimestampedSensorOccData);
		}

		this->mask1_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize1;
		this->mask2_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize1;
		this->disp_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize2;

		// Allocate memory position for the maximum expected data
		this->mask1_mem = malloc(this->mask1_mem_size); 
		this->mask2_mem = malloc(this->mask2_mem_size);
		this->disp_mem = malloc(this->disp_mem_size);

		this->shmem_mask1 = new ShMem(CameraObstacleGridMemoryName_mask1.c_str(), this->mask1_mem_size);

		this->shmem_mask2 = new ShMem(CameraObstacleGridMemoryName_mask2.c_str(), this->mask2_mem_size);

		this->shmem_disp16 = new ShMem(CameraObstacleGridMemoryName_disp16.c_str(), this->disp_mem_size);

		if(this->use_udpconnection)
		{
			//this->udp_con = new UDPConnection();
			//this->udp_con->CreateConnection(this->destiny_IP, this->communication_Port, this->mMaxOutputSize1, false);
			this->shmem_obst = NULL;
		}
		else
		{
			//this->udp_con = NULL;
			this->shmem_obst = new ShMem(CameraObstacleGridMemoryName_obstgrid.c_str(), this->mMaxOutputSize1);
		}
	}
	else
	{
		this->mMaxImageInputSize1 = sizeof(unsigned char)*this->cam_width*this->cam_height;

		this->mMaxOutputSize1 = sizeof(TimestampedSensorOccData);

		this->mask1_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize1;

		// Allocate memory position for the maximum expected data
		this->mask1_mem = malloc(this->mask1_mem_size); 

		this->shmem_mask1 = new ShMem(CameraObstacleGridMemoryName_roadseg.c_str(), this->mask1_mem_size);

		this->shmem_mask2 = NULL;

		this->shmem_disp16 = NULL;

		if(this->use_udpconnection)
		{
			//this->udp_con = new UDPConnection();
			//this->udp_con->CreateConnection(this->destiny_IP, this->communication_Port, this->mMaxOutputSize1, false);
			this->shmem_obst = NULL;
		}
		else
		{
			//this->udp_con = NULL;
			this->shmem_obst = new ShMem(CameraObstacleGridMemoryName_roadgrid.c_str(), this->mMaxOutputSize1);
		}
	}


	QString tmp = QDateTime::currentDateTimeUtc().toString("yyyy_MM_dd-hh_mm_ss") + ".txt";
	saveFile_.setFileName(tmp);
	if (!saveFile_.open(QIODevice::WriteOnly | QIODevice::Text))
		return;
	flux_.setDevice(&saveFile_);
	flux_.setCodec("UTF-8");

	// Run thread
	THREAD_ALIVE = true;
	start();
}

//////////////////////////////////////////////////////////////////////////
/// Called by the ComponentManager to stop the component
void CameraObstacleGridComponent::stopActivity()
{
	LOG_TRACE(Q_FUNC_INFO);

	if(THREAD_ALIVE)
	{
		// Stop thread
		THREAD_ALIVE = false;

		while(is_running)
		{
			msleep(/*MS_DELAY*/10);
		}

		if(this->shmem_mask1)
			delete shmem_mask1;

		this->shmem_mask1 = NULL;

		if(this->shmem_mask2)
			delete shmem_mask2;

		this->shmem_mask2 = NULL;

		if(this->shmem_disp16)
			delete shmem_disp16;

		this->shmem_disp16 = NULL;

		if(this->shmem_obst)
			delete shmem_obst;

		this->shmem_obst = NULL;

		/*if(this->udp_con)
			delete this->udp_con;

		this->udp_con = NULL;*/
	}

	saveFile_.close();

	LOG_INFO("stopped component '" << name() << "'");
}

//////////////////////////////////////////////////////////////////////////
/// Called by the ComponentManager to pass the XML parameters to the
/// component
ComponentBase::COMPONENT_CONFIGURATION CameraObstacleGridComponent::configureComponent(XmlComponentConfig config)
{
	LOG_TRACE(Q_FUNC_INFO);

	// Initialize with default values
	InitDefault();

	if (config.getProperty("run_stereo") != QString::null)
		this->run_stereo = config.getProperty("run_stereo").toInt();

	if (config.getProperty("recording") != QString::null)
		recording = config.getProperty("recording").toInt();

	if (config.getProperty("cam_width") != QString::null)
		this->cam_width = config.getProperty("cam_width").toInt();

	if (config.getProperty("cam_height") != QString::null)
		this->cam_height = config.getProperty("cam_height").toInt();

	if (config.getProperty("cam_fov") != QString::null)
		this->cam_fov = config.getProperty("cam_fov").toDouble();

	if (config.getProperty("cam_fx") != QString::null)
		this->cam_fx = config.getProperty("cam_fx").toDouble();

	if (config.getProperty("cam_fy") != QString::null)
		this->cam_fy = config.getProperty("cam_fy").toDouble();

	if (config.getProperty("cam_cx") != QString::null)
		this->cam_cx = config.getProperty("cam_cx").toDouble();

	if (config.getProperty("cam_cy") != QString::null)
		this->cam_cy = config.getProperty("cam_cy").toDouble();

	if (config.getProperty("cam_baseline") != QString::null)
		this->cam_baseline = config.getProperty("cam_baseline").toDouble();

	if (config.getProperty("cam_tilt_angle") != QString::null)
		this->cam_tilt_angle = config.getProperty("cam_tilt_angle").toDouble()*PI_VALUE/180.0;

	if (config.getProperty("CorrAccuracy") != QString::null)
		this->CorrAccuracy = config.getProperty("CorrAccuracy").toDouble();
	
	if (config.getProperty("cam_valid_col0") != QString::null)
		this->cam_valid_col0 = config.getProperty("cam_valid_col0").toInt();
	else
		this->cam_valid_col0 = 0;

	if (config.getProperty("cam_valid_row0") != QString::null)
		this->cam_valid_row0 = config.getProperty("cam_valid_row0").toInt();
	else
		this->cam_valid_row0 = 0;

	if (config.getProperty("cam_valid_cols") != QString::null)
		this->cam_valid_cols = config.getProperty("cam_valid_cols").toInt();
	else
		this->cam_valid_cols = this->cam_width;

	if (config.getProperty("cam_valid_rows") != QString::null)
		this->cam_valid_rows = config.getProperty("cam_valid_rows").toInt();
	else
		this->cam_valid_rows = this->cam_height;

	if (config.getProperty("data_type") != QString::null)
		this->data_type = config.getProperty("data_type").toInt();

	if (config.getProperty("D_MAX_CAM_GRID") != QString::null)
		this->D_MAX_CAM_GRID = config.getProperty("D_MAX_CAM_GRID").toDouble();

	if (config.getProperty("sub_div") != QString::null)
		this->sub_div = config.getProperty("sub_div").toDouble();

	if (config.getProperty("free_area_guess") != QString::null)
		this->free_area_guess = config.getProperty("free_area_guess").toDouble();

	if (config.getProperty("showdebug") != QString::null)
		this->showdebug = (bool)config.getProperty("showdebug").toInt();

	if (config.getProperty("use_udpconnection") != QString::null)
		this->use_udpconnection = (bool)config.getProperty("use_udpconnection").toInt();

	if ((this->use_udpconnection)&&(this->data_type == 1))
	{
		LOG_WARN("This data type is not valid for UDP connexion! Data type changed to default.");
		this->data_type = 0;
	}

	if (config.getProperty("MAX_DIST_TO_BRAKE") != QString::null)
		this->maxDistToBrake = config.getProperty("MAX_DIST_TO_BRAKE").toDouble();

	if(this->use_udpconnection)
	{
		//this->destiny_IP = (config.getProperty("destiny_IP") != QString::null) ? QHostAddress(config.getProperty("destiny_IP")) : QHostAddress::LocalHost;
		//this->communication_Port = (config.getProperty("communication_Port") != QString::null) ? (quint16)config.getProperty("communication_Port").toInt() : (quint16)1200;
	}
	

	LOG_INFO("configured component '" << name() << "'");
    return ComponentBase::CONFIGURED_OK;
}

/** 
* Initialize default values
*/
void CameraObstacleGridComponent::InitDefault()
{
	// Default
	recording = 0;

	this->maxDistToBrake = 3.0;

	this->run_stereo = 1;		// Select the work mode (stereo camera data or mono camera data)

	// Default values for bumblebee X3 - 6mm - narrow - 400x300
	this->cam_width = 400;		// Image width
	this->cam_height = 300;		// Image height
	this->cam_fov = 43.0;
	this->cam_valid_col0 = 0;		// Initial valid column 
	this->cam_valid_row0 = 0;		// Initial valid row
	this->cam_valid_cols = 400;		// Valid columns 
	this->cam_valid_rows = 300;		// Valid rows
	this->cam_fx = 1.26209;			// Focus in x (pixels)
	this->cam_fy = 1.68279;			// Focus in y (pixels)
	this->cam_cx = 0.50875;			// Focus in x (pixels)
	this->cam_cy = 0.510218;		// Focus in y (pixels)
	this->cam_baseline = 0.12;		// Baseline (meters)
	this->cam_tilt_angle = 0.0;		// Tilt angle (rad)
	this->cam_h = 1.615;			// Camera height
	this->CorrAccuracy = 0.2;		// Estimated camera correlation accuracy
	this->showdebug = false;		// Show frame acquired

	this->use_udpconnection = false;

	//this->destiny_IP = QHostAddress::LocalHost;
	//this->communication_Port = (quint16)1200;

	this->D_MAX_CAM_GRID = D_MAX_CAM_GRID_DEFAULT;
	this->data_type = 0;
	this->sub_div = 0.20;

	this->free_area_guess = 4.0;	// Frontal projected area expected to be a road surface (frontal road surface uncouvered by camera FOV) 
}

// Thread loop for Stereo data
void CameraObstacleGridComponent::run()
{
	LOG_TRACE(Q_FUNC_INFO);

	this->is_running = true;

	if(this->run_stereo)
	{
		if(this->CurrentMask1Frame.cols != this->cam_width)
		{
			this->CurrentMask1Frame = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, 1));
		}
	
		if(this->CurrentMask2Frame.cols != this->cam_width)
		{
			this->CurrentMask2Frame = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, 1));
		}

		// Create the image in which we will save the disparities
		if(this->CurrentDisparityMap16.cols != this->cam_width)
		{
			this->CurrentDisparityMap16 = cv::Mat( this->cam_height, this->cam_width, CV_16S );
		}

		// Keeps the last image timestamp;
		road_time_t last_reading = 0;

		// Time measurement
		road_time_t init_time = 0;
	
		while (THREAD_ALIVE) 
		{
			//init_time = road_time();

			//LOG_INFO("Grab new image");
			// header + image
			this->shmem_mask1->read(this->mask1_mem, this->mask1_mem_size);
			this->shmem_mask2->read(this->mask2_mem, this->mask2_mem_size);
			this->shmem_disp16->read(this->disp_mem, this->disp_mem_size);

			// Header
			memcpy( &this->Mask1ImageHeader, this->mask1_mem, sizeof(TimestampedStructImage)); 
			memcpy( &this->Mask2ImageHeader, this->mask2_mem, sizeof(TimestampedStructImage)); 
			memcpy( &this->DispImageHeader, this->disp_mem, sizeof(TimestampedStructImage)); 
	
			// Check image header
			bool is_ok = false;
			if( (this->Mask1ImageHeader.image.data_size == this->mMaxImageInputSize1) && (this->Mask1ImageHeader.time != last_reading) &&
				(this->Mask2ImageHeader.image.data_size == this->mMaxImageInputSize1) && (this->Mask2ImageHeader.time == this->Mask1ImageHeader.time) &&
				(this->DispImageHeader.image.data_size == this->mMaxImageInputSize2) && (this->DispImageHeader.time == this->Mask1ImageHeader.time) )
			{
				is_ok = true;
				last_reading = this->Mask1ImageHeader.time;

				/*std::cout << "Expected image w:  " << ImageHeader.image.width << std::endl;
				std::cout << "Expected image h:  " << ImageHeader.image.height << std::endl;
				std::cout << "Expected image c:  " << ImageHeader.image.channels << std::endl;
				std::cout << "Expected image data:  " << ImageHeader.image.data_size << std::endl;
				std::cout << "Expected image size:  " << image_mem << std::endl;*/
			}
			/*else
			{
				LOG_ERROR("Error in the image data size!");
			}*/

			//LOG_INFO("Grab new image");
			if(is_ok)
			{
				// Image data
				memcpy( (unsigned char*)(this->CurrentMask1Frame.data), (unsigned char*)((TimestampedStructImage*)this->mask1_mem + 1), this->Mask1ImageHeader.image.data_size); 
				memcpy( (unsigned char*)(this->CurrentMask2Frame.data), (unsigned char*)((TimestampedStructImage*)this->mask2_mem + 1), this->Mask2ImageHeader.image.data_size); 
				memcpy( (unsigned short*)(this->CurrentDisparityMap16.data), (unsigned short*)((TimestampedStructImage*)this->disp_mem + 1), this->DispImageHeader.image.data_size); 

				//======================================= Obstacle Grid Calculation ================================================
				if(this->data_type == 0)
				{
					// Camera to laser obstacles grid
					this->CreateCamera2LaserGrid(this->CurrentMask2Frame, this->CurrentDisparityMap16, this->Camdata2Laser.data.radius, this->Camdata2Laser.data.angle);

					this->Camdata2Laser.time = this->DispImageHeader.time;
					this->Camdata2Laser.timerange = this->DispImageHeader.timerange;
					this->Camdata2Laser.data.num_readings = this->cam_valid_cols;

					if(this->use_udpconnection)
					{
						//this->udp_con->write(&this->Camdata2Laser, this->mMaxOutputSize1);
					}
					else
					{
						this->shmem_obst->write(&this->Camdata2Laser, this->mMaxOutputSize1);
					}
			
					if(this->showdebug)
					{	
						cv::namedWindow( "CameraObstacleGridComponent - Final Result", CV_WINDOW_AUTOSIZE );
						cv::imshow("CameraObstacleGridComponent - Final Result", this->DrawGrid(this->Camdata2Laser.data.radius, this->Camdata2Laser.data.angle));
						cv::waitKey(1);
					}
				}
				else
				{
					// Camera occupancy grid
					this->CreateCameraGrid(this->CurrentMask1Frame, this->CurrentMask2Frame, this->CurrentDisparityMap16, this->CamOccGrid.data);

					this->CamOccGrid.time = this->DispImageHeader.time;
					this->CamOccGrid.timerange = this->DispImageHeader.timerange;

					this->shmem_obst->write(&this->CamOccGrid, this->mMaxOutputSize1);

					if(this->showdebug)
					{	
						cv::namedWindow( "CameraObstacleGridComponent - Final Result", CV_WINDOW_AUTOSIZE );
						cv::imshow("CameraObstacleGridComponent - Final Result", this->DrawGrid(this->CamOccGrid.data) );
						cv::waitKey(1);
					}
				}

				//==================================================================================================================

				//std::cout << componentName.toStdString() << " cicle time: " << (road_time() - init_time)/1000000.0 << std::endl;
			}
			else
			{
				msleep(/*MS_DELAY*/10);
			}

			if(this->showdebug)
				cv::waitKey(1); // Give the system permission
		}
	}
	else
	{
		this->run2();
	}

	this->is_running = false;

	// Destroy the window frame
	if(this->showdebug)
		cvDestroyAllWindows();
}

// Thread loop for mono camera data
void CameraObstacleGridComponent::run2()
{
	if(this->CurrentMask1Frame.cols != this->cam_width)
	{
		this->CurrentMask1Frame = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, 1));
	}

	this->CreateProjectionMatrix(this->D_MAX_CAM_GRID);

	// Keeps the last image timestamp;
	road_time_t last_reading = 0;

	// Time measurement
	road_time_t init_time = 0;
	
	while (THREAD_ALIVE) 
	{
		//init_time = road_time();

		//LOG_INFO("Grab new image");
		// header + image
		this->shmem_mask1->read(this->mask1_mem, this->mask1_mem_size);
		
		// Header
		memcpy( &this->Mask1ImageHeader, this->mask1_mem, sizeof(TimestampedStructImage)); 
		
		// Check image header
		bool is_ok = false;
		if( (this->Mask1ImageHeader.image.data_size == this->mMaxImageInputSize1) && (this->Mask1ImageHeader.time != last_reading) )
		{
			is_ok = true;
			last_reading = this->Mask1ImageHeader.time;

			/*std::cout << "Expected image w:  " << ImageHeader.image.width << std::endl;
			std::cout << "Expected image h:  " << ImageHeader.image.height << std::endl;
			std::cout << "Expected image c:  " << ImageHeader.image.channels << std::endl;
			std::cout << "Expected image data:  " << ImageHeader.image.data_size << std::endl;
			std::cout << "Expected image size:  " << image_mem << std::endl;*/
		}
		/*else
		{
			LOG_ERROR("Error in the image data size!");
		}*/

		//LOG_INFO("Grab new image");
		if(is_ok)
		{
			// Image data
			memcpy( (unsigned char*)(this->CurrentMask1Frame.data), (unsigned char*)((TimestampedStructImage*)this->mask1_mem + 1), this->Mask1ImageHeader.image.data_size); 
			
			//======================================= Obstacle Grid Calculation ================================================
			
			// Camera occupancy grid
			this->CreateMonoCameraGrid(this->CurrentMask1Frame, this->CamOccGrid.data, this->D_MAX_CAM_GRID, 1);

			this->CamOccGrid.time = this->Mask1ImageHeader.time;
			this->CamOccGrid.timerange = this->Mask1ImageHeader.timerange;

			this->shmem_obst->write(&this->CamOccGrid, this->mMaxOutputSize1);

			if(this->showdebug)
			{	
				cv::namedWindow( "CameraObstacleGridComponent2 - Final Result", CV_WINDOW_AUTOSIZE );
				cv::imshow("CameraObstacleGridComponent2 - Final Result", this->DrawGrid(this->CamOccGrid.data) );
				cv::waitKey(1);
			}

			//==================================================================================================================

			//std::cout << componentName.toStdString() << " cicle time: " << (road_time() - init_time)/1000000.0 << std::endl;
		}
		else
		{
			msleep(MS_DELAY);
		}

		if(this->showdebug)
			cv::waitKey(1); // Give the system permission
	}
}

// Function to create the obstacle's grid the mask obstacles image
void CameraObstacleGridComponent::CreateCamera2LaserGrid(cv::Mat mask_obstacles, cv::Mat disp_map16, double* radius_data, double* angle_data)
{
	// Delta angle (rad)
	const double angle_dec = (this->cam_fov/(double)this->cam_width)*CV_PI/180.0; 

	// left angle (rad) (positive to the left and negative to the right)
	const double angle_esq = (this->cam_fov/2.0)*CV_PI/180.0 - (double)this->cam_valid_col0*angle_dec;

	// Current Angle
	double angle = angle_esq;

	// d = f*b/Z
	double disp_limit_max = this->cam_width*this->cam_fx*this->cam_baseline/ROBOT_FRONT_DIST;
	double disp_limit_min = this->cam_width*this->cam_fx*this->cam_baseline/D_MAX_CAM_GRID;
	double max_cam_distance = this->cam_width*this->cam_fx*this->cam_baseline/1.0;

	// Coordenadas no espaco
	double x = 0.0; 
	double y = 0.0; 
	double z = 0.0;

	// Coordenadas na imagem
	int linha, coluna;
	double max_col_disp;

	double dist_min = 100.0;

	//------------------------------------------------------------------------------

	// Auxiliary variables
	unsigned short*     row;

	// Run across the image checking the high disparity value for each column
	for (int i = this->cam_valid_col0; i <= (this->cam_valid_cols + this->cam_valid_col0 - 1); ++i)
	{
		// Max distance = disparity 1.0
		double distance = max_cam_distance;

		max_col_disp = 0.0;

		for (int j = this->cam_valid_row0; j <= (this->cam_valid_rows + this->cam_valid_row0 - 1); ++j)
		{
			// Calcula a posicao do pixel na mascara e no mapa de disparidade 
			int pixel1 =  (j)*mask_obstacles.cols + (i);
			
			row = (unsigned short*)disp_map16.data + j*this->cam_width;

			//se for branco verifica se a disparidade esta dentro de uma margem 2 m e 20 m
			if ((mask_obstacles.data[pixel1] != 0)&&
				(row[i]/16.0 > disp_limit_min)&&(row[i]/16.0 < disp_limit_max)&&(row[i]/16.0 > max_col_disp))
			{						
				max_col_disp = row[i]/16.0;
				
				linha = j;
				coluna = i;
			}
		}

		if (max_col_disp != 0)
		{
			// It is a valid point
			if(this->PointTriangulate( linha , coluna, x, y, z, max_col_disp ) )
			{
				z = z*std::cos(this->cam_tilt_angle);

				double dist_aux = sqrt( x*x + z*z );
				
				if (dist_aux < distance)
					distance = dist_aux;

				if (distance < dist_min)
					dist_min = distance;
			}
		}

		//Insert value in the grid
		radius_data[i - this->cam_valid_col0] = distance;
		angle_data[i - this->cam_valid_col0] = angle;

		// Next angle
		angle = angle - angle_dec;
	}
	
	if (dist_min < maxDistToBrake)
	{
		emit smallestDistance();		
		cout <<  "TIME TO BRAKE: " << dist_min << " m at " << road_time() << endl;
		flux_ <<  "TIME TO BRAKE: " << dist_min << " m at " << road_time() << endl;
	}

	//-----------------------------------------------------------------------------

	return;
}

// Function to create the obstacle's grid the mask obstacles image
void CameraObstacleGridComponent::CreateCameraGrid(cv::Mat mask_free, cv::Mat mask_obstacles, cv::Mat disp_map16, sensor_occ_data &occ_data)
{
	double std_deviation;	// calculated with the camera distance = (f*B*CorrAccuracy/(d*d))/3

	// d = f*b/Z
	double disp_limit_max = this->cam_width*this->cam_fx*this->cam_baseline/ROBOT_FRONT_DIST;
	double disp_limit_min = this->cam_width*this->cam_fx*this->cam_baseline/D_MAX_CAM_GRID;
	double max_cam_distance = this->cam_width*this->cam_fx*this->cam_baseline/1.0;

	// 3D coordinates of the image point
	double x = 0.0; 
	double y = 0.0; 
	double z = 0.0;

	// 2D image coordinates
	int row, col;
	
	// Current grid position
	int grid_x, grid_y, grid_x_aux, grid_y_aux;

	//------------------------------------------------------------------------------
	occ_data.cols = D_MAX_CAM_GRID/this->sub_div;
	occ_data.rows = occ_data.cols;

	occ_data.sensor_x0 = 0.0f;
	occ_data.sensor_y0 = (float)(occ_data.rows/2.0);

	occ_data.ratio = this->sub_div;

	// Auxiliary variables
	unsigned short*     disp16_row;

	float l_0 = 0.5f;

	for (int i = 0; i < (occ_data.cols*occ_data.rows); ++i)
	{
		occ_data.occ_data[i] = l_0;
	}

	//--------------------- Draw uncouvered road surface ---------------------------
	// Triangle points
	cv::Point2f triangle_vertices[3];
	triangle_vertices[0] = cv::Point2f( occ_data.sensor_x0, occ_data.sensor_y0 ); // sensor origin
	
	double img_u = 0.0 - this->cam_cx;
	double img_x = img_u*this->free_area_guess/this->cam_fx;
	triangle_vertices[1] = cv::Point( this->free_area_guess/occ_data.ratio, occ_data.sensor_y0 + (float)(img_x)/occ_data.ratio ); 
	
	img_u = 1.0 - this->cam_cx;
	img_x = img_u*this->free_area_guess/this->cam_fx;
	triangle_vertices[2] = cv::Point( this->free_area_guess/occ_data.ratio, occ_data.sensor_y0 + (float)(img_x)/occ_data.ratio );

	// Triangle top edge line equation y = m*x + b
	double m_1 = (triangle_vertices[1].y - triangle_vertices[0].y)/(triangle_vertices[1].x - triangle_vertices[0].x);
	double b_1 = triangle_vertices[0].y - m_1*triangle_vertices[0].x;

	double m_2 = (triangle_vertices[2].y - triangle_vertices[0].y)/(triangle_vertices[2].x - triangle_vertices[0].x);
	double b_2 = triangle_vertices[0].y - m_1*triangle_vertices[0].x;

	// Fill the blind region in front of the robot
	for (col = (int)(triangle_vertices[0].x + 0.5f); col <= (int)(triangle_vertices[1].x + 0.5f); ++col)
	{
		for (row = (int)(m_1*col + b_1 + 0.5f); row <= (int)(m_2*col + b_2 + 0.5f); ++row)
		{
			int cel_index = row*occ_data.cols + col;

			// The region in front of the vehicle, uncouvered by the camera is just a guess 
			occ_data.occ_data[cel_index] = 0.4f; //0.0001f;
		}
	}
	//------------------------------------------------------------------------------

	// Run across the image...
	for (col = this->cam_valid_col0; col <= (this->cam_valid_cols + this->cam_valid_col0 - 1); ++col)
	{
		for (row = this->cam_valid_row0; row <= (this->cam_valid_rows + this->cam_valid_row0 - 1); ++row)
		{
			// Pixel position in the obstacles mask and disparidade map
			int pixel = (row)*mask_obstacles.cols + (col);
			
			disp16_row = (unsigned short*)disp_map16.data + row*this->cam_width;

			double pixel_disp = (disp16_row[col]/16.0);

			// If the mask_free or mask_obstacles is different of zero and the disparity is in the desired range
			if (((mask_free.data[pixel] != 0)||(mask_obstacles.data[pixel] != 0))&&(pixel_disp > disp_limit_min)&&(pixel_disp < disp_limit_max))
			{						
				// It is a valid point
				if(this->PointTriangulate( row , col, x, y, z, pixel_disp ) )
				{
					z = z*std::cos(this->cam_tilt_angle);

					// Distance from the grid cell to the robot origin
					double D_xz = std::sqrt( x*x + z*z );

					grid_x = (int)(z/this->sub_div + occ_data.sensor_x0 + 0.5);
					grid_y = (int)(x/this->sub_div + occ_data.sensor_y0 + 0.5);

					double D_cell = std::sqrt( (double)((grid_x - occ_data.sensor_x0)*(grid_x - occ_data.sensor_x0) + (grid_y - occ_data.sensor_y0)*(grid_y - occ_data.sensor_y0)) )*this->sub_div;

					if((grid_x >= 0) && (grid_x < occ_data.cols) && (grid_y >= 0) && (grid_y < occ_data.rows))
					{					
						// Estimated standard deviation for the current disparity value
						std_deviation = (this->cam_fx*this->cam_width*this->cam_baseline*this->CorrAccuracy/(pixel_disp*pixel_disp))/3.0;

						// If the grid cell have influence in their neighbours
						if(std_deviation*3.0 > this->CorrAccuracy)
						{
							int n_cels = (int)(std_deviation*3.0/this->sub_div + 0.5);

							for(int i = -n_cels/2; i <= n_cels/2; ++i)
							{
								for(int j = -n_cels/2; j <= n_cels/2; ++j)
								{
									grid_x_aux = grid_x + i;
									grid_y_aux = grid_y + j;

									if((grid_x_aux >= 0) && (grid_x_aux < occ_data.cols) && (grid_y_aux >= 0) && (grid_y_aux < occ_data.rows))
									{
										D_cell = std::sqrt( (double)((grid_x_aux - occ_data.sensor_x0)*(grid_x_aux - occ_data.sensor_x0) + (grid_y_aux - occ_data.sensor_y0)*(grid_y_aux - occ_data.sensor_y0)) )*this->sub_div;
										
										double prob = 0.5;
										int cel_index = (grid_y_aux)*occ_data.cols + (grid_x_aux);

										// Gaussian model for the probability
										if(mask_obstacles.data[pixel] != 0)
										{
											prob = std::exp( -0.5*(((D_xz - D_cell)*(D_xz - D_cell))/(std_deviation*std_deviation)) )/(std::sqrt(2.0*PI_VALUE)*std_deviation);

											prob = (prob > 0.5f) ? prob : 0.5f; 

											occ_data.occ_data[cel_index] = occ_data.occ_data[cel_index] + prob - l_0;
										}
										else // (mask_free.data[pixel] != 0)
										{
											prob = 1.0 - std::exp( -0.5*(((D_xz - D_cell)*(D_xz - D_cell))/(std_deviation*std_deviation)) )/(std::sqrt(2.0*PI_VALUE)*std_deviation);

											prob = (prob < 0.5f) ? prob : 0.5f;

											if(occ_data.occ_data[cel_index] <= l_0)
												occ_data.occ_data[cel_index] = occ_data.occ_data[cel_index] + prob - l_0;
										}//*/
												
										

										if (occ_data.occ_data[cel_index] > 0.9999f) 
											occ_data.occ_data[cel_index] = 0.9999f;
        
										if (occ_data.occ_data[cel_index] < 0.0001f) 
											occ_data.occ_data[cel_index] = 0.0001f;//*/
									} //if
								} // for
							} // for
						} //if
						else // If the point have all the probability
						{
							int cel_index = (grid_y)*occ_data.cols + (grid_x);

							if(mask_obstacles.data[pixel] != 0)
							{
								occ_data.occ_data[cel_index] = 0.9999f;
							}
							else // (mask_free.data[pixel] != 0)
							{
								if(occ_data.occ_data[cel_index] <= l_0)
									occ_data.occ_data[cel_index] = 0.0001f;
							}//*/
						}
					} // if grid
				} // if point triangulate	
			}
		} // for row
	} // for col

	//-----------------------------------------------------------------------------

	return;
}

/*	CreateMonoCameraGrid
		Description:
			Function to create the road surface grid by the mono camera segmented image.
		Parameters:
			mask_free = road surface mask
			occ_data = occupancy grid
			max_dist = max distance to be considerated
			img_step = step between the image rows and cols
	*/ 
void CameraObstacleGridComponent::CreateMonoCameraGrid(cv::Mat mask_free, sensor_occ_data &occ_data, double max_dist, int img_step)
{
	double std_deviation;	// calculated with the camera distance = (f*B*CorrAccuracy/(d*d))/3

	// 3D coordinates of the image point
	double x = 0.0; 
	double y = 0.0; 
	double z = 0.0;

	// 2D image coordinates
	int row, col;
	
	// Current grid position
	int grid_x, grid_y, grid_x_aux, grid_y_aux;

	double* x_ptr; 
	double* x_ptr1;
	double* y_ptr;

	//------------------------------------------------------------------------------
	occ_data.cols = D_MAX_CAM_GRID/this->sub_div;
	occ_data.rows = 2*occ_data.cols;

	occ_data.sensor_x0 = 0.0f;
	occ_data.sensor_y0 = (float)(occ_data.rows/2.0);

	occ_data.ratio = this->sub_div;

	float l_0 = 0.5f;

	for (int i = 0; i < (occ_data.cols*occ_data.rows); ++i)
	{
		occ_data.occ_data[i] = l_0;
	}

	// Run across the image... 
	for (row = this->cam_valid_row0; row <= (this->cam_valid_rows + this->cam_valid_row0 - 1); row = row + img_step)
	{
		x_ptr = (double*)this->CurrentXMatrix.data + row*this->cam_width;
		y_ptr = (double*)this->CurrentYMatrix.data + row*this->cam_width;

		// Estimated standard deviation for the current distance 
		if((row + img_step) < (this->cam_valid_rows + this->cam_valid_row0 - 1))
		{
			x_ptr1 = (double*)this->CurrentXMatrix.data + (row + img_step)*this->cam_width;
			std_deviation = std::fabs(x_ptr[this->cam_valid_col0] - x_ptr1[this->cam_valid_col0]);
		}

		for (col = this->cam_valid_col0; col <= (this->cam_valid_cols + this->cam_valid_col0 - 1); col = col + img_step)
		{
			// Pixel position in the road surface mask
			int pixel = (row)*mask_free.cols + (col);
						
			// If the mask_free is different of zero and the distance is in the max_dist range
			if ((mask_free.data[pixel] != 0)&&(x_ptr[col] < max_dist))
			{						
				// Distance from the grid cell to the robot origin
				double D_xy = std::sqrt( x_ptr[col]*x_ptr[col] + y_ptr[col]*y_ptr[col] );

				grid_x = (int)(x_ptr[col]/this->sub_div + occ_data.sensor_x0 + 0.5);
				grid_y = (int)(y_ptr[col]/this->sub_div + occ_data.sensor_y0 + 0.5);

				double D_cell = std::sqrt( (double)((grid_x - occ_data.sensor_x0)*(grid_x - occ_data.sensor_x0) + (grid_y - occ_data.sensor_y0)*(grid_y - occ_data.sensor_y0)) )*this->sub_div;

				if((grid_x >= 0) && (grid_x < occ_data.cols) && (grid_y >= 0) && (grid_y < occ_data.rows))
				{					
					// If the grid cell have influence in their neighbours
					if(std_deviation*3.0 > this->CorrAccuracy)
					{
						int n_cels = (int)(std_deviation*3.0/this->sub_div + 0.5);

						for(int i = -n_cels/2; i <= n_cels/2; ++i)
						{
							for(int j = -n_cels/2; j <= n_cels/2; ++j)
							{
								grid_x_aux = grid_x + i;
								grid_y_aux = grid_y + j;

								if((grid_x_aux >= 0) && (grid_x_aux < occ_data.cols) && (grid_y_aux >= 0) && (grid_y_aux < occ_data.rows))
								{
									D_cell = std::sqrt( (double)((grid_x_aux - occ_data.sensor_x0)*(grid_x_aux - occ_data.sensor_x0) + (grid_y_aux - occ_data.sensor_y0)*(grid_y_aux - occ_data.sensor_y0)) )*this->sub_div;
										
									double prob = 0.5;
									int cel_index = (grid_y_aux)*occ_data.cols + (grid_x_aux);

									prob = 1.0 - std::exp( -0.5*(((D_xy - D_cell)*(D_xy - D_cell))/(std_deviation*std_deviation)) )/(std::sqrt(2.0*PI_VALUE)*std_deviation);

									prob = (prob < 0.5f) ? prob : 0.5f;

									if(occ_data.occ_data[cel_index] <= l_0)
										occ_data.occ_data[cel_index] = occ_data.occ_data[cel_index] + prob - l_0;	
        
									if (occ_data.occ_data[cel_index] < 0.0001f) 
										occ_data.occ_data[cel_index] = 0.0001f;//*/
								} //if
							} // for
						} // for
					} //if
					else // If the point have all the probability
					{
						int cel_index = (grid_y)*occ_data.cols + (grid_x);

						if(occ_data.occ_data[cel_index] <= l_0)
							occ_data.occ_data[cel_index] = 0.0001f;
						
					}
				} // if grid
			}
		} // for row
	} // for col

	//-----------------------------------------------------------------------------

	return;
}

/*	CreateProjectionMatrix
	Description:
		Function to create the matrixes with the image projection in the 
		robot plane.
	Parameters:
		max_dist = max distance to be considerated
*/ 
void CameraObstacleGridComponent::CreateProjectionMatrix(double max_dist)
{
	this->CurrentXMatrix = cv::Mat::zeros(cv::Size(this->cam_width , this->cam_height), CV_64FC1);
	this->CurrentYMatrix = cv::Mat::zeros(cv::Size(this->cam_width , this->cam_height), CV_64FC1);

	// Run across the the matrixes to complete the X, Y, Z projections
	for (int row = (this->cam_valid_rows + this->cam_valid_row0 - 1); row >= this->cam_valid_row0; --row)
	{
		double* x_ptr = this->CurrentXMatrix.ptr<double>(row);
		double* y_ptr = this->CurrentYMatrix.ptr<double>(row);

		for (int col = this->cam_valid_col0; col <= (this->cam_valid_cols + this->cam_valid_col0 - 1); ++col)
		{
			this->Calc3DPointProjection(row, col, x_ptr[col], y_ptr[col]);
		}

		if(x_ptr[this->cam_valid_col0] >= max_dist)
			break;
	}
} 

/*	PointTriangulate
	Description:
		Calculate the point triangulation in the world
	Parameters:
		row,col = row and column in the image
		x,y,z = world coordinates
		disparity = disparity value
*/
bool CameraObstacleGridComponent::PointTriangulate(int row, int col, double &x, double &y, double &z, double disparity)
{
	bool valid_point = false;

	if(disparity > 0.0 && disparity < 255.0)
	{
		// Z = f*b/d
		z = this->cam_width*this->cam_fx*this->cam_baseline/disparity;
						
		//double u = (double(col) - double(this->cam_width)*this->cam_cx);
		//double v = (double(row) - double(this->cam_height)*this->cam_cy);

		double u = col/(this->cam_width - 1.0) - this->cam_cx;
		double v = row/(this->cam_height - 1.0) - this->cam_cy;

		// X = u*Z/f
		//x = u*z/(this->cam_width*this->cam_fx);
		x = u*z/this->cam_fx;

		// Y = v*Z/f
		//y = v*z/(this->cam_height*this->cam_fy);
		y = v*z/this->cam_fy;

		valid_point = true;
	}

	return valid_point;
}

/*	Calc3DPointProjection
	Description:
		Calculate the 3D point projection in a world plane
	Parameters:
		row, col = row and column in the image
		x_r, y_r = world coordinates (in the robot frame)
*/
void CameraObstacleGridComponent::Calc3DPointProjection(int row, int col, double &x_r, double &y_r)
{	
	double X = (col/(this->cam_width - 1.0) - this->cam_cx)/this->cam_fx;
	double Y = (row/(this->cam_height - 1.0) - this->cam_cy)/this->cam_fy;

	// x_c = X*t_y/(sin(rho) + Ycos(rho))
	double x_c = X*this->cam_h/(std::sin(this->cam_tilt_angle) + Y*std::cos(this->cam_tilt_angle));

	// y_c = Y*t_y/(sin(rho) + Ycos(rho))
	double y_c = Y*this->cam_h/(std::sin(this->cam_tilt_angle) + Y*std::cos(this->cam_tilt_angle));

	// z_c = t_y/(sin(rho) + Ycos(rho))
	double z_c = this->cam_h/(std::sin(this->cam_tilt_angle) + Y*std::cos(this->cam_tilt_angle));

	if((z_c*z_c - this->cam_h*this->cam_h) >= 0)
		x_r = std::sqrt(z_c*z_c - this->cam_h*this->cam_h);
	else
		x_r = 10000.0;

	y_r = x_c;

	return;
}

// Draw the camera obstacle grid
cv::Mat CameraObstacleGridComponent::DrawGrid(double* radius_data, double* angle_data)
{
	double CAM_GRID_DIST = CAM_GRID_ROWS/this->D_MAX_CAM_GRID;					// Pixel/meter ratio

	// Grid image
	cv::Mat GridImage = cv::Mat(cv::Size(CAM_GRID_COLS, CAM_GRID_ROWS), CV_8UC3, cv::Scalar(255,255,255));

	// Obstacle coordinates
	int coord_x, coord_y;
	int coord_x_ant = -1;
	int coord_y_ant = -1;

	// Condition to know if the previous coordinate was an obstacle
	bool Obstaculo_ant = false; 

	const int orig_x = (int)((CAM_GRID_COLS - 1)/2);
	const int orig_y = CAM_GRID_ROWS - 1;
	
	int it_raio, it_angulo;

	for( it_raio = 0, it_angulo = 0; it_raio < this->cam_valid_cols; ++it_raio,++it_angulo)
	{

		if(radius_data[it_raio] != D_MAX_CAM_GRID)
		{
			coord_x = (int)((double)orig_x - CAM_GRID_DIST*((radius_data[it_raio])*sin(angle_data[it_angulo])) + 0.5);
			coord_y = (int)((double)orig_y - CAM_GRID_DIST*((radius_data[it_raio])*cos(angle_data[it_angulo])) + 0.5);

			// Black circle
			cv::circle(GridImage, cv::Point(coord_x, coord_y), 1, CV_RGB(0, 0, 0), 1);

			//// If the previous point was a obstacle, trace a line
			//if( Obstaculo_ant == true)
			//{
			//	cv::line( GridImage, cv::Point(coord_x_ant, coord_y_ant), cv::Point(coord_x, coord_y), CV_RGB(0,0,0), 1, 8 );
			//}

			coord_x_ant = coord_x;
			coord_y_ant = coord_y;

			//Obstaculo_ant = true;
		}
		/*else
		{
			Obstaculo_ant = false;
		}*/
	}

	// Draw the robot limit in blue
	cv::rectangle( GridImage, cvPoint( (orig_x - (int)(CAM_GRID_DIST*ROBOT_FRONT_WIDTH/2.0 + 0.5)), (orig_y - (int)(CAM_GRID_DIST*ROBOT_FRONT_DIST + 0.5))), 
				cvPoint((orig_x + (int)(CAM_GRID_DIST*ROBOT_FRONT_WIDTH/2.0 + 0.5)), orig_y), CV_RGB(0,0,255), 1, 8);

	// Desenha camera em amarelo
	cv::rectangle( GridImage, cv::Point( (orig_x - 4), (orig_y - 1)), cv::Point((orig_x + 4), orig_y), CV_RGB(200,200,0), 2, 8);

	//---------------- Campo de visao --------------------------------------------------------

	// Incremento do angulo
	const double angle_inc = this->cam_fov/(double)this->cam_width; 

	// Angulo mais a esquerda
	const double angle_esq = -1*(this->cam_fov/2.0) + (double)this->cam_valid_col0*angle_inc;
	
	// Calcula variacao em x da reta que representa o campo de visao
	int recuo_x = abs((int)(((double)CAM_GRID_ROWS/cos(angle_esq*2.0*CV_PI/360.0))*sin(angle_esq*2.0*CV_PI/360.0) + 0.5));

	// Desenha as retas do campo de visao em vermelho
	cv::line( GridImage, cv::Point(orig_x, orig_y), cv::Point((orig_x - recuo_x), 0), CV_RGB(255,0,0), 1, 8 );
	cv::line( GridImage, cv::Point(orig_x, orig_y), cv::Point((orig_x + recuo_x), 0), CV_RGB(255,0,0), 1, 8 );

	//-----------------------------------------------------------------------------------------

	// Desenha circulos de distancia em verde
	cv::circle(GridImage, cv::Point(orig_x, orig_y), (int)(5.0*CAM_GRID_DIST), CV_RGB(0, 255, 0), 1);
	cv::circle(GridImage, cv::Point(orig_x, orig_y), (int)(10.0*CAM_GRID_DIST), CV_RGB(0, 255, 0), 1);
	cv::circle(GridImage, cv::Point(orig_x, orig_y), (int)(15.0*CAM_GRID_DIST), CV_RGB(0, 255, 0), 1);
	cv::circle(GridImage, cv::Point(orig_x, orig_y), (int)(20.0*CAM_GRID_DIST), CV_RGB(0, 255, 0), 1);

	return GridImage;
}

// Draw the camera occupancy grid
cv::Mat CameraObstacleGridComponent::DrawGrid(sensor_occ_data &occ_data)
{
	// Grid image
	return cv::Mat(occ_data.rows, occ_data.cols, CV_32FC1, occ_data.occ_data);
	
	
}