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

	Program:		Range Imaging Toolkit (RITK)
	Module:			ritkTruncatedSignedDistanceFunction.cxx
	Version:		$Revision: 3136 $
	Date:			$Date: 2012-04-19 14:42:59 +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 "ritkTruncatedSignedDistanceFunction.h"
#include "ritkTruncatedSignedDistanceFunctionInterfaces.h"

#include <itkImageFileWriter.h>

#include <vnl/vnl_double_3x3.h>
#include <vnl/vnl_double_3.h>

//#define PERFORMANCE_CHECK

namespace ritk
{

//----------------------------------------------------------------------------
TruncatedSignedDistanceFunction::TruncatedSignedDistanceFunction()
{
	// Create the pixel containers for the TSDF
	Superclass::SetPixelContainer(TSDFContainerType::New());
	m_IntermediateTSDF = IntermediateTSDFContainerType::New();

	// Create the range and flag data containers
	m_RangeDataContainer = RangeDataContainerType::New();
	m_FlagDataContainer = FlagDataContainerType::New();

	// Cutoff and invalid values not initialized by the user
	m_Initialized = false;
	m_CutoffValue = 50.f;
	m_InvalidValue = 1000.f;

	m_NumberOfIntegratedRangeImages = 0;
}


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


//----------------------------------------------------------------------------
void
TruncatedSignedDistanceFunction::SetBufferedRegion(const RegionType &Region)
{
	// Update the superclass
	Superclass::SetBufferedRegion(Region);

	// Delegate to the containers
	TSDFContainerType::SizeType TSDFSize;
	TSDFSize[0] = Region.GetSize()[0];
	TSDFSize[1] = Region.GetSize()[1];
	TSDFSize[2] = Region.GetSize()[2];
	TSDFContainerPointer TSDF = dynamic_cast<TSDFContainerType*>(Superclass::GetPixelContainer());
	if ( !TSDF )
		itkExceptionMacro(<<"Invalid container detected!");
	TSDF->SetContainerSize(TSDFSize);
	m_IntermediateTSDF->SetContainerSize(TSDFSize);
}


//----------------------------------------------------------------------------
void
TruncatedSignedDistanceFunction::IntegrateRangeImage(RangeImageConstPointer RI, ProjectionMatrixType P)
{
	// Get the actual TSDF
	TSDFContainerPointer TSDF = dynamic_cast<TSDFContainerType*>(Superclass::GetPixelContainer());
	if ( !TSDF )
		itkExceptionMacro(<<"Invalid container detected!");

	// Reset if not done so
	if ( !m_Initialized )
	{
		Reset();
		m_Initialized = true;
	}

	// Allocate the intermediate TSDF on demand
	if ( m_IntermediateTSDF->Capacity() != TSDF->Capacity() || m_IntermediateTSDF->GetContainerSize() != TSDF->GetContainerSize() )
	{
		m_IntermediateTSDF->ReserveMemory(TSDF->GetContainerSize());
		std::cout << "Initializing intermediate TSDF..." << std::endl;
		for ( unsigned int i = 0; i < TSDF->Capacity(); i++ )
			m_IntermediateTSDF->GetBufferPointer()[i] = m_InvalidValue;
		m_IntermediateTSDF->SynchronizeDevice();
	}


#ifdef PERFORMANCE_CHECK
	cudaEvent_t start_event, stop_event;
    cudaEventCreateWithFlags(&start_event, cudaEventBlockingSync);
    cudaEventCreateWithFlags(&stop_event, cudaEventBlockingSync);
	cudaEventRecord(start_event, 0);
	int NumRuns = 50;
	for ( int i = 0; i < NumRuns; i++ )
	{
#endif


	// Copy the range data to our container
	RangeImageType::RangeImageConstPointer RangeData = RI->GetRangeImage();
	if ( m_RangeDataContainer->Capacity() != RangeData->GetPixelContainer()->Capacity() || m_RangeDataContainer->GetContainerSize()[0] != RI->GetRequestedRegion().GetSize()[0] ||  m_RangeDataContainer->GetContainerSize()[1] != RI->GetRequestedRegion().GetSize()[1] )
	{
		RangeDataContainerType::SizeType RangeDataSize = RangeData->GetRequestedRegion().GetSize();
		m_RangeDataContainer->ReserveMemory(RangeDataSize);
	}
	m_RangeDataContainer->DeepCopy(RangeData->GetPixelContainer());

	// Copy the flag data to our container
	RangeImageType::FlagImageConstPointer FlagData = RI->GetFlagImage();
	if ( m_FlagDataContainer->Capacity() != FlagData->GetPixelContainer()->Capacity() || m_FlagDataContainer->GetContainerSize()[0] != RI->GetRequestedRegion().GetSize()[0] ||  m_FlagDataContainer->GetContainerSize()[1] != RI->GetRequestedRegion().GetSize()[1] )
	{
		FlagDataContainerType::SizeType FlagDataSize = FlagData->GetRequestedRegion().GetSize();
		m_FlagDataContainer->ReserveMemory(FlagDataSize);
	}
	m_FlagDataContainer->DeepCopy(FlagData->GetPixelContainer());

	m_RangeDataContainer->SynchronizeHost();
	m_FlagDataContainer->SynchronizeHost();
	float rmin = 1e16;
	float rmax = -1e16;
	for ( unsigned int i = 0; i < m_RangeDataContainer->Capacity(); i++ )
	{
		float f = m_RangeDataContainer->GetBufferPointer()[i];
		rmin = std::min(rmin,f);
		rmax = std::max(rmax,f);
		if ( f != f )
			std::cout << "qnan" << std::endl;
		if ( f < 400 || f != f )
			m_FlagDataContainer->GetBufferPointer()[i] = 0;
		else
			m_FlagDataContainer->GetBufferPointer()[i] = 1;
	}
	m_FlagDataContainer->SynchronizeDevice();
	std::cout << "min=" << rmin << " max=" << rmax << std::endl;

	


	
	// Set up the integration config
	ritkTSDFIntegrationConfig TSDFIntegrationConfig;

	// Projection matrix
	for ( int r = 0; r < 3; r++ )
		for ( int c = 0; c < 4; c++ )
			TSDFIntegrationConfig.ProjectionMatrix[r*4+c] = P[r][c];

	// Optical center and plane normal using the inverse projection matrix
	vnl_double_3x3 tmp;
	tmp(0,0) = P(0,0); tmp(0,1) = P(0,1); tmp(0,2) = P(0,2);
	tmp(1,0) = P(1,0); tmp(1,1) = P(1,1); tmp(1,2) = P(1,2);
	tmp(2,0) = P(2,0); tmp(2,1) = P(2,1); tmp(2,2) = P(2,2);
	vnl_svd<double> SVD(tmp);
	vnl_double_3x3 IPM = SVD.pinverse();
	vnl_double_3 q;
	q(0) = -1*P(0,3);
	q(1) = -1*P(1,3);
	q(2) = -1*P(2,3);
	q = IPM*q;
	TSDFIntegrationConfig.OpticalCenter[0] = q[0];
	TSDFIntegrationConfig.OpticalCenter[1] = q[1];
	TSDFIntegrationConfig.OpticalCenter[2] = q[2];
	unsigned int CenterX = 320;
	unsigned int CenterY = 240;
	float NormalX = IPM[0][0]*CenterX + IPM[0][1]*CenterY + IPM[0][2];
	float NormalY = IPM[1][0]*CenterX + IPM[1][1]*CenterY + IPM[1][2];
	float NormalZ = IPM[2][0]*CenterX + IPM[2][1]*CenterY + IPM[2][2];
	float Mag = -1.f/sqrt((NormalX)*(NormalX) + (NormalY)*(NormalY) + (NormalZ)*(NormalZ));
	NormalX *= Mag;
	NormalY *= Mag;
	NormalZ *= Mag;
	TSDFIntegrationConfig.PlaneNormal[0] = NormalX;
	TSDFIntegrationConfig.PlaneNormal[1] = NormalY;
	TSDFIntegrationConfig.PlaneNormal[2] = NormalZ;

	// Camera size (in pixel)
	TSDFIntegrationConfig.CameraSize[0] = RangeData->GetRequestedRegion().GetSize()[0];
	TSDFIntegrationConfig.CameraSize[1] = RangeData->GetRequestedRegion().GetSize()[1];

	// The range data array
	TSDFIntegrationConfig.RangeData = m_RangeDataContainer->GetCudaMemoryArray();
	TSDFIntegrationConfig.RangeDataDesc = m_RangeDataContainer->GetChannelDesc();

	// The flag data array
	TSDFIntegrationConfig.FlagData = m_FlagDataContainer->GetCudaMemoryArray();
	TSDFIntegrationConfig.FlagDataDesc = m_FlagDataContainer->GetChannelDesc(); 

	// The TSDF volume
	TSDFIntegrationConfig.TSDF = m_IntermediateTSDF->GetCudaPitchedMemoryPointer();
	TSDFIntegrationConfig.TSDFVolumeOrigin[0] = this->GetOrigin()[0];
	TSDFIntegrationConfig.TSDFVolumeOrigin[1] = this->GetOrigin()[1];
	TSDFIntegrationConfig.TSDFVolumeOrigin[2] = this->GetOrigin()[2];
	TSDFIntegrationConfig.TSDFVolumeSpacing[0] = this->GetSpacing()[0];
	TSDFIntegrationConfig.TSDFVolumeSpacing[1] = this->GetSpacing()[1];
	TSDFIntegrationConfig.TSDFVolumeSpacing[2] = this->GetSpacing()[2];
	TSDFIntegrationConfig.TSDFVolumeSize[0] = this->GetBufferedRegion().GetSize()[0];
	TSDFIntegrationConfig.TSDFVolumeSize[1] = this->GetBufferedRegion().GetSize()[1];
	TSDFIntegrationConfig.TSDFVolumeSize[2] = this->GetBufferedRegion().GetSize()[2];
	TSDFIntegrationConfig.CutoffValue = m_CutoffValue;
	TSDFIntegrationConfig.InvalidValue = m_InvalidValue;

	// Integration kernel invocation
	ritkTSDFIntegration(&TSDFIntegrationConfig);

	m_NumberOfIntegratedRangeImages++;

	// Copy
	TSDF->DeepCopy(m_IntermediateTSDF);
	this->Modified();



#ifdef PERFORMANCE_CHECK
	}
	cudaEventRecord(stop_event, 0);
    cudaEventSynchronize(stop_event);   
	float Time = 0;
    cudaEventElapsedTime(&Time, start_event, stop_event);
	std::cout << "Elapsed time for TSDF update: " << (Time/NumRuns) << std::endl;
#endif



	/*this->SynchronizeHost();

	typedef itk::ImageFileWriter<Superclass> WriterType;
	WriterType::Pointer Writer = WriterType::New();
	Writer->SetInput(this);
	Writer->SetFileName("D:/Temp/TSDF.mha");
	try
	{
		Writer->Update();
	}
	catch ( itk::ExceptionObject &e )
	{
		std::cerr << e << std::endl;
	}*/
}


//----------------------------------------------------------------------------
void
TruncatedSignedDistanceFunction::Reset(float CutoffValue, float InvalidValue)
{
	Superclass::FillBuffer(InvalidValue);
	this->SynchronizeDevice();
	m_IntermediateTSDF = IntermediateTSDFContainerType::New();
	
	m_CutoffValue = 50.f;
	m_InvalidValue = InvalidValue;
}


//----------------------------------------------------------------------------
void
TruncatedSignedDistanceFunction::Synchronize()
{
	TSDFContainerPointer TSDF = dynamic_cast<TSDFContainerType*>(Superclass::GetPixelContainer());
	if ( !TSDF )
		itkExceptionMacro(<<"Invalid container detected!");
	TSDF->Synchronize();
}


//----------------------------------------------------------------------------
void
TruncatedSignedDistanceFunction::SynchronizeHost()
{
	TSDFContainerPointer TSDF = dynamic_cast<TSDFContainerType*>(Superclass::GetPixelContainer());
	if ( !TSDF )
		itkExceptionMacro(<<"Invalid container detected!");
	TSDF->SynchronizeHost();
}


//----------------------------------------------------------------------------
void
TruncatedSignedDistanceFunction::SynchronizeDevice()
{
	TSDFContainerPointer TSDF = dynamic_cast<TSDFContainerType*>(Superclass::GetPixelContainer());
	if ( !TSDF )
		itkExceptionMacro(<<"Invalid container detected!");
	TSDF->SynchronizeDevice();
}


//----------------------------------------------------------------------------
void
TruncatedSignedDistanceFunction::PrintSelf(std::ostream& os, itk::Indent indent) const
{
	Superclass::PrintSelf(os, indent);
	os << indent << "TruncatedSignedDistanceFunction" << std::endl;
}

} //ritk
