#include "KinectSDKSourcePluginRImageSource.h"
#include <iomanip>

// Microsoft SDK Includes
#include <resource.h>
#include <MMSystem.h>
#include <windows.h>

// Calibration and Mapping
#include "itkIdentityTransform.h"
#include "itkBSplineInterpolateImageFunction.h"
#include "itkLinearInterpolateImageFunction.h"
#include "itkResampleImageFilter.h"
#include "itkMultiplyImageFilter.h"
#include "itkDiscreteGaussianImageFilter.h"

// debug
#include "ritkRGBRImage.h"
#include <itkRGBPixel.h>
/* Image I/O operations */
#include <itkImageFileWriter.h>
#include <itkRescaleIntensityImageFilter.h>
#include <iostream>


//#define _RGBVGA

#pragma comment ( lib, "winmm.lib" )

template<class TRImage, class TKinectImage>
KinectSDKSourcePluginSource<TRImage, TKinectImage>
	::KinectSDKSourcePluginSource()
{
	
	m_Connected = false;
	m_Generating = false;

	m_TrackingStarted = false;
	m_CalibrationRequested = false;
	m_Calibrated = false;
	m_FirstGenerateRImageCall = false;

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

	// We require a different output
	ChangeOutputMacro(KinectImageType);

	m_hNextDepthFrameEvent = NULL;
	m_hNextVideoFrameEvent = NULL;
	m_hNextSkeletonEvent = NULL;

	m_pDepthStreamHandle = NULL;
	m_pVideoStreamHandle = NULL;

	m_hThNuiProcess=NULL;
	m_hEvNuiProcessStop=NULL;

	m_ImageBuffer = KinectImageType::New();
	//m_MappingTable = VectorImageType::New();

#ifdef _RGBVGA	
	m_RGBFrameW = 640; //1280;
	m_RGBFrameH = 480; //960;
#else
	m_RGBFrameW = 1280;
	m_RGBFrameH = 960; 
#endif

	m_DepthFrameW = 640;
	m_DepthFrameH = 480;

	m_FramesTotal = m_LastFramesTotal = 0;
	m_LastFPStime = -1;

	m_MappingTable = new std::pair<float,float> [m_RGBFrameW * m_RGBFrameH];

	// initialize Frame Delta in milliseconds
	m_MaxFrameDelta = 50;
	m_EventType = 0;

	// debug timestamp delta
	m_lastTimestamp = -1;

	VF3 Pos(0.f, 0.f, 0.f);
	for ( unsigned int i = 0; i < KinectSkeleton::SKEL_COUNT; i++ )
	{
		m_JointPositions1.push_back(Pos);
		m_JointPositions2.push_back(Pos);
	}
}

template<class TRImage, class TKinectImage>
KinectSDKSourcePluginSource<TRImage, TKinectImage>
	::~KinectSDKSourcePluginSource()
{
	//m_MutexBufferIO.~QMutex();
	if ( m_Connected )
		Disconnect();
}


//----------------------------------------------------------------------------
template<class TRImage, class TKinectImage>
void
	KinectSDKSourcePluginSource<TRImage, TKinectImage>
	::GenerateOutputInformation()
{
	// Call the superclass 
	Superclass::GenerateOutputInformation();

	// Set all regions (adjust to your needs)
	RImageType::SizeType RFImageSize, RGBImageSize;
	
	// Range, flag, and RGB image size
	RFImageSize[0] = m_DepthFrameW;
	RFImageSize[1] = m_DepthFrameH;

	RGBImageSize[0] = m_RGBFrameW;
	RGBImageSize[1] = m_RGBFrameH;

	RImageType::IndexType Index;
	Index[0] = 0;
	Index[1] = 0;
	
	RImageType::RegionType RFRegion, RGBRegion;
	RFRegion.SetIndex(Index);
	RGBRegion.SetIndex(Index);

	RFRegion.SetSize(RFImageSize);
	RGBRegion.SetSize(RGBImageSize);

	CastOutput(RImageType,KinectImageType);

	this->GetOutput()->SetRegions(RFRegion);
	//this->GetOutput()->GetRangeImage()->SetRegions(RFRegion);
	//this->GetOutput()->GetFlagImage()->SetRegions(RFRegion);
	//this->GetOutput()->GetWorldCoordImage()->SetRegions(RGBRegion);

	CastedOutput->GetRGBImage()->SetLargestPossibleRegion(RGBRegion);
	CastedOutput->GetRGBImage()->SetBufferedRegion(RGBRegion);
	CastedOutput->GetRGBImage()->SetRequestedRegion(RGBRegion);

	// set regions for the buffer image	
	m_ImageBuffer->GetFlagImage()->SetRegions(RFRegion);
	m_ImageBuffer->GetRangeImage()->SetRegions(RFRegion);
	m_ImageBuffer->GetRGBImage()->SetRegions(RGBRegion);

	m_ImageBuffer->GetFlagImage()->Allocate();
	m_ImageBuffer->GetRangeImage()->Allocate();
	m_ImageBuffer->GetRGBImage()->Allocate();
	m_ImageBuffer->Allocate();

	// Set the parameters (adjust to your needs)
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::FX] = 5.9421434211923247e+02;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::FY] = 5.9104053696870778e+02;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::CX] = 3.3930780975300314e+02;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::CY] = 2.4273913761751615e+02;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::K1] = -2.6386489753128833e-01;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::K2] = 9.9966832163729757e-01;
	this->GetOutput()->GetCameraParameters()[ritk::CameraParameters::K3] = -1.3053628089976321e+00;

	std::cout << "SOURCE Lar Buf Req" 
		<< CastedOutput->GetRGBImage()->GetLargestPossibleRegion()
		<< CastedOutput->GetRGBImage()->GetBufferedRegion()
		<< 	CastedOutput->GetRGBImage()->GetRequestedRegion() << std::endl;

}


