#include "KinectImage.h"
#include "KinectSkeletonPayloadContainer.h"


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
KinectImage<TRealType, TRGB, NDimension>
::KinectImage()
{
	// Instantiate our range/depth to world coords filter
	m_R2WCFilter = R2WCFilterType::New();
	
	// Append the skeleton payload to the internal payload vector. This will eventually create a valid skeleton object
	m_SkeletonDataPayloadIdx = this->AppendPayloadContainer(KinectSkeletonPayloadContainer::New());
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
KinectImage<TRealType, TRGB, NDimension>
::~KinectImage()
{
}


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

	// The kinect image to copy
	ConstPointer Image = NULL;

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

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

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


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
KinectImage<TRealType, TRGB, NDimension>
::Graft(const DataObject *data)
{
	try
	{
	// Call the superclass' method first
	Superclass::Graft(data);
	}
	catch ( itk::ExceptionObject &e )
	{
		std::cerr << e << std::endl;
	}
	
	// The range image to graft
	ConstPointer Image = NULL;

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

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


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
KinectImage<TRealType, TRGB, 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 KinectImage
	try
	{
		Image = dynamic_cast<const Self*>(data);
	}
	catch( ... )
	{
		itkExceptionMacro(<<"An exception occured during casting!");
	}

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


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


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


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


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


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
KinectImage<TRealType, TRGB, NDimension>
::ComputeRangeToWorldCoords()
{
	// Perform transformation
	//m_R2WCFilter->SetNumberOfThreads(4);
	//m_R2WCFilter->SetConversionParameters(m_XToZ,m_YToZ);
	m_R2WCFilter->SetConversionParameters(this->GetRangeImage()->GetRequestedRegion().GetSize()[0]/this->m_CameraParameters[ritk::CameraParameters::FX],this->GetRangeImage()->GetRequestedRegion().GetSize()[1]/this->m_CameraParameters[ritk::CameraParameters::FY]);
	m_R2WCFilter->SetInput(this->GetRangeImage());
	m_R2WCFilter->SetFlagImageInput(this->GetFlagImage());
	m_R2WCFilter->GetOutput()->SetRequestedRegion(this->GetRangeImage()->GetRequestedRegion());
	try
	{
		m_R2WCFilter->Update();
	}
	catch ( itk::ExceptionObject &e )
	{
		std::cerr << e << std::endl;
	}

	// Set the image and update MTimes
	this->SetWorldCoordImage(m_R2WCFilter->GetOutput());
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
typename KinectImage<TRealType, TRGB, NDimension>::SkeletonConstPointer
KinectImage<TRealType, TRGB, NDimension>
::GetSkeleton() const
{
	return (SkeletonConstPointer)static_cast<KinectSkeleton* const>((itk::DataObject* const)this->GetPayloadContainer(m_SkeletonDataPayloadIdx)->GetPayload());
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
typename KinectImage<TRealType, TRGB, NDimension>::SkeletonPointer
KinectImage<TRealType, TRGB, NDimension>
::GetSkeleton()
{
	return (SkeletonPointer)static_cast<KinectSkeleton*>((itk::DataObject* const)this->GetPayloadContainer(m_SkeletonDataPayloadIdx)->GetPayload());
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
KinectImage<TRealType, TRGB, NDimension>
::SetSkeleton(SkeletonPointer Skel)
{
	this->GetPayloadContainer(m_SkeletonDataPayloadIdx)->SetPayload(Skel);
	this->Modified();
}
