#include "PMDCamCubeRImageIO.h"

#include "ritkDebugManager.h"

#include <itkImageRegionConstIterator.h>

#define CHECK_ERROR(CODE)											\
{																	\
	if ( CODE != PMD_OK )											\
	{																\
		char err[256];												\
		pmdGetLastError(m_PMDHandle,err,256);						\
		itkExceptionMacro(<<"A PMD error occured: " << err);		\
	}																\
}

#define PMDCamCubeRImageIO_V1_1

template<class TRImageBase, class TCamCubeImage>
PMDCamCubeRImageIO<TRImageBase,TCamCubeImage>::
PMDCamCubeRImageIO()
{
	m_CurrentHeader = 0;
	m_SourceDataBuffer = NULL;

	m_MultiThreader = itk::MultiThreader::New();

	// Open the processing plugin	
#ifdef _WIN64
	CHECK_ERROR(pmdOpenProcessingPlugin(&m_PMDHandle, "camcubeproc.W64.ppp", ""));
#else
	CHECK_ERROR(pmdOpenProcessingPlugin(&m_PMDHandle, "camcubeproc.W32.ppp", ""));
#endif

}

template<class TRImageBase, class TCamCubeImage>
PMDCamCubeRImageIO<TRImageBase,TCamCubeImage>::
~PMDCamCubeRImageIO()
{
	if ( m_CurrentHeader )
		delete m_CurrentHeader;

	try
	{
		CHECK_ERROR(pmdClose(m_PMDHandle));
	}
	catch ( itk::ExceptionObject &e )
	{
		std::cerr << e << std::endl;
	}
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TCamCubeImage>
const char* 
PMDCamCubeRImageIO<TRImageBase,TCamCubeImage>::
GetRImageClassName()
{
	return CamCubeImageType::New()->GetNameOfClass();
}


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


//----------------------------------------------------------------------------
template<class TRImageBase, class TCamCubeImage>
void
PMDCamCubeRImageIO<TRImageBase,TCamCubeImage>::
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 TCamCubeImage>
std::string
PMDCamCubeRImageIO<TRImageBase,TCamCubeImage>::
GetFileName()
{
	return m_FileName;
}


//----------------------------------------------------------------------------
template<class TRImageBase, class TCamCubeImage>
void
PMDCamCubeRImageIO<TRImageBase,TCamCubeImage>::
SetFileName(std::string FileName)
{
	if ( FileName == m_FileName )
		return;

	// Update filename
	m_FileName = FileName;

	// Set dirty flags
	m_FileNameDirtyFlag = true;

	// Reset index
	m_Index = 0;
}


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


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

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

	// ???
	NonConstInput->Update();

	// 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(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: <" << GetFileName() <<">");

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


		// Write the header
#if defined(PMDCamCubeRImageIO_V1_0)
		m_OutputStream << "FormatVersion	" << "1.0" << std::endl;
		m_OutputStream << "SizeX	" << NonConstInput->GetRequestedRegion().GetSize()[0] << std::endl;
		m_OutputStream << "SizeY	" << NonConstInput->GetRequestedRegion().GetSize()[1] << std::endl;
		m_OutputStream << "CameraModel	" << "Unknown" << std::endl;
		m_OutputStream << "IntegrationTime	" << "Unknown" << std::endl;
		m_OutputStream << "ModulationFrequency	" << "Unknown" << std::endl;
    m_OutputStream << "FX	" << NonConstInput->GetCameraParameters()[ritk::CameraParameters::FX] << std::endl;
		m_OutputStream << "FY	" << NonConstInput->GetCameraParameters()[ritk::CameraParameters::FY] << std::endl;
		m_OutputStream << "CX	" << NonConstInput->GetCameraParameters()[ritk::CameraParameters::CX] << std::endl;
		m_OutputStream << "CY	" << NonConstInput->GetCameraParameters()[ritk::CameraParameters::CY] << std::endl;
		m_OutputStream << "K1	" << NonConstInput->GetCameraParameters()[ritk::CameraParameters::K1] << std::endl;
		m_OutputStream << "K2	" << NonConstInput->GetCameraParameters()[ritk::CameraParameters::K2] << std::endl;
		m_OutputStream << "K3	" << NonConstInput->GetCameraParameters()[ritk::CameraParameters::K3] << std::endl;
		m_OutputStream << std::endl;
#elif defined(PMDCamCubeRImageIO_V1_1)
#if defined(PMDCamCubeRImageIO_V1_0)
#error Multiple versions defined!
#endif
		m_OutputStream << "FormatVersion	" << "1.1" << std::endl;
		m_OutputStream << "CameraParameters	" << NonConstInput->GetCameraParameters() << std::endl;
		m_OutputStream << "SourceDataSize	" << NonConstInput->GetPMDDataDescription().size << std::endl;
		m_OutputStream << "CameraModel		" << "Unknown" << std::endl;
		m_OutputStream << "IntegrationTime	" << "Unknown" << std::endl;
		m_OutputStream << "ModulationFrequency	" << "Unknown" << std::endl;
		m_OutputStream << std::endl;
#endif

		// 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
#if defined(PMDCamCubeRImageIO_V1_0)
	m_OutputStream.seekp(0,std::ios::end);
	unsigned int SizeX = NonConstInput->GetRequestedRegion().GetSize()[0];
	unsigned int SizeY = NonConstInput->GetRequestedRegion().GetSize()[1];
	m_OutputStream.write((char*)&NonConstInput->GetTimeStamp(),sizeof(RImageBaseType::TimeStampType));
	m_OutputStream.write((itk::Vector<char,3>*)NonConstInput->GetRGBImage()->GetBufferPointer(),SizeX*SizeY*sizeof(RImageBaseType::RGBImageType::PixelType));
	m_OutputStream.write((char*)NonConstInput->GetRangeImage()->GetBufferPointer(),SizeX*SizeY*sizeof(RImageBaseType::RangeImageType::PixelType));
	m_OutputStream.write((char*)NonConstInput->GetAmplitudeImage()->GetBufferPointer(),SizeX*SizeY*sizeof(CamCubeImageType::AmplitudeImageType::PixelType));
	m_OutputStream.write((char*)NonConstInput->GetCCFlagImage()->GetBufferPointer(),SizeX*SizeY*sizeof(CamCubeImageType::CCFlagImageType::PixelType));
	m_OutputStream.seekp(0,std::ios::end);
#elif defined(PMDCamCubeRImageIO_V1_1)
#if defined(PMDCamCubeRImageIO_V1_0)
#error Multiple versions defined!
#endif
	m_OutputStream.seekp(0,std::ios::end);
	m_OutputStream.write((char*)&NonConstInput->GetTimeStamp(),sizeof(RImageBaseType::TimeStampType));
	m_OutputStream.write((char*)&NonConstInput->GetPMDDataDescription(), sizeof(PMDDataDescription));
	m_OutputStream.write(NonConstInput->GetSourceData(), sizeof(char)*NonConstInput->GetPMDDataDescription().size);
	m_OutputStream.seekp(0,std::ios::end);
#endif

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


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

	// Create the result
	CamCubeImagePointer Result = CamCubeImageType::New();

	// Set all regions and allocate
	RImageBaseType::SizeType ImageSize;
	ImageSize[0] = m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SX];
	ImageSize[1] = m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SY];
	RImageBaseType::IndexType Index;
	Index[0] = 0;
	Index[1] = 0;
	RImageBaseType::RegionType Region;
	Region.SetIndex(Index);
	Region.SetSize(ImageSize);
	Result->SetRegions(Region);
	Result->Allocate();

	// 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();
	
	// Depending on the format version
	if ( m_CurrentHeader->FormatVersion == std::string("0.8") ) // WACV '11
	{
		// Some sizes to calculate the offsets
		std::ifstream::pos_type TimeStampSize = sizeof(RImageBaseType::TimeStampType);
		long NumPixels = ImageSize[0]*ImageSize[1];
		std::ifstream::pos_type GrayscaleImageSize = NumPixels*sizeof(CamCubeImageType::GrayscaleType);
		std::ifstream::pos_type RangeImageSize = NumPixels*sizeof(RImageBaseType::RangeType);
		std::ifstream::pos_type AmplitudeImageSize = NumPixels*sizeof(CamCubeImageType::AmplitudeType);
		std::ifstream::pos_type CCFlagImageSize = NumPixels*sizeof(unsigned long);
		std::ifstream::pos_type FrameSize = TimeStampSize + GrayscaleImageSize + RangeImageSize + AmplitudeImageSize + CCFlagImageSize;

		// Read the timestamp
		RImageBaseType::TimeStampType TimeStamp;
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize));
		m_InputStream.read((char*)&TimeStamp,TimeStampSize);
		Result->SetTimeStamp(TimeStamp);

		// Read the grayscale values and convert them to rgb values
		const CamCubeImageType::GrayscaleType *TmpGrayscaleBuffer = Result->GetGrayscaleImage()->GetBufferPointer();
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize));
		m_InputStream.read((char*)TmpGrayscaleBuffer,GrayscaleImageSize);
		Result->GetGrayscaleImage()->Modified();

		// Read the range values and convert data from m to mm
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + GrayscaleImageSize));
		m_InputStream.read((char*)Result->GetRangeImage()->GetBufferPointer(),RangeImageSize);
		RImageBaseType::RangeImageType::PixelType Modifier = 1000.0;
		RImageBaseType::RangeImageType::PixelType *RangeDataBuffer = Result->GetRangeImage()->GetBufferPointer();
		for ( int i = 0; i < Result->GetRangeImage()->GetRequestedRegion().GetNumberOfPixels(); ++i, ++RangeDataBuffer )
			*RangeDataBuffer *= Modifier;
		Result->GetRangeImage()->Modified();

		// Read the amplitudes
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + GrayscaleImageSize + RangeImageSize));
		m_InputStream.read((char*)Result->GetAmplitudeImage()->GetBufferPointer(),AmplitudeImageSize);
		Result->GetAmplitudeImage()->Modified();

		// Read the flags and synchronize the flag image
		typedef itk::Image<unsigned long,2> VersionFlagImageType;
		VersionFlagImageType::Pointer VersionFlagImage = VersionFlagImageType::New();
		VersionFlagImageType::SizeType VersionFlagImageSize;
		VersionFlagImageSize[0] = ImageSize[0];
		VersionFlagImageSize[1] = ImageSize[1];
		VersionFlagImageType::RegionType VersionFlagImageRegion;
		VersionFlagImageRegion.SetSize(VersionFlagImageSize);
		VersionFlagImage->SetRegions(VersionFlagImageRegion);
		VersionFlagImage->Allocate();

		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + GrayscaleImageSize + RangeImageSize + AmplitudeImageSize));
		m_InputStream.read((char*)VersionFlagImage->GetBufferPointer(),CCFlagImageSize);

		typedef itk::Image<unsigned int,2> FlagImageType;
		typedef itk::ImageRegionIterator<VersionFlagImageType> VersionFlagImageIteratorType;
		typedef itk::ImageRegionIterator<FlagImageType> FlagImageIteratorType;
		VersionFlagImageIteratorType itVersionFlag(VersionFlagImage, VersionFlagImage->GetRequestedRegion());
		FlagImageIteratorType itFlag(Result->GetCCFlagImage(), Result->GetCCFlagImage()->GetRequestedRegion());
		for ( itVersionFlag.GoToBegin(),itFlag.GoToBegin(); !itVersionFlag.IsAtEnd(); ++itVersionFlag,++itFlag )
		{
			itFlag.Value() = (unsigned int) itVersionFlag.Value();
		}

		Result->GetCCFlagImage()->Modified();
		Result->SynchronizeFlags();

		// Restore the stream position
		m_InputStream.seekg(DataSectionStreamPos);
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("1.0") )
	{
		// Some sizes to calculate the offsets
		std::ifstream::pos_type TimeStampSize = sizeof(RImageBaseType::TimeStampType);
		long NumPixels = ImageSize[0]*ImageSize[1];
		//std::ifstream::pos_type RGBImageSize = NumPixels*sizeof(RImageBaseType::RGBType);
		std::ifstream::pos_type GrayscaleImageSize = NumPixels*sizeof(CamCubeImageType::GrayscaleType);
		std::ifstream::pos_type RangeImageSize = NumPixels*sizeof(RImageBaseType::RangeType);
		std::ifstream::pos_type AmplitudeImageSize = NumPixels*sizeof(CamCubeImageType::AmplitudeType);
		std::ifstream::pos_type CCFlagImageSize = NumPixels*sizeof(CamCubeImageType::CCFlagType);
		std::ifstream::pos_type FrameSize = TimeStampSize + GrayscaleImageSize + RangeImageSize + AmplitudeImageSize + CCFlagImageSize;

		// Read the timestamp
		RImageBaseType::TimeStampType TimeStamp;
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize));
		m_InputStream.read((char*)&TimeStamp,TimeStampSize);
		Result->SetTimeStamp(TimeStamp);

		// Read the grayscale values and convert them to rgb values
		const CamCubeImageType::GrayscaleType *TmpGrayscaleBuffer = Result->GetGrayscaleImage()->GetBufferPointer();
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize));
		m_InputStream.read((char*)TmpGrayscaleBuffer,GrayscaleImageSize);
		Result->GetGrayscaleImage()->Modified();

		// Read the range values and convert data from m to mm
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + GrayscaleImageSize));
		m_InputStream.read((char*)Result->GetRangeImage()->GetBufferPointer(),RangeImageSize);
		RImageBaseType::RangeImageType::PixelType Modifier = 1000.0;
		RImageBaseType::RangeImageType::PixelType *RangeDataBuffer = Result->GetRangeImage()->GetBufferPointer();
		for ( int i = 0; i < Result->GetRangeImage()->GetRequestedRegion().GetNumberOfPixels(); ++i, ++RangeDataBuffer )
			*RangeDataBuffer *= Modifier;
		Result->GetRangeImage()->Modified();

		// Read the amplitudes
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + GrayscaleImageSize + RangeImageSize));
		m_InputStream.read((char*)Result->GetAmplitudeImage()->GetBufferPointer(),AmplitudeImageSize);
		Result->GetAmplitudeImage()->Modified();

		// Read the flags and synchronize the flag image
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + GrayscaleImageSize + RangeImageSize + AmplitudeImageSize));
		m_InputStream.read((char*)Result->GetCCFlagImage()->GetBufferPointer(),CCFlagImageSize);
		Result->GetCCFlagImage()->Modified();
		Result->SynchronizeFlags();

		// Restore the stream position
		m_InputStream.seekg(DataSectionStreamPos);
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("1.1") )
	{
		// Sizes
		std::ifstream::pos_type TimeStampSize = sizeof(RImageBaseType::TimeStampType);
		std::ifstream::pos_type DataDescriptionSize = sizeof(PMDDataDescription);
		std::ifstream::pos_type SourceDataSize = sizeof(char)*m_CurrentHeader->SourceDataSize;
		std::ifstream::pos_type FrameSize = TimeStampSize + DataDescriptionSize + SourceDataSize;

		// Read the timestamp
		RImageBaseType::TimeStampType TimeStamp;
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize));
		m_InputStream.read((char*)&TimeStamp, TimeStampSize);
		Result->SetTimeStamp(TimeStamp);

		// Read the pmd data description
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize));
		PMDDataDescription Desc;
		m_InputStream.read((char*)&Desc, DataDescriptionSize);

		// Read the source data
		m_InputStream.seekg(DataSectionStreamPos + (std::ifstream::pos_type)(FrameID*FrameSize + TimeStampSize + DataDescriptionSize));
		m_InputStream.read(m_SourceDataBuffer, sizeof(char)*SourceDataSize);

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

		// Compute the images
		Result->ComputeImagesFromSourceData(m_PMDHandle,Desc,m_SourceDataBuffer, m_MultiThreader);

		// Synchronize flags
		Result->SynchronizeFlags();
	}
	else
	{
		itkExceptionMacro(<<"Unknown PMDCamCubeImageIO version: " << m_CurrentHeader->FormatVersion);
	}

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

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


