/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2016 Raytrix GmbH. All rights reserved.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/************************************************************************/
/*  This example demonstrates:                                          */
/*      - Working with a camera.                                        */
/*      - Calculating refocus image                                     */
/*      - Displaying result                                             */
/************************************************************************/

// This class was heavily modified from RX example camera
#include "RxCamera.h"


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// <summary>
/// 	Executes the simulated image capture
/// </summary>
///
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RxCamera::OnSimulatedImage(Poco::Timer&) {
	if (bSimCamera) {

		// create an image here and coppy into CamBuffer
		Rx::CRxImage simImage(imageFormat);

		/************************************************************************/
		/* Write into buffer                                                    */
		/************************************************************************/
		/*if (!m_xCamBuffer.MoveIn(std::move(simImage)))
		{
			// Buffer is full and overwrite is disabled
			// This is a lost frame
			std::cout << "sim : dropped Frame!" << std::endl;
			return;
		}*/
		lock();
		//Rx::CRxImage* tmp;
		//tmp->Create(simImage);
		unsigned char* tmp = new unsigned char[5120 * 5120];
		memcpy(tmp,(unsigned char*)simImage.GetDataPtr(), 5120 * 5120);
		imageQueue.push(tmp);
		unlock();
	}
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// <summary>
/// 	Executes the image captured action.
/// </summary>
///
/// <param name="xImage">  The image. </param>
/// <param name="uCamIdx"> The camera index. </param>
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RxCamera::OnImageCaptured(const Rx::CRxImage& xImage, unsigned uCamIdx)
{
	
	try
	{

		Rx::CRxImage xCapturedImage1;
		// Make a copy of the provided image. We don't know if this image is reused by the camera SDK or by another handler
		//xCapturedImage1.Create(&xImage);


		/************************************************************************/
		/* Write into buffer                                                    */
		/************************************************************************/
		/*if (!m_xCamBuffer.MoveIn(std::move(simImage)))
		{
			// Buffer is full and overwrite is disabled
			// This is a lost frame
			std::cout << "sim : dropped Frame!" << std::endl;
			return;
		}*/
		lock();
		//Rx::CRxImage* tmp;
		//tmp->Create(simImage);
		unsigned char* tmp = new unsigned char[5120 * 5120];
		memcpy(tmp, (unsigned char*)xImage.GetDataPtr(), 5120 * 5120);
		imageQueue.push(tmp);
		unlock();

	}
	catch (Rx::CRxException& ex)
	{
		printf("Exception occured:\n%s\n\n", ex.ToString(true).ToCString());
		printf("Press any key to end program...\n");
		_getch();
	}
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
static void ImageCaptured(const Rx::CRxImage& xImage, unsigned uCamIdx, void* pvContext)
{
	((RxCamera*)pvContext)->OnImageCaptured(xImage, uCamIdx);
}

RxCamera::~RxCamera() {

}

RxCamera::RxCamera() {

	bValidCamera = false;
	bSimCamera = false;
	bWriteImages = false;
	gain = 0.0;
	exposureTime = 0.1;
	frameRate = 60.0f;
	displayRate = frameRate / 2;
	frameCounter = 0;

	imageFormat.m_iHeight = 5120;
	imageFormat.m_iWidth = 5120;
	imageFormat.m_eDataType = Rx::Interop::Runtime28::EDataType::UByte;
	imageFormat.m_ePixelType = Rx::Interop::Runtime28::EPixelType::Lum;

	try
	{
		/************************************************************************/
		/* Initialize and Prepare                                               */
		/************************************************************************/

		std::cout << " in RxCamera" << std::endl;

		_sleep(100);

		// Authenticate MGPU runtime
		printf("Authenticate LFR...\n");
		Rx::LFR::CLightFieldRuntime::Authenticate();

		// Enumerate all CUDA devices at the beginning
		Rx::LFR::CCuda::EnumerateCudaDevices();

		// Iterate over all available Cuda devices
		printf("Listing Cuda devices...\n");
		for (int iIdx = 0; iIdx < Rx::LFR::CCuda::GetDeviceCount(); iIdx++)
		{
			const Rx::LFR::CCudaDevice& xCuda = Rx::LFR::CCuda::GetDevice(iIdx);

			xCuda.GetParams().GetValue(Rx::LFR::Params::ECudaDevice::Name, sxName);

			xCuda.GetParams().GetValue(Rx::LFR::Params::ECudaDevice::ClockFrequencyMHz, dClockMHz);

			xCuda.GetParams().GetValue(Rx::LFR::Params::ECudaDevice::TotalGlobalMemoryMB, dGlobalMemMB);

			printf("%d: %s, Clock: %.0f MHz, Memory: %.0f MB\n", iIdx, sxName.ToCString(), dClockMHz, dGlobalMemMB);
		}

		// Start to find cameras. This is an synchronous call and we wait here until the find process has been finished
		xCamServer.FindCameras();

		// Quit the application if there is no camera
		if (xCamServer.GetCameraCount() == 0)
		{
			printf("No camera found\n");
			bRealCamera = false;
			bSimCamera = true;
		}
		else {
			bRealCamera = true;
			bSimCamera = false;
		}

		printf("Number of cameras available: %d\n", xCamServer.GetCameraCount());

		if (bSimCamera) {
			std::cout << "Using similuated camera..." << std::endl;
		}

		// Quit the application if there is no Cuda device
		if (Rx::LFR::CCuda::GetDeviceCount() == 0)
		{
			printf("No Cuda device found\n");
		}

		printf("Number of Cuda devices available: %d\n", Rx::LFR::CCuda::GetDeviceCount());


		// Work with camera and GPU 0
		uID = 0;
		
		if (bRealCamera) {

			/************************************************************************/
			/* Work with one Camera (Id uID) and one GPU (Id uID)	                */
			/* Get information on available camera calibration settings	            */
			/************************************************************************/

			// Get the camera from the camera server
			Rx::LFR::CCamera& xCamera = xCamServer.GetCamera(uID);

			// Open the camera
			printf("Opening camera 0...");
			xCamera.Open();
			printf("done.\n");

			// Add a image captured callback. This method gets called for every captured camera image and more details are given there
			xCamera.AddImageCapturedCallback(ImageCaptured, this);

			printf("______________________________________________\n");
			printf("Camera %d:\n", uID);

			printf(">> Type: %s\n", xCamera.GetDriverName().ToCString());

			printf(">> ID  : %s\n", xCamera.GetInternalSerial().ToCString());

			printf("\n\n");

			// Load the default calibration of the camera (and load the gray image too)
			Rx::LFR::CCalibrationManager::LoadDefaultCalibration(xDefaultCalibration, xCamera, true);

			// Get the camera name
			sxCameraName = Rx::LFR::CCalibrationManager::GetCameraName(xCamera);

			printf("\n\n");
			printf("Set camera parameter\n");
			printf("____________________\n");

			// Get the current camera exposure
			float fValue;
			xCamera.GetProperty(Rx::Interop::Runtime30::Camera::EProperty::Exposure, fValue);
			printf("Init Exposure: %g\n", fValue);

			// Set the camera exposure in milliseconds
			xCamera.SetProperty(Rx::Interop::Runtime30::Camera::EProperty::Exposure, 10.0f);

			// Set the camera Frame Rate
			xCamera.SetProperty(Rx::Interop::Runtime30::Camera::EProperty::Framerate, frameRate);

			xCamera.SetProperty(Rx::Interop::Runtime30::Camera::EProperty::TriggerMode, Rx::Interop::Runtime30::Camera::ETriggerMode::Camera_FreeRun);

			// Get the new camera exposure
			xCamera.GetProperty(Rx::Interop::Runtime30::Camera::EProperty::Exposure, fValue);
			printf("New Exposure: %g\n", fValue);

		}

		if (bSimCamera) {
			std::cout << "Setting sim camera to " << frameRate << " Hz" << std::endl;
			simCameraCapture.setStartInterval(1000);
			simCameraCapture.setPeriodicInterval(1000 / frameRate);
		}

		// Apply the camera uID and calibration to image processor
		//imgProc.setCameraCalibration(uID, xDefaultCalibration);

		// Set up the image buffer properties
		uBufferSize = 512;
		bOverwrite = false;

		printf("______________________________________________\n");
		printf("Camera Image Buffer %d:\n", uID);

		printf(">> Buffersize: %u\n", uBufferSize);

		printf(">> Overwrite  : %s\n", bOverwrite ? "Yes" : "No");

		printf("\n\n");

		// Create buffer within the given size and with the given overwrite flag
		m_xCamBuffer.Initialize(uBufferSize, bOverwrite);

		// For this example just work with one camera and one GPU
		//xCudaCompute.SetCudaDevice(Rx::LFR::CCuda::GetDevice(uID));
		//xCudaCompute.ApplyCalibration(xDefaultCalibration, true);
		//xCudaCompute.GetParams().SetValue(Rx::LFR::Params::ECudaCompute::PreProc_DataType, (unsigned)Rx::Interop::Runtime28::EDataType::UByte);

		// Get the image access interface.
		//pxImages = static_cast<Rx::LFR::ICudaDataImages*>(xCudaCompute.GetInterface(Rx::LFR::Interfaces::ECudaCompute::Images));

		bValidCamera = true;

	}
	catch (Rx::CRxException& ex)
	{
		printf("Exception occured:\n%s\n\n", ex.ToString(true).ToCString());
		printf("Press any key to end program...\n");
		_getch();
	}
	catch (Rx::IException31& ex)
	{
		printf("Exception occured:\n%s\n\n", ex.GetMessageText());
		printf("Press any key to end program...\n");
		_getch();
	}
}

bool RxCamera::haveCamera() {
	if (!bValidCamera) {
		std::cout << "No valid camera, check power to camera" << std::endl;
		return false;
	}

	return true;
}

void RxCamera::startRecording() {
	std::stringstream ss;
	Poco::Timestamp t;
	ss << "E:\\" << "seq_" << t.utcTime() << ".rays";
	seqWriter.Open(ss.str().c_str(), uBufferSize);
	seqWriter.StartWriting(imageFormat, xDefaultCalibration, metaData, 0);
	bWriteImages = true;
}

void RxCamera::stopRecording() {
	bWriteImages = false;
	seqWriter.Close();
}

bool RxCamera::isRecording() {
	return bWriteImages;
}

int RxCamera::start() {

	std::cout << "starting camera/simulator..." << endl;

	if (!haveCamera())
		return 1;

	if (bRealCamera) {

		Rx::LFR::CCamera& xCamera = xCamServer.GetCamera(uID);
		xCamera.Start();
	}

	if (bSimCamera) {
		Poco::TimerCallback<RxCamera> callback(*this, &RxCamera::OnSimulatedImage);
		simCameraCapture.start(callback);
	}

	//imgProc.start();

	// Start the image processing thread
	startThread();

	return 0;

}

int RxCamera::stop() {

	std::cout << "stoping camera/simulator..." << endl;

	if (!haveCamera())
		return 1;

	// Stop thread
	stopThread();

	if (bRealCamera) {
		Rx::LFR::CCamera& xCamera = xCamServer.GetCamera(uID);

		printf("Stopping camera capture...");
		// Stop capturing
		xCamera.Stop();

		printf("Closing camera...");
		xCamera.Close();
		printf("done.\n");
	}

	if (bSimCamera) {
		simCameraCapture.stop();
	}

	if (bWriteImages)
		seqWriter.Close();

	//imgProc.stop();

   	printf("done.\n");

	// Finalize Cuda and the runtime
	Rx::LFR::CLightFieldRuntime::End();

	return 0;

}

void RxCamera::threadedFunction() {

	while (this->isThreadRunning())
	{

		// Record start time
		auto start = std::chrono::high_resolution_clock::now();

		if (!imageQueue.empty()) {
			try {
				lock();
				unsigned char* tmp = imageQueue.front();
				imageQueue.pop();
				unlock();

				Rx::CRxImage img;
				img.Create(imageFormat.m_iWidth, imageFormat.m_iHeight, imageFormat.m_ePixelType, imageFormat.m_eDataType, tmp);

				frameCounter++;



				// Wait for the image buffer to be not empty
				/*if (!m_xCamBuffer.WaitForNotEmpty(100))
				{
					continue;
				}

				// Check if a new image should be get from the image buffer or the previous image sould be updated
				if (!m_xCamBuffer.MoveOut(xCapturedImage))
				{
					{
						// Wait again! This functions says that it waits until a frame is available
						continue;
					}
				}*/

				// push image to processor
				//imgProc.addImage(xCapturedImage);

					// Write out the image
				if (bWriteImages) {
					seqWriter.WriteFrame(img);
				}

				delete tmp;
			}
			catch (Rx::CRxException& ex)
			{
				printf("Exception occured:\n%s\n\n", ex.ToString(true).ToCString());
				printf("Press any key to end program...\n");
				_getch();
			}

		}

		auto finish = std::chrono::high_resolution_clock::now();
		std::chrono::duration<double> elapsed = finish - start;

		if (frameCounter % (int)frameRate == 0) {

			std::cout << "Elapsed time: " << elapsed.count() << " s";

			if (bSimCamera) {
				std::cout << ", skipped timer calls " << simCameraCapture.skipped();
			}

			if (bWriteImages) {
				std::cout << ", queue size: " << imageQueue.size() << std::endl;
				//std::cout << ", Cam Buf Free: " << m_xCamBuffer.GetFreeSize() << ", Seq Buf Free: " << uBufferSize - seqWriter.GetUsedFrameBufferCount() << std::endl;
			}
			else {
				std::cout << std::endl;
			}
		}


		//_sleep(50);

	}
	std::cout << "thread stopped!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;

}