/*
Copyright (c) 2016, Michael Kazhdan and Fabian Prada
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer. Redistributions in binary form must reproduce
the above copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the distribution. 

Neither the name of the Johns Hopkins University nor the names of its contributors
may be used to endorse or promote products derived from this software without specific
prior written permission. 

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES 
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
TO, PROCUREMENT OF SUBSTITUTE  GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE.
*/

#ifndef LINEAR_SOLVERS_INCLUDE
#define LINEAR_SOLVERS_INCLUDE

#ifdef USE_CHOLMOD
#include <Cholmod/cholmod.h>
#ifdef WIN32
#pragma comment( lib , "CHOLMOD.lib" )
#endif // WIN32
#ifdef DLONG
typedef long long SOLVER_LONG;
#define CHOLMOD( name ) cholmod_l_ ## name
#else // !DLONG
typedef       int SOLVER_LONG;
#define CHOLMOD( name ) cholmod_ ## name
#endif // DLONG
#elif defined(EIGEN_USE_MKL_ALL)
#pragma comment( lib , "mkl_core.lib" )
#pragma comment( lib , "mkl_intel_lp64.lib" )
#pragma comment( lib , "mkl_intel_thread.lib" )
#pragma comment( lib , "mkl_blas95_lp64.lib" )
#pragma comment( lib , "libiomp5md.lib" )
#endif // USE_CHOLMOD

#include <Misha/SparseMatrixInterface.h>

inline double                        SquareNorm( const double* values , int dim ){ double norm2 = 0 ; for( int i=0 ; i<dim ; i++ ) norm2 += values[i] * values[i] ; return norm2; }
inline double                        SquareNorm( const  float* values , int dim ){ double norm2 = 0 ; for( int i=0 ; i<dim ; i++ ) norm2 += values[i] * values[i] ; return norm2; }
template< class Type > inline double SquareNorm( const   Type* values , int dim ){ double norm2 = 0 ; for( int i=0 ; i<dim ; i++ ) norm2 += values[dim].squareNorm()  ; return norm2 ; }

inline double                        SquareDifference( const double* values1 , const double* values2 , int dim ){ double norm2 = 0 ; for( int i=0 ; i<dim ; i++ ) norm2 += ( values1[i] - values2[i] ) * ( values1[i] - values2[i] ) ; return norm2; }
inline double                        SquareDifference( const  float* values1 , const  float* values2 , int dim ){ double norm2 = 0 ; for( int i=0 ; i<dim ; i++ ) norm2 += ( values1[i] - values2[i] ) * ( values1[i] - values2[i] ) ; return norm2; }
template< class Type > inline double SquareDifference( const   Type* values1 , const   Type* values2 , int dim ){ double norm2 = 0 ; for( int i=0 ; i<dim ; i++ ) norm2 += ( values1[dim] - values2[dim] ).squareNorm()  ; return norm2 ; }


