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

#include "DisparityMap.h"

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

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

using namespace std;
using namespace pacpus;

DECLARE_STATIC_LOGGER("pacpus.base.DisparityMap");

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

const int kMaxFilepathLength = 512; // TODO: should be same for all images
static const string DisparityMapMemoryName_imagesrc = "image";
static const string DisparityMapMemoryName_ref = "DisparityMap-ref";
static const string DisparityMapMemoryName_disp = "DisparityMap-disp";


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

	recording = 0;
	THREAD_ALIVE = 0;

	this->cam_width = this->destiny_width = 1280;		// Image width
	this->cam_height = this->destiny_height = 960;		// Image height
	this->cam_channels = 4;		// Image channels
	this->showdebug = false;	// Show frame acquired

	this->min_disp = 0;
	this->num_disp = 256;
	
	// Size of the image data sizeof(char)*width*height*channels
	this->mMaxImageInputSize = sizeof(char)*this->cam_width*this->cam_height*this->cam_channels;

	// Input data
	this->shmem_left = 0;			// Shared memory control access to the image data
	this->shmem_right = 0;			// Shared memory control access to the image data

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

	this->CurrentLeftFrame = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, 3));
	this->CurrentRightFrame = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, 3));
}


////////////////////////////////////////////////////////////////////////////////
/// Destructor
////////////////////////////////////////////////////////////////////////////////
DisparityMap::~DisparityMap()
{
	LOG_TRACE(Q_FUNC_INFO);

	if(this->shmem_left)
		delete shmem_left;

	this->shmem_left = NULL;

	if(this->shmem_right)
		delete shmem_right;

	this->shmem_right = NULL;

	if(this->shmem_ref)
		delete shmem_ref;

	this->shmem_ref = NULL;

	if(this->shmem_disp)
		delete shmem_disp;

	this->shmem_disp = NULL;
}


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

	this->destiny_channels = (this->cam_channels != 1) ? 3 : 1;

	this->mMaxImageInputSize = sizeof(unsigned char) * this->cam_width * this->cam_height * this->cam_channels;
	this->mMaxImageOutputSize1 = sizeof(unsigned char) * this->destiny_width * this->destiny_height * this->destiny_channels;
	this->mMaxImageOutputSize2 = sizeof(unsigned short) * this->destiny_width * this->destiny_height;

	this->left_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize;
	this->right_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize;
	this->ref_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageOutputSize1;
	this->disp_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageOutputSize2;

	// Allocate memory position for the maximum expected data
	this->left_mem = malloc(this->left_mem_size); 
	this->right_mem = malloc(this->right_mem_size);
	this->ref_mem = malloc(this->ref_mem_size);
	this->disp_mem = malloc(this->disp_mem_size);

	// Size of the image data sizeof(char)*width*height*channels
	if (this->img_source_left.size() != 0 &&  this->img_source_right.size() != 0)
	{
		this->shmem_left = new ShMem((this->img_source_left).c_str(), this->left_mem_size);
		this->shmem_right = new ShMem((this->img_source_right).c_str(), this->right_mem_size);
	}
	else
	{
		this->shmem_left = new ShMem((this->img_source + "-left").c_str(), this->left_mem_size);
		this->shmem_right = new ShMem((this->img_source + "-right").c_str(), this->right_mem_size);
	}

	this->shmem_ref = new ShMem(DisparityMapMemoryName_ref.c_str(), this->ref_mem_size);
	this->shmem_disp = new ShMem(DisparityMapMemoryName_disp.c_str(), this->disp_mem_size);
	
	// Run thread
	THREAD_ALIVE = true;
	start();
}


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

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

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

		if(this->shmem_left)
			delete shmem_left;

		this->shmem_left = NULL;

		if(this->shmem_right)
			delete shmem_right;

		this->shmem_right = NULL;

		if(this->shmem_ref)
			delete shmem_ref;

		this->shmem_ref = NULL;

		if(this->shmem_disp)
			delete shmem_disp;

		this->shmem_disp = NULL;

		free(this->left_mem); 
		free(this->right_mem);
		free(this->ref_mem);
		free(this->disp_mem);
	}

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


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

	// Initialize with default values
	InitDefault();

	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_channels") != QString::null)
		this->cam_channels = config.getProperty("cam_channels").toInt();

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

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

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

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

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

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

	if( ((this->destiny_roi_height != this->destiny_height)||(this->destiny_roi_width != this->destiny_width))&&
		((this->destiny_roi_height <= this->destiny_height)&&(this->destiny_roi_width <= this->destiny_width)) )
		this->use_roi = true;

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

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

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

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

	//---------------------------------------------- SBM Configuration --------------------------------------------------------------
	if (config.getProperty("sbm_preFilterCap") != QString::null)
		this->sbm_preFilterCap = config.getProperty("sbm_preFilterCap").toInt();

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

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

	if (config.getProperty("sbm_textureThreshold") != QString::null)
		this->sbm_textureThreshold = config.getProperty("sbm_textureThreshold").toInt();
	
	if (config.getProperty("sbm_uniquenessRatio") != QString::null)
		this->sbm_uniquenessRatio = config.getProperty("sbm_uniquenessRatio").toInt();
	
	if (config.getProperty("sbm_speckleWindowSize") != QString::null)
		this->sbm_speckleWindowSize = config.getProperty("sbm_speckleWindowSize").toInt();
	
	if (config.getProperty("sbm_speckleRange") != QString::null)
		this->sbm_speckleRange = config.getProperty("sbm_speckleRange").toInt();
	
	//---------------------------------------------- SGBM Configuration -------------------------------------------------------------
	if (config.getProperty("sgbm_preFilterCap") != QString::null)
		this->sgbm_preFilterCap = config.getProperty("sgbm_preFilterCap").toInt();

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

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

	if (config.getProperty("sgbm_P2") != QString::null)
		this->sgbm_P2 = config.getProperty("sgbm_P2").toInt();
	
	if (config.getProperty("sgbm_uniquenessRatio") != QString::null)
		this->sgbm_uniquenessRatio = config.getProperty("sgbm_uniquenessRatio").toInt();
	
	if (config.getProperty("sgbm_speckleWindowSize") != QString::null)
		this->sgbm_speckleWindowSize = config.getProperty("sgbm_speckleWindowSize").toInt();
	
	if (config.getProperty("sgbm_speckleRange") != QString::null)
		this->sgbm_speckleRange = config.getProperty("sgbm_speckleRange").toInt();
	
	if (config.getProperty("sgbm_disp12MaxDiff") != QString::null)
		this->sgbm_disp12MaxDiff = config.getProperty("sgbm_disp12MaxDiff").toInt();

	if (config.getProperty("img_source") != QString::null)
		this->img_source = config.getProperty("img_source").toStdString();
	else
		this->img_source = DisparityMapMemoryName_imagesrc;

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

	if (config.getProperty("img_source_right") != QString::null)
		this->img_source_right = config.getProperty("img_source_right").toStdString();
		
	// Size of the image data sizeof(char)*width*height*channels
	this->mMaxImageInputSize = sizeof(char)*this->cam_width*this->cam_height*this->cam_channels;
	
	LOG_INFO("configured component '" << name() << "'");
    return ComponentBase::CONFIGURED_OK;
}


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

	this->cam_width = this->destiny_width = this->destiny_roi_width =  1280;	// Image width
	this->cam_height = this->destiny_height = this->destiny_roi_height = 960;	// Image height
	
	this->destiny_roi_x = this->destiny_roi_y = 0;	// Destiny image roi x and y
	
	this->use_roi = false;

	this->cam_channels = 4;		// Image channels
	this->showdebug = false;	// Show frame acquired
	this->dispmap_type = SGBM;
	this->min_disp = 0;
	this->num_disp = 256;
	
	//---------------------------------------------- SBM Configuration --------------------------------------------------------------
	this->sbm_preFilterCap = 31;		// 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.
	this->sbm_preFilterSize = 41;
	this->sbm_SADWindowSize = 9;
	this->sbm_textureThreshold = 20;	// Validation threashold for texture variation
	this->sbm_uniquenessRatio = 10;		// 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.
	this->sbm_speckleWindowSize = 100;	// 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.
	this->sbm_speckleRange = 32;		// Maximum disparity variation within each connected component. If you do speckle filtering, set 
										// the configeter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is 
										// good enough.

	//---------------------------------------------- SGBM Configuration -------------------------------------------------------------
	this->sgbm_preFilterCap = 63;		// 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.
	this->sgbm_SADWindowSize = 3;
	this->sgbm_P1 = 4*this->sgbm_SADWindowSize*this->sgbm_SADWindowSize;		// The first parameter controlling the disparity smoothness.
	this->sgbm_P2 = 32*this->sgbm_SADWindowSize*this->sgbm_SADWindowSize;		// 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 .
	this->sgbm_uniquenessRatio = 10;			// 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.
	this->sgbm_speckleWindowSize = 100;		// 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.
	this->sgbm_speckleRange = 32;				// 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.
	this->sgbm_disp12MaxDiff = 1;			// Maximum allowed difference (in integer pixel units) in the left-right disparity check. Set it to 
											// a non-positive value to disable the check.
}