//----------------------------------------------------------------------------
template<class TRImage, class TKinectImage>
void
	KinectSDKSourcePluginSource<TRImage, TKinectImage>
	::Connect()
{
	if ( m_Connected )
		itkExceptionMacro(<<"Connected called but already connected!");

	HRESULT                hr;

	// CreateEvent
	// A security descriptor.
	// A Boolean that is true if the event is manually reset.
	// A Boolean that indicates the initial state of the event.
	// A string that contains the name of the event
	m_hNextDepthFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL ); 
	m_hNextVideoFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL ); 
	m_hNextSkeletonEvent = CreateEvent( NULL, TRUE, FALSE, NULL );


	// Initialization of the Kinect Sensor
	hr = NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH | NUI_INITIALIZE_FLAG_USES_SKELETON | NUI_INITIALIZE_FLAG_USES_COLOR);

	if( FAILED( hr ) )
	{
		itkExceptionMacro(<<"Sensor initialization failed "<< hr);
		return;
	}


	// Enable Skeleton Tracking
	hr = NuiSkeletonTrackingEnable(m_hNextSkeletonEvent,0);
	if( FAILED( hr ) )
	{
		itkExceptionMacro(<<"Skeleton tracking initialization failed "<< hr);
		return;
	}
	
	// Open RGB Stream NUI_IMAGE_RESOLUTION_1280x1024 or NUI_IMAGE_RESOLUTION_640x480
#ifdef _RGBVGA
	hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480,0,2,
		m_hNextVideoFrameEvent, &m_pVideoStreamHandle );
#else
	hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_1280x1024,0,2,
		m_hNextVideoFrameEvent, &m_pVideoStreamHandle );
#endif


	if( FAILED( hr ) )
	{
		itkExceptionMacro(<<"Failed to open image stream "<< hr);
		return;
	}

	// Open Range Stream


	hr = NuiImageStreamOpen( NUI_IMAGE_TYPE_DEPTH, NUI_IMAGE_RESOLUTION_640x480, 0, 2,
		m_hNextDepthFrameEvent, &m_pDepthStreamHandle );

	if( FAILED( hr ) )
	{
		itkExceptionMacro(<<"Failed to open depth and player index stream "<< hr);
		return;
	}

	// Start the Nui processing thread
	m_hEvNuiProcessStop=CreateEvent(NULL,FALSE,FALSE,NULL);
	m_hThNuiProcess=CreateThread(NULL,0,Nui_ProcessThread,this,0,NULL);

	// Update the connected flag
	m_Connected = true;
}



//----------------------------------------------------------------------------
template<class TRImage, class TKinectImage>
void
KinectSDKSourcePluginSource<TRImage, TKinectImage>
::Nui_GotVideoAlert()
{

	const NUI_IMAGE_FRAME * pImageFrame = NULL;

	HRESULT hr = NuiImageStreamGetNextFrame(
		m_pVideoStreamHandle,
		0,
		&pImageFrame );
	if( FAILED( hr ) )
	{
		return;
	}

	INuiFrameTexture * pTexture = pImageFrame->pFrameTexture;

	// get stream resolution
	if (!m_RGBFrameW)
	{
		switch(pImageFrame->eResolution)
		{
		case NUI_IMAGE_RESOLUTION_640x480:
			m_RGBFrameW = 640;
			m_RGBFrameH = 480;
			break;
		case NUI_IMAGE_RESOLUTION_1280x1024:
			m_RGBFrameW = 1280;
			m_RGBFrameH = 960; //1024;
			break;
		default:
			itkExceptionMacro(<<"No valid video stream resolution"<< hr);
			return;
		}
		std::cout << "VideoResolution: " << m_RGBFrameW << "x" << m_RGBFrameH << std::endl;
	}
	
	NUI_LOCKED_RECT LockedRect;
	pTexture->LockRect( 0, &LockedRect, NULL, 0 );
	if( LockedRect.Pitch != 0 )
	{
		// draw the bits to the bitmap
		KinectImageType::RGBImageType::PixelType *RGBDataBuffer = m_ImageBuffer->GetRGBImage()->GetBufferPointer();

		BYTE * pBuffer = (BYTE*) LockedRect.pBits;

		for( int y = 0 ; y < m_RGBFrameH * m_RGBFrameW; y++, ++RGBDataBuffer )
		{
			RGBQUAD * quad = ((RGBQUAD*)pBuffer) + y;

			(*RGBDataBuffer)[0] = quad->rgbRed;
			(*RGBDataBuffer)[1] = quad->rgbGreen;
			(*RGBDataBuffer)[2] = quad->rgbBlue;
		}
	}
	else
	{
		itkExceptionMacro(<<"Buffer length of received texture is bogus\n" );
	}

	NuiImageStreamReleaseFrame( m_pVideoStreamHandle, pImageFrame );

}


