#include "KinectImageDepthToWorldFilter.h"

#include <itkImageRegionIterator.h>


//----------------------------------------------------------------------------
template<class TRangeImage, class TFlagImage, class TWorldCoordImage>
KinectImageDepthToWorldFilter<TRangeImage,TFlagImage,TWorldCoordImage>
::KinectImageDepthToWorldFilter()
{
	m_XToZ = 0;
	m_YToZ = 0;

	this->SetNumberOfThreads(2);
}


//----------------------------------------------------------------------------
template<class TRangeImage, class TFlagImage, class TWorldCoordImage>
KinectImageDepthToWorldFilter<TRangeImage,TFlagImage,TWorldCoordImage>
::~KinectImageDepthToWorldFilter()
{
}


//----------------------------------------------------------------------------
template<class TRangeImage, class TFlagImage, class TWorldCoordImage>
void
KinectImageDepthToWorldFilter<TRangeImage,TFlagImage,TWorldCoordImage>
::SetConversionParameters(float XToZ, float YToZ)
{
	m_XToZ = XToZ;
	m_YToZ = YToZ;
}


//----------------------------------------------------------------------------
template<class TRangeImage, class TFlagImage, class TWorldCoordImage>
void
KinectImageDepthToWorldFilter<TRangeImage,TFlagImage,TWorldCoordImage>
::GetConversionParameters(float *XToZ, float *YToZ)
{
	*XToZ = m_XToZ;
	*YToZ = m_YToZ;
}


//----------------------------------------------------------------------------
template<class TRangeImage, class TFlagImage, class TWorldCoordImage>
void
KinectImageDepthToWorldFilter<TRangeImage,TFlagImage,TWorldCoordImage>
::BeforeThreadedGenerateData()
{
	// Sanity check
	if ( fabs(m_XToZ) < 1e-3 || fabs(m_YToZ) < 1e-3 )
		itkExceptionMacro(<<"Invalid conversion parameters detected!");
}


//----------------------------------------------------------------------------
template<class TRangeImage, class TFlagImage, class TWorldCoordImage>
void
KinectImageDepthToWorldFilter<TRangeImage,TFlagImage,TWorldCoordImage>
::ThreadedGenerateData(const typename OutputImageType::RegionType &OutputRegionForThread,int threadId)
{
#if 0
	// Set up iterators
	typedef itk::ImageRegionConstIterator<InputImageType> RangeImageIteratorType;
	typedef itk::ImageRegionIterator<OutputImageType> WorldCoordIteratorType;
	RangeImageIteratorType itRange(this->GetInput(), OutputRegionForThread);
	WorldCoordIteratorType itWC(this->GetOutput(), OutputRegionForThread);

	//InputImageType::SizeType Resolution = GetInput()->GetRequestedRegion().GetSize();

	float Resolution[2];
	Resolution[0] = 640.f;
	Resolution[1] = 480.f;

	// Convert
	for ( itRange.GoToBegin(),itWC.GoToBegin(); !itRange.IsAtEnd(); ++itRange,++itWC )
	{
		// Get index
		typename InputImageType::IndexType Index = itWC.GetIndex();

		float fNormalizedX = (Index[0] / Resolution[0] - 0.5);
		itWC.Value()[0] = (float)(fNormalizedX * itRange.Value() * m_XToZ);

		float fNormalizedY = (0.5 - Index[1] / Resolution[1]);
		itWC.Value()[1] = (float)(fNormalizedY * itRange.Value() * m_YToZ);

		itWC.Value()[2] = itRange.Value();

		/*XnPoint3D Projective = xnCreatePoint3D(Index[0], Index[1], itRange.Value());
		XnPoint3D RealWorld;
		m_DepthGenerator->ConvertProjectiveToRealWorld(1,&Projective,&RealWorld);

		if ( fabs(itWC.Value()[0]-RealWorld.X) > 1e-3 )
			std::cout << "Mismatch in x-coord detected: " << itWC.Value()[0] << " vs. " << RealWorld.X << std::endl;
		if ( fabs(itWC.Value()[1]-RealWorld.Y) > 1e-3 )
			std::cout << "Mismatch in y-coord detected: " << itWC.Value()[1] << " vs. " << RealWorld.Y << std::endl;
		if ( fabs(itWC.Value()[2]-RealWorld.Z) > 1e-3 )
			std::cout << "Mismatch in z-coord detected: " << itWC.Value()[2] << " vs. " << RealWorld.Z << std::endl;*/
	}
#else
	// For convenience
	typename InputImageType::SizeType Resolution = this->GetInput()->GetRequestedRegion().GetSize();
	float invResX = 1.f / Resolution[0];
	float invResY = 1.f / Resolution[1];

	// Initialize pointers
	const typename InputImageType::PixelType *RangePtr = this->GetInput()->GetBufferPointer() + this->GetInput()->ComputeOffset(OutputRegionForThread.GetIndex());
	typename OutputImageType::PixelType *WCPtr = this->GetOutput()->GetBufferPointer() + this->GetOutput()->ComputeOffset(OutputRegionForThread.GetIndex());

	// Loop
	for ( unsigned int y = OutputRegionForThread.GetIndex()[1]; y < OutputRegionForThread.GetIndex()[1]+OutputRegionForThread.GetSize()[1]; y++ )
	{
		for ( unsigned int x = OutputRegionForThread.GetIndex()[0]; x < OutputRegionForThread.GetIndex()[0]+OutputRegionForThread.GetSize()[0]; x++ )
		{
			// Get the range value
			typename InputImageType::PixelType Range = *RangePtr;

			// Comput x-coordinate
			typename InputImageType::PixelType  fNormalizedX = (x * invResX - 0.5);
			(*WCPtr)[0] = (typename InputImageType::PixelType )(fNormalizedX * Range * m_XToZ);

			// Compute y-coordinate
			typename InputImageType::PixelType  fNormalizedY = (0.5 - y * invResY);
			(*WCPtr)[1] = (typename InputImageType::PixelType )(fNormalizedY * Range * m_YToZ);

			// Store z-component
			(*WCPtr)[2] = Range;

			// Update pointers
			RangePtr++;
			WCPtr++;
		}
	}
#endif
}


//----------------------------------------------------------------------------
template<class TRangeImage, class TFlagImage, class TWorldCoordImage>
void
KinectImageDepthToWorldFilter<TRangeImage,TFlagImage,TWorldCoordImage>
::AfterThreadedGenerateData()
{
}
