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

*/
#include <iostream>

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

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

using namespace std;
using namespace NICE;
using namespace OBJREC;

const int maxiterations = 30;
const bool exponential_noise = true;

CholeskyRobustAuto::CholeskyRobustAuto ( const CholeskyRobustAuto & src )
	: CholeskyRobust ( src )
{
}

double CholeskyRobustAuto::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 << "CholeskyRobustAuto::robustChol: A " << A.rows() << " x " << A.cols() << endl;

	int i = 0;
	double noise = 0.0;
	double noiseStepExp = m_noiseStep;
	bool robust = true;
	do {
		robust = true;

		if ( m_verbose )
			cerr << "CholeskyRobustAuto::robustChol: Cholesky decomposition" << endl;

		try {
			//choleskyDecomp ( ARegularized, cholA );
			#ifdef NICE_USELIB_CUDACHOLESKY
			if ( m_useCuda ) 
				CudaCholeskyNICE::choleskyDecomp ( ARegularized, cholA );
			else
			#endif
				choleskyDecompLargeScale ( ARegularized, cholA );
#ifdef NICE_USELIB_CUDACHOLESKY
		} catch ( CudaError & e ) {
			fthrow(CudaError, e.what() );
#endif
		} catch ( Exception ) {
			if ( m_verbose )
				cerr << "CholeskyRobustAuto::robustChol: failed!" << endl;
			robust = false;
		}

		if ( robust && cholA.containsNaN() ) 
		{
			if ( m_verbose )
				cerr << "CholeskyRobustAuto::robustChol: cholesky matrix contains NaN values" << endl;
			robust = false; 
		}

		m_logDetMatrix = m_minMatrixLogDeterminant;
		if ( robust ) 
		{
			m_logDetMatrix = 2*triangleMatrixLogDet ( cholA );
			if ( m_verbose ) 
				cerr << "CholeskyRobustAuto::robustChol: Cholesky condition: " << m_logDetMatrix << endl;
		}


		if ( isnan(m_logDetMatrix) || (m_logDetMatrix < m_minMatrixLogDeterminant) ) {	
			robust = false;
		}

		if ( !robust ) {	
			robust = false;
			if ( m_verbose ) 
				cerr << "CholeskyRobustAuto::robustChol: Adding noise " << noiseStepExp << endl;
			ARegularized.addIdentity ( noiseStepExp );
			noise += noiseStepExp;

			if ( exponential_noise ) 
				noiseStepExp *= 10.0;
		}

		i++;
	} while ( !robust && (i < maxiterations) );

	if ( !robust ) 
		fthrow( Exception, "Inverse matrix is instable (please adjust the noise step term)" );

	return noise;
}

CholeskyRobustAuto::~CholeskyRobustAuto ()
{
}

CholeskyRobustAuto::CholeskyRobustAuto ( bool verbose, double noiseStep, double minMatrixLogDeterminant, bool useCuda ) :
	CholeskyRobust ( verbose, noiseStep, minMatrixLogDeterminant, useCuda )
{
}
		
CholeskyRobustAuto *CholeskyRobustAuto::clone(void) const
{
	return new CholeskyRobustAuto ( *this );
}