#include "KinectSDKImage.h"


#include <itkImageRegionIterator.h>
#include <itkSmartPointer.h>

#include "ritkCudaRegularMemoryImportImageContainer.h"


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
typename KinectSDKImage<TRealType, TRGB, NDimension>::R2WCFilterType::Pointer
KinectSDKImage<TRealType, TRGB, NDimension>
::s_R2WCFilter = NULL;


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
KinectSDKImage<TRealType, TRGB, NDimension>
::KinectSDKImage()
{
	// somewhere there is a additional reference count set...
	if( !s_R2WCFilter )
		s_R2WCFilter = R2WCFilterType::New();
	
	// Append the skeleton payload to the internal payload vector
	AppendGenericPayloadContainerMacro(m_SkeletonDataPayloadIdx, Skeleton::New(), "SkeletonData");
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
KinectSDKImage<TRealType, TRGB, NDimension>
::~KinectSDKImage()
{
	//// why do I 
	//if( s_R2WCFilter )
	//	s_R2WCFilter.~SmartPointer();
}


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


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


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


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

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


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


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


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


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


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


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


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


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


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


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

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

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


	// Copy Skeleton data for both skeletons (cannot set const skeleton data)
	SkeletonConstPointer InputSkeleton = Image->GetSkeleton();
	SkeletonPointer NewSkeleton = SkeletonType::New();

	for (int i = 0; i < 2; i++)
	{
		for (Skeleton::JointTypes joint = Skeleton::SKEL_HIP_Center; joint != Skeleton::SKEL_COUNT; joint = static_cast<Skeleton::JointTypes> (joint + 1))
		{
			NewSkeleton->SetJoint(joint, InputSkeleton->GetJoint(joint, i), i);		
			NewSkeleton->SetTrackingState(joint, InputSkeleton->GetTrackingState(joint, i),i);
		}
	}

	this->SetSkeleton(NewSkeleton);
	
}


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

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

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

	SkeletonPointer NewSkeleton = SkeletonType::New();
	SkeletonConstPointer InputSkeleton = Image->GetSkeleton(); 


	if ( !InputSkeleton )
	{
		itkExceptionMacro(<<"An exception occured during casting!");
	}

	// Copy of Skeleton data (# joints = 20)
	for (int i = 0; i < 2; i++)
	{
		for (Skeleton::JointTypes joint = Skeleton::SKEL_HIP_Center; joint != Skeleton::SKEL_COUNT; joint = static_cast<Skeleton::JointTypes> (joint + 1))
		{
			NewSkeleton->SetJoint(joint, InputSkeleton->GetJoint(joint, i),i);		
			NewSkeleton->SetTrackingState(joint, InputSkeleton->GetTrackingState(joint, i),i);
		}
	}
	

	this->SetSkeleton(NewSkeleton);

	
}


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


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


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


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


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

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

	cudaThreadSynchronize();

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



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

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



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


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