// This is the conjugate gradients solver.
// The assumption is that the class SPDOperator defines a method operator()( const Real* , Real* ) which corresponds to applying a symmetric positive-definite operator.
template< class Real >
struct CGScratch
{
	Real *r , *d , *q;
	CGScratch( void ) : r(NULL) , d(NULL) , q(NULL) , _dim(0){ ; }
	CGScratch( int dim ) : r(NULL) , d(NULL) , q(NULL){ resize(dim); }
	~CGScratch( void ){ resize(0); }
	void resize( int dim )
	{
		if( dim!=_dim )
		{
			if( r ) delete[] r ; r = NULL;
			if( d ) delete[] d ; d = NULL;
			if( q ) delete[] q ; q = NULL;
			if( dim ) r = new Real[dim] , d = new Real[dim] , q = new Real[dim];
			_dim = dim;
		}
	}
protected:
	int _dim;
};
template< class Real >
struct PreconditionedCGScratch : public CGScratch< Real >
{
	Real *s;
	PreconditionedCGScratch( void ) : CGScratch< Real >() , s(NULL){ ; }
	PreconditionedCGScratch( int dim ) : CGScratch< Real >() { resize(dim); }
	~PreconditionedCGScratch( void ){ resize(0); }
	void resize( int dim )
	{
		if( dim!=CGScratch< Real >::_dim )
		{
			if( s ) delete[] s; s = NULL;
			if( dim ) s = new Real[dim];
		}
		CGScratch< Real >::resize( dim );
	}
};
template< class Real >
struct DiagonalPreconditioner
{
	Real* iDiagonal;
	DiagonalPreconditioner( void ) : iDiagonal(NULL) , _dim(0){ ; }
	~DiagonalPreconditioner( void ){ if( iDiagonal ) delete[] iDiagonal ; iDiagonal = NULL; }
	template< class MatrixRowIterator >
	void set( const SparseMatrixInterface< Real , MatrixRowIterator >& M )
	{
		if( _dim!=M.Rows() )
		{
			_dim = (int)M.Rows();
			if( iDiagonal ) delete[] iDiagonal , iDiagonal = NULL;
			if( _dim>0 ) iDiagonal = new Real[_dim];
		}
		memset( iDiagonal , 0 , sizeof(Real)*_dim );
#pragma omp parallel for
		for( int i=0 ; i<M.Rows() ; i++ )
		{
			for( MatrixRowIterator iter=M.begin(i) ; iter!=M.end(i) ; iter++ ) if( iter->N==i ) iDiagonal[i] += iter->Value;
			iDiagonal[i] = (Real)1./iDiagonal[i];
		}
	}
	void operator()( const Real* in , Real* out ) const
	{
#pragma omp parallel for
		for( int i=0 ; i<_dim ; i++ ) out[i] = in[i] * iDiagonal[i];
	}
protected:
	int _dim;
};

