/*======================================================================================

	Program:		Range Imaging Toolkit (RITK)
	Module:			ritkRGBRImage.h
	Version:		$Revision: 3133 $
	Date:			$Date: 2012-04-19 10:50:33 +0200 (Do, 19 Apr 2012) $

	Copyright (c) 2010-2011
	Pattern Recognition Lab, 
	University Erlangen-Nuremberg, Germany
	All rights reserved.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 
	See RITKCopyright.txt for more information.

	http://www5.informatik.uni-erlangen.de/research/software/ritk

========================================================================================*/

#include "ritkRGBRImage.h"
#include "ritkImagePayloadContainer.h"
#include <itkVectorExpandImageFilter.h>

namespace ritk
{

//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
RGBRImage<TRealType, TRGB, NDimension>
::RGBRImage()
{
	m_MappedRGBDataPayloadIdx = -1;

	// Append the rgb payload to the internal payload vector
	AppendImagePayloadContainerMacro(m_RGBDataPayloadIdx, RGBImageType, RGBImageType::New(), "RGBData");
	AppendImagePayloadContainerMacro(m_MappedRGBDataPayloadIdx, RGBImageType, RGBImageType::New(), "MappedRGBData");

	// By default no mapping will be done
	m_MappingActive = false;
	
	// Initialize the range to world coord filter
	m_MappingFilter = MappingFilterType::New();
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
RGBRImage<TRealType, TRGB, NDimension>
::~RGBRImage()
{
}


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

	this->GetRGBImage()->SetLargestPossibleRegion(Region);

	this->GetMappedRGBImage()->SetLargestPossibleRegion(Region);
}


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

	this->GetRGBImage()->SetRequestedRegion(Region);

