/** 
* @file CholeskyRobust.cpp
* @brief robust cholesky decomposition by adding some noise
* @author Erik Rodner
* @date 01/06/2010

*/
#include <iostream>

#include "CholeskyRobust.h"
#include "core/vector/Algorithms.h"

#ifdef NICE_USELIB_CUDACHOLESKY
#include "cholesky-gpu/niceinterface/CudaCholeskyNICE.h"
#endif

using namespace std;
using namespace NICE;
using namespace OBJREC;
		
		
CholeskyRobust::CholeskyRobust ( const CholeskyRobust & src )
	: m_verbose(src.m_verbose), m_noiseStep(src.m_noiseStep),
	  m_minMatrixLogDeterminant(src.m_minMatrixLogDeterminant),
	  m_useCuda(src.m_useCuda), m_logDetMatrix(src.m_logDetMatrix)
{
}

double CholeskyRobust::robustChol ( const Matrix & A, Matrix & cholA )
{
	// this additional memory requirement could be skipped
	// with a modified cholesky routine
	Matrix ARegularized (A);

	if ( m_verbose )
		cerr << "CholeskyRobust::robustChol: Adding noise " << m_noiseStep << endl;
	ARegularized.addIdentity ( m_noiseStep );

#ifdef NICE_USELIB_CUDACHOLESKY
	if ( m_useCuda ) 
		CudaCholeskyNICE::choleskyDecomp ( ARegularized, cholA );
	else
#endif
		choleskyDecompLargeScale ( ARegularized, cholA );
	
	m_logDetMatrix = 2*triangleMatrixLogDet ( cholA );
	if ( m_verbose ) 
			cerr << "CholeskyRobust::robustChol: Cholesky condition: " << m_logDetMatrix << endl;

	if ( !finite(m_logDetMatrix) )
	{
		fthrow(Exception, "The matrix has infinite or not defined log determinant !" );
	}

	return m_noiseStep;
}

double CholeskyRobust::robustCholInv ( const Matrix & A, Matrix & invA )
{
	Matrix G;
	double noise = robustChol ( A, G  );
	if ( m_verbose )
			cerr << "CholeskyRobust::robustChol: Cholesky inversion" << endl;

	choleskyInvertLargeScale ( G, invA );

	return noise;
}


CholeskyRobust::~CholeskyRobust ()
{
}

CholeskyRobust::CholeskyRobust ( bool verbose, double noiseStep, double minMatrixLogDeterminant, bool useCuda ) :
	m_verbose(verbose), m_noiseStep(noiseStep), m_minMatrixLogDeterminant(minMatrixLogDeterminant), m_useCuda ( useCuda )
{
	m_logDetMatrix = m_minMatrixLogDeterminant;
}

CholeskyRobust *CholeskyRobust::clone(void) const
{
	CholeskyRobust *dst = new CholeskyRobust ( *this );
	return dst;
}