template< class Real , class SPDOperator >
int SolveCG( SPDOperator& L , int iters , int dim , const Real* b , Real* x , CGScratch< Real >* scratch=NULL , double eps=1e-8 , int threads=1 , bool verbose=false )
{
	eps *= eps;
	Real *r , *d , *q;
	if( scratch ) r = scratch->r    , d = scratch->d    , q = scratch->q;
	else          r = new Real[dim] , d = new Real[dim] , q = new Real[dim];
	memset( r , 0 , sizeof(Real)*dim ) , memset( d , 0 , sizeof(Real)*dim ) , memset( q , 0 , sizeof(Real)*dim );
	double delta_new = 0 , delta_0;

	L( x , r );
#pragma omp parallel for num_threads( threads ) reduction( + : delta_new )
	for( int i=0 ; i<dim ; i++ ) d[i] = r[i] = b[i] - r[i] , delta_new += r[i] * r[i];

	delta_0 = delta_new;
	if( delta_new<eps )
	{
		if( !scratch ) delete[] r , delete[] d , delete[] q;
		return 0;
	}

	int ii;
	for( ii=0 ; ii<iters && delta_new>eps*delta_0 ; ii++ )
	{
		L( d , q );
        double dDotQ = 0;
#pragma omp parallel for num_threads( threads ) reduction( + : dDotQ )
		for( int i=0 ; i<dim ; i++ ) dDotQ += d[i] * q[i];
		Real alpha = Real( delta_new / dDotQ );

		double delta_old = delta_new;
		delta_new = 0;

		const int RESET_COUNT = 50;
		if( (ii%RESET_COUNT)==(RESET_COUNT-1) )
		{
#pragma omp parallel for num_threads( threads )
			for( int i=0 ; i<dim ; i++ ) x[i] += d[i] * alpha;
			L( x , r );
#pragma omp parallel for num_threads( threads ) reduction ( + : delta_new )
			for( int i=0 ; i<dim ; i++ ) r[i] = b[i] - r[i] , delta_new += r[i] * r[i];
		}
		else
#pragma omp parallel for num_threads( threads ) reduction( + : delta_new )
			for( int i=0 ; i<dim ; i++ ) r[i] -= q[i] * alpha , delta_new += r[i] * r[i] , x[i] += d[i] * alpha;

		Real beta = Real( delta_new / delta_old );
#pragma omp parallel for num_threads( threads )
		for( int i=0 ; i<dim ; i++ ) d[i] = r[i] + d[i] * beta;
	}
	if( verbose )
	{
		L( x , r );
#pragma omp parallel for num_threads( threads )
		for( int i=0 ; i<dim ; i++ ) r[i] -= b[i];
		printf( "CG: %d %g -> %g\n" , ii , SquareNorm( b , dim ) , SquareNorm( r , dim ) );
	}
	if( !scratch ) delete[] r , delete[] d , delete[] q;
	return ii;
}
template< class Real , class SPDOperator , class SPDPreconditioner >
int SolvePreconditionedCG( SPDOperator& L , SPDPreconditioner& Pinverse , int iters , int dim , const Real* b , Real* x , PreconditionedCGScratch< Real >* scratch=NULL , double eps=1e-8 , int threads=1 , bool verbose=false )
{
	eps *= eps;
	Real *r , *d , *q , *s;
	if( scratch ) r = scratch->r    , d = scratch->d    , q = scratch->q , s = scratch->s;
	else          r = new Real[dim] , d = new Real[dim] , q = new Real[dim] , s = new Real[dim];
	memset( r , 0 , sizeof(Real)*dim ) , memset( d , 0 , sizeof(Real)*dim ) , memset( q , 0 , sizeof(Real)*dim ) , memset( s , 0 , sizeof(Real)*dim );
	double delta_new = 0 , delta_0;

	L( x , r );
#pragma omp parallel for num_threads( threads )
	for( int i=0 ; i<dim ; i++ ) r[i] = b[i] - r[i];
	Pinverse( r , d );
#pragma omp parallel for num_threads( threads ) reduction( + : delta_new )
	for( int i=0 ; i<dim ; i++ ) delta_new += r[i] * d[i];

	delta_0 = delta_new;
	if( delta_new<eps )
	{
		if( !scratch ) delete[] r , delete[] d , delete[] q;
		return 0;
	}
	int ii;
	for( ii=0 ; ii<iters && delta_new>eps*delta_0 ; ii++ )
	{
		L( d , q );
        double dDotQ = 0;
#pragma omp parallel for num_threads( threads ) reduction( + : dDotQ )
		for( int i=0 ; i<dim ; i++ ) dDotQ += d[i] * q[i];
		Real alpha = Real( delta_new / dDotQ );

		const int RESET_COUNT = 50;
#pragma omp parallel for num_threads( threads )
		for( int i=0 ; i<dim ; i++ ) x[i] += d[i] * alpha;
		if( (ii%RESET_COUNT)==(RESET_COUNT-1) )
		{
			L( x , r );
#pragma omp parallel for num_threads( threads )
			for( int i=0 ; i<dim ; i++ ) r[i] = b[i] - r[i];
		}
		else
#pragma omp parallel for num_threads( threads ) reduction( + : delta_new )
			for( int i=0 ; i<dim ; i++ ) r[i] -= q[i] * alpha;
		Pinverse( r , s );

		double delta_old = delta_new;
		delta_new = 0;
#pragma omp parallel for num_threads( threads ) reduction( + : delta_new )
		for( int i=0 ; i<dim ; i++ ) delta_new += r[i] * s[i];

		Real beta = Real( delta_new / delta_old );
#pragma omp parallel for num_threads( threads )
		for( int i=0 ; i<dim ; i++ ) d[i] = s[i] + d[i] * beta;
	}
	if( verbose )
	{
		L( x , r );
#pragma omp parallel for num_threads( threads )
		for( int i=0 ; i<dim ; i++ ) r[i] -= b[i];
		printf( "PCCG: %d %g -> %g\n" , ii , SquareNorm( b , dim ) , SquareNorm( r , dim ) );
	}
	if( !scratch ) delete[] r , delete[] d , delete[] q , delete[] s;
	return ii;
}


