#include "KinectImageIO.h"
#include "ritkDebugManager.h"

template<class TRImageBase, class TKinectImage>
KinectImageIO<TRImageBase,TKinectImage>::
KinectImageIO()
{
	m_FileNameDirtyFlag = true;

	m_ClassName = KinectImageType::New()->GetNameOfClass();
	m_CurrentHeader = 0;
	m_uiSkeletons = NUMBEROFSKELETONS;
	m_uiQualityFlags = 1;
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TKinectImage>
KinectImageIO<TRImageBase,TKinectImage>::
~KinectImageIO()
{
	if ( m_CurrentHeader )
		delete m_CurrentHeader;
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TKinectImage>
const char* 
KinectImageIO<TRImageBase,TKinectImage>::
GetRImageClassName()
{
	return m_ClassName.c_str();
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TKinectImage>
unsigned int
KinectImageIO<TRImageBase,TKinectImage>::
GetNumberOfFrames()
{
	OpenFileStreamAndReadHeader();
	
	return m_CurrentHeader->NumFrames;
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TKinectImage>
void
KinectImageIO<TRImageBase,TKinectImage>::
GetResolution(unsigned int *SX, unsigned int *SY)
{
	OpenFileStreamAndReadHeader();

	*SX = m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SX];
	*SY = m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SY];
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TKinectImage>
void
KinectImageIO<TRImageBase,TKinectImage>::
Close()
{
	if ( m_OutputStream.is_open() )
		m_OutputStream.close();
	if ( m_InputStream.is_open() )
		m_InputStream.close();
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TKinectImage>
void
KinectImageIO<TRImageBase,TKinectImage>::
WriteFrame(RImageBaseConstPointer Frame)
{
	// Sanity checks
	if ( !Frame )
		itkExceptionMacro(<<"No input set!");
	if ( Superclass::m_FileName.size() == 0 )
		itkExceptionMacro(<<"No file name set!");

	// Update the pipeline
	KinectImagePointer NonConstInput = const_cast<KinectImageType*>(dynamic_cast<const KinectImageType*>((const RImageBaseType*)Frame));
	if ( !NonConstInput )
		itkExceptionMacro(<<"Invalid input detected: Is not a KinectSDK image!");

	unsigned int RGBSizeX = NonConstInput->GetRGBImage()->GetRequestedRegion().GetSize()[0];
	unsigned int RGBSizeY = NonConstInput->GetRGBImage()->GetRequestedRegion().GetSize()[1];

	unsigned int DepthAndFlagSizeX = NonConstInput->GetRangeImage()->GetRequestedRegion().GetSize()[0];
	unsigned int DepthAndFlagSizeY = NonConstInput->GetRangeImage()->GetRequestedRegion().GetSize()[1];
	
	// Dirty flag => close old stream open new one and write the header
	if ( m_FileNameDirtyFlag )
	{
		// Close
		if ( m_OutputStream.is_open() )
			m_OutputStream.close();
		
		// Open new file
		m_OutputStream.open(this->GetFileName().c_str(),std::ios::out|std::ios::trunc|std::ios::binary);

		// Check for errors
		if ( !m_OutputStream.is_open() || m_OutputStream.fail() )
			itkExceptionMacro(<< "Could not open output file: <" << this->GetFileName() <<">");

		// Write the type
		m_OutputStream << this->GetRImageClassName() << std::endl;

		// Write the header
		m_OutputStream << "FormatVersion	" << "2.0" << std::endl;
		m_OutputStream << "RGBResolution	" << RGBSizeX << "	" << RGBSizeY << std::endl;
		m_OutputStream << "RangeResolution	" << DepthAndFlagSizeX << "	" << DepthAndFlagSizeY << std::endl;

		m_OutputStream << "CameraParameters	" << NonConstInput->GetCameraParameters() << std::endl;
		
		// The number of frames
		m_OutputStream << "NumFrames	";
		// Remember the position to the end of the header
		m_HeaderEndPos = m_OutputStream.tellp();
		//std::cout << pos = " << m_HeaderEndPos << std::endl;
		m_OutputStream << "000000" << std::endl;

		// Reset index
		m_Index = 0;

		// Finally, clear the dirty flag
		m_FileNameDirtyFlag = false;
	}


	// Update the number of frames at the correct position
	m_OutputStream.seekp(m_HeaderEndPos);
	m_Index++;
	if ( m_Index > 1000000 )
		itkExceptionMacro(<<"Can not handle so many frames...");
	if ( m_Index < 10 )
		m_OutputStream << "00000";
	else if ( m_Index < 100 )
		m_OutputStream << "0000";
	else if ( m_Index < 1000 )
		m_OutputStream << "000";
	else if ( m_Index < 10000 )
		m_OutputStream << "00";
	else if ( m_Index < 100000 )
		m_OutputStream << "0";
	m_OutputStream << m_Index << std::endl;

	// Write the data
	m_OutputStream.seekp(0,std::ios::end);

	// Time stamp
	m_OutputStream.write((char*)&NonConstInput->GetTimeStamp(),sizeof(typename RImageBaseType::TimeStampType)); 

	// Conversion parameters
	typename KinectImageType::RealType XToZ;
	typename KinectImageType::RealType YToZ;
	NonConstInput->GetConversionParameters(&XToZ,&YToZ);
	m_OutputStream.write((char*)&XToZ,sizeof(typename KinectImageType::RealType));	
	m_OutputStream.write((char*)&YToZ,sizeof(typename KinectImageType::RealType));

	// Superclass' data
	m_OutputStream.write((char*)NonConstInput->GetRGBImage()->GetBufferPointer(),RGBSizeX*RGBSizeY*sizeof(typename KinectImageType::RGBImageType::PixelType));
	m_OutputStream.write((char*)NonConstInput->GetRangeImage()->GetBufferPointer(),DepthAndFlagSizeX*DepthAndFlagSizeY*sizeof(typename RImageBaseType::RangeImageType::PixelType));
	m_OutputStream.write((char*)NonConstInput->GetFlagImage()->GetBufferPointer(),DepthAndFlagSizeX*DepthAndFlagSizeY*sizeof(typename RImageBaseType::FlagImageType::PixelType));


	// Skeleton data (joint positions and CoM)
	for ( unsigned int i = 0; i < m_uiSkeletons; i++ )
	{
		for (KinectSkeleton::JointTypes joint = KinectSkeleton::SKEL_HIP_Center; joint != KinectSkeleton::SKEL_COUNT; joint = static_cast<KinectSkeleton::JointTypes> (joint + 1))
		{
			
			VF3 tmpJoint  = NonConstInput->GetSkeleton()->GetJoint(joint,i);
			short tmpTrackingState = NonConstInput->GetSkeleton()->GetTrackingState(joint,i);

			m_OutputStream.write((char *) &tmpJoint,sizeof(VF3));
			m_OutputStream.write((char *) &tmpTrackingState,sizeof(short));
		}
	}

	// Reposition stream
	m_OutputStream.seekp(0,std::ios::end);

	// Check for errors
	if ( m_OutputStream.fail() )
		itkExceptionMacro(<<"Failed during writing!");
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TKinectImage>
typename KinectImageIO<TRImageBase,TKinectImage>::RImageBasePointer
KinectImageIO<TRImageBase,TKinectImage>::
ReadFrame(unsigned int FrameID)
{
	// Open the stream, read the header
	OpenFileStreamAndReadHeader();

	// Create the result
	KinectImagePointer Result = NULL;

	// Search the pool for an unused image and make it our output
	bool ImageFound = false;
	while ( !ImageFound )
	{
		// Loop over the current pool
		for ( unsigned int i = 0; i < m_RImagePool.size(); i++ )
		{
			// Check the reference count
			int rc = m_RImagePool[i]->GetReferenceCount();
			if ( rc == 1 )
			{
				Result = m_RImagePool[i];
				// Update our flag
				ImageFound = true;
				break;
			}
		}

		// Found no unused image -> Create/Resize the current pool
		if ( !ImageFound )
		{
			int OldSize = m_RImagePool.size();
			if ( OldSize == 0 )
				m_RImagePool.resize(16, NULL);
			else
				m_RImagePool.resize(OldSize+16, NULL);
			LOG_DEB("Resizing image pool: N=" << m_RImagePool.size());

			for ( unsigned int i = OldSize; i < m_RImagePool.size(); i++ )
			{
				// Create output image type to match the input
				KinectImagePointer tmp = KinectImageType::New();

				// Set the regions and allocate the image (explicitely allocate the world coord sub image)
				typename RImageBaseType::SizeType RGBImageSize, RangeImageSize;
				RGBImageSize[0] = m_RGBWidth;
				RGBImageSize[1] = m_RGBHeight;

				RangeImageSize[0] = m_RangeWidth;
				RangeImageSize[1] = m_RangeHeight;
				
				typename RImageBaseType::IndexType Index;
				Index[0] = 0;
				Index[1] = 0;

				typename RImageBaseType::RegionType RGBRegion, RangeRegion;
				RGBRegion.SetIndex(Index);
				RGBRegion.SetSize(RGBImageSize);
				RangeRegion.SetIndex(Index);
				RangeRegion.SetSize(RangeImageSize);

				tmp->SetRegions(RangeRegion);
				tmp->GetRGBImage()->SetRegions(RGBRegion);
				tmp->Allocate();

				// Add the image to the pool
				m_RImagePool[i] = tmp;
			}
		}
	}
	

	// Set the parameters
	for ( unsigned int i = 0; i < ritk::CameraParameters::NumCameraParameters; i++ )
		Result->GetCameraParameters()[i] = m_CurrentHeader->CurrCameraParameters[i];

	// Sanity check
	if ( FrameID >= m_CurrentHeader->NumFrames )
		itkExceptionMacro(<<"Invalid frame requested: " << FrameID << " vs. " << m_CurrentHeader->NumFrames);

	// Safe the stream position
	std::ifstream::pos_type DataSectionStreamPos = m_InputStream.tellg();

	// Some sizes to calculate the offsets
	long NumPixelsRGB = Result->GetRGBImage()->GetRequestedRegion().GetSize()[0]*Result->GetRGBImage()->GetRequestedRegion().GetSize()[1];
	long NumPixelsRF = Result->GetRangeImage()->GetRequestedRegion().GetSize()[0]*Result->GetRangeImage()->GetRequestedRegion().GetSize()[1];

	std::ifstream::pos_type TimeStampSize = sizeof(typename RImageBaseType::TimeStampType);
	std::ifstream::pos_type ConversionParamsSize = 2*sizeof(typename RImageBaseType::RealType);
	std::ifstream::pos_type RGBImageSize = NumPixelsRGB*sizeof(typename KinectImageType::RGBType);
	std::ifstream::pos_type RangeImageSize = NumPixelsRF*sizeof(typename RImageBaseType::RangeType);
	std::ifstream::pos_type FlagImageSize = NumPixelsRF*sizeof(typename RImageBaseType::FlagType);	

	// Depending on the format version
	if ( m_CurrentHeader->FormatVersion == std::string("1.0") ||  m_CurrentHeader->FormatVersion == std::string("1.1") )
	{
		// Full framesize
		std::ifstream::pos_type FrameSize = TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize + FlagImageSize;
		
		// Read the timestamp
		typename RImageBaseType::TimeStampType TimeStamp;
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize));
		m_InputStream.read((char*)&TimeStamp,TimeStampSize);
		Result->SetTimeStamp(TimeStamp);

		// Conversion parameters
		typename KinectImageType::RealType ConvParams[2];
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize));
		m_InputStream.read((char*)ConvParams,ConversionParamsSize);
		Result->SetConversionParameters(ConvParams[0],ConvParams[1]);

		// Read the RGB values
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize));
		m_InputStream.read((char*)Result->GetRGBImage()->GetBufferPointer(),RGBImageSize);
		Result->GetRGBImage()->Modified();

		// Read the range values
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize + RGBImageSize));
		m_InputStream.read((char*)Result->GetRangeImage()->GetBufferPointer(),RangeImageSize);
		Result->GetRangeImage()->Modified();

		// Read the flags
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize));
		m_InputStream.read((char*)Result->GetFlagImage()->GetBufferPointer(),FlagImageSize);
		Result->GetFlagImage()->Modified();

		// Restore the stream position
		m_InputStream.seekg(DataSectionStreamPos);
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("1.2") )
	{
		std::ifstream::pos_type SkeletonDataSize = KinectSkeleton::SKEL_COUNT * m_uiSkeletons * (sizeof(VF3) + m_uiQualityFlags * sizeof(short));

		// Full framesize
		std::ifstream::pos_type FrameSize = TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize + FlagImageSize + SkeletonDataSize;
		
		// Read the timestamp
		typename RImageBaseType::TimeStampType TimeStamp;
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize));
		m_InputStream.read((char*)&TimeStamp,TimeStampSize);
		//std::cout<< "Time: " << TimeStamp << std::endl;
		Result->SetTimeStamp(TimeStamp);

		// Conversion parameters
		typename KinectImageType::RealType ConvParams[2];
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize));
		m_InputStream.read((char*)ConvParams,ConversionParamsSize);
		Result->SetConversionParameters(ConvParams[0],ConvParams[1]);

		// Read the RGB values
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize));
		m_InputStream.read((char*)Result->GetRGBImage()->GetBufferPointer(),RGBImageSize);
		Result->GetRGBImage()->Modified();

		// Read the range values
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize + RGBImageSize));
		m_InputStream.read((char*)Result->GetRangeImage()->GetBufferPointer(),RangeImageSize);
		Result->GetRangeImage()->Modified();

		// Read the flags
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize));
		m_InputStream.read((char*)Result->GetFlagImage()->GetBufferPointer(),FlagImageSize);
		Result->GetFlagImage()->Modified();

		std::ifstream::pos_type SkeletonBegin = DataSectionStreamPos + 
			(std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize + FlagImageSize);

		// Read the skeleton data
		for ( unsigned int i = 0, j = 0; i < m_uiSkeletons; i++)
		{
			for (KinectSkeleton::JointTypes joint = KinectSkeleton::SKEL_HIP_Center; joint != KinectSkeleton::SKEL_COUNT; joint = static_cast<KinectSkeleton::JointTypes> (joint + 1), j++)
			{
				VF3 tmpJoint;
				short trackingState = 0;

				// go to joint position and read
				m_InputStream.seekg(SkeletonBegin + (std::ifstream::pos_type) (j * sizeof(VF3) + j * m_uiQualityFlags * sizeof(short)));
				m_InputStream.read((char *) &tmpJoint, sizeof(VF3));
				
				
				if (m_uiQualityFlags)
				{
					// go to tracking State position and read
					m_InputStream.seekg(SkeletonBegin + (std::ifstream::pos_type) ((j+1) * sizeof(VF3) + j * sizeof(short)));
					m_InputStream.read((char *) &trackingState, sizeof(short));

				}
				
				// set data
				Result->GetSkeleton()->SetJoint(joint, tmpJoint, i);
				Result->GetSkeleton()->SetTrackingState(joint, trackingState, i);
				
			}
		}

		// go to position
		m_InputStream.seekg(DataSectionStreamPos + 
				(std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize + FlagImageSize + SkeletonDataSize));
		Result->GetSkeleton()->Modified();

		// Restore the stream position
		m_InputStream.seekg(DataSectionStreamPos);
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("2.0") )
	{
		
		std::ifstream::pos_type SkeletonDataSize = KinectSkeleton::SKEL_COUNT * m_uiSkeletons * (sizeof(VF3) + m_uiQualityFlags * sizeof(short));

		// Full framesize
		std::ifstream::pos_type FrameSize = TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize + FlagImageSize + SkeletonDataSize;
		
		// Read the timestamp
		typename RImageBaseType::TimeStampType TimeStamp;
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize));
		m_InputStream.read((char*)&TimeStamp,TimeStampSize);
		//std::cout<< "Time: " << TimeStamp << std::endl;
		Result->SetTimeStamp(TimeStamp);

		// Conversion parameters
		typename KinectImageType::RealType ConvParams[2];
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize));
		m_InputStream.read((char*)ConvParams,ConversionParamsSize);
		Result->SetConversionParameters(ConvParams[0],ConvParams[1]);

		// Read the RGB values
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize));
		m_InputStream.read((char*)Result->GetRGBImage()->GetBufferPointer(),RGBImageSize);
		Result->GetRGBImage()->Modified();

		// Read the range values
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize + RGBImageSize));
		m_InputStream.read((char*)Result->GetRangeImage()->GetBufferPointer(),RangeImageSize);
		Result->GetRangeImage()->Modified();

		// Read the flags
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize));
		m_InputStream.read((char*)Result->GetFlagImage()->GetBufferPointer(),FlagImageSize);
		Result->GetFlagImage()->Modified();

		std::ifstream::pos_type SkeletonBegin = DataSectionStreamPos + 
			(std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize + FlagImageSize);

		// Read the skeleton data
		for ( unsigned int i = 0, j = 0; i < m_uiSkeletons; i++)
		{
			for (KinectSkeleton::JointTypes joint = KinectSkeleton::SKEL_HIP_Center; joint != KinectSkeleton::SKEL_COUNT; joint = static_cast<KinectSkeleton::JointTypes> (joint + 1), j++)
			{
				VF3 tmpJoint;
				short trackingState = 0;

				// go to joint position and read
				m_InputStream.seekg(SkeletonBegin + (std::ifstream::pos_type) (j * sizeof(VF3) + j * m_uiQualityFlags * sizeof(short)));
				m_InputStream.read((char *) &tmpJoint, sizeof(VF3));
				
				
				if (m_uiQualityFlags)
				{
					// go to tracking State position and read
					m_InputStream.seekg(SkeletonBegin + (std::ifstream::pos_type) ((j+1) * sizeof(VF3) + j * sizeof(short)));
					m_InputStream.read((char *) &trackingState, sizeof(short));

				}
				
				// set data
				Result->GetSkeleton()->SetJoint(joint, tmpJoint, i);
				Result->GetSkeleton()->SetTrackingState(joint, trackingState, i);
				
			}
		}

		// go to position
		m_InputStream.seekg(DataSectionStreamPos + 
				(std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize + FlagImageSize + SkeletonDataSize));
		Result->GetSkeleton()->Modified();

		// Restore the stream position
		m_InputStream.seekg(DataSectionStreamPos);
	}

	else
	{
		itkExceptionMacro(<<"Unknown KinectImageIO version1: " << m_CurrentHeader->FormatVersion);
	}

	// Check for errors
	if ( m_InputStream.fail() )
		itkExceptionMacro(<<"Could not read frame!");

	return dynamic_cast<RImageBaseType*>((KinectImageType*)Result);
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TKinectImage>
void
KinectImageIO<TRImageBase,TKinectImage>::
OpenFileStreamAndReadHeader()
{
	// If the dirty flag for the filename is not set we don't have to do anything
	if ( !m_FileNameDirtyFlag )
	{
		return;
	}

	// Sanity check
	if ( Superclass::m_FileName.size() == 0 )
		itkExceptionMacro(<<"No file name set!");

	// Close old stream and open new stream
	if ( m_InputStream.is_open() )
		m_InputStream.close();
	m_InputStream.open(this->GetFileName().c_str(),std::ios::in|std::ios::binary);
	if ( !m_InputStream.is_open() )
		itkExceptionMacro(<<"Could not open input file!");

	// Read the header
	if ( m_CurrentHeader )
		delete m_CurrentHeader;
	m_CurrentHeader = new HeaderStruct();
	std::string dummy;

	// Image type
	m_InputStream >> dummy;
	if ( dummy.compare(std::string(this->GetRImageClassName())) != 0 )
		itkExceptionMacro(<<"Invalid image type detected: " << dummy << " Can only handle " << this->GetRImageClassName() << "!");

	// Format version
	m_InputStream >> dummy;
	m_InputStream >> m_CurrentHeader->FormatVersion;
	LOG_DEB("File <" << Superclass::m_FileName << ">: KinectImageIO version " << m_CurrentHeader->FormatVersion);

	// Based on the old format version
	if ( m_CurrentHeader->FormatVersion == std::string("1.0") )
	{
		m_uiSkeletons = 0;
		m_uiQualityFlags = 0;
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("1.1") )
	{
		m_uiSkeletons = 0;
		m_uiQualityFlags = 0;
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("1.2") )
	{
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("2.0") )
	{
		// RGB
		m_InputStream >> dummy;
		m_InputStream >> m_RGBWidth;
		m_InputStream >> m_RGBHeight;
		// Range
		m_InputStream >> dummy;
		m_InputStream >> m_RangeWidth;
		m_InputStream >> m_RangeHeight;
	}

	else
	{
		itkExceptionMacro(<<"Unknown KinectImageIO version2: " << m_CurrentHeader->FormatVersion);
	}

	// The camera parameters
	m_InputStream >> dummy;
	m_InputStream >> m_CurrentHeader->CurrCameraParameters;

	// Previous versions have always the same resolution for different data
	if ( m_CurrentHeader->FormatVersion != std::string("2.0") )
	{
		m_RGBWidth = m_RangeWidth = m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SX];
		m_RGBHeight = m_RangeHeight = m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SY];
	}

	// The number of available frames
	m_InputStream.clear();
	m_InputStream >> dummy;
	m_InputStream >> m_CurrentHeader->NumFrames;

	std::cout << "NumFrames: " << m_CurrentHeader->NumFrames << std::endl;

	// Set the stream position to the beginning of the data section
	m_InputStream.seekg(((unsigned long)m_InputStream.tellg())+1);

	// Finally, clear the dirty flag
	m_FileNameDirtyFlag = false;

	// Initializing the timestamps is done here as well
	InitTimeStamps();

	// Check for errors
	if ( m_InputStream.fail() )
		itkExceptionMacro(<<"Could not read header");
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TKinectImage>
void
KinectImageIO<TRImageBase,TKinectImage>::
InitTimeStamps()
{
	LOG_DEB("Initializing timestamps...");
	// Safe the stream position
	std::ifstream::pos_type DataSectionStreamPos = m_InputStream.tellg();

	// Depending on the current header
	if (m_CurrentHeader->FormatVersion == std::string("1.0")  || m_CurrentHeader->FormatVersion == std::string("1.1") )
	{
		// Some offsets
		long NumPixels = m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SX]*m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SY];
		std::ifstream::pos_type TimeStampSize = sizeof(typename RImageBaseType::TimeStampType);
		std::ifstream::pos_type ConversionParamsSize = 2*sizeof(typename RImageBaseType::RealType);
		std::ifstream::pos_type RGBImageSize = NumPixels*sizeof(typename KinectImageType::RGBType);
		std::ifstream::pos_type RangeImageSize = NumPixels*sizeof(typename RImageBaseType::RangeType);
		std::ifstream::pos_type FlagImageSize = NumPixels*sizeof(typename RImageBaseType::FlagType);
		std::ifstream::pos_type FrameSize = TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize + FlagImageSize;

		// Extract timestamps
		Superclass::m_TimeStamps.resize(m_CurrentHeader->NumFrames);
		for ( long FrameID = 0; FrameID < m_CurrentHeader->NumFrames; FrameID++ )
		{
			typename RImageBaseType::TimeStampType TimeStamp;
			m_InputStream.seekg(DataSectionStreamPos + FrameID*FrameSize);
			m_InputStream.read((char*)&TimeStamp,TimeStampSize);
			Superclass::m_TimeStamps[FrameID] = TimeStamp;
		}
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("1.2") || m_CurrentHeader->FormatVersion == std::string("2.0"))
	{
		// Some offsets
		int NumPixelsRGB = m_RGBWidth * m_RGBHeight;
		int NumPixelsRange = m_RangeHeight * m_RangeWidth;
		std::cout << "Timestamp: " << NumPixelsRGB << " " << NumPixelsRange << std::endl;
		std::ifstream::pos_type TimeStampSize = sizeof(typename RImageBaseType::TimeStampType);
		std::ifstream::pos_type ConversionParamsSize = 2*sizeof(typename RImageBaseType::RealType);
		std::ifstream::pos_type RGBImageSize = NumPixelsRGB*sizeof(typename KinectImageType::RGBType);
		std::ifstream::pos_type RangeImageSize = NumPixelsRange*sizeof(typename RImageBaseType::RangeType);
		std::ifstream::pos_type FlagImageSize = NumPixelsRange*sizeof(typename RImageBaseType::FlagType);
		std::ifstream::pos_type SkeletonDataSize = KinectSkeleton::SKEL_COUNT * m_uiSkeletons * (sizeof(VF3) + m_uiQualityFlags * sizeof(short));

		std::ifstream::pos_type FrameSize = TimeStampSize + ConversionParamsSize + RGBImageSize + RangeImageSize + FlagImageSize + SkeletonDataSize;

		// Extract timestamps
		Superclass::m_TimeStamps.resize(m_CurrentHeader->NumFrames);
		for ( long FrameID = 0; FrameID < m_CurrentHeader->NumFrames; FrameID++ )
		{
			typename RImageBaseType::TimeStampType TimeStamp;
			m_InputStream.seekg(DataSectionStreamPos + FrameID*FrameSize);
			m_InputStream.read((char*)&TimeStamp,TimeStampSize);
			Superclass::m_TimeStamps[FrameID] = TimeStamp;
		}
	}
	else
	{
		itkExceptionMacro(<<"Unknown KinectImageIO version: " << m_CurrentHeader->FormatVersion);
	}

	// Restore the stream position
	m_InputStream.seekg(DataSectionStreamPos);
}