//----------------------------------------------------------------------------
template<class TRImage, class TKinectImage>
void
	KinectSDKSourcePluginSource<TRImage, TKinectImage>
	::Nui_GotDepthAlert()
{

	const NUI_IMAGE_FRAME * pImageFrame = NULL;

	// get the new image frame
	HRESULT hr = NuiImageStreamGetNextFrame(
		m_pDepthStreamHandle,
		0,
		&pImageFrame );

	if( FAILED( hr ) )
	{
		return;
	}

	
	if (!m_Calibrated)
	{
		/* get the depth stream resolution */

		switch(pImageFrame->eResolution)
		{
		case NUI_IMAGE_RESOLUTION_80x60:
			m_DepthFrameW = 80;
			m_DepthFrameH = 60;
			m_DepthScale = 4;
			break;
		case NUI_IMAGE_RESOLUTION_320x240:
			m_DepthFrameW = 320;
			m_DepthFrameH = 240;
			m_DepthScale = 1;
			break;
		case NUI_IMAGE_RESOLUTION_640x480:
			m_DepthFrameW = 640;
			m_DepthFrameH = 480;
			m_DepthScale = 0.5;
			break;
		default:
			itkExceptionMacro(<<"No valid depth stream resolution"<< hr);
			return;
		}
	   std::cout << "DepthResolution: " << m_RGBFrameW << "x" << m_RGBFrameH << std::endl;

	   /* Calibration Image initialization */
	   
	   // Set the regions (adjust to your needs)
		UIImageType::SizeType CalibrationImageSize;
	
		// Range, flag, and RGB image size
		CalibrationImageSize[0] = m_DepthFrameW;
		CalibrationImageSize[1] = m_DepthFrameH;

		UIImageType::IndexType Index;
		Index[0] = 0;
		Index[1] = 0;

		UIImageType::RegionType CalibrationRegion; 
		CalibrationRegion.SetIndex(Index);
		CalibrationRegion.SetSize(CalibrationImageSize);

		// initialize the images
		CalibrationX = UIImageType::New();
		CalibrationY = UIImageType::New();
		CalibrationX->SetRegions(CalibrationRegion);
		CalibrationY->SetRegions(CalibrationRegion);
		CalibrationX->Allocate();
		CalibrationY->Allocate();

	}

	// Pointer for simply references in further code
	INuiFrameTexture * pTexture = pImageFrame->pFrameTexture;

	NUI_LOCKED_RECT LockedRect;

	pTexture->LockRect( 0, &LockedRect, NULL, 0 );
	if( LockedRect.Pitch != 0 )
	{

		BYTE * pBuffer = (BYTE*) LockedRect.pBits;

		// draw the bits to the bitmap
		USHORT * pBufferRun = (USHORT*) pBuffer;
		USHORT * pBufferRunTmp = (USHORT*) pBuffer;

		RImageType::RangeImageType::PixelType *RangeDataBuffer = this->m_ImageBuffer->GetRangeImage()->GetBufferPointer();
		RImageType::FlagImageType::PixelType *FlagDataBuffer = this->m_ImageBuffer->GetFlagImage()->GetBufferPointer();
		//itk::Image<unsigned char, 2>::PixelType *MappingTable = this->m_MappingTable->GetBufferPointer();


		// image is mirrored if the pure depth stream is taken (hint: no mirroring with the depht+player stream!)
		for( int y = 0 ; y < m_DepthFrameH ; y++ )
		{
			for( int x = m_DepthFrameW - 1; x >= 0 ; x--, RangeDataBuffer++, FlagDataBuffer++, pBufferRunTmp++)
			{
				// first 3 bits for Player index, 
				// following 12 bits for depth value in mm
				USHORT RealDepth = (*pBufferRun & 0xfff8) >> 3;
				USHORT Player = *pBufferRun & 7;
				USHORT Depth = *pBufferRun;

				// Copy Depth Values
				pBufferRunTmp = pBufferRun + x;
				*RangeDataBuffer = *pBufferRunTmp; // + 2*x; //RealDepth

				if ( *RangeDataBuffer != 0 )
				{
					*FlagDataBuffer = RImageType::VALID;	
				} else {
					*FlagDataBuffer = ~RImageType::VALID;
				}


				if (!m_Calibrated) 
				{
					LONG colorIndexX, colorIndexY;

					NuiImageGetColorPixelCoordinatesFromDepthPixel(pImageFrame->eResolution, NULL, x*m_DepthScale, y*m_DepthScale, 0, &colorIndexX, &colorIndexY);

					// ensure that the coordinates are within the RGB range
					colorIndexX = (std::min) ((int)colorIndexX,m_DepthFrameW - 1);
					colorIndexY = (std::min) ((int)colorIndexY,m_DepthFrameH - 1);
					colorIndexX = (std::max) ((int)colorIndexX,0);
					colorIndexY = (std::max) ((int)colorIndexY,0);

					float colorX = colorIndexX;
					float colorY = colorIndexY;

					UIImageType::IndexType Index;
					Index[0] = x;
					Index[1] = y;

					CalibrationX->SetPixel(Index, colorX);
					CalibrationY->SetPixel(Index, colorY);

				}


			}
			pBufferRun += m_DepthFrameW;
			
		}

		// resample and scale the calibration image, initialize the mapping table
		if (!m_Calibrated)
		{
					
			if (m_RGBFrameW == 640)
			{				
				// initialize the mapping table
				typedef itk::ImageRegionIterator<UIImageType> CalibrationIteratorType;
				CalibrationIteratorType     itX  (CalibrationX,    CalibrationX->GetLargestPossibleRegion());
				CalibrationIteratorType     itY  (CalibrationY,    CalibrationY->GetLargestPossibleRegion());

				int i = 0;
				for (itX.GoToBegin(), itY.GoToBegin(); !itX.IsAtEnd(); ++itY, ++itX, i++)
				{
					m_MappingTable[i]  = std::make_pair(itX.Value(), itY.Value());
				}
			} 
			else
			{

				// Calculate Upscaled Mapping Pattern

				// *** Rescale Mapping Images ***//

				// identity transform
				typedef itk::IdentityTransform<double, 2> TransformType;
				// interpolation type
				typedef itk::BSplineInterpolateImageFunction<UIImageType, double, double> InterpolatorType;
				//typedef itk::LinearInterpolateImageFunction<UIImageType, double> InterpolatorType;
				// the resample filter itself
				typedef itk::ResampleImageFilter<UIImageType, UIImageType> ResampleFilterType;

				// prepare the resampler
				// Instantiate the transform and specify it should be the id transform.
				TransformType::Pointer pTransform = TransformType::New();
				pTransform->SetIdentity();

				// Instantiate the resampler. Wire in the transform and the interpolator.
				ResampleFilterType::Pointer pResizeFilterX = ResampleFilterType::New();
				ResampleFilterType::Pointer pResizeFilterY = ResampleFilterType::New();
				pResizeFilterX->SetTransform(pTransform.GetPointer());
				pResizeFilterY->SetTransform(pTransform.GetPointer());
				
				const double vfOutputOrigin[2]  = { 0.0, 0.0 };
				pResizeFilterY->SetOutputOrigin(vfOutputOrigin);

				// Fetch original image size.
				const UIImageType::RegionType& inputRegion = CalibrationX->GetLargestPossibleRegion();
				const UIImageType::SizeType& vnInputSize = inputRegion.GetSize();
				unsigned int nOldWidth = vnInputSize[0];
				unsigned int nOldHeight = vnInputSize[1];

				UIImageType::SizeType vnOutputSize = { {m_RGBFrameW, m_RGBFrameH} };
				pResizeFilterX->SetSize(vnOutputSize);
				pResizeFilterY->SetSize(vnOutputSize);

				 // Fetch original image spacing.
				const UIImageType::SpacingType& vfInputSpacing = CalibrationX->GetSpacing();

				double vfOutputSpacing[2];
				vfOutputSpacing[0] = vfInputSpacing[0] * (double) nOldWidth / (double) m_RGBFrameW;
				vfOutputSpacing[1] = vfInputSpacing[1] * (double) nOldHeight / (double) m_RGBFrameH;
				pResizeFilterX->SetOutputSpacing(vfOutputSpacing);
				pResizeFilterY->SetOutputSpacing(vfOutputSpacing);

				pResizeFilterX->SetInput(CalibrationX);
				pResizeFilterX->Update();
				pResizeFilterY->SetInput(CalibrationY);
				pResizeFilterY->Update();

				//****  Multiply the mapping images by a constant to get into the higher image range **** ///
				UIImageType::Pointer constantImage = UIImageType::New();

				itk::Index<2> start;
				start.Fill(0);

				// Set all regions (adjust to your needs)
				UIImageType::SizeType RGBImageSize;	
				RGBImageSize[0] = m_RGBFrameW;
				RGBImageSize[1] = m_RGBFrameH;

				UIImageType::IndexType Index;
				Index[0] = 0;
				Index[1] = 0;
	
				UIImageType::RegionType RGBRegion;
				RGBRegion.SetIndex(Index);
				RGBRegion.SetSize(RGBImageSize);

				constantImage->SetRegions(RGBRegion);
				constantImage->Allocate();
				constantImage->FillBuffer(2);
				constantImage->Update();

				typedef itk::MultiplyImageFilter<UIImageType, UIImageType, UIImageType> MultiplyImageFilterType;
				MultiplyImageFilterType::Pointer multiplyImageFilterX = MultiplyImageFilterType::New();
				MultiplyImageFilterType::Pointer multiplyImageFilterY = MultiplyImageFilterType::New();
				multiplyImageFilterX->SetInput1(pResizeFilterX->GetOutput());
				multiplyImageFilterX->SetInput2(constantImage);
				multiplyImageFilterY->SetInput1(pResizeFilterY->GetOutput());
				multiplyImageFilterY->SetInput2(constantImage);
				multiplyImageFilterX->Update();
				multiplyImageFilterY->Update();	

				//**** Gauss filter for eliminate discretization errors caused by upscaling ****//
				typedef itk::DiscreteGaussianImageFilter<UIImageType, UIImageType>  GaussfilterType;

				// Create and setup a Gaussian filter
				double variance = 4.0;

				GaussfilterType::Pointer gaussianFilterX = GaussfilterType::New();
				gaussianFilterX->SetInput( multiplyImageFilterX->GetOutput() );
				gaussianFilterX->SetVariance(variance);
				GaussfilterType::Pointer gaussianFilterY = GaussfilterType::New();
				gaussianFilterY->SetInput( multiplyImageFilterY->GetOutput() );
				gaussianFilterY->SetVariance(variance);

				
				// Get the new mapping images
				UIImageType::Pointer NewImageX = gaussianFilterX->GetOutput();
				NewImageX->Update();
				UIImageType::Pointer NewImageY = gaussianFilterY->GetOutput();
				NewImageY->Update();

				// initialize the mapping LUT
				typedef itk::ImageRegionIterator<UIImageType> CalibrationIteratorType;
				CalibrationIteratorType     itX  (NewImageX,    NewImageX->GetLargestPossibleRegion());
				CalibrationIteratorType     itY  (NewImageY,    NewImageY->GetLargestPossibleRegion());
				int i = 0;
				for (itX.GoToBegin(), itY.GoToBegin(); !itX.IsAtEnd(); ++itY, ++itX, i++)
				{
					
					m_MappingTable[i]  = std::make_pair(itX.Value(), itY.Value());
					
				}
			} // end of creating LUT
		} // end if(!Calibrated)
	}
	else
	{
		itkExceptionMacro(<<"Size of one row of the current frame is zero");
	}


	NuiImageStreamReleaseFrame( m_pDepthStreamHandle, pImageFrame );
	if (!m_Calibrated) m_Calibrated = true;
	//CastedOutput->SetTimeStamp(m_Clock->GetTimeStamp());

}