#ifdef USE_EIGEN
#define STORE_EIGEN_MATRIX
#ifdef EIGEN_USE_MKL_ALL
#include <Eigen/PardisoSupport>
#else // !EIGEN_USE_MKL_ALL
#include <Eigen/Sparse>
#endif // EIGEN_USE_MKL_ALL

template< class Real , class MatrixRowIterator >
struct EigenSolver
{
	virtual void update( const SparseMatrixInterface< Real , MatrixRowIterator >& M ) = 0;
	virtual void solve( ConstPointer( Real ) b , Pointer( Real ) x ) = 0;
	virtual size_t dimension( void ) const = 0;
};

template< class Real , class MatrixRowIterator >
class EigenSolverCholeskyLLt : public EigenSolver< Real , MatrixRowIterator >
{
#ifdef EIGEN_USE_MKL_ALL
	typedef Eigen::PardisoLLT< Eigen::SparseMatrix< double > > Eigen_Solver;
	typedef Eigen::VectorXd                                    Eigen_Vector;
#else // !EIGEN_USE_MKL_ALL
	typedef Eigen::SimplicialLLT< Eigen::SparseMatrix< double > > Eigen_Solver;
	typedef Eigen::VectorXd                                       Eigen_Vector;
#endif // EIGEN_USE_MKL_ALL
	Eigen_Solver _solver;
	Eigen_Vector _eigenB;
#ifdef STORE_EIGEN_MATRIX
	Eigen::SparseMatrix< double > _eigenM;
#endif // STORE_EIGEN_MATRIX
public:
	EigenSolverCholeskyLLt( const SparseMatrixInterface< Real , MatrixRowIterator >& M , bool analyzeOnly=false )
	{
#ifdef STORE_EIGEN_MATRIX
		_eigenM.resize( int( M.Rows() ) , int( M.Rows() ) );
#else // !STORE_EIGEN_MATRIX
		Eigen::SparseMatrix< double > eigenM( int( M.Rows() ) , int( M.Rows() ) );
#endif // STORE_EIGEN_MATRIX
		std::vector< Eigen::Triplet< double > > triplets;
		triplets.reserve( M.Entries() );
		for( int i=0 ; i<M.Rows() ; i++ ) for( MatrixRowIterator iter=M.begin(i) ; iter!=M.end(i) ; iter++ ) triplets.push_back( Eigen::Triplet< double >( i , iter->N , iter->Value ) );
#ifdef STORE_EIGEN_MATRIX
		_eigenM.setFromTriplets( triplets.begin() , triplets.end() );
		_solver.analyzePattern( _eigenM );
#else // !STORE_EIGEN_MATRIX
		eigenM.setFromTriplets( triplets.begin() , triplets.end() );
		_solver.analyzePattern( eigenM );
#endif // STORE_EIGEN_MATRIX
		if( !analyzeOnly )
		{
#ifdef STORE_EIGEN_MATRIX
			_solver.factorize( _eigenM );
#else // !STORE_EIGEN_MATRIX
			_solver.factorize( eigenM );
#endif // STORE_EIGEN_MATRIX
			if( _solver.info()!=Eigen::Success ) fprintf( stderr , "[ERROR] EigenSolverCholeskyLLt::EigenSolverCholeskyLLt Failed to factorize matrix\n" ) , exit(0);
		}
		_eigenB.resize( M.Rows() );
	}
	void update( const SparseMatrixInterface< Real , MatrixRowIterator >& M )
	{
#ifdef STORE_EIGEN_MATRIX
#pragma omp parallel for
		for( int i=0 ; i<M.Rows() ; i++ ) for( MatrixRowIterator iter=M.begin(i) ; iter!=M.end(i) ; iter++ ) _eigenM.coeffRef( i , iter->N ) = iter->Value;
		_solver.factorize( _eigenM );
#else // !STORE_EIGEN_MATRIX
		Eigen::SparseMatrix< double > eigenM( int( M.Rows() ) , int( M.Rows() ) );
		std::vector< Eigen::Triplet< double > > triplets;
		triplets.reserve( M.Entries() );
		for( int i=0 ; i<M.Rows() ; i++ ) for( MatrixRowIterator iter=M.begin(i) ; iter!=M.end(i) ; iter++ ) triplets.push_back( Eigen::Triplet< double >( i , iter->N , iter->Value ) );
		eigenM.setFromTriplets( triplets.begin() , triplets.end() );
		_solver.factorize( eigenM );
#endif // STORE_EIGEN_MATRIX
		switch( _solver.info() )
		{
		case Eigen::Success: break;
		case Eigen::NumericalIssue: fprintf( stderr , "[ERROR] EigenSolverCholeskyLLt::update Failed to factorize matrix (numerical issue)\n" ) , exit(0);
		case Eigen::NoConvergence:  fprintf( stderr , "[ERROR] EigenSolverCholeskyLLt::update Failed to factorize matrix (no convergence)\n" ) , exit(0);
		case Eigen::InvalidInput:   fprintf( stderr , "[ERROR] EigenSolverCholeskyLLt::update Failed to factorize matrix (invalid input)\n" ) , exit(0);
		default: fprintf( stderr , "[ERROR] EigenSolverCholeskyLLt::update Failed to factorize matrix\n" ) , exit(0);
		}
	}
	void solve( ConstPointer( Real ) b , Pointer( Real ) x )
	{
#pragma omp parallel for
		for( int i=0 ; i<_eigenB.size() ; i++ ) _eigenB[i] = b[i];
		Eigen_Vector eigenX = _solver.solve( _eigenB );
#pragma omp parallel for
		for( int i=0 ; i<eigenX.size() ; i++ ) x[i] = (Real)eigenX[i];
	}
	size_t dimension( void ) const { return _eigenB.size(); }
	static void Solve( const SparseMatrixInterface< Real , MatrixRowIterator >& M , ConstPointer( Real ) b , Pointer( Real ) x ){ EigenSolverCholeskyLLt solver( M ) ; solver.solve( b , x ); }
};
template< class Real , class MatrixRowIterator >
class EigenSolverCholeskyLDLt : public EigenSolver< Real , MatrixRowIterator >
{
#ifdef EIGEN_USE_MKL_ALL
	typedef Eigen::PardisoLDLT< Eigen::SparseMatrix< double > > Eigen_Solver;
	typedef Eigen::VectorXd                                     Eigen_Vector;
#else // !EIGEN_USE_MKL_ALL
	typedef Eigen::SimplicialLDLT< Eigen::SparseMatrix< double > > Eigen_Solver;
	typedef Eigen::VectorXd                                        Eigen_Vector;
#endif // EIGEN_USE_MKL_ALL
	Eigen_Solver _solver;
	Eigen_Vector _eigenB;
public:
	EigenSolverCholeskyLDLt( const SparseMatrixInterface< Real , MatrixRowIterator >& M , bool analyzeOnly=false )
	{
		Eigen::SparseMatrix< double > eigenM( int( M.Rows() ) , int( M.Rows() ) );
		std::vector< Eigen::Triplet<double> > triplets;
		triplets.reserve( M.Entries() );
		for( int i=0 ; i<M.Rows() ; i++ ) for( MatrixRowIterator iter=M.begin(i) ; iter!=M.end(i) ; iter++ ) triplets.push_back( Eigen::Triplet< double >( i , iter->N , iter->Value ) );
		eigenM.setFromTriplets( triplets.begin() , triplets.end() );
		_solver.analyzePattern( eigenM );
		if( !analyzeOnly )
		{
			_solver.factorize( eigenM );
			if( _solver.info()!=Eigen::Success ) fprintf( stderr , "[ERROR] EigenSolverCholeskyLDLt::EigenSolverCholeskyLDLt Failed to factorize matrix\n" ) , exit(0);
		}
		_eigenB.resize( M.Rows() );
	}
	void update( const SparseMatrixInterface< Real , MatrixRowIterator >& M )
	{
		Eigen::SparseMatrix< double > eigenM( int( M.Rows() ) , int( M.Rows() ) );
		std::vector< Eigen::Triplet<double> > triplets;
		triplets.reserve( M.Entries() );
		for( int i=0 ; i<M.Rows() ; i++ ) for( MatrixRowIterator iter=M.begin(i) ; iter!=M.end(i) ; iter++ ) triplets.push_back( Eigen::Triplet< double >( i , iter->N , iter->Value ) );
		eigenM.setFromTriplets( triplets.begin() , triplets.end() );
		_solver.factorize( eigenM );
		if( _solver.info()!=Eigen::Success ) fprintf( stderr , "[ERROR] EigenSolverCholeskyLDLt::update Failed to factorize matrix\n" ) , exit(0);
	}
	void solve( ConstPointer( Real ) b , Pointer( Real ) x )
	{
#pragma omp parallel for
		for( int i=0 ; i<_eigenB.size() ; i++ ) _eigenB[i] = b[i];
		Eigen_Vector eigenX = _solver.solve( _eigenB );
#pragma omp parallel for
		for( int i=0 ; i<eigenX.size() ; i++ ) x[i] = (Real)eigenX[i];
	}
	size_t dimension( void ) const { return _eigenB.size(); }
	static void Solve( const SparseMatrixInterface< Real , MatrixRowIterator >& M , ConstPointer( Real ) b , Pointer( Real ) x ){ EigenSolverCholeskyLDLt solver( M ) ; solver.solve( b , x ); }
};
template< class Real , class MatrixRowIterator >
class EigenSolverCG : public EigenSolver< Real , MatrixRowIterator >
{
#if 1
//	Eigen::ConjugateGradient< Eigen::SparseMatrix< double > , Eigen::Lower , Eigen::IncompleteLUT< double > > _solver;
	Eigen::ConjugateGradient< Eigen::SparseMatrix< double > > _solver;
#else
	Eigen::BiCGSTAB< Eigen::SparseMatrix< double > > _solver;
#endif
	Eigen::VectorXd _eigenB , _eigenX;
	Eigen::SparseMatrix< double > _eigenM;
public:
	EigenSolverCG( const SparseMatrixInterface< Real , MatrixRowIterator >& M , int iters=20 )
	{
		_eigenM.resize( (int)M.Rows() , (int)M.Rows() );
		std::vector< Eigen::Triplet< double > > triplets;
		triplets.reserve( M.Entries() );
		for( int i=0 ; i<M.Rows() ; i++ ) for( MatrixRowIterator iter=M.begin(i) ; iter!=M.end(i) ; iter++ ) triplets.push_back( Eigen::Triplet< double >( i , iter->N , iter->Value ) );
		_eigenM.setFromTriplets( triplets.begin() , triplets.end() );
		_solver.compute( _eigenM );
		_solver.analyzePattern( _eigenM );
		if( _solver.info()!=Eigen::Success ) fprintf( stderr , "[ERROR] EigenSolverCG::EigenSolverCG Failed to factorize matrix\n" ) , exit(0);
		_eigenB.resize( M.Rows() ) , _eigenX.resize( M.Rows() );
		_solver.setMaxIterations( iters );
	}
	void update( const SparseMatrixInterface< Real , MatrixRowIterator >& M )
	{
#pragma omp parallel for
		for( int i=0 ; i<M.Rows() ; i++ ) for( MatrixRowIterator iter=M.begin(i) ; iter!=M.end(i) ; iter++ ) _eigenM.coeffRef( i , iter->N ) = iter->Value;
		_solver.compute( _eigenM );
		_solver.analyzePattern( _eigenM );
		if( _solver.info()!=Eigen::Success ) fprintf( stderr , "[ERROR] EigenSolverCG::update Failed to factorize matrix\n" ) , exit(0);
	}