//----------------------------------------------------------------------------
template<class TRImageBase, class TCamCubeImage>
void
PMDCamCubeRImageIO<TRImageBase,TCamCubeImage>::
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 ( 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(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(GetRImageClassName())) != 0 )
		itkExceptionMacro(<<"Invalid image type detected: " << dummy << " Can only handle " << GetRImageClassName() << "!");

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

	// Based on the format version
	if ( m_CurrentHeader->FormatVersion == std::string("0.8") ) // WACV '11
	{
		// The size of the image
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SX];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SY];

		// Unused
		for ( unsigned int i = 0; i < 3; i++ )
		{
			m_InputStream >> dummy;
			m_InputStream >> dummy;
		}

		// The actual parameters
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::FX];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::FY];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::CX];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::CY];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::K1];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::K2];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::K3];
	}	
	else if ( m_CurrentHeader->FormatVersion == std::string("1.0") )
	{
		// The size of the image
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SX];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SY];

		// Unused
		for ( unsigned int i = 0; i < 3; i++ )
		{
			m_InputStream >> dummy;
			m_InputStream >> dummy;
		}

		// The actual parameters
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::FX];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::FY];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::CX];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::CY];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::K1];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::K2];
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::K3];
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("1.1") )
	{
		// The camera parameters
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->CurrCameraParameters;

		// The source data size. At this, also allocate the buffer
		m_InputStream >> dummy;
		m_InputStream >> m_CurrentHeader->SourceDataSize;
		if ( m_SourceDataBuffer )
			delete[] m_SourceDataBuffer;
		m_SourceDataBuffer = new char[m_CurrentHeader->SourceDataSize];

		// Camera model, integration time and modulation frequency are not used atm
		for ( unsigned int i = 0; i < 3; i++ )
		{
			m_InputStream >> dummy;
			m_InputStream >> dummy;
		}
	}
	else
	{
		itkExceptionMacro(<<"Unknown PMDCamCubeImageIO version: " << m_CurrentHeader->FormatVersion);
	}

	// The number of available frames
	m_InputStream >> dummy;
	m_InputStream >> m_CurrentHeader->NumFrames;

	// 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 TCamCubeImage>
