/*******************************************************************************
//  created:    2013/06/19 - 18:40
//  filename:   DisparityMap.h
//
//  author:     Danilo Alves de Lima and Pierre Hudelaine
//              Copyright Heudiasyc UMR UTC/CNRS 6599
// 
//  version:    $Id: $
//
//  purpose:	Disparity map calculation from stereo image data    
//
*******************************************************************************/

#ifndef DISPARITYMAP_H
#define DISPARITYMAP_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 "../SensorsApplication/SensorsApplicationStructures.h"

#include <QThread>
#include <QMutex>

namespace pacpus {

/** Class to provide the disparity map from a stereo pair */
class STEREOVISIONDISPARITY_API DisparityMap:	public QThread, 
												public ComponentBase
{
public:
	//============================= DEFAULT ELEMENTS ===============================================
    DisparityMap(QString name);
    ~DisparityMap();

    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;

public:

	//enum { STEREO_BM=0, STEREO_SGBM, STEREO_HH };
	enum Type{SAD, SGBM, SGBM_HH, SAD_PtG}; // Disparity type

	void InitDefault();

private:

	// Cam settings
	int cam_width;			// Image width
	int cam_height;			// image height
	int cam_channels;		// image channels
	
	// Image destination settings
	int destiny_width;		// Destiny image width
	int destiny_height;		// Destiny image height
	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
	int destiny_channels;   // Destiny image channels

	// Processing settings
	bool use_roi;			// If is to use roi
	Type dispmap_type;		// Type of disparity map calculation:
							//		0 = SAD (Sum of Absolute Differences algorithm)
							//		1 = SGBM (Semi-Global Block Matching algorithm)
							//		2 = SGBM+HH (full-scale two-pass dynamic programming algorithm)
							//		3 = SAD from PtGrey
	int min_disp;			// Minimum disparity value (max-min) must be divisible by 16
	int num_disp;			// Number of disparities (max-min) must be divisible by 16
		
	//---------------------------------------------- SBM Configuration --------------------------------------------------------------
	int sbm_preFilterCap;				// Truncation value for the prefiltered image pixels. 
										// The algorithm first computes x-derivative at each pixel and clips its 
										// value by [-preFilterCap, preFilterCap] interval. The result values are
										// passed to the Birchfield-Tomasi pixel cost function.
	int sbm_preFilterSize;
	int sbm_SADWindowSize;				// Correlation window size, odd value >= 5
	int sbm_textureThreshold;			// Validation threashold for texture variation
	int sbm_uniquenessRatio;			// Margin in percentage by which the best (minimum) computed cost function value should “win” 
										// the second best value to consider the found match correct. Normally, a value within the 5-15 
										// range is good enough.
	int sbm_speckleWindowSize;			// Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set 
										// it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
	int sbm_speckleRange;				// Maximum disparity variation within each connected component. If you do speckle filtering, set 
										// the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is 
										// good enough.

	//---------------------------------------------- SGBM Configuration -------------------------------------------------------------
	int sgbm_preFilterCap;				// Truncation value for the prefiltered image pixels. 
										// The algorithm first computes x-derivative at each pixel and clips its 
										// value by [-preFilterCap, preFilterCap] interval. The result values are
										// passed to the Birchfield-Tomasi pixel cost function.
	int sgbm_SADWindowSize;				// Correlation window size, odd value >= 3
	int sgbm_P1;						// The first parameter controlling the disparity smoothness.
	int sgbm_P2;						// The second parameter controlling the disparity smoothness. The larger the values are, 
										// the smoother the disparity is. P1 is the penalty on the disparity change by plus or 
										// minus 1 between neighbor pixels. P2 is the penalty on the disparity change by more than 
										// 1 between neighbor pixels. The algorithm requires P2 > P1 .
	int sgbm_uniquenessRatio;			// Margin in percentage by which the best (minimum) computed cost function value should “win” 
										// the second best value to consider the found match correct. Normally, a value within the 5-15 
										// range is good enough.
	int sgbm_speckleWindowSize;			// Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set 
										// it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
	int sgbm_speckleRange;				// Maximum disparity variation within each connected component. If you do speckle filtering, set 
										// the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is 
										// good enough.
	int sgbm_disp12MaxDiff;				// Maximum allowed difference (in integer pixel units) in the left-right disparity check. Set it to 
										// a non-positive value to disable the check.

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

	std::string img_source;		// Image source
	std::string img_source_left; // Left image source (if stereovision)
	std::string img_source_right;// Right image source (if stereovision)

	int mMaxImageInputSize;		// Size of the input image data in the memory
	int mMaxImageOutputSize1;	// Size of the output image data in the memory
	int mMaxImageOutputSize2;	// Size of the output image data in the memory

	TimestampedStructImage LeftImage;		// Header for the left image
	TimestampedStructImage RightImage;		// Header for the right image
	TimestampedStructImage RefImage;		// Header for the reference image
	TimestampedStructImage DispImage;		// Header for the disp image

	void* left_mem;
	void* right_mem;
	void* ref_mem;
	void* disp_mem;
	size_t left_mem_size;	// Image shared memory position size
	size_t right_mem_size;	// Image shared memory position size
	size_t ref_mem_size;	// Image shared memory position size
	size_t disp_mem_size;	// Image shared memory position size

	// Imput data
	ShMem * shmem_left;			// Shared memory control access to the image data
	ShMem * shmem_right;		// Shared memory control access to the image data

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

	cv::Mat CurrentLeftFrame;		// Image of the left camera
	cv::Mat CurrentRightFrame;		// Image of the right camera
	cv::Mat CurrentDisparityMap;	// DisparityMap

	/*	CalcDisparityMap
		Description:
			Calculate the Disparity Map of a stereo pair
		Parameters:
			Left_img = left image
			Right_img = right image
			Disp_map = disparity map
			type = Method:  0 = SAD (Sum of Absolute Differences algorithm)
							1 = SGBM (Semi-Global Block Matching algorithm)
							2 = SGBM+HH (full-scale two-pass dynamic programming algorithm)			
	*/
	void CalcDisparityMap(cv::Mat Left_img, cv::Mat Right_img, cv::Mat &Disp_map, Type type);


	// tmp
	bool recording, THREAD_ALIVE;
	bool showdebug;			// Show frame acquired
};

}
#endif // DISPARITYMAP