	void setIters( int iters ){ _solver.setMaxIterations( iters ); }
	void solve( const Real* b , Real* x )
	{
#pragma omp parallel for
		for( int i=0 ; i<_eigenB.size() ; i++ ) _eigenB[i] = b[i] , _eigenX[i] = x[i];
		_eigenX = _solver.solveWithGuess( _eigenB , _eigenX );
#pragma omp parallel for
		for( int i=0 ; i<_eigenX.size() ; i++ ) x[i] = _eigenX[i];
	}
	size_t dimension( void ) const { return _eigenB.size(); }
	static void Solve( const SparseMatrixInterface< Real , MatrixRowIterator >& M , const Real* b , Real* x , int iters ){ EigenSolverCG solver( M , iters ) ; solver.solve( b , x ); }
};
#endif // USE_EIGEN

#ifdef USE_CHOLMOD
class CholmodSolver
{
	const static bool LOWER_TRIANGULAR = true;
	int dim;
	cholmod_factor* cholmod_L;
	cholmod_dense*  cholmod_b;
	cholmod_sparse* cholmod_M;
	std::vector< bool > flaggedValues;
	template< class Real , class MatrixRowIterator > void _init( const SparseMatrixInterface< Real , MatrixRowIterator >& M );
public:
	static cholmod_common cholmod_C;
	static bool cholmod_C_set;

