/*********************************************************************
//  created:    2013/06/27 - 18:40
//  filename:   CameraObstacleGridComponent.h
//
//  author:     Danilo Alves de Lima
//              Copyright Heudiasyc UMR UTC/CNRS 6599
// 
//  version:    $Id: $
//
//  purpose:    
*********************************************************************/

#ifndef CAMERAOBSTACLEGRIDCOMPONENT_H
#define CAMERAOBSTACLEGRIDCOMPONENT_H

#include <fstream>
#include <qcoreevent.h>
#include <qthread.h>
#include <string>

#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include "Pacpus/kernel/ComponentBase.h"
#include "Pacpus/kernel/DbiteFile.h"
#include "Pacpus/PacpusTools/ShMem.h"
//#include "Pacpus/PacpusTools/UDPConnection.h"
#include "StereoVisionDisparityExp.h"
#include "SensorsApplicationStructures.h"

#include <QThread>
#include <QMutex>
#include <QFile.h>
#include <QTextStream>

namespace pacpus {

/** Class to convert the image classification masks of free space and obstacles in a 2D grid */
class STEREOVISIONDISPARITY_API CameraObstacleGridComponent:	public QThread, 
															public ComponentBase
{

	Q_OBJECT

public:
	//============================= DEFAULT ELEMENTS ===============================================
    CameraObstacleGridComponent(QString name);
    ~CameraObstacleGridComponent();

	// Run for stereo camera data
    void run();

	// Run for mono camera data 
	void run2();

    virtual void stopActivity(); /*!< to stop the processing thread */
    virtual void startActivity(); /*!< to start the processing thread */
    virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config);
	//==============================================================================================
		
protected:

	// Indicates that thread is running
	bool is_running;

public:

	/** 
	* Initialize default values
	*/
	void InitDefault();

private:

	int run_stereo;		// Select the work mode (stereo camera data or mono camera data)

	bool use_udpconnection;	// Uses the udp connection to send the obstacles data

	//QHostAddress destiny_IP;		// Destiny IP
    //quint16 communication_Port;		// Communication port

	int cam_width;			// Image width
	int cam_height;			// image height
	double cam_fov;			// Field of view of the camera
	int cam_valid_col0;		// Initial valid column 
	int cam_valid_row0;		// Initial valid row
	int cam_valid_cols;		// Valid columns 
	int cam_valid_rows;		// Valid rows
	double cam_fx;			// Focus in x (pixels)
	double cam_fy;			// Focus in y (pixels)
	double cam_cx;			// Center in x (pixels)
	double cam_cy;			// Center in y (pixels)
	double cam_baseline;	// Baseline (meters)
	double cam_tilt_angle;	// Inclination angle of the camera
	double cam_h;			// Camera height
	double CorrAccuracy;	// Estimated camera correlation accuracy
	bool showdebug;			// Show frame acquired
		
	int mMaxImageInputSize1;		// Size of the input image data in the memory
	int mMaxImageInputSize2;		// Size of the input image data in the memory

	int mMaxOutputSize1;			// Size of the out data in the memory

	TimestampedStructImage Mask1ImageHeader;		// Header for the mask image 1
	TimestampedStructImage Mask2ImageHeader;		// Header for the mask image 2
	TimestampedStructImage DispImageHeader;			// Header for the disp image

	//------------------------- Camera occupancy grid projection -----------------------------
	int data_type;									// 0 = camera data to laser format | 1 = camera data to occupancy grid format
	TimestampedCameraOccData Camdata2Laser;			// Camera obstacle data			( for projection like a laser )

	TimestampedSensorOccData CamOccGrid;			// Occupancy grid mapping from the stereo image		( for complete projection of the image data )
	
	double D_MAX_CAM_GRID;		// Sensor max valid distance in the grid
	double sub_div;				// Grid subdivision

	double free_area_guess;		// Frontal projected area expected to be a road surface (frontal road surface uncouvered by camera FOV) 
	//-----------------------------------------------------------------------------------------

	void* mask1_mem;
	void* mask2_mem;
	void* disp_mem;

	size_t mask1_mem_size;		// Image shared memory position size
	size_t mask2_mem_size;		// Image shared memory position size
	size_t disp_mem_size;		// Image shared memory position size

	// Imput data
	ShMem * shmem_mask1;			// Shared memory control access to the image data
	ShMem * shmem_mask2;			// Shared memory control access to the image data
	ShMem * shmem_disp16;			// Shared memory control access to the image data

	// Output data
	ShMem * shmem_obst;				// Shared memory control access to the image data

	// Output data via UDP
//	UDPConnection* udp_con;		

	cv::Mat CurrentMask1Frame;		// Mask1 = free space image
	cv::Mat CurrentMask2Frame;		// Mask2 = obstacles image
	cv::Mat CurrentDisparityMap16;	// Disparity Map 16 bits

	cv::Mat CurrentXMatrix;			// Matrix with the x mapping (robot frame)
	cv::Mat CurrentYMatrix;			// Matrix with the y mapping (robot frame)
	
	// Function to create the obstacle's grid by the obstacles mask image. Similar to a laser.
	void CreateCamera2LaserGrid(cv::Mat mask_obstacles, cv::Mat disp_map16, double* radius_data, double* angle_data);

	// Function to create the obstacle's grid by the road and obstacles mask images.
	void CreateCameraGrid(cv::Mat mask_free, cv::Mat mask_obstacles, cv::Mat disp_map16, sensor_occ_data &occ_data);

	/*	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 CreateMonoCameraGrid(cv::Mat mask_free, sensor_occ_data &occ_data, double max_dist, int img_step);

	/*	CreateProjectionMatrix
		Description:
			Function to create the matrixes with the image projection in the 
			robot plane.
		Parameters:
			max_dist = max distance to be considerated
	*/ 
	void CreateProjectionMatrix(double max_dist);

	/*	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 PointTriangulate(int row, int col, double &x, double &y, double &z, double disparity);

	/*	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 Calc3DPointProjection(int row, int col, double &x_r, double &y_r);

	// Draw the camera obstacle grid
	cv::Mat DrawGrid(double* radius_data, double* angle_data);

	// Draw the camera occupancy grid
	cv::Mat DrawGrid(sensor_occ_data &occ_data);

	bool recording, THREAD_ALIVE;

	double maxDistToBrake;
	QFile saveFile_;
	QTextStream flux_;

Q_SIGNALS:
	void smallestDistance();
};

}
#endif // CAMERAOBSTACLEGRIDCOMPONENT