#include "KinectNITERImageSource.h"

#define XNSafeCall(ret)													\
{																		\
	if ( ret != XN_STATUS_OK )											\
		itkExceptionMacro(<<"XN error: " << xnGetStatusString(ret));	\
}


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

	m_TrackingStarted = false;
	m_CalibrationRequested = false;

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

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


//----------------------------------------------------------------------------
template<class TRImage, class TKinectImage>
KinectNITERImageSource<TRImage, TKinectImage>
::~KinectNITERImageSource()
{
	if ( m_Connected )
		Disconnect();
}


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

	// Set all regions (adjust to your needs)
	typename RImageType::SizeType ImageSize;
	ImageSize[0] = 640;
	ImageSize[1] = 480;
	typename RImageType::IndexType Index;
	Index[0] = 0;
	Index[1] = 0;
	typename RImageType::RegionType Region;
	Region.SetIndex(Index);
	Region.SetSize(ImageSize);
	this->GetOutput()->SetRegions(Region);

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

	// The correct output
	CastOutput(RImageType,KinectImageType)
	CastedOutput->GetRGBImage()->SetRegions(Region);
}


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

	// Initialize the OpenNI status 
	XNSafeCall(m_Context.Init());

	// Create a depth map generator and set its resolution
	XNSafeCall(m_DepthGenerator.Create(m_Context));
	XnMapOutputMode DepthMode;
	m_DepthGenerator.GetMapOutputMode(DepthMode);
	DepthMode.nXRes = xn::Resolution((XnResolution)XN_RES_VGA).GetXResolution();
	DepthMode.nYRes = xn::Resolution((XnResolution)XN_RES_VGA).GetYResolution();
	XNSafeCall(m_DepthGenerator.SetMapOutputMode(DepthMode));
	{
		XnUInt32 NumModes = 10;
		XnMapOutputMode *SupportedModes = new XnMapOutputMode[NumModes];
		XNSafeCall(m_DepthGenerator.GetSupportedMapOutputModes(SupportedModes, NumModes));
		for ( unsigned int i = 0; i < NumModes; i++ )
		{
			std::cout << "DepthModes #" << i << std::endl;
			std::cout << "	Nx=" << SupportedModes[i].nXRes << std::endl;
			std::cout << "	Ny=" << SupportedModes[i].nYRes << std::endl;
			std::cout << "	FPS=" << SupportedModes[i].nFPS << std::endl;
		}
		delete[] SupportedModes;
	}

	// Create an image generator and set its resolution
#ifndef USEIR
	XNSafeCall(m_ImageGenerator.Create(m_Context));
	XnMapOutputMode ImageMode;
	m_ImageGenerator.GetMapOutputMode(ImageMode);
	ImageMode.nXRes = xn::Resolution((XnResolution)XN_RES_VGA).GetXResolution();
	ImageMode.nYRes = xn::Resolution((XnResolution)XN_RES_VGA).GetYResolution();
	XNSafeCall(m_ImageGenerator.SetMapOutputMode(ImageMode));
	{
		XnUInt32 NumModes = 10;
		XnMapOutputMode *SupportedModes = new XnMapOutputMode[NumModes];
		XNSafeCall(m_ImageGenerator.GetSupportedMapOutputModes(SupportedModes, NumModes));
		for ( unsigned int i = 0; i < NumModes; i++ )
		{
			std::cout << "ImageModes #" << i << std::endl;
			std::cout << "	Nx=" << SupportedModes[i].nXRes << std::endl;
			std::cout << "	Ny=" << SupportedModes[i].nYRes << std::endl;
			std::cout << "	FPS=" << SupportedModes[i].nFPS << std::endl;
		}
		delete[] SupportedModes;
	}
#endif

#ifdef USEIR
	// Create the IR generator and set its resolution
	XNSafeCall(m_IRGenerator.Create(m_Context));
	XnMapOutputMode IRMode;
	m_IRGenerator.GetMapOutputMode(IRMode);
	IRMode.nXRes = XN_VGA_X_RES;
	IRMode.nYRes = XN_VGA_Y_RES;
	IRMode.nFPS = 30;
	XNSafeCall(m_IRGenerator.SetMapOutputMode(IRMode));

	XnUInt32 NumModes = 10;
	XnMapOutputMode *SupportedModes = new XnMapOutputMode[NumModes];
	XNSafeCall(m_IRGenerator.GetSupportedMapOutputModes(SupportedModes, NumModes));
	for ( unsigned int i = 0; i < NumModes; i++ )
	{
		std::cout << "Mode #" << i << std::endl;
		std::cout << "	Nx=" << SupportedModes[i].nXRes << std::endl;
		std::cout << "	Ny=" << SupportedModes[i].nYRes << std::endl;
		std::cout << "	FPS=" << SupportedModes[i].nFPS << std::endl;
	}
	delete[] SupportedModes;
