/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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"

void RxCamera::estimateFrameRate() {

	auto finish = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double> elapsed = finish - frameClockStart;
	frameRateEst = 1 / elapsed.count();
	frameClockStart = std::chrono::high_resolution_clock::now();

}

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


	// Loop sequence for testing
	if (seqReader.GetFrameIndex() >= seqReader.GetFrameCount()) {
		seqReader.SetFrameIndex(0);
	}

	// Read image from sequence queue
	seqReader.ReadFrame(readImage);
	using namespace std::chrono;
	readImage.SetTimestamp(duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count());

	// Send image to be added to thread-safe buffer and estimate frame rate
	if (!m_xCamBuffer.MoveIn(std::move(readImage))) {
		std::cout << "cam: dropped Frame!" << std::endl;
	}

}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// <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)
{


	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);

	// Set the mage timestamp from the system time
	using namespace std::chrono;
	xCapturedImage1.SetTimestamp(duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count());

	// Send image to be added to thread-safe buffer and estimate frame rate
	if (!m_xCamBuffer.MoveIn(std::move(xCapturedImage1))) {
		std::cout << "cam: dropped Frame!" << std::endl;
	}



}

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

RxCamera::~RxCamera() {

}

RxCamera::RxCamera(double exposureTime, double frameRate, double recordRate, double displayRate, double logRate) {

	bValidCamera = false;
	bSimCamera = false;
	bWriteImages = false;
	imageUpdated = false;
	gain = 0.0;
	dFocus = 0.5;
	this->exposureTime = exposureTime;
	this->frameRate = frameRate;
	this->displayRate =  displayRate;
	this->saveToDiskRate = recordRate;
	this->logRate = logRate;
	frameCounter = 0;
	displayType = RAW;
	lastFrameCount = 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);

			imgProc.setCameraCalibration(uID, xDefaultCalibration);

			// 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,this-> exposureTime);

			// Set the camera Frame Rate
			xCamera.SetProperty(Rx::Interop::Runtime30::Camera::EProperty::Framerate, this->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;
			std::cout << "Loading test image sequence..." << std::endl;
			seqReader.Open("E:\\test_sequence.rays", 100);
			seqReader.StartReading(readImage);
			imgProc.setCameraCalibration(uID, readImage.GetCalibration());
			simCameraCapture.setStartInterval(10);
			simCameraCapture.setPeriodicInterval(1000/frameRate);
		}

		// Apply the camera uID and calibration to image processor
		imgProc.setImageFormat(imageFormat);
		imgProc.initCudaWithImage(); // Slow!!
		_sleep(2000);

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

		m_xCamBuffer.Initialize(uBufferSize, bOverwrite);

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

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

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

		printf("\n\n");

		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();
	}
}

void RxCamera::recStatMsg(const lcm::ReceiveBuffer* rbuf, const std::string& chan, const exlcm::recorder_status* msg) {
	if (msg->recording && !this->bWriteImages) {
		this->startRecording();
	}
	else if (!msg->recording && this->bWriteImages) {
		this->stopRecording();
	}
}

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;
	recordClockStart = std::chrono::high_resolution_clock::now();
	ss << "E:\\" << "EyeRIS_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) {
		seqReader.ReadFrame(readImage);
		Poco::TimerCallback<RxCamera> callback(*this, &RxCamera::OnSimulatedImage);
		simCameraCapture.start(callback);
	}

	displayClockStart = std::chrono::high_resolution_clock::now();

	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::setRates(double frameRate, double recordRate, double displayRate, double logRate) {
	this->frameRate = frameRate;
	this->saveToDiskRate = recordRate;
	this->displayRate = displayRate;
	this->logRate = logRate;
}

void RxCamera::setDisplayType(RxProcessor::IMAGE_DISPLAY_TYPE t) {
	imgProc.setDisplayType(t);
}

void RxCamera::threadedFunction() {

	while (this->isThreadRunning())
	{

		
		
		/*// Wait for the image buffer to be not empty
		if (m_xCamBuffer.IsEmpty())
		{
			sleep(8);
			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
			sleep(8);
			continue;
		}


		// Check record
		auto finish = std::chrono::high_resolution_clock::now();
		std::chrono::duration<double> elapsed = finish - recordClockStart;

		// Write out the image if time has elapsed and recording is enabled
		if (bWriteImages && (elapsed.count() >= 1.0 / saveToDiskRate)) {
				recordClockStart = std::chrono::high_resolution_clock::now();
				seqWriter.WriteFrame(xCapturedImage);
					
		}

		finish = std::chrono::high_resolution_clock::now();
		elapsed = finish - displayClockStart;

		if (elapsed.count() >= 1.0 / displayRate) {
			displayClockStart = std::chrono::high_resolution_clock::now();
			imgProc.addImage(xCapturedImage);
		}

		estimateFrameRate();

		frameCounter++;

		finish = std::chrono::high_resolution_clock::now();
		elapsed = finish - logClockStart;

		if (elapsed.count() >= 1 / logRate) {

			logClockStart = std::chrono::high_resolution_clock::now();

			double frameDelta = xCapturedImage.GetTimestamp() - lastFrameTimestamp;


			lastFrameTimestamp = xCapturedImage.GetTimestamp();

			char buf[128];

			std::cout.precision(14);
			std::cout << "FrameRate: " << (frameCounter - lastFrameCount)/logRate << " fps";
			lastFrameCount = frameCounter;

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


			std::cout << ", cam queue size: " << m_xCamBuffer.GetSize();
			std::cout << ", proc queue size: " << imgProc.queueSize();

			if (bWriteImages) {
				std::cout << " RECORDING, " << seqWriter.GetFrameCount() << " frames.";
			}

			std::cout << endl;
		}

	}

}