//----------------------------------------------------------------------------
template<class TRImage, class TKinectImage>
void
	KinectSDKSourcePluginSource<TRImage, TKinectImage>
	::Nui_GotSkeletonAlert()
{

	NUI_SKELETON_FRAME SkeletonFrame;

	HRESULT hr = NuiSkeletonGetNextFrame( 0, &SkeletonFrame );
	m_JointPositions1[0][0] = 0.0;
	m_JointPositions2[0][0] = 0.0;
	int SkeletonCount = 0;

	for( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ )
	{
		if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED )
		{
			SkeletonCount++;
		}
	}

	// return, if no skeleton is found
	if( !SkeletonCount || !m_DepthFrameW)
	{
		return;
	}

	//CastedOutput->m_Skeleton->SetJoint(KinectSkeleton::JointTypes(j), JointPosition.position.X, JointPosition.position.Y, JointPosition.position.Z);

	// filters skeleton positions to reduce jitter between frames
	//NuiTransformSmooth(&SkeletonFrame,NULL);

	// Threadsafe handling of writing into the buffer

	int skelIDX = 0;

	for( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ )
	{
		
		if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED )
		{
			//if (skelIDX == 1)
			//	std::cout << "2 Skeletons found:" << std::endl;

			float fx=0,fy=0,fz=0;

			// Set joint positions for one of two possible skeletons
			for (int j = 0; j < KinectSkeleton::SKEL_COUNT - 1; j++)
			{
				NuiTransformSkeletonToDepthImageF(SkeletonFrame.SkeletonData[i].SkeletonPositions[j], &fx, &fy );
				fz = SkeletonFrame.SkeletonData[i].SkeletonPositions[j].z;
				short trackingState = SkeletonFrame.SkeletonData[i].eSkeletonPositionTrackingState[j];

				// Set CoM
				if (j == KinectSkeleton::SKEL_COM) 
				{
					NuiTransformSkeletonToDepthImageF(SkeletonFrame.SkeletonData[i].Position, &fx, &fy );	
					fz = SkeletonFrame.SkeletonData[i].Position.z;
					trackingState = SkeletonFrame.SkeletonData[i].dwQualityFlags;
				}

				if (skelIDX == 0)
				{			
					m_JointPositions1[j][0] = m_DepthFrameW * fx;
					m_JointPositions1[j][1] = m_DepthFrameH * fy;
					m_JointPositions1[j][2] = fz;
				}
				else
				{
					m_JointPositions2[j][0] = m_DepthFrameW * fx;
					m_JointPositions2[j][1] = m_DepthFrameH * fy;
					m_JointPositions2[j][2] = fz;
				}

				// Set tracking state of joint
				m_TrackingState[skelIDX*KinectSkeleton::SKEL_COUNT + j] = trackingState;

			}

			skelIDX++;
		}
	}
}