#endif

	// Create a user generator
	XNSafeCall(m_UserGenerator.Create(m_Context));
		
	// Set skeleton profile
	if (!m_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
		printf("Supplied user generator doesn't support skeleton\n");
	m_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);


	// Camera registration
	if ( m_DepthGenerator.IsCapabilitySupported(XN_CAPABILITY_ALTERNATIVE_VIEW_POINT) )
	{
		XNSafeCall(m_DepthGenerator.GetAlternativeViewPointCap().SetViewPoint(m_ImageGenerator));
	}
	else
	{
		std::cout << "Alternative view point not supported by the depth generator..." << std::endl;
	}
#ifdef USEIR
	if ( m_IRGenerator.IsCapabilitySupported(XN_CAPABILITY_ALTERNATIVE_VIEW_POINT) )
	{
		//XNSafeCall(m_IRGenerator.GetAlternativeViewPointCap().SetViewPoint(m_DepthGenerator));
	}
	else
	{
		std::cout << "Alternative view point not supported by the IR generator..." << std::endl;
	}
#endif

	// Mirror data
	XNSafeCall(m_Context.SetGlobalMirror(!m_Context.GetGlobalMirror()));

	// Start data generation
	XNSafeCall(m_Context.StartGeneratingAll());

	// Update the connected flag
	m_Connected = true;
}


//----------------------------------------------------------------------------
template<class TRImage, class TKinectImage>
void
KinectNITERImageSource<TRImage, TKinectImage>
::Disconnect()
{
	if ( !m_Connected )
		itkExceptionMacro(<<"Disconnect called but not connected!");

	XNSafeCall(m_Context.StopGeneratingAll());
	m_Connected = false;
}


