// Ladybug.cpp : définit les fonctions exportées pour l'application DLL.
//

#include "LadybugComponent.h"

#ifdef WIN32
#include <Windows.h>
#endif 

#include <qfile.h>
#include <qapplication.h>

#include <iostream>
//#include <sstream>
//#include <string>
#include <qdebug.h>
#include <shlobj.h>

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

using namespace std;
using namespace pacpus;

namespace pacpus{

DECLARE_STATIC_LOGGER("pacpus.base.LadybugComponent");

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

/* *Constructor*/
LadybugComponent::LadybugComponent(QString name) : ComponentBase(name)
{
	lbFrameCount = 0;
	lbCurrentTime = 0;
	lbElapsedTime = 0;
	lbLastUpdateTime = 0;

	LOG_TRACE ("constructor(" << name << ")");
}

/* *Destructor */
LadybugComponent::~LadybugComponent()
{
	LOG_TRACE ("destructor");
}

/* *Called by the ComponentManager to start the component*/
void LadybugComponent::startActivity()
{
	lbTotalMBWritten = 0.0;
	lbTotalNumberOfImagesWritten = 0;
	lbNumOfFrames = 0;
	THREAD_ALIVE = true;
	start();
	//LOG_TRACE(Q_FUNC_INFO);
}

/* *Called by the ComponentManager to stop the component*/
void LadybugComponent::stopActivity()
{
	printf (" \n*** Stopping Activity ***\n");
	LadybugError error;

	lbIsRecording = false;
	THREAD_ALIVE = false;

    // wait the thread termination
    if (!wait(1000)) {
        LOG_ERROR(name() << ":The thread doesn't respond for 1 second, it has been terminated");
        terminate();
    }

	/*error = ladybugStopStream (lbStreamContext);
	HandleError(error);

	// Allocate memory for the 6 processed images
    for (unsigned int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++)
    {
		delete lbArpBuffers[uiCamera];
    }

	printf( "Stopping camera...\n" );
    error = ladybugStop(lbContext);
	HandleError(error);

	error = ladybugDestroyStreamContext ( &lbStreamContext );
	HandleError(error);

	// Destroy the context
    printf( "Destroying context...\n" );
    error = ladybugDestroyContext(&lbContext);
    HandleError(error);*/
}

/* *Called by the ComponentManager to pass the XML parameters to the component*/
ComponentBase::COMPONENT_CONFIGURATION LadybugComponent::configureComponent(XmlComponentConfig config)
{
	if (config.getProperty("saveVideoStream") != QString::null) {
		lbSaveVideoStream = config.getProperty("saveVideoStream").toInt();
	}
	if (config.getProperty("saveJPGImages") != QString::null) {
		lbSaveJPGImages = config.getProperty("saveJPGImages").toInt();
	}
	if (config.getProperty("savePanoImage") != QString::null) {
		lbSavePanoImage = config.getProperty("savePanoImage").toInt();
	}
	if (config.getProperty("trigger") != QString::null) {
		lbTriggerOn = config.getProperty("trigger").toInt();
	}
	if (config.getProperty("trigger_mode") != QString::null) {
		lbTriggerMode = config.getProperty("trigger_mode").toInt();
	}
	if (config.getProperty("trigger_source") != QString::null) {
		lbTriggerSource = config.getProperty("trigger_source").toInt();
	}
	if (config.getProperty("trigger_param") != QString::null) {
		lbTriggerParam = config.getProperty("trigger_param").toInt();
	}
	if (config.getProperty("trigger_polarity") != QString::null) {
		lbTriggerPolarity = config.getProperty("trigger_polarity").toInt();
	}

	return ComponentBase::CONFIGURED_OK;
}

/* *Initialize default values to the camera*/
void LadybugComponent::InitDefault()
{

}

/* *Thread loop*/
void LadybugComponent::run()
{
	LadybugError error;
	LadybugImage image;

	StartCamera();
	CreateImageBuffers();

	while (THREAD_ALIVE)
	{
		lbCurrentTime = GetTickCount(); // road_time_t timestamp = road_time(); // ui64 ; unit microsec ; Orig 01/01/1970 00:00
		lbFrameCount++;

		lbElapsedTime = lbCurrentTime - lbLastUpdateTime;
		if (lbElapsedTime >= 1000)
		{
			std::cout << "FPS: " << (lbFrameCount * 1000.0 / lbElapsedTime) << std::endl;
			lbFrameCount = 0;
			lbLastUpdateTime = lbCurrentTime;
		}

		error = ladybugGrabImage( lbContext, &image );
		// t = road_time(); // timestamp 
		HandleError(error);

		if (error == LADYBUG_OK)
		{
			// Save videostream
			if ( lbStreamContext != NULL  && lbSaveVideoStream)
			{
				SaveVideoStream(&image);
			}

			// Save JPG for each camera
			if (lbSaveJPGImages)
			{
				SaveJPGImages(&image);
			}

			// Save Panoramic Images
			if (lbSavePanoImage)
			{
				SavePanoImage(&image);
			}
			lbNumOfFrames++;
		}
	}
	
	
	//error = ladybugConvertImage( context, &image, NULL );
	LOG_INFO("Finishing acquisition...\n");

	if (lbSaveVideoStream)
	{
		error = ladybugStopStream (lbStreamContext);
		HandleError(error);
	}

	triggerMode.bOnOff = false;

	error = ladybugSetTriggerMode( lbContext, &triggerMode);
    HandleError(error);

	//
	// Clean up the buffers
	//
	for( int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++ )
	{
		delete [] lbArpBuffers[ uiCamera ];
	}

	printf (" \n*** Stopping Activity ***\n");

	printf( "Stopping camera...\n" );
    error = ladybugStop(lbContext);
	HandleError(error);

	error = ladybugDestroyStreamContext ( &lbStreamContext );
	HandleError(error);

	// Destroy the context
    printf( "Destroying context...\n" );
    error = ladybugDestroyContext(&lbContext);
    HandleError(error);
}

void LadybugComponent::StartCamera()
{
	LadybugError error;

	// Initialize context
	error = ladybugCreateContext(&lbContext);
	HandleError(error);
	
	error = ladybugCreateStreamContext(&lbStreamContext);
	HandleError(error);
    
	error = ladybugInitializeFromIndex(lbContext, 0);
	HandleError(error);

	error = ladybugGetCameraInfo(lbContext, &lbCamInfo);
	HandleError(error);
	
	error = ladybugLoadConfig(lbContext, NULL);
	HandleError(error);

	error = ladybugSetAutoShutterRange(lbContext, LADYBUG_AUTO_SHUTTER_MOTION);
	HandleError(error);

	/*// Initialize Trigger
	error = ladybugGetTriggerModeInfo( lbContext, &triggerModeInfo );
	HandleError(error);

	error = ladybugGetTriggerMode( lbContext, &triggerMode);
    HandleError(error);

    // Set camera to trigger mode 0
    // A source of 7 means software trigger
    triggerMode.bOnOff = false;
    triggerMode.uiSource = 0; //7
    triggerMode.uiMode = 0;
    triggerMode.uiParameter = 0;
    triggerMode.uiPolarity = 0;*/

	error = ladybugSetPanoramicViewingAngle( lbContext, LADYBUG_FRONT_0_POLE_5);
	HandleError(error);

	error = ladybugSetColorProcessingMethod(lbContext, LADYBUG_NEAREST_NEIGHBOR_FAST);
	HandleError(error); 

    // Configure output images in Ladybug library
	error = ladybugConfigureOutputImages( lbContext, LADYBUG_PANORAMIC );
    HandleError(error);

    error = ladybugSetOffScreenImageSize(lbContext, LADYBUG_PANORAMIC, PANORAMIC_IMAGE_WIDTH, PANORAMIC_IMAGE_HEIGHT );  
    HandleError(error);

	if (lbSaveVideoStream)
	{
		char pszOutputFilePath[256] = {0};
		char pszStreamNameToOpen[256] = {0};
		char buf[_MAX_PATH];
		HRESULT hres = SHGetFolderPath( NULL, CSIDL_PERSONAL, NULL, 0, buf);
		if ( hres == S_OK)
		{
			// Set stream name to "My Documents" folder
			sprintf( pszStreamNameToOpen, "%s\\Ladybug\\PGR\\%s\0", buf, "LadybugCameraStream" );
		}
		else
		{
			// Set stream name to current folder
			strcpy( pszOutputFilePath, buf);
		}

		error = ladybugInitializeStreamForWriting(lbStreamContext, pszStreamNameToOpen, lbContext, pszOutputFilePath, false);
		HandleError(error);
		//printf( "Recording to %s\n", pszOutputFilePath );
	}

	printf("Starting %s (%u)...\n", lbCamInfo.pszModelName, lbCamInfo.serialHead);
	error = ladybugStart(lbContext, LADYBUG_DATAFORMAT_HALF_HEIGHT_RAW8 );
	HandleError(error);
}

void LadybugComponent::SaveVideoStream(LadybugImage* image)
{
	LadybugError error;
	error = ladybugWriteImageToStream( lbStreamContext, image,	&lbTotalMBWritten, &lbTotalNumberOfImagesWritten ); 
	if ( error != LADYBUG_OK )
	{
		printf("Error while saving images...\n");
		//
		// Stop recording if a write error occurs
		// If the return value is LADYBUG_ERROR_DISK_NOT_ENOUGH_SPACE, 
		// the disk is full.
		//
		lbIsRecording = false;
		ladybugStopStream ( lbStreamContext );
		_DISPLAY_ERROR_MSG_AND_RETURN
	}
}

void LadybugComponent::SaveJPGImages(LadybugImage* image)
{
	LadybugError error;
	cv::Mat col_img;

	char pszPath[256] = {0};
	char pszOutputFilePath[256] = {0};
	char buf[_MAX_PATH];
	HRESULT hres = SHGetFolderPath( NULL, CSIDL_PERSONAL, NULL, 0, buf);
	if ( hres == S_OK)
	{
		sprintf( pszPath, "%s\\Ladybug\\Images\0", buf);
	}
	else
	{
		// Set stream name to current folder
		strcpy( pszPath, buf);
	}

	error = ladybugConvertImage(lbContext, image, lbArpBuffers, LADYBUG_BGRU);
	HandleError(error);

	// Save the image as 6 individual raw (unstitched, distorted) images
	for (unsigned int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++)
	{
		LadybugProcessedImage processedImage;
		processedImage.pData = lbArpBuffers[uiCamera];
		processedImage.pixelFormat = LADYBUG_BGRU;
		processedImage.uiCols = image->uiCols;
		processedImage.uiRows = image->uiRows;

		col_img = cv::Mat(processedImage.uiRows, processedImage.uiCols, CV_8UC4);
		col_img.data = processedImage.pData; 
				
		sprintf(pszOutputFilePath, "%s\\ladybug_%d_cam_%02u.bmp", pszPath, lbTotalNumberOfImagesWritten, uiCamera);
		error = ladybugSaveImage(lbContext, &processedImage, pszOutputFilePath, LADYBUG_FILEFORMAT_BMP); 
		HandleError(error);

		//printf("Saved camera %u image to %s.\n", uiCamera, pszOutputFilePath);

		/*// Showing the result
		cv::String name = cv::format("Gray%d",uiCamera);
		cv::resize(col_img, col_img, cv::Size(), 0.25, 0.25, cv::INTER_NEAREST);
		cv::namedWindow( name, CV_WINDOW_AUTOSIZE );
		cv::imshow( name, col_img );
		cv::waitKey(10);*/
	}
}

void LadybugComponent::SavePanoImage(LadybugImage* image)
{
	LadybugError error;
	char pszPath[256] = {0};
	char pszOutputFilePath[256] = {0};
	char buf[_MAX_PATH];
	HRESULT hres = SHGetFolderPath( NULL, CSIDL_PERSONAL, NULL, 0, buf);
	if ( hres == S_OK)
	{
		sprintf( pszPath, "%s\\Ladybug\\Panoramic\0", buf);
	}
	else
	{
		// Set stream name to current folder
		strcpy( pszPath, buf);
	}

	// Convert the image to 6 RGB buffers
	error = ladybugConvertImage(lbContext, image, lbArpBuffers);
	HandleError(error);

	// Send the RGB buffers to the graphics card
	error = ladybugUpdateTextures(lbContext, LADYBUG_NUM_CAMERAS, const_cast<const unsigned char**>( lbArpBuffers ));
	HandleError(error);

	// Stitch the images (inside the graphics card) and retrieve the output to the user's memory
	LadybugProcessedImage processedImage;
	error = ladybugRenderOffScreenImage(lbContext, LADYBUG_PANORAMIC, LADYBUG_BGR, &processedImage);
	HandleError(error);

	// Save the output image to a file
	sprintf(pszOutputFilePath, "%s\\PanoStitchOutput_%d.jpg", pszPath, lbNumOfFrames);
	error = ladybugSaveImage(lbContext, &processedImage, pszOutputFilePath, LADYBUG_FILEFORMAT_JPG); 
	HandleError(error);

}

void LadybugComponent::CreateImageBuffers()
{
	LadybugError error;
	LadybugImage image;

	// Creating buffers for images
    error = LADYBUG_FAILED;
    for ( int i = 0; i < 10 && error != LADYBUG_OK; i++)
    {
        error = ladybugGrabImage(lbContext, &image);
    }
	HandleError(error);
	
	unsigned int uiRawCols, uiRawRows;
	uiRawCols = image.uiCols;// / 2;
	uiRawRows = image.uiRows;// / 2;

	error = ladybugInitializeAlphaMasks( lbContext, uiRawCols, uiRawRows );
	HandleError(error);
	error = ladybugSetAlphaMasking( lbContext, true );
    HandleError(error);
	
	/// Allocate memory for the 6 processed images
    for (unsigned int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++)
    {
		lbArpBuffers[uiCamera] = new unsigned char[image.uiRows * image.uiCols * 4];
        // Initialize the entire buffer so that the alpha channel has a valid (maximum) value.
		memset(lbArpBuffers[uiCamera], 0xff, image.uiRows * image.uiCols * 4);
    }
}

void LadybugComponent::HandleError(LadybugError error)
{
	if( error != LADYBUG_OK )
	{
		printf ("Error: Ladybug library reported - %s\n", ladybugErrorToString( error ) );
		exit( 1 );
	}
	//else 
		//printf("ok...\n");
};

}