	template< class Real , class MatrixRowIterator >
	CholmodSolver( const SparseMatrixInterface< Real , MatrixRowIterator >& M , bool analyzeOnly=false );
	~CholmodSolver( void );

	template< class Real > void solve( ConstPointer( Real ) b , Pointer( Real ) x );
	template< class Real , class MatrixRowIterator > bool update( const SparseMatrixInterface< Real , MatrixRowIterator >& M );
	int nonZeros( void ) const;

};
bool CholmodSolver::cholmod_C_set = false;
cholmod_common CholmodSolver::cholmod_C;

template< class Real , class MatrixRowIterator > CholmodSolver::CholmodSolver( const SparseMatrixInterface< Real , MatrixRowIterator >& M , bool analyzeOnly ){ _init( M ) ; if( !analyzeOnly ) update( M ); }
template< class Real , class MatrixRowIterator >
void CholmodSolver::_init( const SparseMatrixInterface< Real , MatrixRowIterator >& M )
{
	{
		if( !cholmod_C_set ) CHOLMOD(start)( &cholmod_C );
		cholmod_C_set = true;
	}
	dim = (int)M.Rows();

	int maxEntries;
	if( LOWER_TRIANGULAR )
	{
		maxEntries = (int)( ( M.Entries()-M.Rows() ) / 2 + M.Rows() );
		cholmod_M = CHOLMOD(allocate_sparse)( dim , dim , maxEntries , 0 , 1 , -1 , CHOLMOD_REAL , &cholmod_C );
	}
	else
	{
		maxEntries = (int)M.Entries();
		cholmod_M = CHOLMOD(allocate_sparse)( dim , dim , maxEntries , 0 , 1 ,  0 , CHOLMOD_REAL , &cholmod_C );
	}
	cholmod_M->i = malloc( sizeof( SOLVER_LONG ) * maxEntries );
	cholmod_M->x = malloc( sizeof( double ) * maxEntries );

	SOLVER_LONG *_p = (SOLVER_LONG*)cholmod_M->p;
	SOLVER_LONG *_i = (SOLVER_LONG*)cholmod_M->i;

	int off = 0;
	dim = 0;

	for( int i=0 ; i<M.Rows() ; i++ )
	{
		_p[dim++] = off;
		for( MatrixRowIterator iter=M.begin(i) ; iter!=M.end(i) ; iter++ ) if( !LOWER_TRIANGULAR || iter->N>=i ) _i[off++] = iter->N;
	}
	_p[dim] = off;

	cholmod_L = CHOLMOD(analyze)( cholmod_M , &cholmod_C );
	cholmod_b = CHOLMOD(allocate_dense)( dim , 1 , dim , cholmod_M->xtype , &cholmod_C );
}
template< class Real , class MatrixRowIterator >
bool CholmodSolver::update( const SparseMatrixInterface< Real , MatrixRowIterator >& M )
{
	double *_x = (double*)cholmod_M->x;
	int off = 0;

	SOLVER_LONG *_p = (SOLVER_LONG*)cholmod_M->p;
#pragma omp parallel for
	for( int i=0 ; i<M.Rows() ; i++ )
	{
		int off = (int)_p[i];
		for( MatrixRowIterator iter=M.begin(i) ; iter!=M.end(i) ; iter++ )if( !LOWER_TRIANGULAR || iter->N>=i ) _x[off++] = double( iter->Value );
	}

	cholmod_C.print = 0;
	CHOLMOD(factorize)( cholmod_M , cholmod_L , &cholmod_C );
	if( cholmod_C.status==CHOLMOD_NOT_POSDEF )
	{
		fprintf( stderr , "[WARNING] Matrix not positive-definite\n" );
		return false;
	}
	else if( cholmod_C.status==CHOLMOD_OUT_OF_MEMORY )
	{
		fprintf( stderr , "[WARNING] CHOLMOD ran out of memory\n" );
		return false;
	}
	else if( cholmod_C.status!=CHOLMOD_OK )
	{
		fprintf( stderr , "[WARNING] CHOLMOD status not OK: %d\n" , cholmod_C.status );
		return false;
	}
	return true;
}
CholmodSolver::~CholmodSolver( void )
{
	if( cholmod_L ) CHOLMOD(free_factor)( &cholmod_L , &cholmod_C ) , cholmod_L = NULL;
	if( cholmod_b ) CHOLMOD(free_dense )( &cholmod_b , &cholmod_C ) , cholmod_b = NULL;
	if( cholmod_M ) CHOLMOD(free_sparse)( &cholmod_M , &cholmod_C ) , cholmod_M = NULL;
}