//----------------------------------------------------------------------------
template<class TRImage, class TKinectImage>
void
KinectNITERImageSource<TRImage, TKinectImage>
::GenerateRImage()
{
	// Sanity check
	if ( !m_Connected )
		itkExceptionMacro(<<"Not connected!");

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

	// The correct output
	CastOutput(RImageType,KinectImageType)

	// Wait for an update
	XNSafeCall(m_Context.WaitAndUpdateAll());


	// Skeleton readout
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	m_UserGenerator.GetUsers(aUsers, nUsers);

	if ( nUsers == 0 )
	{
		m_TrackingStarted = false;
		m_CalibrationRequested = false;
	}

	// Create new skeleton
	KinectSkeleton::Pointer Skel = KinectSkeleton::New();

	// Update the skeleton for all users
	for ( unsigned int i = 0; i < nUsers; ++i )
	{
		// Center of mass
		XnPoint3D CoM;
		m_UserGenerator.GetCoM(aUsers[i], CoM);
		//CastedOutput->m_Skeleton->SetCoM(CoM.X, CoM.Y, CoM.Z);
		
		if(!m_CalibrationRequested)
		{
			m_UserGenerator.GetSkeletonCap().RequestCalibration(aUsers[i], TRUE);
			m_CalibrationRequested = true;
		}
		else
		{
			if (m_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
			{
				// Tracking
				//std::cout << "tracking" << std::endl;
			}
			else if (m_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
			{
				// Calibrating
				//std::cout << "calibrating" << std::endl;
			}
			else if (m_UserGenerator.GetSkeletonCap().IsCalibrated(aUsers[i]))
			{
				//std::cout << "is calibrated" << std::endl;
				if(!m_TrackingStarted)
				{
					m_UserGenerator.GetSkeletonCap().StartTracking(aUsers[i]);
					m_TrackingStarted = true;
				}
			}

			if(m_TrackingStarted)
			{
				//std::cout << "Active user #" << i << std::endl; // ": " << m_UserGenerator.GetSkeletonCap().IsJointActive(XN_SKEL_RIGHT_ELBOW) << std::endl;
				//std::cout << "Available: " << m_UserGenerator.GetSkeletonCap().IsJointAvailable(XN_SKEL_RIGHT_ELBOW) << std::endl;
				
				XnSkeletonJointPosition JointPosition;
				for ( unsigned int j = 1; j <= 24; j++ )
				{
					m_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XnSkeletonJoint(j), JointPosition);
				
					// Conversion from NITE
					bool valid = true;
					KinectSkeleton::JointTypes Joint;
					switch ( j )
					{
						case 1:
							Joint = KinectSkeleton::SKEL_HEAD;
							break;
						case 2:
							Joint = KinectSkeleton::SKEL_NECK;
							break;
						case 3:
							Joint = KinectSkeleton::SKEL_TORSO;
							break;
						case 4:
							Joint = KinectSkeleton::SKEL_HIP_Center;
							break;
						case 5:
							valid = false;
							break;
						case 6:
							Joint = KinectSkeleton::SKEL_LEFT_SHOULDER;
							break;
						case 7:
							Joint = KinectSkeleton::SKEL_LEFT_ELBOW;
							break;
						case 8:
							Joint = KinectSkeleton::SKEL_LEFT_WRIST;
							break;
						case 9:
							Joint = KinectSkeleton::SKEL_LEFT_HAND;
							break;
						case 10:
							valid = false;
							break;
						case 11:
							valid = false;
							break;
						case 12:
							Joint = KinectSkeleton::SKEL_RIGHT_SHOULDER;
							break;
						case 13:
							Joint = KinectSkeleton::SKEL_RIGHT_ELBOW;
							break;
						case 14:
							Joint = KinectSkeleton::SKEL_RIGHT_WRIST;
							break;
						case 15:
							Joint = KinectSkeleton::SKEL_RIGHT_HAND;
							break;
						case 16:
							valid = false;
							break;
						case 17:
							Joint = KinectSkeleton::SKEL_LEFT_HIP;
							break;
						case 18:
							Joint = KinectSkeleton::SKEL_LEFT_KNEE;
							break;
						case 19:
							Joint = KinectSkeleton::SKEL_LEFT_ANKLE;
							break;
						case 20:
							Joint = KinectSkeleton::SKEL_LEFT_FOOT;
							break;
						case 21:
							Joint = KinectSkeleton::SKEL_RIGHT_HIP;
							break;
						case 22:
							Joint = KinectSkeleton::SKEL_RIGHT_KNEE;
							break;
						case 23:
							Joint = KinectSkeleton::SKEL_RIGHT_ANKLE;
							break;
						case 24:
							Joint = KinectSkeleton::SKEL_RIGHT_FOOT;
							break;
						default:
							valid = false;
					}

					// Only valid joints
					if ( valid )
					{
						Skel->SetJoint(Joint, JointPosition.position.X, JointPosition.position.Y, JointPosition.position.Z, i);
					}
				}
			}
		}
	}

	// Set the skeleton to our output
	CastedOutput->SetSkeleton(Skel);

	// Get the meta data
	xn::DepthMetaData DepthMD;
	m_DepthGenerator.GetMetaData(DepthMD);
#ifndef USEIR
	xn::ImageMetaData ImageMD;
	m_ImageGenerator.GetMetaData(ImageMD);
#endif
#ifdef USEIR
	xn::IRMetaData IRMD;
	m_IRGenerator.GetMetaData(IRMD);
#endif

	// Copy the depth data and initialize the flag image
	this->GetOutput()->GetFlagImage()->FillBuffer(0);
	this->GetOutput()->GetRangeImage()->FillBuffer(0);
	const XnDepthPixel* DepthData = DepthMD.Data();
	typename RImageType::RangeImageType::PixelType *RangeDataBuffer = this->GetOutput()->GetRangeImage()->GetBufferPointer();
	typename RImageType::FlagImageType::PixelType *FlagDataBuffer = this->GetOutput()->GetFlagImage()->GetBufferPointer();
	for ( XnUInt y = 0; y < DepthMD.YRes(); y++ )
	{
		for ( XnUInt x = 0; x < DepthMD.XRes(); x++, ++DepthData, ++RangeDataBuffer, ++FlagDataBuffer )
		{
			*RangeDataBuffer = *DepthData;
			if ( *RangeDataBuffer != 0 )
				*FlagDataBuffer = RImageType::VALID;
		}
	}
	//this->GetOutput()->GetFlagImage()->Modified();
	//this->GetOutput()->GetRangeImage()->Modified();

	// Convert and copy the color data
#ifndef USEIR
	const XnRGB24Pixel* ImageData = ImageMD.RGB24Data();
	typename KinectImageType::RGBImageType::PixelType *RGBDataBuffer = CastedOutput->GetRGBImage()->GetBufferPointer();
	for ( XnUInt y = 0; y < ImageMD.YRes(); y++ )
	{
		for ( XnUInt x = 0; x < ImageMD.XRes(); x++, ++ImageData,++RGBDataBuffer )
		{
			(*RGBDataBuffer)[0] = (*ImageData).nRed;
			(*RGBDataBuffer)[1] = (*ImageData).nGreen;
			(*RGBDataBuffer)[2] = (*ImageData).nBlue;
		}
	}
	//this->GetOutput()->GetRGBImage()->Modified();
#endif
#ifdef USEIR
	const XnIRPixel* ImageData = IRMD.Data();
	typename RImageType::GrayscaleImageType::PixelType *GrayscaleDataBuffer = this->GetOutput()->GetGrayscaleImage()->GetBufferPointer();
	typename KinectImageType::RGBImageType::PixelType *RGBDataBuffer = CastedOutput->GetRGBImage()->GetBufferPointer();
	for ( XnUInt y = 0; y < IRMD.YRes(); y++ )
	{
		for ( XnUInt x = 0; x < IRMD.XRes(); x++, ++ImageData, ++GrayscaleDataBuffer,++RGBDataBuffer )
		{
			*GrayscaleDataBuffer = *ImageData;
		}
	}
#endif

	/** Conversion from projective to reald world is done as:  X_RW = (X_proj / X_res - 1/2) * Z * x_to_z
	 * \code
	 *	 XnDouble fNormalizedX = (aProjective[i].X / outputMode.nXRes - 0.5);
	 *	 aRealWorld[i].X = (XnFloat)(fNormalizedX * aProjective[i].Z * fXToZ);
	 *
	 *	 XnDouble fNormalizedY = (0.5 - aProjective[i].Y / outputMode.nYRes);
	 *	 aRealWorld[i].Y = (XnFloat)(fNormalizedY * aProjective[i].Z * fYToZ);
	 *
	 *	 aRealWorld[i].Z = aProjective[i].Z;
	 * \endcode
	 */

	// Safe the parameters used to compute the world coords out of the range data
	/*XnPoint3D RealWorld;
	XnPoint3D Projective;
	Projective.X = 1;
	Projective.Y = 1;
	Projective.Z = 1;
	m_DepthGenerator.ConvertProjectiveToRealWorld(1, &Projective, &RealWorld);
	float XToZ = RealWorld.X / (((Projective.X/DepthMD.XRes())-0.5)*Projective.Z);
	float YToZ = RealWorld.Y / ((0.5-(Projective.Y/DepthMD.YRes()))*Projective.Z);
	CastedOutput->SetConversionParameters(XToZ,YToZ);*/

	
	// Safe the parameters used to compute the world coords out of the range data
	// We do NOT operate on the range intrinsic as the range data is registered to the RGB camera
	XnMapOutputMode ImageMode;
	m_ImageGenerator.GetMapOutputMode(ImageMode);
	XnDouble fx = 5.2921508098293293e+02;
    XnDouble fy = 5.2556393630057437e+02;
	XnDouble fovx = 2 * atan(ImageMode.nXRes / (2 * fx));
	XnDouble fovy = 2 * atan(ImageMode.nYRes / (2 * fy));
	CastedOutput->SetConversionParameters(tan(fovx/2)*2,tan(fovy/2)*2); // m_fRealWorldXtoZ = tan(FOV.fHFOV/2)*2; m_fRealWorldYtoZ = tan(FOV.fVFOV/2)*2;


	/*XnFieldOfView DepthFOV;
	m_DepthGenerator.GetFieldOfView(DepthFOV);
	std::cout << "Depth info" << std::endl;
	std::cout << "	FOVx=" << DepthFOV.fHFOV << std::endl;
	std::cout << "	FOVy=" << DepthFOV.fVFOV << std::endl;
	std::cout << "RGB info" << std::endl;
	std::cout << "	XRes=" << ImageMode.nXRes << std::endl;
	std::cout << "	YRes=" << ImageMode.nYRes << std::endl;
	std::cout << "	FOVx=" << fovx << std::endl;
	std::cout << "	FOVy=" << fovy << std::endl;
	std::cout << "	XToZ=" << (tan(fovx/2)*2) << " vs. " << XToZ << std::endl;
	std::cout << "	YToZ=" << (tan(fovy/2)*2) << " vs. " << YToZ << std::endl;*/

	CastedOutput->SetTimeStamp(m_Clock->GetTimeStamp());

	this->Modified();
	this->GetOutput()->Modified();
	m_Generating = false;
}


#if 0

XnUInt64 nZPD;
	nRetVal = GetIntProperty(XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE, nZPD);
	XN_IS_STATUS_OK(nRetVal);

	XnDouble fZPPS;
	nRetVal = GetRealProperty(XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE, fZPPS);
	XN_IS_STATUS_OK(nRetVal);

	m_FOV.fHFOV = 2*atan(fZPPS*XN_SXGA_X_RES/2/nZPD);
	m_FOV.fVFOV = 2*atan(fZPPS*XN_VGA_Y_RES*2/2/nZPD);

	m_fRealWorldXtoZ = tan(FOV.fHFOV/2)*2;
	m_fRealWorldYtoZ = tan(FOV.fVFOV/2)*2;

XnStatus XnSensorDepthGenerator::UpdateRealWorldTranslationData()
{
	XnStatus nRetVal = XN_STATUS_OK;

	XnUInt64 nZPD;
	nRetVal = GetIntProperty(XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE, nZPD);
	XN_IS_STATUS_OK(nRetVal);

	XnDouble fZPPS;
	nRetVal = GetRealProperty(XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE, fZPPS);
	XN_IS_STATUS_OK(nRetVal);

	m_FOV.fHFOV = 2*atan(fZPPS*XN_SXGA_X_RES/2/nZPD);
	m_FOV.fVFOV = 2*atan(fZPPS*XN_VGA_Y_RES*2/2/nZPD);

	nRetVal = m_fovChangedEvent.Raise();
	XN_IS_STATUS_OK(nRetVal);

	return (XN_STATUS_OK);
}

void xn::DepthPrivateData::OnFieldOfViewChanged()
{
	XnFieldOfView FOV;
	xnGetDepthFieldOfView(m_hNode, &FOV);

	m_fRealWorldXtoZ = tan(FOV.fHFOV/2)*2;
	m_fRealWorldYtoZ = tan(FOV.fVFOV/2)*2;
}

XN_C_API XnStatus xnConvertProjectiveToRealWorld(XnNodeHandle hInstance, XnUInt32 nCount, const XnPoint3D* aProjective, XnPoint3D* aRealWorld)
{
	XnStatus nRetVal = XN_STATUS_OK;
	
	XN_VALIDATE_INTERFACE_TYPE(hInstance, XN_HIERARCHY_TYPE_DEPTH_GENERATOR);

	/**
	 * X_RW = (X_proj / X_res - 1/2) * Z * x_to_z
	 */

	XnMapOutputMode outputMode;
	nRetVal = xnGetMapOutputMode(hInstance, &outputMode);
	XN_IS_STATUS_OK(nRetVal);

	xn::DepthPrivateData* pDepthPrivate = (xn::DepthPrivateData*)hInstance->pPrivateData;
	XnDouble fXToZ = pDepthPrivate->GetRealWorldXtoZ();
	XnDouble fYToZ = pDepthPrivate->GetRealWorldYtoZ();

	for (XnUInt32 i = 0; i < nCount; ++i)
	{
		XnDouble fNormalizedX = (aProjective[i].X / outputMode.nXRes - 0.5);
		aRealWorld[i].X = (XnFloat)(fNormalizedX * aProjective[i].Z * fXToZ);

		XnDouble fNormalizedY = (0.5 - aProjective[i].Y / outputMode.nYRes);
		aRealWorld[i].Y = (XnFloat)(fNormalizedY * aProjective[i].Z * fYToZ);

		aRealWorld[i].Z = aProjective[i].Z;
	}

	return (XN_STATUS_OK);
}


	
#endif
	


