#include "KinectSkeletonPayloadContainer.h"

//----------------------------------------------------------------------------
KinectSkeletonPayloadContainer::KinectSkeletonPayloadContainer()
{
	this->SetIdentifier("KinectSkeleton");
	this->SetPayload(KinectSkeleton::New());
}


//----------------------------------------------------------------------------
KinectSkeletonPayloadContainer::~KinectSkeletonPayloadContainer()
{
}


//----------------------------------------------------------------------------
void
KinectSkeletonPayloadContainer::SetPayload(PayloadType *payload)
{
	// Sanity check
	KinectSkeletonPayloadPointer test = dynamic_cast<KinectSkeletonPayloadType*>((KinectSkeletonPayloadType*)payload);
	if ( !test )
		itkExceptionMacro(<<"Could not cast payload to a Kinect skeleton!");

	// Set the payload via the superclass
	Superclass::SetPayload(payload);
}


//----------------------------------------------------------------------------
void
KinectSkeletonPayloadContainer::Graft(const DataObject *data)
{
	// Sanity check for correct image container payload type
	Self::ConstPointer KSC = dynamic_cast<const Self*>((const Self*)data);
	if ( !KSC )
		itkExceptionMacro(<<"Could not cast argument to Kinect skeleton payload container!");
	if ( KSC->GetIdentifier().compare(this->GetIdentifier()) != 0 )
		itkExceptionMacro(<<"Invalid payload identifiers detected: " << KSC->GetIdentifier() << " vs. " << this->GetIdentifier());

	// Only copy valid skeleton payloads
	KinectSkeletonPayloadPointer SourceSkeleton = dynamic_cast<KinectSkeletonPayloadType*>((KinectSkeletonPayloadType*)KSC->GetPayload());
	KinectSkeletonPayloadPointer TargetSkeleton = dynamic_cast<KinectSkeletonPayloadType*>((KinectSkeletonPayloadType*)this->GetPayload());
	if ( SourceSkeleton && TargetSkeleton )
	{
		//std::cout << "Grafting skeleton payload..." << std::endl;
		// Copy
		for ( int i = 0; i < 2; i++ )
		{
			for ( KinectSkeleton::JointTypes joint = KinectSkeleton::SKEL_HIP_Center; joint != KinectSkeleton::SKEL_COUNT; joint = static_cast<KinectSkeleton::JointTypes>(joint + 1) )
			{
				TargetSkeleton->SetJoint(joint, SourceSkeleton->GetJoint(joint, i), i);		
				TargetSkeleton->SetTrackingState(joint, SourceSkeleton->GetTrackingState(joint, i),i);
			}
		}
	}
}


//----------------------------------------------------------------------------
void
KinectSkeletonPayloadContainer::DeepCopy(const DataObject *data)
{
	// Sanity check for correct image container payload type
	Self::ConstPointer KSC = dynamic_cast<const Self*>((const Self*)data);
	if ( !KSC )
		itkExceptionMacro(<<"Could not cast argument to Kinect skeleton payload container!");
	if ( KSC->GetIdentifier().compare(this->GetIdentifier()) != 0 )
		itkExceptionMacro(<<"Invalid payload identifiers detected: " << KSC->GetIdentifier() << " vs. " << this->GetIdentifier());

	// Only copy valid skeleton payloads
	KinectSkeletonPayloadPointer SourceSkeleton = dynamic_cast<KinectSkeletonPayloadType*>((KinectSkeletonPayloadType*)KSC->GetPayload());
	KinectSkeletonPayloadPointer TargetSkeleton = dynamic_cast<KinectSkeletonPayloadType*>((KinectSkeletonPayloadType*)this->GetPayload());
	if ( SourceSkeleton && TargetSkeleton )
	{
		//std::cout << "Copying skeleton payload..." << std::endl;
		// Copy
		for ( int i = 0; i < 2; i++ )
		{
			for ( KinectSkeleton::JointTypes joint = KinectSkeleton::SKEL_HIP_Center; joint != KinectSkeleton::SKEL_COUNT; joint = static_cast<KinectSkeleton::JointTypes>(joint + 1) )
			{
				TargetSkeleton->SetJoint(joint, SourceSkeleton->GetJoint(joint, i), i);		
				TargetSkeleton->SetTrackingState(joint, SourceSkeleton->GetTrackingState(joint, i),i);
			}
		}
	}
}


//----------------------------------------------------------------------------
void
KinectSkeletonPayloadContainer::PrintSelf(std::ostream &os, itk::Indent indent) const
{
	Superclass::PrintSelf(os, indent);
	os << indent << "KinectSkeletonPayloadContainer" << std::endl;
	os << indent << indent << "Payload: " << this->GetPayload() << std::endl;
}
