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

#ifndef OBSTACLEDETECTIONCOMPONENT_H
#define OBSTACLEDETECTIONCOMPONENT_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 "StereoVisionDisparityExp.h"

#include "../../StdDbtPlayerComponents/ImageBaseStructure.h"

#include <QThread>
#include <QMutex>

namespace pacpus {

/** Class to provide the obstacle detection from a disparity map data */
class STEREOVISIONDISPARITY_API ObstacleDetectionComponent:	public QThread, 
															public ComponentBase
{
public:
	//============================= DEFAULT ELEMENTS ===============================================
    ObstacleDetectionComponent(QString name);
    ~ObstacleDetectionComponent();

    void run();

    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, THREAD_ALIVE;


public:

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

private:

	double ANG_VARIATION, ANG_VARIATION2;

	int cam_width;			// Image width
	int cam_height;			// image height
	int cam_channels;		// number of channels
	bool showdebug;			// Show frame acquired
	int min_disp;			// Minimum disparity value to detect obstacles in the u/v-disp map
	int max_disp;			// Maximum disparity value to detect obstacles in the u/v-disp map
	int min_disp_norm;		// Minimum disparity value to equalize the disp map 16
	int max_disp_norm;		// Maximum disparity value to equalize the disp map 16

	int destiny_roi_x;		// Destiny image roi x
	int destiny_roi_y;		// Destiny image roi y
	int destiny_roi_width;	// Destiny image roi width
	int destiny_roi_height;	// Destiny image roi height

	bool use_roi;			// If is to use roi

	int mMaxImageInputSize1;		// Size of the input image data in the memory
	int mMaxImageInputSize2;		// Size of the input image data in the memory
	int mMaxImageOutputSize;		// Size of the output image data in the memory

	TimestampedStructImage RefImageHeader;			// Header for the reference image
	TimestampedStructImage DispInImageHeader;		// Header for the input disp image
	TimestampedStructImage Mask1ImageHeader;		// Header for the mask image 1
	TimestampedStructImage Mask2ImageHeader;		// Header for the mask image 2
	TimestampedStructImage DispOutImageHeader;		// Header for the output disp image

	void * ref_mem;
	void * dispin_mem;
	void * mask1_mem;
	void * mask2_mem;
	void * dispout_mem;

	size_t ref_mem_size;		// Image shared memory position size
	size_t dispin_mem_size;		// Image shared memory position size
	size_t mask1_mem_size;		// Image shared memory position size
	size_t mask2_mem_size;		// Image shared memory position size
	size_t dispout_mem_size;	// Image shared memory position size

	// Imput data
	ShMem * shmem_ref;			// Shared memory control access to the image data
	ShMem * shmem_dispin;			// Shared memory control access to the image data

	// Output data
	ShMem * shmem_mask1;		// Shared memory control access to the image data (free space mask)
	ShMem * shmem_mask2;		// Shared memory control access to the image data (obstacles mask)
	ShMem * shmem_dispout;		// Shared memory control access to the image data (disparity map 16)

	ShMem * shmem_dispMapNormalized;
	ShMem * shmem_result;

	cv::Mat CurrentReferenceFrame;	// Reference image
	cv::Mat CurrentDisparityMap16;	// Disparity Map

	cv::Mat CurrentSurfaceMask;		// Surface mask
	cv::Mat CurrentObstaclesMask;	// Obstacles mask

	// Function to calculate the U/V disparity map
	std::pair<cv::Mat, cv::Mat> CalcUVDisparityMap(cv::Mat disp_map);

	/*	CalcUVDisparityMapNorm
		Description:
			Function to calculate the U/V disparity map from a disp map normalized
		Parameters:
			disp_map16 = original disparity map 16
			disp_map_norm = resulted disparity map normalized
			min_d_norm = Minimum disparity value to equalize the disp map 16
			max_d_norm = Maximum disparity value to equalize the disp map 16
	*/
	void CalcUVDisparityMapNorm(cv::Mat disp_map16, cv::Mat &v_disp_map, cv::Mat &u_disp_map, cv::Mat &disp_map_norm, int min_d_norm, int max_d_norm);

	//Function to check the points order, making the second one with the highest y ever
	void CheckPoints(cv::Point &pt1, cv::Point &pt2);

	// Function to calculate the line slope of pt1 to pt2
	double Inclination(cv::Point pt1, cv::Point pt2);

	// Function to calculate the free space (v_disp_1) and obstacles (v_disp_2) masks from 
	// a red highlighted V-Disparity map
	std::pair<cv::Mat, cv::Mat> MaskSurface2(cv::Mat disp_map, cv::Mat v_disp_1, cv::Mat v_disp_2, int min_d = 0, int max_d = 255, int value = 0);

	// Function to calculate the free space (v_disp_1) and obstacles (v_disp_2/u_disp) masks from 
	// a red highlighted V-Disparity map
	std::pair<cv::Mat, cv::Mat> MaskSurface3(cv::Mat disp_map, cv::Mat v_disp_1, cv::Mat v_disp_2, cv::Mat u_disp, int min_d = 0, int max_d = 255, int value = 0);
	
	// Function to find the free space surface from a V-disparity map
	cv::Mat FindSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2);

	/*	FindSurface2
		Description:
			Function to find the free space surface from a V-disparity map, based on the frontal plane. 
			Return the V-dysparity map with the red line representing the free surface.
		Parameters:
			v_disp_map = Original V-disparity map
			v_disp_map2 = Orignal V-disparity map less the surface detected
	*/
	cv::Mat FindSurface2(cv::Mat &v_disp_map, cv::Mat &v_disp_map2);

	// Function to find the free space surface from a V-disparity map with mean average
	cv::Mat FindAverageSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2);

	// Function to find the near obstacles from a v-Disparity map
	cv::Mat FindNearObstacles(cv::Mat v_disp_map, int min_d = 0, int max_d = 255);

	// Function to find the near obstacles from the v/u-Disparity maps
	std::pair<cv::Mat, cv::Mat> FindNearObstaclesUV(cv::Mat v_disp_map, cv::Mat u_disp_map, int min_d = 0, int max_d = 255);

	/*	LinesFiltering
		Description:
			Filter the detected lines related to the distance and angle between them.
		Parameters:
			lines = line list (point 1 and 2)
	*/
	std::vector<cv::Point> LinesFiltering(std::vector<cv::Vec4i> lines);

	/*	CalcMedian
		Description:
			Calcule the median value of a vector.
		Parametros:
			vector = vector with data to calculate the median
	*/
	template<class A>
	A CalcMedian(std::vector<A> vetor) const;

};

}
#endif // OBSTACLEDETECTIONCOMPONENT