#include "PMDCamCubeRImage.h"

#include "ritkGenericImportImageContainer.h"

#include <itkImageRegionIterator.h>

#include "ritkImagePayloadContainer.h"


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
PMDCamCubeRImage<TRealType, NDimension>
::PMDCamCubeRImage()
{
	// Append the amplitude payload to the internal payload vector
	AppendImagePayloadContainerMacro(m_AmplitudeDataPayloadIdx, AmplitudeImageType, AmplitudeImageType::New(), "Amplitudes");

	// Append the grayscale payload to the internal payload vector
	AppendImagePayloadContainerMacro(m_GrayscaleDataPayloadIdx, GrayscaleImageType, GrayscaleImageType::New(), "GrayscaleData");

	// Append the CC flag data
	AppendImagePayloadContainerMacro(m_CCFlagDataPayloadIdx, CCFlagImageType, CCFlagImageType::New(), "CamCubeFlags");

	// Set the PMD data description to invalid
	m_PMDDataDescription.size = 0;

	// Set the source data to NULL
	m_SourceData = NULL;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
PMDCamCubeRImage<TRealType, NDimension>
::~PMDCamCubeRImage()
{
	if ( m_SourceData )
		delete[] m_SourceData;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::Modified() const
{
	Superclass::Modified();
	GetAmplitudeImage()->Modified();
	GetGrayscaleImage()->Modified();
	GetCCFlagImage()->Modified();
}


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


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


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


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


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


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


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


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


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


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


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


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


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::SetDirection(const DirectionType Direction)
{
	Superclass::SetDirection(Direction);
	GetAmplitudeImage()->SetDirection(Direction);
	GetGrayscaleImage()->SetDirection(Direction);
	GetCCFlagImage()->SetDirection(Direction);
}


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


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::Graft(const DataObject *data)
{
	// Call the superclass' method first. This should also graft all payloads
	Superclass::Graft(data);


	
	// The range image to graft
	ConstPointer Image = NULL;

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

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

#if 0
	// Graft the internal images
	GetAmplitudeImage()->Graft(Image->GetAmplitudeImage());
	GetGrayscaleImage()->Graft(Image->GetGrayscaleImage());
	GetCCFlagImage()->Graft(Image->GetCCFlagImage());
#endif
	

	// Copy the PMD data description
	this->SetPMDDataDescription(Image->GetPMDDataDescription());

	// Copy the source buffer
	if ( this->GetPMDDataDescription().size > 0 )
	{
		memcpy
		(
			(void*)this->GetSourceData(),
			(void*)Image->GetSourceData(),
			sizeof(char)*this->GetPMDDataDescription().size
		);
	}
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::DeepCopy(const DataObject *data)
{
	// Call the superclass' method first. This should also deep copy all payloads
	Superclass::DeepCopy(data);
	
	// The range image to graft
	ConstPointer Image = NULL;

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

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

#if 0
	GIIC_IMAGE_DEEPCOPY(AmplitudeImageType, Image->GetAmplitudeImage(), this->GetAmplitudeImage());
	GIIC_IMAGE_DEEPCOPY(GrayscaleImageType, Image->GetGrayscaleImage(), this->GetGrayscaleImage());
	GIIC_IMAGE_DEEPCOPY(CCFlagImageType, Image->GetCCFlagImage(), this->GetCCFlagImage());
#endif

	// Copy the PMD data description
	this->SetPMDDataDescription(Image->GetPMDDataDescription());

	// Copy the source buffer
	if ( this->GetPMDDataDescription().size > 0 )
	{
		memcpy
		(
			(void*)this->GetSourceData(),
			(void*)Image->GetSourceData(),
			sizeof(char)*this->GetPMDDataDescription().size
		);
	}
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::DisconnectPipeline()
{
	Superclass::DisconnectPipeline();
	if ( GetAmplitudeImage() )
		GetAmplitudeImage()->DisconnectPipeline();
	if ( GetCCFlagImage() )
		GetCCFlagImage()->DisconnectPipeline();
	if ( GetGrayscaleImage() )
		GetGrayscaleImage()->DisconnectPipeline();
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
typename PMDCamCubeRImage<TRealType, NDimension>::AmplitudeImagePointer
PMDCamCubeRImage<TRealType, NDimension>
::GetAmplitudeImage()
{
	return (AmplitudeImagePointer)static_cast<AmplitudeImageType*>(this->GetPayloadContainer(m_AmplitudeDataPayloadIdx)->GetPayload());
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
typename PMDCamCubeRImage<TRealType, NDimension>::AmplitudeImageConstPointer
PMDCamCubeRImage<TRealType, NDimension>
::GetAmplitudeImage() const
{
	return (AmplitudeImageConstPointer)static_cast<const AmplitudeImageType*>(this->GetPayloadContainer(m_AmplitudeDataPayloadIdx)->GetPayload());
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
typename PMDCamCubeRImage<TRealType, NDimension>::GrayscaleImagePointer
PMDCamCubeRImage<TRealType, NDimension>
::GetGrayscaleImage()
{
	return (GrayscaleImagePointer)static_cast<GrayscaleImageType*>(this->GetPayloadContainer(m_GrayscaleDataPayloadIdx)->GetPayload());
}

//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
typename PMDCamCubeRImage<TRealType, NDimension>::GrayscaleImageConstPointer
PMDCamCubeRImage<TRealType, NDimension>
::GetGrayscaleImage() const
{
	return (GrayscaleImageConstPointer)static_cast<const GrayscaleImageType*>(this->GetPayloadContainer(m_GrayscaleDataPayloadIdx)->GetPayload());
}



//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
typename PMDCamCubeRImage<TRealType, NDimension>::CCFlagImagePointer
PMDCamCubeRImage<TRealType, NDimension>
::GetCCFlagImage()
{
	return (CCFlagImagePointer)static_cast<CCFlagImageType*>(this->GetPayloadContainer(m_CCFlagDataPayloadIdx)->GetPayload());
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
typename PMDCamCubeRImage<TRealType, NDimension>::CCFlagImageConstPointer
PMDCamCubeRImage<TRealType, NDimension>
::GetCCFlagImage() const
{
	return (CCFlagImageConstPointer)static_cast<const CCFlagImageType*>(this->GetPayloadContainer(m_CCFlagDataPayloadIdx)->GetPayload());
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::SetAmplitudeImage(AmplitudeImagePointer AI)
{
	// Sanity check
	if ( this->GetRequestedRegion() != AI->GetRequestedRegion() )
		itkExceptionMacro(<<"Invalid regions detected!");

	this->GetPayloadContainer(m_AmplitudeDataPayloadIdx)->SetPayload(AI);
	this->Modified();
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::SetGrayscaleImage(GrayscaleImagePointer GI)
{
	// Sanity check
	if ( this->GetRequestedRegion() != GI->GetRequestedRegion() )
		itkExceptionMacro(<<"Invalid regions detected:" << this->GetRequestedRegion() << " vs. " << GI->GetRequestedRegion());

	this->GetPayloadContainer(m_GrayscaleDataPayloadIdx)->SetPayload(GI);
	this->Modified();
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::SetCCFlagImage(CCFlagImagePointer FI)
{
	// Sanity check
	if ( this->GetRequestedRegion() != FI->GetRequestedRegion() )
		itkExceptionMacro(<<"Invalid regions detected:" << this->GetRequestedRegion() << " vs. " << FI->GetRequestedRegion());

	this->GetPayloadContainer(m_CCFlagDataPayloadIdx)->SetPayload(FI);
	this->Modified();
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::SynchronizeFlags()
{
	typedef itk::ImageRegionIterator<FlagImageType> FlagIteratorType;
	typedef itk::ImageRegionIterator<CCFlagImageType> CCFlagIteratorType;
	FlagIteratorType itFlag(GetFlagImage(),GetFlagImage()->GetRequestedRegion());
	CCFlagIteratorType itCCFlag(GetCCFlagImage(),GetCCFlagImage()->GetRequestedRegion());
	for ( itFlag.GoToBegin(),itCCFlag.GoToBegin(); !itFlag.IsAtEnd(); ++itFlag,++itCCFlag )
	{
		itFlag.Value() = RImage::VALID;
		if ( itCCFlag.Value()&PMD_FLAG_INVALID )
			itFlag.Value() = itFlag.Value()&(~RImage::VALID);
	}
	this->Modified();
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
PMDDataDescription
PMDCamCubeRImage<TRealType, NDimension>
::GetPMDDataDescription() const
{
	return m_PMDDataDescription;
}



//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::SetPMDDataDescription(PMDDataDescription DD)
{
	if ( m_SourceData )
		delete[] m_SourceData;

	m_SourceData = new char[DD.size];
	m_PMDDataDescription = DD;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
char*
PMDCamCubeRImage<TRealType, NDimension>
::GetSourceData()
{
	return m_SourceData;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
const char*
PMDCamCubeRImage<TRealType, NDimension>
::GetSourceData() const
{
	return m_SourceData;
}


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


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
void
PMDCamCubeRImage<TRealType, NDimension>
::ComputeImagesFromSourceData(const PMDHandle &Handle, const PMDDataDescription &Descr, const char *SourceData, itk::MultiThreader *Threader)
{
	// Copy the data description and source data
	SetPMDDataDescription(Descr);
	memcpy(m_SourceData, SourceData, sizeof(char)*Descr.size);

  // Set up structs for multi-threaded data generation
	m_ThreadedComputeSubImagesStruct.Handle = Handle;
	m_ThreadedComputeSubImagesStruct.NumPixels = m_PMDDataDescription.img.numColumns*m_PMDDataDescription.img.numRows;
	m_ThreadedComputeSubImagesStruct.DataDescription = m_PMDDataDescription;
	m_ThreadedComputeSubImagesStruct.SourceBuffer = m_SourceData;
	m_ThreadedComputeSubImagesStruct.GrayscaleData = GetGrayscaleImage()->GetBufferPointer();
	m_ThreadedComputeSubImagesStruct.RangeData = GetRangeImage()->GetBufferPointer();
	m_ThreadedComputeSubImagesStruct.AmplitudeData = GetAmplitudeImage()->GetBufferPointer();
	m_ThreadedComputeSubImagesStruct.CCFlagData = GetCCFlagImage()->GetBufferPointer();

	// Multi-threaded data generation
	Threader->SetNumberOfThreads(4);
	Threader->SetMultipleMethod(0,ThreadedCalcAmplitudes,&m_ThreadedComputeSubImagesStruct);
	Threader->SetMultipleMethod(1,ThreadedCalcDistances,&m_ThreadedComputeSubImagesStruct);
	Threader->SetMultipleMethod(2,ThreadedCalcIntensities,&m_ThreadedComputeSubImagesStruct);
	Threader->SetMultipleMethod(3,ThreadedCalcFlags,&m_ThreadedComputeSubImagesStruct);
	Threader->MultipleMethodExecute();

	// Convert range data from m to mm
	Superclass::RangeImageType::PixelType Modifier = 1000.0;
	Superclass::RangeImageType::PixelType *RangeDataBuffer = GetRangeImage()->GetBufferPointer();
	for ( int i = 0; i < GetRangeImage()->GetRequestedRegion().GetNumberOfPixels(); ++i, ++RangeDataBuffer )
		*RangeDataBuffer *= Modifier;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
ITK_THREAD_RETURN_TYPE 
PMDCamCubeRImage<TRealType, NDimension>
::ThreadedCalcAmplitudes(void *arg)
{
	// Get the thread struct that contains a pointer to the reader
	ThreadedComputeSubImagesStruct *str = (ThreadedComputeSubImagesStruct*)(((itk::MultiThreader::ThreadInfoStruct*)(arg))->UserData);
	pmdCalcAmplitudes(str->Handle,str->AmplitudeData,str->NumPixels*sizeof(float),str->DataDescription,str->SourceBuffer);

	return 0;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
ITK_THREAD_RETURN_TYPE 
PMDCamCubeRImage<TRealType, NDimension>
::ThreadedCalcDistances(void *arg)
{
	// Get the thread struct that contains a pointer to the reader
	ThreadedComputeSubImagesStruct *str = (ThreadedComputeSubImagesStruct*)(((itk::MultiThreader::ThreadInfoStruct*)(arg))->UserData);
	pmdCalcDistances(str->Handle,str->RangeData,str->NumPixels*sizeof(float),str->DataDescription,str->SourceBuffer);

	return 0;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
ITK_THREAD_RETURN_TYPE 
PMDCamCubeRImage<TRealType, NDimension>
::ThreadedCalcIntensities(void *arg)
{
	// Get the thread struct that contains a pointer to the reader
	ThreadedComputeSubImagesStruct *str = (ThreadedComputeSubImagesStruct*)(((itk::MultiThreader::ThreadInfoStruct*)(arg))->UserData);
	pmdCalcIntensities(str->Handle,str->GrayscaleData,str->NumPixels*sizeof(float),str->DataDescription,str->SourceBuffer);

  return 0;
}


//----------------------------------------------------------------------------
template<class TRealType, unsigned int NDimension>
ITK_THREAD_RETURN_TYPE 
PMDCamCubeRImage<TRealType, NDimension>
::ThreadedCalcFlags(void *arg)
{
	// Get the thread struct that contains a pointer to the reader
	ThreadedComputeSubImagesStruct *str = (ThreadedComputeSubImagesStruct*)(((itk::MultiThreader::ThreadInfoStruct*)(arg))->UserData);
	pmdCalcFlags(str->Handle,str->CCFlagData,str->NumPixels*sizeof(unsigned),str->DataDescription,str->SourceBuffer);

	return 0;
}


