#include "PMDCamCubeRImageSource.h"
#include <QMessageBox>

#include <float.h>
#include <fstream>

template<class TRImage, class TCamCubeImage>
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::PMDCamCubeRImageSource()
{
	m_PMDHandle = 0;

	m_Error = false;

	m_SourceBufferSize = 0;
	m_ReceiverSourceBuffer = 0;
	m_SharedSourceBuffer = 0;
	m_ProcessorSourceBuffer = 0;

	m_ImageSize[0] = 0;
	m_ImageSize[1] = 0;

	m_Connected = false;

	m_HaltReceiverSemaphore = 0;
	m_HaltProcessorSemaphore = 0;

	m_ReceiverThreadID = 0; //tbd

	m_LastImageTimeStamp = 0;

	m_AverageSourceData = false;

	m_NumFramesToAverageSourceData = 1;

	// Initialize clock
	m_Clock = itk::RealTimeClock::New();

	// We require a different output
	ChangeOutputMacro(CamCubeImageType);
}


template<class TRImage, class TCamCubeImage>
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::~PMDCamCubeRImageSource()
{
	if ( m_Connected )
		Disconnect();
}


//----------------------------------------------------------------------------
template<class TRImage, class TCamCubeImage>
void
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::CheckError(int Code)
{
	if ( Code != PMD_OK )
	{
		char err[256];
		pmdGetLastError(m_PMDHandle,err,256);
		//itkExceptionMacro(<<"A PMD error occured: " << err);
		std::cout << "An error occurred! " << err << std::endl;

		m_Error = true;
	}
}


//----------------------------------------------------------------------------
template<class TRImage, class TCamCubeImage>
bool
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::Connect()
{
	// Sanity check
	if ( m_Connected )
		itkExceptionMacro(<<"Connect requested but reader already connected to camera!");

	// Establish connection
#ifdef _WIN64
	CheckError(pmdOpen(&m_PMDHandle, "camcube.W64.pap", "", "camcubeproc.W64.ppp", ""));
#else
	CheckError(pmdOpen(&m_PMDHandle, "camcube.W32.pap", "", "camcubeproc.W32.ppp", ""));
#endif

	// Update camera
	if(!m_Error)
		CheckError(pmdUpdate(m_PMDHandle));

	// Set the integration time
	if(!m_Error)
		CheckError(pmdSetIntegrationTime(m_PMDHandle,0,250));

	// Unset internal distortion correction
	char buffer[256];

	if(!m_Error)
		CheckError(pmdSourceCommand(m_PMDHandle,buffer,256,"SetLensCalibration Off"));

	// Get the data description
	PMDDataDescription DataDescription;
	if(!m_Error)
	{
		CheckError(pmdGetSourceDataDescription(m_PMDHandle,&DataDescription));

		// Set the image size member (regions are set in GenerateOutputInformation() using these values)
		m_ImageSize[0] = DataDescription.img.numColumns;
		m_ImageSize[1] = DataDescription.img.numRows;

		// Allocate buffers
		m_SourceBufferSize = DataDescription.size;
		m_ReceiverSourceBuffer = new char[m_SourceBufferSize];
		m_SharedSourceBuffer = new char[m_SourceBufferSize];
		m_ProcessorSourceBuffer = new char[m_SourceBufferSize];
		m_AccumulatedSourceData = new long[m_SourceBufferSize];
		memset(m_AccumulatedSourceData,0,sizeof(long)*m_SourceBufferSize);

		// Initialize semaphores
		m_HaltReceiverSemaphore = itk::Semaphore::New();
		m_HaltReceiverSemaphore->Initialize(1);
		m_HaltProcessorSemaphore = itk::Semaphore::New();
		m_HaltProcessorSemaphore->Initialize(0);

		// Spawn receiver thread
		m_ThreadedCameraUpdateStruct.Reader = this;
		m_ReceiverThreadID = this->GetMultiThreader()->SpawnThread(this->ThreadedCameraUpdate,&m_ThreadedCameraUpdateStruct);

		// Set the flag
		m_Connected = true;
	}

	// Get camera serial number
	//CheckError(pmdSourceCommand(m_PMDHandle, m_SerialNumber, sizeof(m_SerialNumber),"GetSerialNumber"));
	// Get modulation frequency
	//CheckError(pmdGetModulationFrequency(m_PMDHandle, &m_ModulationFrequency, 0));
	// Get integration time
	//CheckError(pmdGetIntegrationTime(m_PMDHandle, &m_IntegrationTime, 0));
	return !m_Error;
}