////////////////////////////////////////////////////////////////////////////////
/// Thread loop
////////////////////////////////////////////////////////////////////////////////
void DisparityMap::run()
{
	LOG_TRACE(Q_FUNC_INFO);

	this->is_running = true;
	
	// "channels" is a vector of 4 Mat arrays: BGRU
	cv::Mat Img_BGRU;
	cv::Mat Img_BGRU_resized;
	std::vector<cv::Mat> channels(4);
	
	Img_BGRU = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, this->cam_channels));
	Img_BGRU_resized = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, this->cam_channels));
		
	this->CurrentLeftFrame = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, this->destiny_channels));
	this->CurrentRightFrame = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, this->destiny_channels));


	// Create the image in which we will save the disparities
	if(this->CurrentDisparityMap.cols != this->destiny_width)
		this->CurrentDisparityMap = cv::Mat( this->destiny_height, this->destiny_width, CV_16S, cv::Scalar(0));

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

	// Initialize the output images header
	this->RefImage.image.width = this->destiny_width;
	this->RefImage.image.height = this->destiny_height;
	this->RefImage.image.channels = 3;
	this->RefImage.image.width_step = (size_t)(this->RefImage.image.height * this->RefImage.image.channels);
	this->RefImage.image.data_size = this->mMaxImageOutputSize1;

	this->DispImage.image.width = this->destiny_width;
	this->DispImage.image.height = this->destiny_height;
	this->DispImage.image.channels = 1;
	this->DispImage.image.width_step = (size_t)(sizeof(unsigned short)*this->DispImage.image.height * this->DispImage.image.channels);
	this->DispImage.image.data_size = this->mMaxImageOutputSize2;

	// Time measurement
	road_time_t init_time = 0;

	while (THREAD_ALIVE) 
	{
		init_time = road_time();

		//LOG_INFO("Grab new image");
		// header + image
		this->shmem_left->read(this->left_mem, this->left_mem_size);
		this->shmem_right->read(this->right_mem, this->right_mem_size);

		// Header
		memcpy( &this->LeftImage, this->left_mem, sizeof(TimestampedStructImage)); 
		memcpy( &this->RightImage, this->right_mem, sizeof(TimestampedStructImage)); 
	
		// Check image header
		bool is_ok = false;
		if( (this->LeftImage.image.data_size == this->mMaxImageInputSize) && (this->LeftImage.time != last_reading) &&
			(this->RightImage.image.data_size == this->mMaxImageInputSize) && (this->LeftImage.time == this->RightImage.time) )
		{
			is_ok = true;
			last_reading = this->LeftImage.time;

			//cout << "Left - Right = " << (int64_t)this->LeftImage.time - (int64_t)this->RightImage.time << endl;
		}

		if(is_ok)
		{
			if (this->cam_channels == 3)
			{
				if(this->cam_width != this->destiny_width)
				{
					memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size); 
					
					cv::resize(Img_BGRU, this->CurrentLeftFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);					

					memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size); 
					
					cv::resize(Img_BGRU, this->CurrentRightFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);	
				}
				else
				{
					// Image data
					memcpy( (unsigned char*)(this->CurrentLeftFrame.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size); 
					memcpy( (unsigned char*)(this->CurrentRightFrame.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size); 
				}
				
			}
			else if(this->cam_channels == 4)//this->cam_channels == 4, use bumblebee image
			{
				// If is to resize the image...
				if(this->cam_width != this->destiny_width)
				{
					memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size); 					
					cv::resize(Img_BGRU, Img_BGRU_resized, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
					cv::cvtColor(Img_BGRU_resized, this->CurrentLeftFrame, CV_BGRA2BGR);

					memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size); 					
					cv::resize(Img_BGRU, Img_BGRU_resized, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);	
					cv::cvtColor(Img_BGRU_resized, this->CurrentRightFrame, CV_BGRA2BGR);
				}
				else
				{
					// Image data
					memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
					cv::cvtColor(Img_BGRU, this->CurrentLeftFrame, CV_BGRA2BGR);

					// Image data
					memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
					cv::cvtColor(Img_BGRU, this->CurrentRightFrame, CV_BGRA2BGR);
				}
			}
			else if (this->cam_channels == 1)
			{
				// If is to resize the image...
				if(this->cam_width != this->destiny_width)
				{
					// this is a gray image!!!
					memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);					
					cv::resize(Img_BGRU, this->CurrentLeftFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);

					memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);					
					cv::resize(Img_BGRU, this->CurrentRightFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
				}
				else
				{
					memcpy( (unsigned char*)(this->CurrentLeftFrame.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);

					memcpy( (unsigned char*)(this->CurrentRightFrame.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
				}
			}
									
			if(this->showdebug)
			{			
				cv::namedWindow( "DisparityMapComponent - Current Left Frame", CV_WINDOW_NORMAL );
				cv::imshow("DisparityMapComponent - Current Left Frame",this->CurrentLeftFrame);
				cv::namedWindow( "DisparityMapComponent - Current Right Frame", CV_WINDOW_NORMAL );
				cv::imshow("DisparityMapComponent - Current Right Frame",this->CurrentRightFrame);
				cv::waitKey(1);
			}

			//======================================= Disparity Map Calculation ================================================
		
			this->CalcDisparityMap(this->CurrentLeftFrame, this->CurrentRightFrame, this->CurrentDisparityMap, this->dispmap_type);

			// Complete timestamp header of the left image
			this->RefImage.time = this->LeftImage.time;
			this->RefImage.timerange = this->LeftImage.timerange;

			// Copy images header and data to memory
			memcpy(this->ref_mem, &RefImage, sizeof(TimestampedStructImage)); 
			memcpy((void*)((TimestampedStructImage*)this->ref_mem + 1), (void*)this->CurrentLeftFrame.data, this->RefImage.image.data_size); 
			this->shmem_ref->write(this->ref_mem, this->ref_mem_size);

			// Complete timestamp header of the disp image
			this->DispImage.time = this->LeftImage.time;
			this->DispImage.timerange = this->LeftImage.timerange;

			// Copy images header and data to memory
			memcpy(this->disp_mem, &DispImage, sizeof(TimestampedStructImage)); 
			memcpy((void*)((TimestampedStructImage*)this->disp_mem + 1), (void*)this->CurrentDisparityMap.data, this->DispImage.image.data_size); 
			this->shmem_disp->write(this->disp_mem, this->disp_mem_size);

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

			if(this->showdebug)
			{
				cv::Mat imgDisparity8U = cv::Mat( this->CurrentDisparityMap.rows, this->CurrentDisparityMap.cols, CV_8UC1 );
					
				this->CurrentDisparityMap = this->CurrentDisparityMap/16;
					
					// Display it as a CV_8UC1 image
				this->CurrentDisparityMap.convertTo( imgDisparity8U, CV_8UC1);
		
				cv::namedWindow( "DisparityMapComponent - Disparity Map", CV_WINDOW_NORMAL );
				imshow( "DisparityMapComponent - Disparity Map", imgDisparity8U );
			}
		}
		else
			msleep(/*MS_DELAY*/10);

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

	this->is_running = false;

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

////////////////////////////////////////////////////////////////////////////////
///	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)
///						3 = SAD from PtGrey
////////////////////////////////////////////////////////////////////////////////
void DisparityMap::CalcDisparityMap(cv::Mat Left_img, cv::Mat Right_img, cv::Mat &Disp_map, Type type)
{	
	//Left_img = cv::imread("C:/Bumblebee_Images/BumblebeeXB3_2013-09-17/rectified-left-00102.ppm");
	//Right_img = cv::imread("C:/Bumblebee_Images/BumblebeeXB3_2013-09-17/rectified-right-00102.ppm");

	cv::Mat Left_img_gray, Right_img_gray;

	int SADWindowSize_ratio = type == 0 ? this->sbm_SADWindowSize/2 : this->sgbm_SADWindowSize/2;

	cv::Mat Left_img_exp, Right_img_exp, Disp_map_exp;

	// Expand the image size to better process the disparity range
	if(this->use_roi)
	{
		Left_img_exp = cv::Mat(this->destiny_roi_height + 2*SADWindowSize_ratio, this->destiny_roi_width + 2*this->num_disp, CV_MAKETYPE(CV_8U, this->destiny_channels), cv::Scalar(0));
		Right_img_exp = cv::Mat(this->destiny_roi_height + 2*SADWindowSize_ratio, this->destiny_roi_width + 2*this->num_disp, CV_MAKETYPE(CV_8U, this->destiny_channels), cv::Scalar(0));
		Disp_map_exp = cv::Mat(this->destiny_roi_height + 2*SADWindowSize_ratio, this->destiny_roi_width + 2*this->num_disp, CV_16S);
 
		(Left_img(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
			Left_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height)));

		(Right_img(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
			Right_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height)));
	}
	else
	{
		Left_img_exp = cv::Mat(Left_img.rows + 2*SADWindowSize_ratio, Left_img.cols + 2*this->num_disp, CV_MAKETYPE(CV_8U, this->destiny_channels), cv::Scalar(0));
		Right_img_exp = cv::Mat(Right_img.rows + 2*SADWindowSize_ratio, Right_img.cols + 2*this->num_disp, CV_MAKETYPE(CV_8U, this->destiny_channels), cv::Scalar(0));
		Disp_map_exp = cv::Mat(Disp_map.rows + 2*SADWindowSize_ratio, Disp_map.cols + 2*this->num_disp, CV_16S);

		Left_img.copyTo(Left_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Left_img.cols, Left_img.rows)));
		Right_img.copyTo(Right_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Right_img.cols, Right_img.rows)));
	}
	
	// Disparity Map by the SAD algorithm
	if(type == SAD)
	{
		cv::StereoBM sbm;
		
		// Gray scale images
		if(this->destiny_channels != 1)
		{
			Left_img_gray = cv::Mat(cv::Size(Left_img_exp.cols, Left_img_exp.rows), CV_8UC1);
			cv::cvtColor(Left_img_exp, Left_img_gray, CV_BGR2GRAY);
			Right_img_gray = cv::Mat(cv::Size(Right_img_exp.cols, Right_img_exp.rows), CV_8UC1);
			cv::cvtColor(Right_img_exp, Right_img_gray, CV_BGR2GRAY);
		}
		else
		{
			Left_img_gray = Left_img_exp;
			Right_img_gray = Right_img_exp;
		}

		// Configuration
		//sbm.state->roi1 = roi1;		// Valid pixels ROI of the left image
		//sbm.state->roi2 = roi2;		// Valid pixels ROI of the right image
		sbm.state->preFilterCap = this->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.
		sbm.state->preFilterSize = this->sbm_preFilterSize >= 5 ? this->sbm_preFilterSize : 5;
		sbm.state->SADWindowSize = this->sbm_SADWindowSize >= 5 ? this->sbm_SADWindowSize : 11;  // Matched block size. It must be an odd number >=1. 
																			// Normally, it should be somewhere in the 3..11 range
		sbm.state->minDisparity = this->min_disp;	// Minimum possible disparity value. Normally, it is zero but sometimes 
										// rectification algorithms can shift images, so this parameter needs to be adjusted accordingly.
		sbm.state->numberOfDisparities = this->num_disp > 0 ? this->num_disp : Left_img.cols/8;	// Maximum disparity minus minimum disparity. 
																											// The value is always greater than zero. 
																											// In the current implementation, this parameter must be 
																											// divisible by 16.
		sbm.state->textureThreshold = this->sbm_textureThreshold;	// Validation threashold for texture variation
		sbm.state->uniquenessRatio = this->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.
		sbm.state->speckleWindowSize = this->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.
		sbm.state->speckleRange = this->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.
		
		// Calculate the disparity image (16S = scaled by 16, must be divided by 16; 32F = original value)
		sbm(Left_img_gray, Right_img_gray, Disp_map_exp, CV_16S);
	}
	else if(type == SGBM || type == SGBM_HH)
	{
		cv::StereoSGBM sgbm;
	
		sgbm.preFilterCap = this->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.
		sgbm.SADWindowSize = this->sgbm_SADWindowSize > 0 ? this->sgbm_SADWindowSize : 7;			// Matched block size. It must be an odd number >=1. 
																			// Normally, it should be somewhere in the 3..11 range
		sgbm.P1 = this->sgbm_P1;								// The first parameter controlling the disparity smoothness.
		sgbm.P2 = this->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 .
		sgbm.minDisparity = this->min_disp;			// Minimum possible disparity value. Normally, it is zero but sometimes 
											// rectification algorithms can shift images, so this parameter needs to be adjusted accordingly.
		sgbm.numberOfDisparities = this->num_disp > 0 ? this->num_disp : Left_img.cols/8;	// Maximum disparity minus minimum disparity. 
																									// The value is always greater than zero. 
																									// In the current implementation, this parameter must be 
																									// divisible by 16.
		sgbm.uniquenessRatio = this->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.
		sgbm.speckleWindowSize = this->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.
		sgbm.speckleRange = this->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.
		sgbm.disp12MaxDiff = this->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.
		sgbm.fullDP = type == SGBM;							// Set it to true to run the full-scale two-pass dynamic programming algorithm. 
																	// It will consume O(W*H*numDisparities) bytes, which is large for 640x480 stereo 
																	// and huge for HD-size pictures.

		sgbm(Left_img_exp, Right_img_exp, Disp_map_exp);
	}

	if(this->use_roi)
	{
		(Disp_map_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
			Disp_map(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)));
	}
	else
	{
		(Disp_map_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Disp_map.cols, Disp_map.rows))).copyTo(Disp_map);
	}
}