/*******************************************************************************
//  created:    2013/10/25 - 19:36
//  filename:   Flea3Grabber.cpp
//
//  author:     Danilo Alves de Lima and Pierre Hudelaine
//              Copyright Heudiasyc UMR UTC/CNRS 6599
// 
//  version:    $Id: $
//
//  purpose:   Grabbe images from multiple flea3
//			   
*******************************************************************************/

#include "Flea3Grabber.h"

#include <iomanip>
#include <iostream>

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

using namespace std;
using namespace pacpus;

DECLARE_STATIC_LOGGER("pacpus.base.Flea3Grabber");

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

const int kMaxFilepathLength = 512; // TODO: should be same for all images


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

	this->nbrCamera_ = 1;
	this->masterCamera_ = 0;
	this->img2send_ = 0;
}


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

	for (int i = 0; i < nbrCamera_; i++)
	{
		if (this->shmem_images_[i])
			delete this->shmem_images_[i];

		this->shmem_images_[i] = NULL;
	}
}


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

	for (int i = 0; i < nbrCamera_; i++)
	{

		if (((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO8)||
			((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO12)||
			((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO16))
		{
			this->settings_[i].cam_channels = 1;	
		}
		else
		{
			if (this->settings_[i].cam_ColorProcessingAlgorithm == 1)
				this->settings_[i].cam_channels = 1;
			else
				this->settings_[i].cam_channels = 3;
		}

		// If do not rescale the image before save
		this->settings_[i].mMaxImageOutputSize = sizeof(unsigned char) * this->settings_[i].cam_width * this->settings_[i].cam_height * this->settings_[i].cam_channels;

		if (this->settings_[i].image_scale == 1.0)
			this->settings_[i].mSaveImageSize = this->settings_[i].mMaxImageOutputSize;
		else
			this->settings_[i].mSaveImageSize = sizeof(unsigned char)*((int)(this->settings_[i].cam_width * this->settings_[i].image_scale)) * ((int)(this->settings_[i].cam_height * this->settings_[i].image_scale)) * this->settings_[i].cam_channels;

		// Create output files for recording
		if (isRecording()) 
		{
			try 
			{
				QString dbtFileName_ = this->settings_[i].mOutputDirectory.filePath(name() + ".dbt");

				if (this->settings_[i].save2dbt)
				{	
					//std::cout << "Save file : " << dbtFileName_.toStdString() << std::endl;
					this->mDbtImage.open(dbtFileName_.toStdString(), WriteMode, FILE_DBT_UNKNOWN, this->settings_[i].mSaveImageSize);
					//this->mDbtImage.open(dbtFileName_.toStdString(), WriteMode, FILE_JPEG, kMaxFilepathLength);
				}
				else
				{
					//std::cout << "Save file : " << dbtFileName_.toStdString() << std::endl;
					this->mDbtImage.open(dbtFileName_.toStdString(), WriteMode, FILE_IMAGE, kMaxFilepathLength);
					//this->mDbtImage.open(dbtFileName_.toStdString(), WriteMode, FILE_JPEG, kMaxFilepathLength);
				}
			} 
			catch (DbiteException & e) 
			{
				cerr << "error opening dbt file: " << e.what() << endl;
			}
		}
	}
		
	// Run thread
	THREAD_ALIVE = true;
	start();
}


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

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

		while(is_running)
		{
			msleep(10);
		}
	}

	// Close DBT file
	if (isRecording()) 
	{
		this->mDbtImage.close();
	}

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


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

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

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

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

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

	if (config.getProperty("cam_translationx") != QString::null)
		trans_[0] = config.getProperty("cam_translationx").toFloat();
	if (config.getProperty("cam_translationy") != QString::null)
		trans_[1] = config.getProperty("cam_translationy").toFloat();
	if (config.getProperty("cam_translationz") != QString::null)
		trans_[2] = config.getProperty("cam_translationz").toFloat();

	if (config.getProperty("cam_rotationx") != QString::null)
		rot_[0] = config.getProperty("cam_rotationx").toFloat();
	if (config.getProperty("cam_rotationy") != QString::null)
		rot_[1] = config.getProperty("cam_rotationy").toFloat();
	if (config.getProperty("cam_rotationz") != QString::null)
		rot_[2] = config.getProperty("cam_rotationz").toFloat();
	
	for (int i = 0; i < nbrCamera_; i++)
	{
		// Initialize with default values
		settings_.push_back(camSetting());
		this->InitDefault(i);

		if (config.getProperty("cam_serial" + QString::number(i)) != QString::null)
			settings_[i].cam_serial = config.getProperty("cam_serial" + QString::number(i)).toInt();

		//---------------------------------------- Camera configuration --------------------------------------------
		if (config.getProperty("auto_FrameRate" + QString::number(i)) != QString::null)
			settings_[i].auto_FrameRate = (config.getProperty("auto_FrameRate" + QString::number(i)) == "true" ? true : false);

		if (config.getProperty("cam_FrameRate" + QString::number(i)) != QString::null)
			settings_[i].cam_FrameRate = config.getProperty("cam_FrameRate" + QString::number(i)).toDouble();

		if (config.getProperty("auto_Gain" + QString::number(i)) != QString::null)
			settings_[i].auto_Gain = (config.getProperty("auto_Gain" + QString::number(i)) == "true" ? true : false);

		if (config.getProperty("cam_Gain" + QString::number(i)) != QString::null)
			settings_[i].cam_Gain = config.getProperty("cam_Gain" + QString::number(i)).toDouble();

		if (config.getProperty("auto_Exposure" + QString::number(i)) != QString::null)
			settings_[i].auto_Exposure = (config.getProperty("auto_Exposure" + QString::number(i)) == "true" ? true : false);

		if (config.getProperty("cam_Exposure" + QString::number(i)) != QString::null)
			settings_[i].cam_Exposure = config.getProperty("cam_Exposure" + QString::number(i)).toDouble();

		if (config.getProperty("auto_Shutter" + QString::number(i)) != QString::null)
			settings_[i].auto_Shutter = (config.getProperty("auto_Shutter" + QString::number(i)) == "true" ? true : false);

		if (config.getProperty("cam_Shutter" + QString::number(i)) != QString::null)
			settings_[i].cam_Shutter = config.getProperty("cam_Shutter" + QString::number(i)).toDouble();

		if (config.getProperty("auto_ExposurebyCode" + QString::number(i)) != QString::null)
			settings_[i].auto_ExposurebyCode = (config.getProperty("auto_ExposurebyCode" + QString::number(i)) == "true" ? true : false);

		if (config.getProperty("cam_ExposurebyCode_tshold" + QString::number(i)) != QString::null)
			settings_[i].cam_ExposurebyCode_tshold = config.getProperty("cam_ExposurebyCode_tshold" + QString::number(i)).toDouble();
		//----------------------------------------------------------------------------------------------------------

		if (config.getProperty("cam_video_mode" + QString::number(i)) != QString::null)
			settings_[i].cam_video_mode = config.getProperty("cam_video_mode" + QString::number(i)).toInt();

		if (config.getProperty("cam_mode" + QString::number(i)) != QString::null)
			settings_[i].cam_mode = config.getProperty("cam_mode" + QString::number(i)).toInt();

		if (config.getProperty("cam_PixelFormat" + QString::number(i)) != QString::null)
			settings_[i].cam_PixelFormat = config.getProperty("cam_PixelFormat" + QString::number(i)).toInt();

		if (config.getProperty("cam_ColorProcessingAlgorithm" + QString::number(i)) != QString::null)
			settings_[i].cam_ColorProcessingAlgorithm = config.getProperty("cam_ColorProcessingAlgorithm" + QString::number(i)).toInt();

		if (config.getProperty("cam_start_point_left" + QString::number(i)) != QString::null)
			settings_[i].cam_start_point_left = config.getProperty("cam_start_point_left" + QString::number(i)).toInt();

		if (config.getProperty("cam_start_point_top" + QString::number(i)) != QString::null)
			settings_[i].cam_start_point_top = config.getProperty("cam_start_point_top" + QString::number(i)).toInt();

		if (config.getProperty("cam_width" + QString::number(i)) != QString::null)
			settings_[i].cam_width = config.getProperty("cam_width" + QString::number(i)).toInt();

		if (config.getProperty("cam_height" + QString::number(i)) != QString::null)
			settings_[i].cam_height = config.getProperty("cam_height" + QString::number(i)).toInt();

		//-------------------------------- Trigger/strobe options -------------------------------------------

		if (config.getProperty("cam_trigger_mode" + QString::number(i)) != QString::null)
			settings_[i].cam_trigger_mode = config.getProperty("cam_trigger_mode" + QString::number(i)).toInt();

		if (config.getProperty("cam_trigger_enable" + QString::number(i)) != QString::null)
			settings_[i].cam_trigger_enable = config.getProperty("cam_trigger_enable" + QString::number(i)).toInt();

		if (config.getProperty("cam_trigger_parameter" + QString::number(i)) != QString::null)
			settings_[i].cam_trigger_parameter = config.getProperty("cam_trigger_parameter" + QString::number(i)).toInt();

		if (config.getProperty("cam_trigger_polarity" + QString::number(i)) != QString::null)
			settings_[i].cam_trigger_polarity = config.getProperty("cam_trigger_polarity" + QString::number(i)).toInt();

		if (config.getProperty("cam_trigger_source" + QString::number(i)) != QString::null)
			settings_[i].cam_trigger_source = config.getProperty("cam_trigger_source" + QString::number(i)).toInt();

		if (config.getProperty("cam_trigger_dest" + QString::number(i)) != QString::null)
			settings_[i].cam_trigger_dest = config.getProperty("cam_trigger_dest" + QString::number(i)).toInt();

		if (config.getProperty("cam_strobe_enable" + QString::number(i)) != QString::null)
			settings_[i].cam_strobe_enable = config.getProperty("cam_strobe_enable" + QString::number(i)).toInt();

		if (config.getProperty("cam_strobe_source" + QString::number(i)) != QString::null)
			settings_[i].cam_strobe_source = config.getProperty("cam_strobe_source" + QString::number(i)).toInt();

		if (config.getProperty("cam_strobe_polarity" + QString::number(i)) != QString::null)
			settings_[i].cam_strobe_polarity = config.getProperty("cam_strobe_polarity" + QString::number(i)).toInt();

		if (config.getProperty("cam_strobe_delay" + QString::number(i)) != QString::null)
			settings_[i].cam_strobe_delay = config.getProperty("cam_strobe_delay" + QString::number(i)).toInt();

		if (config.getProperty("cam_strobe_duration" + QString::number(i)) != QString::null)
			settings_[i].cam_strobe_duration = config.getProperty("cam_strobe_duration" + QString::number(i)).toInt();

		//-------------------------------- Camera settings --------------------------------------------------

		if (config.getProperty("cam_fx" + QString::number(i)) != QString::null)
			settings_[i].tmp_matrix[0][0] = config.getProperty("cam_fx" + QString::number(i)).toDouble();// / settings_[i].cam_width;
		if (config.getProperty("cam_fy" + QString::number(i)) != QString::null)
			settings_[i].tmp_matrix[1][1] = config.getProperty("cam_fy" + QString::number(i)).toDouble();// / settings_[i].cam_height;
		if (config.getProperty("cam_cx" + QString::number(i)) != QString::null)
			settings_[i].tmp_matrix[0][2] = config.getProperty("cam_cx" + QString::number(i)).toDouble();// / settings_[i].cam_width;
		if (config.getProperty("cam_cy" + QString::number(i)) != QString::null)
			settings_[i].tmp_matrix[1][2] = config.getProperty("cam_cy" + QString::number(i)).toDouble();// / settings_[i].cam_height;
		settings_[i].tmp_matrix[2][2] = 1.0;

		for (int j = 0; j < 8; j++) // 8 max parameters for the distortion in OpenCV
			if (config.getProperty("cam_distk" + QString::number(j + 1) + QString::number(i)) != QString::null)
				settings_[i].distCoe[j] = config.getProperty("cam_distk" + QString::number(j + 1) + QString::number(i)).toDouble();

		//-------------------------------- Recording options ------------------------------------------------
		if (config.getProperty("recording") != QString::null)
			setRecording(config.getProperty("recording").toInt());

		if (config.getProperty("image_scale" + QString::number(i)) != QString::null)
			settings_[i].image_scale = config.getProperty("image_scale" + QString::number(i)).toDouble();

		if (config.getProperty("image_compact" + QString::number(i)) != QString::null)
			settings_[i].image_compact = config.getProperty("image_compact" + QString::number(i)).toInt();

		if (config.getProperty("save2dbt" + QString::number(i)) != QString::null)
			settings_[i].save2dbt = config.getProperty("save2dbt" + QString::number(i)).toInt();
		//---------------------------------------------------------------------------------------------------

		if (config.getProperty("showdebug" + QString::number(i)) != QString::null)
			settings_[i].showdebug = (bool)config.getProperty("showdebug" + QString::number(i)).toInt();

		if (config.getProperty("outputdir" + QString::number(i)) != QString::null)
		{
			if (isRecording()) 
			{
				settings_[i].mOutputDirectory.mkdir(config.getProperty("outputdir" + QString::number(i)));
				settings_[i].mOutputDirectory.mkdir(config.getProperty("outputdir" + QString::number(i) + "/" + name()));
				settings_[i].mOutputDirectory.setPath(config.getProperty("outputdir" + QString::number(i)));
			}
		}
		else
		{
			if (isRecording()) 
			{
				settings_[i].mOutputDirectory.mkdir(name());
				settings_[i].mOutputDirectory.setPath(settings_[i].mOutputDirectory.currentPath());
			}
		}
	}

	if (masterCamera_ >= nbrCamera_)
	{
	    LOG_ERROR("masterCamera number error");
		ComponentManager::getInstance()->stop(name());
	}

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


////////////////////////////////////////////////////////////////////////////////
/// Initialize default values
////////////////////////////////////////////////////////////////////////////////
void Flea3Grabber::InitDefault(int indice)
{
	// Default
	setRecording(0);

	this->settings_[indice].cam_serial = 0;												// Camera index to connect
	
	// Camera configuration
	this->settings_[indice].auto_FrameRate = true;					// Set auto frame rate
	this->settings_[indice].cam_FrameRate = 15.0;						// Frame rates in frames per second = FlyCapture2::FRAMERATE_15
	this->settings_[indice].auto_Gain = true;						// Set auto gain
	this->settings_[indice].cam_Gain = 1.0;							// Gain value in db
	this->settings_[indice].auto_Exposure = true;						// Set auto exposure
	this->settings_[indice].cam_Exposure = 1.322;						// Auto exposure in EV
	this->settings_[indice].auto_Shutter = true;						// Set auto shutter
	this->settings_[indice].cam_Shutter = 66.639;						// Shutter in miliseconds
	this->settings_[indice].auto_ExposurebyCode = false;				// Set auto exposure by pos processing method
	this->settings_[indice].cam_ExposurebyCode_tshold = 20.0;			// Pecentage of white pixels for threshold
	this->settings_[indice].cam_start_point_left = 0;					// Image left point (for standard modes only)
	this->settings_[indice].cam_start_point_top = 0;					// Image top point (for standard modes only)
	this->settings_[indice].cam_width = 1280;							// Image width (for standard modes only)
	this->settings_[indice].cam_height = 960;						// image height (for standard modes only)
	this->settings_[indice].cam_video_mode = FlyCapture2::VIDEOMODE_FORMAT7;				// DCAM video modes
	this->settings_[indice].cam_mode = FlyCapture2::MODE_0;								// Camera modes for DCAM formats as well as Format7
	this->settings_[indice].cam_PixelFormat = FlyCapture2::PIXEL_FORMAT_RAW8;				// Pixel formats available for Format7 modes7
	this->settings_[indice].cam_trigger_mode = 0;		// Trigger mode
	this->settings_[indice].cam_trigger_enable = 0;	// Trigger enable (1 on // 0 off)
	this->settings_[indice].cam_trigger_parameter = 0;	
	this->settings_[indice].cam_trigger_polarity = 0;	// Trigger polarity (0 low // 1 high)
	this->settings_[indice].cam_trigger_source = 0;	// Source (GPIO)
	this->settings_[indice].cam_trigger_dest = 0;		// Dest (GPIO) input/output
	this->settings_[indice].cam_strobe_source = 0;
	this->settings_[indice].cam_strobe_enable = 0;				// Enable strobe mode
	this->settings_[indice].cam_strobe_polarity = 0;	// Strobe polarity
	this->settings_[indice].cam_strobe_delay = 0;
	this->settings_[indice].cam_strobe_duration = 0;
	this->settings_[indice].cam_ColorProcessingAlgorithm = FlyCapture2::NEAREST_NEIGHBOR;	/**
																		* Color processing algorithms. Please refer to our knowledge base at
																		* article at http://www.ptgrey.com/support/kb/index.asp?a=4&q=33 for
																		* complete details for each algorithm.
																		*/
	this->settings_[indice].showdebug = false;	// Show frame acquired
		
	if (this->settings_[indice].cam_ColorProcessingAlgorithm == 1)
		this->settings_[indice].cam_channels = 1;
	else
		this->settings_[indice].cam_channels = 3;

	// Size of the image data sizeof(char)*width*height*channels
	this->settings_[indice].mMaxImageOutputSize = sizeof(char) * this->settings_[indice].cam_width * this->settings_[indice].cam_height * this->settings_[indice].cam_channels;
	this->settings_[indice].image_scale = 1.0;				// Default scale
	this->settings_[indice].image_compact = 0;				// Don't compact the image data
	this->settings_[indice].save2dbt = 0;

	for (int i = 0; i < 3; i++)
		for (int j = 0; j < 3; j++)
			settings_[indice].tmp_matrix[i][j] = 0.0;

	for (int i = 0; i < 8; i++) // 8 max for dist coeffs
		settings_[indice].distCoe[i] = 0.0;
}


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

	this->is_running = true;
	bool triggered = false;

	// Tmp matrice used because of intelligent pointer of OpenCV and "intelligent" memory free 
	cv::Mat_<double> tmp_cameraMatrix(3,3); // 3x3 matrix
	cv::Mat_<double> tmp_distCoeffs(8,1);   // 5x1 matrix
	cv::Mat_<double> R(3,1);            // 3x1 matrix, rotation left to right camera
	cv::Mat_<double> T(3,1);            // 3x1 matrix, translation left to right proj. center
	cv::Mat R1, R2, P1, P2, Q;
	std::vector<cv::Mat> map1(nbrCamera_);
	std::vector<cv::Mat> map2(nbrCamera_);
	std::vector<cv::Mat> correctedImg(nbrCamera_);

	for (int i = 0; i < nbrCamera_; i++)
	{
		correctedImg.push_back(cv::Mat(settings_[i].cam_height, settings_[i].cam_width, CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels)));
		map1.push_back(cv::Mat());
		map2.push_back(cv::Mat());

		tmp_cameraMatrix << settings_[i].tmp_matrix[0][0], settings_[i].tmp_matrix[0][1], settings_[i].tmp_matrix[0][2], settings_[i].tmp_matrix[1][0], settings_[i].tmp_matrix[1][1], settings_[i].tmp_matrix[1][2], settings_[i].tmp_matrix[2][0], settings_[i].tmp_matrix[2][1], settings_[i].tmp_matrix[2][2];
		settings_[i].matrix = tmp_cameraMatrix;
		LOG_INFO(settings_[i].matrix);
		tmp_distCoeffs << settings_[i].distCoe[0], settings_[i].distCoe[1], settings_[i].distCoe[2], settings_[i].distCoe[3], settings_[i].distCoe[4], settings_[i].distCoe[5], settings_[i].distCoe[6], settings_[i].distCoe[7];
		settings_[i].distCoeffs = tmp_distCoeffs;
		LOG_INFO(settings_[i].distCoeffs);
	}

	// Used only for two cameras
	if (nbrCamera_ == 2)
	{
		// Initialisation des matrices pour les calculs OpenCV
		R << rot_[0], rot_[1], rot_[2];
		T << trans_[0], trans_[1], trans_[2];
		
		cv::stereoRectify(settings_[0].matrix, settings_[0].distCoeffs, settings_[1].matrix, settings_[1].distCoeffs, cv::Size(settings_[0].cam_width, settings_[0].cam_height), R, T, R1, R2, P1, P2, Q, CV_CALIB_ZERO_DISPARITY);

		cv::initUndistortRectifyMap(settings_[0].matrix, settings_[0].distCoeffs, R1, P1, cv::Size(settings_[0].cam_width, settings_[0].cam_height), CV_16SC2, map1.at(0), map2.at(0));
		cv::initUndistortRectifyMap(settings_[1].matrix, settings_[1].distCoeffs, R2, P2, cv::Size(settings_[1].cam_width, settings_[1].cam_height), CV_16SC2, map1.at(1), map2.at(1));	
	}

	std::vector<FlyCapture2::Camera *> cams;
	std::vector<FlyCapture2::Image> convertedImage;

	//==================================== Camera initialization =================================================
	// Check the number of cameras
	FlyCapture2::BusManager busMgr;
    unsigned int numCameras;
    FlyCapture2::Error error = busMgr.GetNumOfCameras(&numCameras);

    if (error != FlyCapture2::PGRERROR_OK)
    {
        LOG_FATAL(error.GetDescription());
		ComponentManager::getInstance()->stop(name());
	}

	if (this->nbrCamera_ != numCameras)
    {
        LOG_FATAL("Incoorect number of cameras...");
        ComponentManager::getInstance()->stop(name());
    }

	for (int i = 0; i < this->nbrCamera_; i++)
	{	
		if (this->use_shmem)
		{
			this->settings_[i].ImageHeader.image.width = this->settings_[i].cam_width;
			this->settings_[i].ImageHeader.image.height = this->settings_[i].cam_height;
			this->settings_[i].ImageHeader.image.channels = this->settings_[i].cam_channels;
			this->settings_[i].ImageHeader.image.width_step = this->settings_[i].cam_width * this->settings_[i].cam_channels * sizeof(unsigned char);
			this->settings_[i].ImageHeader.image.data_size = this->settings_[i].cam_width * this->settings_[i].cam_height;

			this->settings_[i].img_mem_size = sizeof(TimestampedStructImage) + settings_[i].mMaxImageOutputSize;
			this->settings_[i].img_mem = malloc(this->settings_[i].img_mem_size); 
	
			ShMem * tmp_shmem = new ShMem((name() + "_" + QString().setNum(settings_[i].cam_serial)).toStdString().c_str(), this->settings_[i].img_mem_size);

			shmem_images_.push_back(tmp_shmem);
		}

		// Get camera from serial (in this case first camera in the bus)
		FlyCapture2::PGRGuid guid;		
		error = busMgr.GetCameraFromSerialNumber(this->settings_[i].cam_serial, &guid);
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}
	
		cams.push_back(new FlyCapture2::Camera);	// FlyCapture camera class

		// Connect to a camerak
		error = cams[i]->Connect(&guid);
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}
		
		// Get the camera information
		FlyCapture2::CameraInfo camInfo;
		error = cams[i]->GetCameraInfo(&camInfo);
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}

		// Just for visualization
		/*printf(
			"\n*** CAMERA INFORMATION ***\n"
			"Serial number - %u\n"
			"Camera model - %s\n"
			"Camera vendor - %s\n"
			"Sensor - %s\n"
			"Resolution - %s\n"
			"Firmware version - %s\n"
			"Firmware build time - %s\n\n",
			camInfo.serialNumber,
			camInfo.modelName,
			camInfo.vendorName,
			camInfo.sensorInfo,
			camInfo.sensorResolution,
			camInfo.firmwareVersion,
			camInfo.firmwareBuildTime);*/

		// Query for available Format 7 modes
		FlyCapture2::Format7Info fmt7Info;
		bool supported;
		fmt7Info.mode = (FlyCapture2::Mode)this->settings_[i].cam_mode;
		error = cams[i]->GetFormat7Info(&fmt7Info, &supported);
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}
	    
		if (((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat & fmt7Info.pixelFormatBitField) == 0)
		{
			// Pixel format not supported!
			LOG_ERROR("Pixel format is not supported\n");
			ComponentManager::getInstance()->stop(name());
		}
    
		// Configurate custom image
		FlyCapture2::Format7ImageSettings fmt7ImageSettings;
		fmt7ImageSettings.mode = (FlyCapture2::Mode)this->settings_[i].cam_mode;
		fmt7ImageSettings.offsetX = this->settings_[i].cam_start_point_left;
		fmt7ImageSettings.offsetY = this->settings_[i].cam_start_point_top;
		fmt7ImageSettings.width = this->settings_[i].cam_width;
		fmt7ImageSettings.height = this->settings_[i].cam_height;
		fmt7ImageSettings.pixelFormat = (FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat;

		bool valid;
		FlyCapture2::Format7PacketInfo fmt7PacketInfo;

		// Validate the settings to make sure that they are valid
		error = cams[i]->ValidateFormat7Settings(
			&fmt7ImageSettings,
			&valid,
			&fmt7PacketInfo);
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}

		if (!valid)
		{
			// Settings are not valid
			LOG_ERROR("Format7 settings are not valid");
			ComponentManager::getInstance()->stop(name());
		}

		// Set the settings to the camera
		error = cams[i]->SetFormat7Configuration(
			&fmt7ImageSettings,
			fmt7PacketInfo.recommendedBytesPerPacket);
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}

		// Define the color algorithm
		FlyCapture2::Image::SetDefaultColorProcessing((FlyCapture2::ColorProcessingAlgorithm)this->settings_[i].cam_ColorProcessingAlgorithm);

		// Start capturing images
		error = cams[i]->StartCapture();
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}

		// Set frame rate property
		FlyCapture2::Property cam_property;
		cam_property.type = (FlyCapture2::PropertyType)16; //FlyCapture2::PropertyType::FRAME_RATE;
		cam_property.absControl = true;
		cam_property.autoManualMode = this->settings_[i].auto_FrameRate;
		cam_property.onOff = true;
		cam_property.onePush = false;
		cam_property.absValue = (float)this->settings_[i].cam_FrameRate;
		error = cams[i]->SetProperty(&cam_property);
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}

		// Set gain property
		cam_property.type = (FlyCapture2::PropertyType)13; //FlyCapture2::PropertyType::GAIN; 
		cam_property.absControl = true;
		cam_property.autoManualMode = this->settings_[i].auto_Gain;
		cam_property.onOff = true;
		cam_property.onePush = false;	
		cam_property.absValue = (float)this->settings_[i].cam_Gain;
		error = cams[i]->SetProperty(&cam_property);
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}

		// Set exposure property
		cam_property.type = (FlyCapture2::PropertyType)1; //FlyCapture2::PropertyType::AUTO_EXPOSURE; 
		cam_property.absControl = true;
		cam_property.onePush = false;
		cam_property.onOff = true;
		cam_property.autoManualMode = this->settings_[i].auto_Exposure;
		cam_property.absValue = (float)this->settings_[i].cam_Exposure;
		error = cams[i]->SetProperty(&cam_property);
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}

		// Set shutter property
		cam_property.type = (FlyCapture2::PropertyType)12; //FlyCapture2::PropertyType::SHUTTER; 
		cam_property.absControl = true;
		cam_property.onePush = false;
		cam_property.onOff = true;
		cam_property.autoManualMode = this->settings_[i].auto_Shutter;
		cam_property.absValue = (float)this->settings_[i].cam_Shutter;
		error = cams[i]->SetProperty(&cam_property);
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}

		if (settings_[i].cam_trigger_enable)
		{
			FlyCapture2::TriggerMode trigger;
			trigger.mode = settings_[i].cam_trigger_mode;
			trigger.onOff = settings_[i].cam_trigger_enable;
			trigger.parameter = settings_[i].cam_trigger_parameter;
			trigger.polarity = settings_[i].cam_trigger_polarity;
			trigger.source = settings_[i].cam_trigger_source;
			error = cams[i]->SetTriggerMode(&trigger);
			if (error != FlyCapture2::PGRERROR_OK)
			{
				LOG_ERROR(error.GetDescription());
				ComponentManager::getInstance()->stop(name());
			}

			error = cams[i]->SetGPIOPinDirection(settings_[i].cam_trigger_dest, (i == masterCamera_)? 1 : 0);
			if (error != FlyCapture2::PGRERROR_OK)
			{
				LOG_ERROR(error.GetDescription());
				ComponentManager::getInstance()->stop(name());
			}

			triggered = true;
		}

		if (settings_[i].cam_strobe_enable)
		{
			FlyCapture2::StrobeControl strobe;
			strobe.onOff = settings_[i].cam_strobe_enable;
			strobe.source = settings_[i].cam_strobe_source;
			strobe.delay = settings_[i].cam_strobe_delay;
			strobe.duration = settings_[i].cam_strobe_duration;
			strobe.polarity = settings_[i].cam_strobe_polarity;
			error = cams[i]->SetStrobe(&strobe);
			if (error != FlyCapture2::PGRERROR_OK)
			{
				LOG_ERROR(error.GetDescription());
				ComponentManager::getInstance()->stop(name());
			}
		}

		//============================================================================================================
	
		//---------------------------------- Recording parameters ------------------------------------
		//size_t imageCount = 0;
	
		// Timestamp
		//road_time_t time = 0;
		//road_timerange_t tr = 0;

		const char * const imagePrefix = "image";
		const char * const imageNameDelimiter = "-";
		const char * const imageExtension = this->settings_[i].image_compact ? ".jpg" : 
												(((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO8)||
												((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO12)||
												((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO16)) ?  ".pgm" : ".ppm";
		
		const char kFillCharacter = '0';
		const int kNumberWidth = 5;
		//--------------------------------------------------------------------------------------------
		FlyCapture2::Image tmp_img;
		convertedImage.push_back(tmp_img);
	}
	
	// Raw image
	FlyCapture2::Image rawImage;	

	//bool shmen_init = false;
	// Because the API doesn't work without (saw this with FlyCapture2.exe)
	if (settings_[masterCamera_].cam_trigger_enable)
	{
		cams[masterCamera_]->FireSoftwareTrigger();
		usleep(50000);

		cams[masterCamera_]->FireSoftwareTrigger();
		usleep(50000);
	}

	img_sending_ = new ShMem("Flea3Grabber_sending", settings_[0].mMaxImageOutputSize); 


	road_time_t time;
	
	while (THREAD_ALIVE) 
	{
		if (settings_[masterCamera_].cam_trigger_enable)
		{
			error = cams[masterCamera_]->FireSoftwareTrigger();
			if (error != FlyCapture2::PGRERROR_OK)
			{
					LOG_ERROR(error.GetDescription());
					continue;
			}			
		}

		time = road_time();

		//cout << settings_[1].timeStamp.microSeconds - settings_[0].timeStamp.microSeconds << endl;

		for (int i = 0; i < this->nbrCamera_; i++)
		{
			// Retrieve an image
			error = cams[i]->RetrieveBuffer(&rawImage);
			if (error != FlyCapture2::PGRERROR_OK)
			{
				LOG_ERROR(error.GetDescription());
				continue;
			}


			// Image timestamp
			//settings_[i].timeStamp = rawImage.GetTimeStamp();

			//time = road_time(); // Use the same time base of the other sensors

			if (this->settings_[i].cam_channels == 1)
			{
				// Convert the raw image
				error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage[i]);
				if (error != FlyCapture2::PGRERROR_OK)
				{
					LOG_ERROR(error.GetDescription());
					ComponentManager::getInstance()->stop(name());
				}
			}
			else
			{
				// Convert the raw image
				error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &convertedImage[i]);
				if (error != FlyCapture2::PGRERROR_OK)
				{
					LOG_ERROR(error.GetDescription());
					ComponentManager::getInstance()->stop(name());
				}
			}

			cv::remap(cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage[i].GetData(), (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())), correctedImg[i], map1.at(i), map2.at(i), CV_INTER_LINEAR);
		
			if (this->use_shmem)
			{
				// Complete timestamp header of the right image
				this->settings_[i].ImageHeader.time = time;
				this->settings_[i].ImageHeader.timerange = 0;

				// Copy images header and data to memory
				memcpy(this->settings_[i].img_mem, &settings_[i].ImageHeader, sizeof(TimestampedStructImage)); 
				
				if (shmem_corrected)
					memcpy((void*)((TimestampedStructImage*)this->settings_[i].img_mem + 1), (void*)correctedImg[i].data, this->settings_[i].ImageHeader.image.data_size); 
				else
					memcpy((void*)((TimestampedStructImage*)this->settings_[i].img_mem + 1), (void*)convertedImage[i].GetData(), this->settings_[i].ImageHeader.image.data_size); 
				
				this->shmem_images_[i]->write(this->settings_[i].img_mem, this->settings_[i].img_mem_size);
			}

			if (this->settings_[i].showdebug)
			{
				cv::namedWindow(QString("Flea3Component - Current Reference Frame " + QString::number(i)).toStdString(), CV_WINDOW_NORMAL);
				cv::imshow(QString("Flea3Component - Current Reference Frame " + QString::number(i)).toStdString(), cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage[i].GetData(), (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())));

				cv::namedWindow(QString("Flea3Component - Current Corrected Frame " + QString::number(i)).toStdString(), CV_WINDOW_NORMAL);
				cv::imshow(QString("Flea3Component - Current Corrected Frame " + QString::number(i)).toStdString(), cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), correctedImg[i].data, (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())));
				cv::waitKey(1);
			}

			/////////////////////
			// Write images to disk
			/*if (isRecording()) 
			{
				if (this->settings_[i].save2dbt)
				{
					//------------------------------------------------ Save image in the dbt file ------------------------------------------------------------
					if (this->settings_[i].image_scale == 1.0)
					{
						try 
						{
							this->mDbtImage.writeRecord(time, tr, (char *)convertedImage.GetData(), convertedImage.GetDataSize());
						} 
						catch (DbiteException & e) 
						{
							cerr << "error opening dbt file: " << e.what() << endl;
						}
					}
					else
					{
						// convert to cv::mat
						unsigned int rowBytes = (int)((double)convertedImage.GetReceivedDataSize()/(double)convertedImage.GetRows()); 
						cv::Mat Img_BGR = cv::Mat(convertedImage.GetRows(), convertedImage.GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage.GetData(), rowBytes);

						// Scaled image
						cv::Mat img_resized;

						cv::resize(Img_BGR, img_resized, cv::Size((int)(Img_BGR.cols * this->settings_[i].image_scale), (int)(Img_BGR.rows * this->settings_[i].image_scale)), 0.0, 0.0, CV_INTER_CUBIC);

						try 
						{
							this->mDbtImage.writeRecord(time, tr, (char*)img_resized.data, this->mSaveImageSize);
						} 
						catch (DbiteException & e) 
						{
							cerr << "error opening dbt file: " << e.what() << endl;
						}
					}
					//----------------------------------------------------------------------------------------------------------------------------------------
				}
				else
				{
					//------------------------------------------------------------ Save image file ------------------------------------------------------------
					// Save the image to file
					stringstream imageNameSs; 
				
					imageNameSs << name().toStdString() << "/" << imagePrefix << imageNameDelimiter << setfill(kFillCharacter) << setw(kNumberWidth) << imageCount << imageExtension;
					string imageName = this->settings_[i].mOutputDirectory.filePath(imageNameSs.str().c_str()).toStdString();
			
					//LOG_INFO("Recording frame...");
					if ((this->settings_[i].image_compact == 0) && (this->settings_[i].image_scale == 1.0))
					{
						// Save the image. If a file format is not passed in, then the file
						// extension is parsed to attempt to determine the file format.
						error = convertedImage.Save(const_cast<char *>(imageName.c_str()));
						if (error != FlyCapture2::PGRERROR_OK)
						{
							LOG_ERROR(error.GetDescription());
							ComponentManager::getInstance()->stop(name());
						}  				
					}
					else if (this->settings_[i].image_scale == 1.0)
					{
						// convert to cv::mat
						unsigned int rowBytes = (int)((double)convertedImage.GetReceivedDataSize()/(double)convertedImage.GetRows()); 
						cv::Mat Img_BGR = cv::Mat(convertedImage.GetRows(), convertedImage.GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage.GetData(), rowBytes);

						// Save the image. If a file format is not passed in, then the file
						// extension is parsed to attempt to determine the file format.
						if (!cv::imwrite(const_cast<char *>(imageName.c_str()), Img_BGR))
						{
							LOG_ERROR("Error to save the Flea image!");
							ComponentManager::getInstance()->stop(name());
						}  
					}
					else
					{
						// convert to cv::mat
						unsigned int rowBytes = (int)((double)convertedImage.GetReceivedDataSize()/(double)convertedImage.GetRows()); 
						cv::Mat Img_BGR = cv::Mat(convertedImage.GetRows(), convertedImage.GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage.GetData(), rowBytes);

						// Scaled image
						cv::Mat img_resized;

						cv::resize(Img_BGR, img_resized, cv::Size((int)(Img_BGR.cols * this->settings_[i].image_scale), (int)(Img_BGR.rows * this->settings_[i].image_scale)), 0.0, 0.0, CV_INTER_CUBIC);

						// Save the image. If a file format is not passed in, then the file
						// extension is parsed to attempt to determine the file format.
						if (!cv::imwrite(const_cast<char *>(imageName.c_str()), img_resized))
						{
							LOG_ERROR("Error to save the Flea image!");
							ComponentManager::getInstance()->stop(name());
						}  
					}

					try 
					{
						this->mDbtImage.writeRecord(time, tr, this->settings_[i].mOutputDirectory.filePath(imageName.c_str()).toStdString().c_str(), kMaxFilepathLength);
					} 
					catch (DbiteException & e) 
					{
						cerr << "error opening dbt file: " << e.what() << endl;
					}
					//--------------------------------------------------------------------------------------------------------------------------------------------
				}
			}*/

			//LOG_TRACE("image no. " << imageCount << " acquired successfully");
			//++imageCount;
		}
		
		switch (img2send_)
		{
		case 0:
			this->img_sending_->write((void*)convertedImage[0].GetData(), settings_[0].mMaxImageOutputSize);
			break;
		case 1:
			this->img_sending_->write((void*)correctedImg[0].data, settings_[0].mMaxImageOutputSize);
			break;
		case 2:
			this->img_sending_->write((void*)convertedImage[1].GetData(), settings_[0].mMaxImageOutputSize);
			break;
		case 3:
			this->img_sending_->write((void*)correctedImg[1].data, settings_[0].mMaxImageOutputSize);
			break;
		default:
			// Should not happen
			break; 
		}
  	}
		
	for (int i = 0; i < this->nbrCamera_; i++)
	{
		//====================================== Camera finalization =================================================
		LOG_INFO(QString("Finished grabbing images!" + QString::number(settings_[i].cam_serial)));
		// Stop capturing images
		error = cams[i]->StopCapture();
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}      

		// Disconnect the camera
		error = cams[i]->Disconnect();
		if (error != FlyCapture2::PGRERROR_OK)
		{
			LOG_ERROR(error.GetDescription());
			ComponentManager::getInstance()->stop(name());
		}
		//============================================================================================================

		if (this->shmem_images_[i])
			delete shmem_images_[i];

			this->shmem_images_[i] = NULL;

		this->is_running = false;

		// Destroy the window frame
		if (this->settings_[i].showdebug)
			cvDestroyAllWindows();
	}
}


////////////////////////////////////////////////////////////////////////////////
/// Slot to change the image
////////////////////////////////////////////////////////////////////////////////
void Flea3Grabber::changeImage()
{
	img2send_ = ++img2send_ & 0x03;
}