#include "KinectPoseImage.h"

#include <itkImageRegionIterator.h>

template<class TRealType, unsigned int NDimension>
KinectPoseImage<TRealType, NDimension>
::KinectPoseImage()
{
	m_R2WCFilter = R2WCFilterType::New();
}


template<class TRealType, unsigned int NDimension>
KinectPoseImage<TRealType, NDimension>
::~KinectPoseImage()
{
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetRegions(RegionType region)
{
	Self::SetLargestPossibleRegion(region);
	Self::SetBufferedRegion(region);
	Self::SetRequestedRegion(region);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetRegions(SizeType size)
{
	RegionType region; 
	region.SetSize(size);
	Self::SetRegions(region);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetLargestPossibleRegion(const RegionType &Region)
{
	Superclass::SetLargestPossibleRegion(Region);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetRequestedRegion(const RegionType &Region)
{
	Superclass::SetRequestedRegion(Region);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetBufferedRegion(const RegionType &Region)
{
	Superclass::SetBufferedRegion(Region);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::Allocate()
{
	// Call superclass' implementation
	Superclass::Allocate();
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetSpacing(const SpacingType &Spacing)
{
	Superclass::SetSpacing(Spacing);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetSpacing(const double Spacing[ImageDimension] )
{
	SpacingType s(Spacing);
	SetSpacing(s);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetSpacing(const float Spacing[ImageDimension] )
{
	itk::Vector<float, ImageDimension> sf(Spacing);
	SpacingType s;
	s.CastFrom(sf);
	SetSpacing(s);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetOrigin(const PointType &Origin)
{
	Superclass::SetOrigin(Origin);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetOrigin(const double Origin[ImageDimension])
{
	PointType p(Origin);
	this->SetOrigin(p);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetOrigin(const float Origin[ImageDimension])
{
	itk::Point<float, ImageDimension> of(Origin);
	PointType p;
	p.CastFrom(of);
	this->SetOrigin(p);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetDirection(const DirectionType Direction)
{
	Superclass::SetDirection(Direction);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::CopyInformation(const DataObject *data)
{
	// Call the superclass' method first
	Superclass::CopyInformation(data);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::Graft(const DataObject *data)
{
	// Call the superclass' method first
	Superclass::Graft(data);
	
	// The range image to graft
	ConstPointer Image = NULL;

	// Check if we have a KinectPoseImage
	try
	{
		Image = dynamic_cast<const Self*>(data);
	}
	catch( ... )
	{
		itkExceptionMacro(<<"An exception occured during casting!");
	}

	// This is not a KinectPoseImage
	if ( !Image )
	{
		return;
	}

	// Copy conversion parameters
	m_XToZ = Image->m_XToZ;
	m_YToZ = Image->m_YToZ;

	m_DepthGenerator = Image->m_DepthGenerator;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::DeepCopy(const DataObject *data)
{
	// Call the superclass' method first
	Superclass::DeepCopy(data);
	
	// The range image to graft
	ConstPointer Image = NULL;

	// Check if we have a KinectPoseImage
	try
	{
		Image = dynamic_cast<const Self*>(data);
	}
	catch( ... )
	{
		itkExceptionMacro(<<"An exception occured during casting!");
	}

	// This is not a KinectPoseImage
	if ( !Image )
	{
		return;
	}

	// Copy conversion parameters
	m_XToZ = Image->m_XToZ;
	m_YToZ = Image->m_YToZ;

	m_DepthGenerator = Image->m_DepthGenerator;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::DisconnectPipeline()
{
	Superclass::DisconnectPipeline();
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::SetConversionParameters(float XToZ, float YToZ)
{
	m_XToZ = XToZ;
	m_YToZ = YToZ;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::GetConversionParameters(float *XToZ, float *YToZ)
{
	*XToZ = m_XToZ;
	*YToZ = m_YToZ;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::PrintSelf(std::ostream& os, itk::Indent indent) const
{
	os << "KinectPoseImage" << std::endl;
	Superclass::PrintSelf(os,indent);
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
KinectPoseImage<TRealType, NDimension>
::ComputeRangeToWorldCoords()
{
	// Initialize timer
	LONGLONG Frequency;
	QueryPerformanceFrequency((LARGE_INTEGER*)&Frequency);
	LONGLONG C1;
	QueryPerformanceCounter((LARGE_INTEGER*)&C1);

	// Convert
#if 0
	// For convenience
	float Resolution[2];
	Resolution[0] = 640.f;
	Resolution[1] = 480.f;

	// Allocate the world coordinate image
	GetWorldCoordImage()->Allocate();

	// Set up iterators
	typedef itk::ImageRegionConstIterator<RangeImageType> RangeImageIteratorType;
	typedef itk::ImageRegionIterator<WorldCoordImageType> WorldCoordIteratorType;
	RangeImageIteratorType itRange(GetRangeImage(),GetRangeImage()->GetRequestedRegion());
	WorldCoordIteratorType itWC(GetWorldCoordImage(), GetWorldCoordImage()->GetRequestedRegion());
	for ( itRange.GoToBegin(),itWC.GoToBegin(); !itRange.IsAtEnd(); ++itRange,++itWC )
	{
		// Get index
		RangeImageType::IndexType Index = itRange.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;*/
	}
#endif

#if 1
	// Allocate the world coordinate image
	GetWorldCoordImage()->Allocate();

	float Resolution[2];
	Resolution[0] = GetRangeImage()->GetBufferedRegion().GetSize()[0];
	Resolution[1] = GetRangeImage()->GetBufferedRegion().GetSize()[1];

	const RangeImageType::PixelType *RangePtr = GetRangeImage()->GetBufferPointer();
	WorldCoordImageType::PixelType *WCPtr = GetWorldCoordImage()->GetBufferPointer();
	float invResX = 1.f / Resolution[0];
	float invResY = 1.f / Resolution[1];
	for ( int y = 0; y < Resolution[1]; y++ )
	{
		for ( int x = 0; x < Resolution[0]; x++ )
		{
			// Get the range value
			RangeImageType::PixelType Range = *RangePtr;

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

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

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

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

#if 0
	// Perform transformation
	//m_R2WCFilter->SetNumberOfThreads(4);
	m_R2WCFilter->SetConversionParameters(m_XToZ,m_YToZ);
	m_R2WCFilter->SetInput(GetRangeImage());
	m_R2WCFilter->SetFlagImageInput(GetFlagImage());
	m_R2WCFilter->GetOutput()->SetRequestedRegion(this->GetRequestedRegion());
	m_R2WCFilter->Update();
	//GetRangeImage()->Modified();

	// Set the image and update MTimes
	this->SetWorldCoordImage(m_R2WCFilter->GetOutput());
	//this->GetWorldCoordImage()->DisconnectPipeline();
#endif


	LONGLONG C2;
	QueryPerformanceCounter((LARGE_INTEGER*)&C2);
	double Diff = (((double)(C2-C1))/((double)Frequency))*1000.0;
	//std::cout << "Elapsed time: " << Diff << std::endl;
}




#if 0
// Perform transformation
	//m_R2WCFilter->SetNumberOfThreads(4);
	m_R2WCFilter->SetConversionParameters(m_XToZ,m_YToZ);
	m_R2WCFilter->SetInput(GetRangeImage());
	m_R2WCFilter->SetFlagImageInput(GetFlagImage());
	m_R2WCFilter->GetOutput()->SetRequestedRegion(this->GetRequestedRegion());
	m_R2WCFilter->Update();
	//GetRangeImage()->Modified();

	// Set the image and update MTimes
	this->SetWorldCoordImage(m_R2WCFilter->GetOutput());
	this->GetWorldCoordImage()->DisconnectPipeline();
#endif






#if 0
	const RangeImageType::PixelType *RangePtr = GetRangeImage()->GetBufferPointer();
	WorldCoordImageType::PixelType *WCPtr = GetWorldCoordImage()->GetBufferPointer();
	float invResX = 1.f / Resolution[0];
	float invResY = 1.f / Resolution[1];
	for ( int y = 0; y < Resolution[1]; y++ )
	{
		for ( int x = 0; x < Resolution[0]; x++ )
		{
			// Get the range value
			RangeImageType::PixelType Range = *RangePtr;

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

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

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

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