void
PMDCamCubeRImageIO<TRImageBase,TCamCubeImage>::
InitTimeStamps()
{
	// Safe the stream position
	std::ifstream::pos_type DataSectionStreamPos = m_InputStream.tellg();

	// Depending on the current header
	if ( m_CurrentHeader->FormatVersion == std::string("0.8") ) // WACV '11
	{
		// Some offsets
		long NumPixels = m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SX]*m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SY];
		std::ifstream::pos_type TimeStampSize = sizeof(RImageBaseType::TimeStampType);
		std::ifstream::pos_type GrayscaleImageSize = NumPixels*sizeof(CamCubeImageType::GrayscaleType);
		std::ifstream::pos_type RangeImageSize = NumPixels*sizeof(RImageBaseType::RangeType);
		std::ifstream::pos_type AmplitudeImageSize = NumPixels*sizeof(CamCubeImageType::AmplitudeType);
		std::ifstream::pos_type CCFlagImageSize = NumPixels*sizeof(unsigned long);
		std::ifstream::pos_type FrameSize = TimeStampSize + GrayscaleImageSize + RangeImageSize + AmplitudeImageSize + CCFlagImageSize;

		// Extract timestamps
		m_TimeStamps.resize(m_CurrentHeader->NumFrames);
		std::cout << m_CurrentHeader->NumFrames << std::endl;
		for ( long FrameID = 0; FrameID < m_CurrentHeader->NumFrames; FrameID++ )
		{
			// Generate artificial timestamps			
			itk::RealTimeClock::Pointer m_Clock = itk::RealTimeClock::New();
			m_TimeStamps[FrameID] = m_Clock->GetTimeStamp();;
		}
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("1.0") )
	{
		// Some offsets
		long NumPixels = m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SX]*m_CurrentHeader->CurrCameraParameters[ritk::CameraParameters::SY];
		std::ifstream::pos_type TimeStampSize = sizeof(RImageBaseType::TimeStampType);
		std::ifstream::pos_type GrayscaleImageSize = NumPixels*sizeof(CamCubeImageType::GrayscaleType);
		std::ifstream::pos_type RangeImageSize = NumPixels*sizeof(RImageBaseType::RangeType);
		std::ifstream::pos_type AmplitudeImageSize = NumPixels*sizeof(CamCubeImageType::AmplitudeType);
		std::ifstream::pos_type CCFlagImageSize = NumPixels*sizeof(CamCubeImageType::CCFlagType);
		std::ifstream::pos_type FrameSize = TimeStampSize + GrayscaleImageSize + RangeImageSize + AmplitudeImageSize + CCFlagImageSize;

		// Extract timestamps
		m_TimeStamps.resize(m_CurrentHeader->NumFrames);
		for ( long FrameID = 0; FrameID < m_CurrentHeader->NumFrames; FrameID++ )
		{
			RImageBaseType::TimeStampType TimeStamp;
			m_InputStream.seekg(DataSectionStreamPos + FrameID*FrameSize);
			m_InputStream.read((char*)&TimeStamp,TimeStampSize);
			m_TimeStamps[FrameID] = TimeStamp;
		}
	}
	else if ( m_CurrentHeader->FormatVersion == std::string("1.1") )
	{
		// Sizes
		std::ifstream::pos_type TimeStampSize = sizeof(RImageBaseType::TimeStampType);
		std::ifstream::pos_type DataDescriptionSize = sizeof(PMDDataDescription);
		std::ifstream::pos_type SourceDataSize = sizeof(char)*m_CurrentHeader->SourceDataSize;
		std::ifstream::pos_type FrameSize = TimeStampSize + DataDescriptionSize + SourceDataSize;

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

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


/*__declspec(dllexport) std::ostream& operator<<(std::ostream &out, PMDImageData& DD);
__declspec(dllexport) std::istream& operator>>(std::istream &in, PMDImageData& DD);
__declspec(dllexport) std::ostream& operator<<(std::ostream &out, PMDGenericData& DD);
__declspec(dllexport) std::istream& operator>>(std::istream &in, PMDGenericData& DD);
__declspec(dllexport) std::ostream& operator<<(std::ostream &out, PMDDataDescription &DD);
__declspec(dllexport) std::istream& operator>>(std::istream &in, PMDDataDescription &DD);


std::ostream& operator<<(std::ostream &out, PMDImageData& DD)
{
	for ( unsigned int i = 0; i < 24; i++ )
		out << DD.reserved[i] << " ";
	for ( unsigned int i = 0; i < 4; i++ )
		out << DD.integrationTime[i] << " ";
	for ( unsigned int i = 0; i < 4; i++ )
		out << DD.modulationFrequency[i] << " ";
	for ( unsigned int i = 0; i < 4; i++ )
		out << DD.offset[i] << " ";
	out << DD.subType << " " << DD.numColumns << " " << DD.numRows << " " << DD.numSubImages << " " << DD.pixelAspectRatio << " " << DD.pixelOrigin << " " << DD.timeStampHi << " " << DD.timeStampLo << " " << DD.userData0;
	return out;
}

std::istream& operator>>(std::istream &in, PMDImageData& DD)
{
	for ( unsigned int i = 0; i < 24; i++ )
		in >> DD.reserved[i];
	for ( unsigned int i = 0; i < 4; i++ )
		in >> DD.integrationTime[i];
	for ( unsigned int i = 0; i < 4; i++ )
		in >> DD.modulationFrequency[i];
	for ( unsigned int i = 0; i < 4; i++ )
		in >> DD.offset[i];
	in >> DD.subType >> DD.numColumns >> DD.numRows >> DD.numSubImages >> DD.pixelAspectRatio >> DD.pixelOrigin >> DD.timeStampHi >> DD.timeStampLo >> DD.userData0;
	return in;
}


std::ostream& operator<<(std::ostream &out, PMDGenericData& DD)
{
	out << DD.subType << " " << DD.numElem << " " << DD.sizeOfElem;
	return out;
}


std::istream& operator>>(std::istream &in, PMDGenericData& DD)
{
	in >> DD.subType >> DD.numElem >> DD.sizeOfElem;
	return in;
}


std::ostream& operator<<(std::ostream &out, PMDDataDescription &DD)
{
	for ( unsigned int i = 0; i < 108; i++ )
		out << DD.fillUpToSizeOfStructure[i] << " ";
	out << DD.DID << " " << DD.gen << " " << DD.img << " " << DD.PID << " " << DD.size << " " << DD.std << " " << DD.subHeaderType << " " << DD.type;
	return out;
}

std::istream& operator>>(std::istream &in, PMDDataDescription &DD)
{
	for ( unsigned int i = 0; i < 108; i++ )
		in >> DD.fillUpToSizeOfStructure[i];
	in >> DD.DID >> DD.gen >> DD.img >> DD.PID >> DD.size >> DD.std >> DD.subHeaderType >> DD.type;
	return in;
}*/