template<class TRImage, class TKinectImage>
DWORD WINAPI
	KinectSDKSourcePluginSource<TRImage, TKinectImage>
	::Nui_ProcessThread(LPVOID pParam)
{

	// Cast
	KinectSDKSourcePluginSource *pthis=(KinectSDKSourcePluginSource *) pParam; 

	// wait for the first Call of GenerateRImage (wait for Allocation)
	while(!pthis->m_FirstGenerateRImageCall)
		Sleep(500);

	HANDLE                 hEvents[4];
	int                    nEventIdx; //,t,dt;

	// Configure events to be listened on
	hEvents[0]=pthis->m_hEvNuiProcessStop;
	hEvents[1]=pthis->m_hNextDepthFrameEvent;
	hEvents[2]=pthis->m_hNextVideoFrameEvent;
	hEvents[3]=pthis->m_hNextSkeletonEvent;

	int depthframes = 0;
	int videoframes = 0;
	int skeletons = 0;

	double dVideoImageTimeStamp = 0;
	double dRangeImageTimeStamp = 0;
	double dSkeletonImageTimeStamp = 0;

	// debug
	double averagePerFrame = 0;
	//double timeDeltaMax[100];

	int i = 0;

	// Main thread loop
	while(1)
	{

		// Locked while all data is not complete for a frame

		pthis->m_MutexBufferIO.lock();
		int frameSync = 0;

		while (frameSync != 15)
		{
			// Wait for an event to be signalled
			nEventIdx=WaitForMultipleObjects(sizeof(hEvents)/sizeof(hEvents[0]),hEvents,FALSE,100);

			// If the stop event, stop looping and exit
			if(nEventIdx==0) {
				pthis->m_MutexBufferIO.unlock();
				return (0);
			}

			// Process signal events
			switch(nEventIdx)
			{
			case 1:
				{
					// max 2 ms
					//double begin = timeGetTime();

					//std::cout << " 1 ";
					dRangeImageTimeStamp = timeGetTime();
					pthis->Nui_GotDepthAlert(); 

					//if ((frameSync & 2) == 2)
					{
						frameSync = frameSync | 1;
					}
					//std::cout<< dRangeImageTimeStamp-begin << std::endl;
					break;
				}
			case 2:
				{
					//std::cout << " 2 ";
					// max 1 ms
					//double begin = timeGetTime();	
					dVideoImageTimeStamp = timeGetTime();
					pthis->Nui_GotVideoAlert();
					frameSync = frameSync | 2;
					
					videoframes ++;
					//std::cout<< dVideoImageTimeStamp-begin << std::endl;
					break;
				}
			case 3:
				{
					//std::cout << " 3 ";
					// < 1 ms
					//double begin = timeGetTime();
					dSkeletonImageTimeStamp = timeGetTime();
					skeletons ++;
					pthis->Nui_GotSkeletonAlert();
					//if ((frameSync & 2) == 2 && (frameSync & 1) == 1)
					{
						frameSync = frameSync | 4;
					}
					
					//std::cout<< dSkeletonImageTimeStamp-begin << std::endl;
					break;
				}
			}

			// Check if Depth/Video/Skeleton is written and if the given data is within the given timeinterval
			if (frameSync == 7) {
				
				double oldest = std::min(std::min(dSkeletonImageTimeStamp, dRangeImageTimeStamp),dVideoImageTimeStamp);
				double newest = std::max(std::max(dSkeletonImageTimeStamp, dRangeImageTimeStamp),dVideoImageTimeStamp);

				if (newest - oldest < pthis->m_MaxFrameDelta)
				{

					frameSync = frameSync | 8;
					
				} 
				else // the timestamps of the data differs too much TODO
				{	
					frameSync = 0;
					//std::cout << "Frames asynchron" << std::endl;
				}
			}
		}

		pthis->m_MutexBufferIO.unlock();
		pthis->m_FramesTotal++;

		// initialize for new Frame
		dSkeletonImageTimeStamp = dRangeImageTimeStamp = dSkeletonImageTimeStamp = frameSync= 0;
	}

	return (0);
}

	//----------------------------------------------------------------------------
	template<class TRImage, class TKinectImage>
	void
		KinectSDKSourcePluginSource<TRImage, TKinectImage>
		::Disconnect()
	{
		// Stop the Nui processing thread
		if(m_hEvNuiProcessStop!=NULL)
		{
			// Signal the thread
			SetEvent(m_hEvNuiProcessStop);

			// Wait for thread to stop
			if(m_hThNuiProcess!=NULL)
			{
				WaitForSingleObject(m_hThNuiProcess,100);
				CloseHandle(m_hThNuiProcess);
			}
			CloseHandle(m_hEvNuiProcessStop);
		}

		// Disconnect sensor
		NuiShutdown( );
		if( m_hNextSkeletonEvent && ( m_hNextSkeletonEvent != INVALID_HANDLE_VALUE ) )
		{
			CloseHandle( m_hNextSkeletonEvent );
			m_hNextSkeletonEvent = NULL;
		}
		if( m_hNextDepthFrameEvent && ( m_hNextDepthFrameEvent != INVALID_HANDLE_VALUE ) )
		{
			CloseHandle( m_hNextDepthFrameEvent );
			m_hNextDepthFrameEvent = NULL;
		}
		if( m_hNextVideoFrameEvent && ( m_hNextVideoFrameEvent != INVALID_HANDLE_VALUE ) )
		{
			CloseHandle( m_hNextVideoFrameEvent );
			m_hNextVideoFrameEvent = NULL;
		}

		// Update the connected flag
		m_Connected = false;

	}


	//----------------------------------------------------------------------------
	template<class TRImage, class TKinectImage>
	void
		KinectSDKSourcePluginSource<TRImage, TKinectImage>
		::GenerateRImage()
	{

		// Sanity check
		if ( !m_Connected )
			itkExceptionMacro(<<"Not connected!");

		if (!m_FirstGenerateRImageCall)
			m_FirstGenerateRImageCall = true;

		if ( m_Generating )
			std::cout << "Warning: Generate called but already generating..." << std::endl;

		{
			
			QMutexLocker lockBuffer(& m_MutexBufferIO);
			CastOutput(RImageType,KinectImageType);
			//double begin = timeGetTime();	

			
			KinectImageType::RGBImageType::PixelType *RGBDataBufferTarget = CastedOutput->GetRGBImage()->GetBufferPointer();
			KinectImageType::RGBImageType::PixelType *RGBDataBufferSource = this->m_ImageBuffer->GetRGBImage()->GetBufferPointer();
			KinectImageType::RGBImageType::PixelType *RGBDataBufferSourceBegin = this->m_ImageBuffer->GetRGBImage()->GetBufferPointer();

			if (CastedOutput->GetRGBImage()->GetLargestPossibleRegion() != m_ImageBuffer->GetRGBImage()->GetLargestPossibleRegion())
				std::cout<< "Regionen passen nicht" << std::endl;


			/* COPY DEPTH, VIDEO, and SKELETON data from BUFFER into OUTPUT */

			float scaleX = ((float) m_DepthFrameW) / m_RGBFrameW;
			float scaleY = ((float) m_DepthFrameH) / m_RGBFrameH;

			// Copy the video buffer
			for(int i = 0 ; i < m_RGBFrameH * m_RGBFrameW ; i++, ++RGBDataBufferTarget,++RGBDataBufferSource)
			{

				if (!m_Calibrated)
				{
					RGBDataBufferSource = RGBDataBufferSourceBegin++;
				}
				else 
				{
					std::pair<float,float> tmpPair = m_MappingTable[i];

					float offsetX = tmpPair.first + 0.5;
					float offsetY = tmpPair.second + 0.5;

					int tmpY = offsetY;
					int offset =  offsetX;
					offset += m_RGBFrameW * tmpY;

					RGBDataBufferSource = RGBDataBufferSourceBegin + offset;
				}
					

				(*RGBDataBufferTarget)[0] = (*RGBDataBufferSource)[0];
				(*RGBDataBufferTarget)[1] = (*RGBDataBufferSource)[1];
				(*RGBDataBufferTarget)[2] = (*RGBDataBufferSource)[2];

			}

#if 0 // write RGB images
	// allocate memory for new images
	typedef itk::RGBPixel<unsigned char>		ITKRGBPixelType;
	typedef itk::Image<ITKRGBPixelType, 2u>		ITKRGBImageType;
	typedef ITKRGBImageType::Pointer			ITKRGBImagePointer;

	ITKRGBImagePointer m_ITKRGBImage = ITKRGBImageType::New();
	ITKRGBImagePointer m_ITKRGBImageOriginal = ITKRGBImageType::New();

	m_ITKRGBImage->SetRegions(CastedOutput->GetRGBImage()->GetLargestPossibleRegion());
	m_ITKRGBImageOriginal->SetRegions(CastedOutput->GetRGBImage()->GetLargestPossibleRegion());
	m_ITKRGBImage->Allocate();
	m_ITKRGBImageOriginal->Allocate();
	
	// RTIK RGB image iterator
	typedef ritk::RGBRImageUCF2					RGBRImageType;
	typedef itk::ImageRegionIterator<RGBRImageType::RGBImageType> RGBRImageIteratorType;
	RGBRImageIteratorType     itRGB_RITK  (CastedOutput->GetRGBImage(),    CastedOutput->GetRGBImage()->GetLargestPossibleRegion());	
	RGBRImageIteratorType     itRGB_Original  (m_ImageBuffer->GetRGBImage(),    m_ImageBuffer->GetRGBImage()->GetLargestPossibleRegion());	
	
	// ITK RGB image iterator
	typedef itk::ImageRegionIterator<ITKRGBImageType> RGBITKIteratorType;
	RGBITKIteratorType itRGB_ITK (m_ITKRGBImage, m_ITKRGBImage->GetRequestedRegion());
	RGBITKIteratorType itRGBOrig_ITK (m_ITKRGBImageOriginal, m_ITKRGBImageOriginal->GetRequestedRegion());


	// create itkRGBImage and calculate simple Grayscale image
	int i = 0;
	for (itRGB_RITK.GoToBegin(), itRGBOrig_ITK.GoToBegin(), itRGB_ITK.GoToBegin(), itRGB_Original.GoToBegin(); 
		!itRGB_RITK.IsAtEnd(); ++itRGB_RITK, itRGB_ITK++, i++, itRGB_Original++, itRGBOrig_ITK++)
	{

		itRGB_ITK.Value()[0] = itRGB_RITK.Value()[0];
		itRGB_ITK.Value()[1] = itRGB_RITK.Value()[1];
		itRGB_ITK.Value()[2] = itRGB_RITK.Value()[2];

		itRGBOrig_ITK.Value()[0] = itRGB_Original.Value()[0];
		itRGBOrig_ITK.Value()[1] = itRGB_Original.Value()[1];
		itRGBOrig_ITK.Value()[2] = itRGB_Original.Value()[2];
		
	}
	std::cout << i << " pixel uebertragen zum Plot" << std::endl;

		// write RGB image
	typedef itk::ImageFileWriter<itk::Image<itk::RGBPixel<unsigned char>, 2u>> RGBWriterType;
	RGBWriterType::Pointer rgbWriter = RGBWriterType::New();

	rgbWriter->SetFileName("CoilDetection/MappedRGB.png");
	rgbWriter->SetInput(m_ITKRGBImage);
	rgbWriter->Update();

	rgbWriter->SetFileName("CoilDetection/OriginalRGB.png");
	rgbWriter->SetInput(m_ITKRGBImageOriginal);
	rgbWriter->Update();

#endif

			// Copy the depth buffer and initialize the flag image
			this->GetOutput()->GetFlagImage()->FillBuffer(0);
			this->GetOutput()->GetRangeImage()->FillBuffer(0);

			// Range Image Source(Buffer) and Target (Output)
			RImageType::RangeImageType::PixelType *RangeDataBufferTarget = this->GetOutput()->GetRangeImage()->GetBufferPointer();
			RImageType::RangeImageType::PixelType *RangeDataBufferSource = this->m_ImageBuffer->GetRangeImage()->GetBufferPointer();

			// Flag Image Source(Buffer) and Target (Output)
			RImageType::FlagImageType::PixelType *FlagDataBufferTarget = this->GetOutput()->GetFlagImage()->GetBufferPointer();
			RImageType::FlagImageType::PixelType *FlagDataBufferSource = this->m_ImageBuffer->GetFlagImage()->GetBufferPointer();

			// Copy Depth and Flag Image
			for( int y = 0 ; y < m_DepthFrameH ; y++ )
			{
				for( int x = 0 ; x < m_DepthFrameW ; x++, FlagDataBufferSource++, FlagDataBufferTarget++, RangeDataBufferSource++, RangeDataBufferTarget++)
				{
					*RangeDataBufferTarget = *RangeDataBufferSource;
					*FlagDataBufferTarget = *FlagDataBufferSource;
				}
			}


#if 0 // write Range image for debug purpose
	// rescale image
				// Output Caster
	typedef float AccumulatorPixelType;
	typedef itk::Image< AccumulatorPixelType, 2 >	AccumulatorImageType;

	// rescale image
	typedef itk::RescaleIntensityImageFilter<AccumulatorImageType, itk::Image<unsigned char>> RescalerType;
	RescalerType::Pointer Rescaler = RescalerType::New();

		// write grayscale image
	Rescaler->SetInput(this->GetOutput()->GetRangeImage());
	Rescaler->SetOutputMinimum(0);
	Rescaler->SetOutputMaximum(255);
	Rescaler->Update();



	typedef itk::ImageFileWriter< itk::Image<unsigned char>> ucWriterType;
	ucWriterType::Pointer ucWriter = ucWriterType::New();

	ucWriter->SetFileName("CoilDetection/Range.png");
	ucWriter->SetInput(Rescaler->GetOutput());
	ucWriter->Update();
#endif

			// Undistort and Copy Skeleton Data if any
			RImageType::RangeImageType::PixelType *RangeDataBufferSourceBegin = this->m_ImageBuffer->GetRangeImage()->GetBufferPointer();
			RImageType::RangeImageType::PixelType *RangeDataJoint = NULL;

			int j;
			KinectSkeleton::JointTypes joint;

			for (int skelIDX = 0; skelIDX < 2; skelIDX++)
			{
				for (j = 0, joint = KinectSkeleton::SKEL_HIP_Center; joint != KinectSkeleton::SKEL_COUNT; joint = static_cast<KinectSkeleton::JointTypes> (joint + 1), j++)
				{	
					VF3 undistJointPos;
					if (skelIDX == 0)
					{
						undistJointPos = Undistort(m_JointPositions1[j]);
					}
					else
					{
						undistJointPos = Undistort(m_JointPositions2[j]);
					}

					CastedOutput->GetSkeleton()->SetJoint(KinectSkeleton::JointTypes(joint),undistJointPos,skelIDX);
					CastedOutput->GetSkeleton()->SetTrackingState(KinectSkeleton::JointTypes(joint), m_TrackingState[skelIDX*KinectSkeleton::SKEL_COUNT + j], skelIDX);
				}			
			}

			if (! m_JointPositions1[0][0] )
				CastedOutput->GetSkeleton()->SetJoint(KinectSkeleton::SKEL_HIP_Center,0,0,0,0);
			
			double fx = 5.2921508098293293e+02;
			double fy = 5.2556393630057437e+02;
			CastedOutput->SetConversionParameters(m_DepthFrameW/fx,m_DepthFrameH/fy);
			CastedOutput->SetTimeStamp(m_Clock->GetTimeStamp());

		}

		return;
	}

	
	//----------------------------------------------------------------------------
	template<class TRImage, class TKinectImage>
	VF3
	KinectSDKSourcePluginSource<TRImage, TKinectImage>
	::Undistort(VF3 InputPoint)
	{
		double X2Z = m_DepthFrameW/5.2921508098293293e+02;
		double Y2Z = m_DepthFrameH/5.2556393630057437e+02;
		VF3 OutputPoint;

		// The range value
		float z = InputPoint(2); // tex2D(InputPoint(2), InputPoint(0), InputPoint(1));
		// MS SDK depth value is given in meters, we need milimeters
		z *= 1000;

		// The corresponding x,y,z triple
		if ( z < 500.f )
			z = sqrtf(-1.0f);

		// calculate and set world coordinates
		float fNormalizedX = InputPoint(0) / m_DepthFrameW - 0.5f; // check for float
		OutputPoint(0) = fNormalizedX * z * X2Z;

		float fNormalizedY = 0.5f - InputPoint(1) / m_DepthFrameH;
		OutputPoint(1) = fNormalizedY * z * Y2Z;
		
		OutputPoint(2) = z;

		return(OutputPoint);
	}