template< class Real >
void CholmodSolver::solve( ConstPointer( Real ) b , Pointer( Real ) x )
{
	double* _b = (double*)cholmod_b->x;
	for( int i=0 ; i<dim ; i++ ) _b[i] = (double)b[i];

	cholmod_dense* cholmod_x = CHOLMOD(solve)( CHOLMOD_A , cholmod_L , cholmod_b , &cholmod_C );
	double* _x = (double*)cholmod_x->x;
	for( int i=0 ; i<dim ; i++ ) x[i] = (Real)_x[i];

	CHOLMOD(free_dense)( &cholmod_x , &cholmod_C );
}
int CholmodSolver::nonZeros( void ) const
{
	long long nz = 0;
	if( cholmod_L->xtype != CHOLMOD_PATTERN && !(cholmod_L->is_super ) ) for( int i=0 ; i<cholmod_L->n ; i++ ) nz += ((SOLVER_LONG*)cholmod_L->nz)[i];
	bool examine_super = false;
	if( cholmod_L->xtype != CHOLMOD_PATTERN ) examine_super = true ;
	else                                      examine_super = ( ((int*)cholmod_L->s)[0] != (-1));
	if( examine_super )
	{
		/* check and print each supernode */
		for (int s = 0 ; s < cholmod_L->nsuper ; s++)
		{
			int k1 = ((int*)cholmod_L->super) [s] ;
			int k2 = ((int*)cholmod_L->super) [s+1] ;
			int psi = ((int*)cholmod_L->pi)[s] ;
			int psend = ((int*)cholmod_L->pi)[s+1] ;
			int nsrow = psend - psi ;
			int nscol = k2 - k1 ;
			nz += nscol * nsrow - (nscol*nscol - nscol)/2 ;
		}
	}
	return (int)nz;
}
#endif // USE_CHOLMOD
#endif // LINEAR_SOLVERS_INCLUDE