//----------------------------------------------------------------------------
template<class TRImage, class TCamCubeImage>
void
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::Disconnect()
{
	// Sanity check
	if ( !m_Connected )
		itkExceptionMacro(<<"Disconnect requested but reader not initialized!");

	// Close connection
	CheckError(pmdClose(m_PMDHandle));

	// Terminate receiver thread
	if ( m_ReceiverThreadID )
	{
		this->GetMultiThreader()->TerminateThread(m_ReceiverThreadID);
		m_ReceiverThreadID = 0;
	}

	// Release buffer memory
	if ( m_ReceiverSourceBuffer )
		delete[] m_ReceiverSourceBuffer;
	if ( m_SharedSourceBuffer )
		delete[] m_SharedSourceBuffer;
	if ( m_ProcessorSourceBuffer )
		delete[] m_ProcessorSourceBuffer;
	if ( m_AccumulatedSourceData )
		delete[] m_AccumulatedSourceData;
	while ( m_SourceBufferQueue.size() > 0 )
	{
		// Get the oldest source data
		long *OldBuffer = m_SourceBufferQueue.front();
		m_SourceBufferQueue.pop();
		delete[] OldBuffer;
	}

	// Update flag
	m_Connected = false;
}


//----------------------------------------------------------------------------
template<class TRImage, class TCamCubeImage>
void
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::SetIntegrationTime(unsigned int Value)
{
	m_PMDMutex.Lock();
	CheckError(pmdSetIntegrationTime(m_PMDHandle,0,Value));
	CheckError(pmdGetIntegrationTime(m_PMDHandle, &m_IntegrationTime, 0));	
	m_PMDMutex.Unlock();
}


//----------------------------------------------------------------------------
template<class TRImage, class TCamCubeImage>
void
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::AverageSourceData(bool b)
{
	if ( m_AverageSourceData != b )
	{
		m_AverageSourceData = b;
		this->Modified();
	}
}


//----------------------------------------------------------------------------
template<class TRImage, class TCamCubeImage>
void
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::SetNumberOfFramesToAverageSourceData(unsigned int num)
{
	if ( m_NumFramesToAverageSourceData != num )
	{
		m_NumFramesToAverageSourceData = num;
		this->Modified();
	}
}


//----------------------------------------------------------------------------
template<class TRImage, class TCamCubeImage>
void
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::GenerateOutputInformation()
{
	// Sanity check
	if ( !m_Connected )
		itkExceptionMacro(<<"Camera reader is not initialized");

	// Call superclass' implementation
	Superclass::GenerateOutputInformation();

	// Set the regions
	RImageType::SizeType ImageSize;
	ImageSize[0] = m_ImageSize[0];
	ImageSize[1] = m_ImageSize[1];
	RImageType::IndexType Index;
	Index[0] = 0;
	Index[1] = 0;
	RImageType::RegionType Region;
	Region.SetIndex(Index);
	Region.SetSize(ImageSize);
	this->GetOutput()->SetRegions(Region);

	// Set the camera parameters
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::FX] = 281.7116589;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::FY] = 281.8992216;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::CX] = 102.7888459;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::CY] = 111.8996916;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::K1] = -0.42198390521073870;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::K2] = 0.13503147659513920;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::K3] = 0.18505155340116997;
	
	/*this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::CAM_SERIALNUMBER] = atof(m_SerialNumber);
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::MODULATION_FREQUENCY] = m_ModulationFrequency;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::INTEGRATION_TIME] = m_IntegrationTime;*/
}