	this->GetMappedRGBImage()->SetRequestedRegion(Region);
}


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

	this->GetRGBImage()->SetBufferedRegion(Region);

	this->GetMappedRGBImage()->SetBufferedRegion(Region);
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::Finish()
{
	Superclass::Finish();
	if(m_MappingActive)
	{
		MapRGBData();
	}
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::SetCameraParametersRGB(CameraParameters CameraParameters)
{
	m_CameraParametersRGB = CameraParameters;
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
CameraParameters&
RGBRImage<TRealType, TRGB, NDimension>
::GetCameraParametersRGB()
{
	return m_CameraParametersRGB;
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
const CameraParameters&
RGBRImage<TRealType, TRGB, NDimension>
::GetCameraParametersRGB() const
{
	return (const CameraParameters&)m_CameraParametersRGB;
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::SetCameraTranslation(TransVectorType CameraTranslation)
{
	m_CameraTranslation = CameraTranslation;
	m_MappingFilter->SetCameraTranslation(m_CameraTranslation);
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
const typename RGBRImage<TRealType, TRGB, NDimension>::TransVectorType&
RGBRImage<TRealType, TRGB, NDimension>
::GetCameraTranslation() const
{
	return m_CameraTranslation;
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::SetCameraRotation(RotMatrixType CameraRotation)
{
	m_CameraRotation = CameraRotation;
	m_MappingFilter->SetCameraRotation(m_CameraRotation);
}

//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
const typename RGBRImage<TRealType, TRGB, NDimension>::RotMatrixType&
RGBRImage<TRealType, TRGB, NDimension>
::GetCameraRotation() const
{
	return m_CameraRotation;
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::MapRGBData()
{
	//// Set all regions and allocate
	//SizeType ImageSize;
	//ImageSize[0] = 640;
	//ImageSize[1] = 500;
	//IndexType Index;
	//Index[0] = 0;
	//Index[1] = 0;
	//RegionType Region;
	//Region.SetIndex(Index);
	//Region.SetSize(ImageSize);
	//ritk::RImageF2::Pointer testImg = ritk::RImageF2::New();
	//testImg->SetRegions(Region);
	//testImg->Allocate();
	//float* rangePtr = testImg->GetRangeImage()->GetBufferPointer();
	//for(int i=0; i< 640*500; ++i, ++rangePtr)
	//	*rangePtr = -50;
	//testImg->GetRangeImage()->Modified();
	//CameraParameters tmpCameraParameters = m_CameraParameters;
	//tmpCameraParameters[CameraParameters::CX] *= 10;
	//tmpCameraParameters[CameraParameters::CY] *= 10;
	//tmpCameraParameters[CameraParameters::FX] *= 10;
	//tmpCameraParameters[CameraParameters::FY] *= 10;
	//testImg->SetCameraParameters(tmpCameraParameters);
	//testImg->ComputeRangeToWorldCoords();

	//typedef itk::VectorExpandImageFilter< typename Superclass::WorldCoordImageType, typename Superclass::WorldCoordImageType > VectorExpandFilterType;
	//typename VectorExpandFilterType::Pointer vectorExpandFilter = VectorExpandFilterType::New();
	//vectorExpandFilter->SetInput(this->GetWorldCoordImage());
	//RegionType Region1 = this->GetWorldCoordImage()->GetRequestedRegion();
	//vectorExpandFilter->SetExpandFactors(10.0);
	//vectorExpandFilter->Update();

	// Perform transformation
	m_MappingFilter->SetCameraParametersRGB(m_CameraParametersRGB);
	m_MappingFilter->SetUseUndistortion(this->m_UseDistortion);

	//m_MappingFilter->SetInput(vectorExpandFilter->GetOutput());
	//m_MappingFilter->GetOutput()->SetRequestedRegion(this->GetMappedRGBImage()->GetRequestedRegion());

	m_MappingFilter->GetOutput()->SetRequestedRegion(this->GetWorldCoordImage()->GetRequestedRegion());
	m_MappingFilter->SetInput(this->GetWorldCoordImage());

	m_MappingFilter->SetRGBImageInput(this->GetRGBImage());

	m_MappingFilter->Update();

	this->GetMappedRGBImage()->SetRegions(this->GetWorldCoordImage()->GetLargestPossibleRegion());

	this->SetMappedRGBImage(m_MappingFilter->GetOutput());	
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::SetMapping(bool activate)
{
	m_MappingActive = activate;
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::Modified() const
{
	Superclass::Modified();
	this->GetRGBImage()->Modified();
	this->GetMappedRGBImage()->Modified();
}


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

	// Return if we have an invalid data object
	if ( !data )
		return;

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

	// Check if we have a vaild data object
	try
	{
		Image = dynamic_cast<const Self*>(data);
	}
	catch( ... )
	{
		itkExceptionMacro(<<"Could not graft unknown type: exception occured!");
	}
	if ( !Image )
	{
		itkExceptionMacro(<<"Could not graft unknown type: could not cast!");
	}

	CopyImageMetaData(Image->GetRGBImage(),this->GetRGBImage());
	CopyImageMetaData(Image->GetMappedRGBImage(),this->GetMappedRGBImage());

	// Copy the camera parameters
	for ( unsigned int i = 0; i < CameraParameters::NumCameraParameters; i++ )
		m_CameraParametersRGB[i] = Image->GetCameraParametersRGB()[i];

	// Copy translation
	for ( unsigned int i = 0; i < 3; i++ )
		m_CameraTranslation[i] = Image->GetCameraTranslation()[i];

	// Copy translation
	for ( unsigned int i = 0; i < 3; i++ )
		for ( unsigned int j = 0; j < 3; j++)
			m_CameraRotation(i,j) = Image->GetCameraRotation()(i,j);

	this->SetCameraRotation(Image->GetCameraRotation());
	this->SetCameraTranslation(Image->GetCameraTranslation());

}



//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::Allocate()
{
	Superclass::Allocate();
	this->GetRGBImage()->Allocate();
	this->GetMappedRGBImage()->Allocate();
}


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

	// Update the payload
	this->GetRGBImage()->SetSpacing(Spacing);
	this->GetMappedRGBImage()->SetSpacing(Spacing);
}


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

	// Update the payload
	this->GetRGBImage()->SetOrigin(Origin);
	this->GetMappedRGBImage()->SetOrigin(Origin);
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::SetDirection(const DirectionType Direction)
{
	// Update the superclass
	Superclass::SetDirection(Direction);

	// Update the payload
	this->GetRGBImage()->SetDirection(Direction);
	this->GetMappedRGBImage()->SetDirection(Direction);
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::DisconnectPipeline()
{
	// Disconnect the superclass
	Superclass::DisconnectPipeline();

	// Disconnect the payload
	if ( this->GetRGBImage() )
		this->GetRGBImage()->DisconnectPipeline();
	if ( this->GetMappedRGBImage() )
		this->GetMappedRGBImage()->DisconnectPipeline();
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::SynchronizeGenericImportImageContainers()
{
	Superclass::SynchronizeGenericImportImageContainers();

	// For conveniences
	typedef unsigned long ElementIdentifier;

	// RGB data
	typedef GenericImportImageContainer<ElementIdentifier, RGBType> RGBContainerType;
	typename RGBContainerType::Pointer RGBContainer = dynamic_cast<RGBContainerType*>(this->GetRGBImage()->GetPixelContainer());
	if ( RGBContainer )
	{
		RGBContainer->Synchronize();
	}

	// Mapped RGB data
	typename RGBContainerType::Pointer MappedRGBContainer = dynamic_cast<RGBContainerType*>(this->GetMappedRGBImage()->GetPixelContainer());
	if ( MappedRGBContainer )
	{
		MappedRGBContainer->Synchronize();
	}
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
typename RGBRImage<TRealType, TRGB, NDimension>::RGBImagePointer
RGBRImage<TRealType, TRGB, NDimension>
::GetMappedRGBImage()
{
	if(m_MappingActive)
	{
		return (RGBImagePointer)static_cast<RGBImageType*>(this->GetPayloadContainer(m_MappedRGBDataPayloadIdx)->GetPayload());
	}
	else
	{
		return (RGBImagePointer)static_cast<RGBImageType*>(this->GetPayloadContainer(m_RGBDataPayloadIdx)->GetPayload());
	}
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
typename RGBRImage<TRealType, TRGB, NDimension>::RGBImageConstPointer
RGBRImage<TRealType, TRGB, NDimension>
::GetMappedRGBImage() const
{
	if(m_MappingActive)
	{
		return (RGBImageConstPointer)static_cast<const RGBImageType*>(this->GetPayloadContainer(m_MappedRGBDataPayloadIdx)->GetPayload());
	}
	else
	{
		return (RGBImageConstPointer)static_cast<const RGBImageType*>(this->GetPayloadContainer(m_RGBDataPayloadIdx)->GetPayload());
	}
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::SetMappedRGBImage(RGBImagePointer RI)
{
	this->GetPayloadContainer(m_MappedRGBDataPayloadIdx)->SetPayload(RI);
	this->Modified();
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
typename RGBRImage<TRealType, TRGB, NDimension>::RGBImagePointer
RGBRImage<TRealType, TRGB, NDimension>
::GetRGBImage()
{
	return (RGBImagePointer)static_cast<RGBImageType*>(this->GetPayloadContainer(m_RGBDataPayloadIdx)->GetPayload());
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
typename RGBRImage<TRealType, TRGB, NDimension>::RGBImageConstPointer
RGBRImage<TRealType, TRGB, NDimension>
::GetRGBImage() const
{
	return (RGBImageConstPointer)static_cast<const RGBImageType*>(this->GetPayloadContainer(m_RGBDataPayloadIdx)->GetPayload());
}


//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::SetRGBImage(RGBImagePointer RI)
{
	// Sanity check
	/** if ( this->GetRequestedRegion(RGB) != RI->GetRequestedRegion() )
		itkExceptionMacro(<<"Invalid regions detected:" << this->GetRequestedRegion(RGB) << " vs. " << RI->GetRequestedRegion()); */

	this->GetPayloadContainer(m_RGBDataPayloadIdx)->SetPayload(RI);
	this->Modified();
}


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

//----------------------------------------------------------------------------
/*template<class TRealType, class TRGB, unsigned int NDimension>
typename RGBRImage<TRealType,TRGB,NDimension>::RegionType
RGBRImage<TRealType, TRGB, NDimension>
::GetRequestedRegion(int FrameType)
{
	if (FrameType == DEPTH_AND_FLAG)
	{
		return Superclass::GetRequestedRegion();
	}
	else if (FrameType == RGB)
	{
		return this->GetRGBImage()->GetRequestedRegion();
	}
	else
	{
		itkExceptionMacro(<<"Unknown FrameType in GetRequestedRegion(FrameType) ");
	}
}*/

//----------------------------------------------------------------------------
template<class TRealType, class TRGB, unsigned int NDimension>
void
RGBRImage<TRealType, TRGB, NDimension>
::DeepCopy(const DataObject *data)
{

	Superclass::DeepCopy(data);

#if 0
	// The range image to copy
	ConstPointer Source = NULL;

	// Check if we have a vaild data object
	try
	{
		Source = dynamic_cast<const Self*>(data);
	}
	catch( ... )
	{
		itkExceptionMacro(<<"Could not graft unknown type");
	}
	if ( !Source )
	{
		itkExceptionMacro(<<"Could not cast unknown type");
	}

	// Copy the meta-information and remaining regions
	this->CopyInformation(Source);

	this->GetRangeImage()->SetLargestPossibleRegion(Source->GetRangeImage()->GetLargestPossibleRegion());
	this->GetRangeImage()->SetBufferedRegion(Source->GetRangeImage()->GetBufferedRegion());
	this->GetRangeImage()->SetRequestedRegion(Source->GetRangeImage()->GetRequestedRegion());

	this->GetFlagImage()->SetLargestPossibleRegion(Source->GetFlagImage()->GetLargestPossibleRegion());
	this->GetFlagImage()->SetBufferedRegion(Source->GetFlagImage()->GetBufferedRegion());
	this->GetFlagImage()->SetRequestedRegion(Source->GetFlagImage()->GetRequestedRegion());

	this->GetRGBImage()->SetLargestPossibleRegion(Source->GetRGBImage()->GetLargestPossibleRegion());
	this->GetRGBImage()->SetBufferedRegion(Source->GetRGBImage()->GetBufferedRegion());
	this->GetRGBImage()->SetRequestedRegion(Source->GetRGBImage()->GetRequestedRegion());

	// Allocate all subimages. Note that this should not allocate any memory if previous call to allocate used the same regions!
	this->Allocate();

	// First, check if this image stores more payloads than the input. This is a case that we can't handle...
	if ( Superclass::m_PayloadContainers.size() > Source->m_PayloadContainers.size() )
		itkExceptionMacro(<<"Cannot graft an RImage that stores less payload containers: " << Source->m_PayloadContainers.size() << " vs. " << Superclass::m_PayloadContainers.size());

	// Second, check if the input image stores more payloads. If so, clone these containers and append them
	if ( Source->m_PayloadContainers.size() > Superclass::m_PayloadContainers.size() )
	{
		for ( unsigned int i = Superclass::m_PayloadContainers.size(); i < Source->m_PayloadContainers.size(); i++ )
		{
			GenericPayloadContainerPointer tmp = dynamic_cast<ritk::GenericPayloadContainer*>((itk::LightObject*)Source->m_PayloadContainers[i]->CreateAnother());
			tmp->SetIdentifier(Source->m_PayloadContainers[i]->GetIdentifier());
			this->AppendPayloadContainer(tmp);
		}
	}

	// Deep copy all payload containers
	for ( unsigned int i = 0; i < Superclass::m_PayloadContainers.size(); i++ )
		Superclass::m_PayloadContainers[i]->DeepCopy(Source->m_PayloadContainers[i]);

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

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

	// This is not a RGBRImage
	if ( !Image )
	{
		return;
	}
#endif
}


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

#if 0
	
	// The range image to graft
	ConstPointer Source = NULL;

	// Check if we have a vaild data object
	try
	{
		Source = dynamic_cast<const Self*>(data);
	}
	catch( ... )
	{
		itkExceptionMacro(<<"Could not graft unknown type");
	}
	if ( !Source )
	{
		itkExceptionMacro(<<"Could not cast unknown type");
	}

	// Copy the meta-information and remaining regions
	this->CopyInformation(Source);
	
	this->GetRangeImage()->SetLargestPossibleRegion(Source->GetRangeImage()->GetLargestPossibleRegion());
	this->GetRangeImage()->SetBufferedRegion(Source->GetRangeImage()->GetBufferedRegion());
	this->GetRangeImage()->SetRequestedRegion(Source->GetRangeImage()->GetRequestedRegion());

	this->GetFlagImage()->SetLargestPossibleRegion(Source->GetFlagImage()->GetLargestPossibleRegion());
	this->GetFlagImage()->SetBufferedRegion(Source->GetFlagImage()->GetBufferedRegion());
	this->GetFlagImage()->SetRequestedRegion(Source->GetFlagImage()->GetRequestedRegion());

	this->GetRGBImage()->SetLargestPossibleRegion(Source->GetRGBImage()->GetLargestPossibleRegion());
	this->GetRGBImage()->SetBufferedRegion(Source->GetRGBImage()->GetBufferedRegion());
	this->GetRGBImage()->SetRequestedRegion(Source->GetRGBImage()->GetRequestedRegion());

	// First, check if this image stores more payloads than the input. This is a case that we can't handle...
	if ( Superclass::m_PayloadContainers.size() > Source->m_PayloadContainers.size() )
		itkExceptionMacro(<<"Cannot graft an RImage that stores less payload containers: " << Source->m_PayloadContainers.size() << " vs. " << Superclass::m_PayloadContainers.size());

	// Second, check if the input image stores more payloads. If so, clone these containers and append them
	if ( Source->m_PayloadContainers.size() > Superclass::m_PayloadContainers.size() )
	{
		for ( unsigned int i = Superclass::m_PayloadContainers.size(); i < Source->m_PayloadContainers.size(); i++ )
		{
			GenericPayloadContainerPointer tmp = dynamic_cast<GenericPayloadContainer*>((itk::LightObject*)Source->m_PayloadContainers[i]->CreateAnother());
			tmp->SetIdentifier(Source->m_PayloadContainers[i]->GetIdentifier());
			this->AppendPayloadContainer(tmp);
		}
	}

	// Graft all payload containers
	for ( unsigned int i = 0; i < Superclass::m_PayloadContainers.size(); i++ )
		Superclass::m_PayloadContainers[i]->Graft(Source->m_PayloadContainers[i]);
#endif
}


} // end namespace ritk