//----------------------------------------------------------------------------
template<class TRImage, class TCamCubeImage>
void
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::GenerateRImage()
{
	// Sanity check
	if ( !m_Connected )
		itkExceptionMacro(<<"Camera reader is not initialized");
	
	// The correct output
	CastOutput(RImageType,CamCubeImageType)

	// Wait until valid data is available
	while ( true )
	{
		m_HaltProcessorSemaphore->Down();
		m_SharedMutex.Lock();
		TimeStampType CurrentTime = m_Clock->GetTimeStamp();
		if ( CurrentTime - m_LastImageTimeStamp > 0.05 )
		{
			m_SharedMutex.Unlock();
			m_HaltReceiverSemaphore->Up();
			continue;
		}
		break;
	}	

	// Copy data from shared to own buffer. Note that we still have the mutex locked if we reach this point
	PMDDataDescription DataDescription = m_SharedDataDescription;
	memcpy(m_ProcessorSourceBuffer,m_SharedSourceBuffer,m_SourceBufferSize);
	m_SharedMutex.Unlock();	

	// Average data if requested
	if ( m_AverageSourceData )
	{
		// Add the current source data to the averaged data and remember the pointer
		long *TmpSourceBuffer = new long[m_SourceBufferSize];
		for ( unsigned int i = 0; i < m_SourceBufferSize; i++ )
		{
			TmpSourceBuffer[i] = m_ProcessorSourceBuffer[i];
			m_AccumulatedSourceData[i] += TmpSourceBuffer[i];
		}
		m_SourceBufferQueue.push(TmpSourceBuffer);

		// Discard old source data
		while ( m_SourceBufferQueue.size() > m_NumFramesToAverageSourceData )
		{
			// Get the oldest source data
			long *OldBuffer = m_SourceBufferQueue.front();
			m_SourceBufferQueue.pop();

			// Subtract from the averaged data and free memory
			for ( unsigned int i = 0; i < m_SourceBufferSize; i++ )
				m_AccumulatedSourceData[i] -= OldBuffer[i];
			delete[] OldBuffer;
		}

		// Normalize and set to the correct buffer
		for ( unsigned int i = 0; i < m_SourceBufferSize; i++ )
			m_ProcessorSourceBuffer[i] = m_AccumulatedSourceData[i]/m_SourceBufferQueue.size();
	}

	// Update semaphore to indicate that the data is actually processed
	m_HaltReceiverSemaphore->Up();

	// Compute the range images using the PMDCamCubeRImage's built in mechanism. This will also update the Image's internal PMD datadescription and source data
	CastedOutput->ComputeImagesFromSourceData(m_PMDHandle, DataDescription, m_ProcessorSourceBuffer, this->GetMultiThreader());

	// Set the timestamp for the output image
	CastedOutput->SetTimeStamp(m_Clock->GetTimeStamp());

	// Synchronize the flag image
	CastedOutput->SynchronizeFlags();


	// Set the output and ourself to modified
	this->Modified();
	this->GetOutput()->Modified();
	CastedOutput->Modified();
}


//----------------------------------------------------------------------------
template<class TRImage, class TCamCubeImage>
ITK_THREAD_RETURN_TYPE 
PMDCamCubeRImageSource<TRImage,TCamCubeImage>
::ThreadedCameraUpdate(void *arg)
{
	// Get the thread struct that contains a pointer to the reader
	ThreadedCameraUpdateStruct *str = (ThreadedCameraUpdateStruct*)(((itk::MultiThreader::ThreadInfoStruct*)(arg))->UserData);
	Pointer Reader = str->Reader;

	// Used to set the time stamp for the last image
	itk::RealTimeClock::Pointer Clock = itk::RealTimeClock::New();


	// Loop
	while ( true )
	{
		//itk::RealTimeClock::TimeStampType Start = Clock->GetTimeStamp();

		// Ensure that the data is actually processed
		Reader->m_HaltReceiverSemaphore->Down();

		// Update the camera
		Reader->CheckError(pmdUpdate(Reader->m_PMDHandle));

		// Lock PMD
		Reader->m_PMDMutex.Lock();

		// Get the data description
		PMDDataDescription DataDescription;
		Reader->CheckError(pmdGetSourceDataDescription(Reader->m_PMDHandle,&DataDescription));

		// Get the raw data
		Reader->CheckError(pmdGetSourceData(Reader->m_PMDHandle,Reader->m_ReceiverSourceBuffer,DataDescription.size));

		Reader->m_PMDMutex.Unlock();

		// Reset semaphore, copy data to shared buffer and update data description
		Reader->m_SharedMutex.Lock();
		//Reader->m_HaltReceiverSemaphore->Initialize(5);
		memcpy(Reader->m_SharedSourceBuffer,Reader->m_ReceiverSourceBuffer,DataDescription.size);
		Reader->m_SharedDataDescription = DataDescription;
		Reader->Modified();
		Reader->m_LastImageTimeStamp = Clock->GetTimeStamp();
		//std::cout << "Setting  timestamp to " << Reader->m_TimeStamp << std::endl;
		Reader->m_SharedMutex.Unlock();

		// Update semaphore to indicate that data is available
		Reader->m_HaltProcessorSemaphore->Up();

		//itk::RealTimeClock::TimeStampType End = Clock->GetTimeStamp();
		//std::cout << "Elapsed time for one camera update: " << (1000*(End-Start)) << "ms" << std::endl;
	}
}



	


