/*  CHOLCOND.C
 *
 *  Factors a real symmetric positive definite matrix and
 *  estimates the inverse condition number of the matrix.
 *
 *  Adapted from:
 *  Dongarra et al.  Linpack User's Guide.  Philadelphia, SIAM, 1979
 *  Golub, Gene.  Matrix Computations.  Baltimore,
 *      Johns Hopkins University Press, 1983.
 *
 *
 *  local functions:
 *  static double matrixVectorDot( const Matrix, int, const Vector, int );
 *  static double vecWtSum2( const Matrix, int, Vector, int, double );
 *  double cholCond( const Matrix );
 *
 */

#include "atsmath.h"
#include <math.h>
#include <stddef.h>

static double matrixVectorDot ( const Matrix A, int col, const Vector Z, int numElts ){
/*
 *  Calculates a dot product of the first numElts+1 of a specified
 *  column of the given matrix with the first numElts+1 of the given vector.
 *
 */
	int j;
	double tmp=0;

	for (j=0; j <= numElts; j++){
		tmp += matAt( A, j, col ) * vecAt( Z, j );
	}

	return tmp;
}

static Vector vecWtSum2 ( const Matrix A, int col, Vector Z, int numElts, double t ) {
/*
 *   Calculates the sum of a scalar t multiplied to the first numElts+1
 *   of a specified column of matrix A added to the first numElts+1
 *   of vector Z.
 *
 *   The result is stored in vector Z.
 *
 */

	int j;

	for (j=0; j<=numElts; j++){
		vecAt( Z, j ) += t * matAt( A, j, col );
	}

	return Z;

}


double cholCond( const Matrix A )
/*  Explanation of parameters to cholCond():
 *
 *  A:  the symmetric matrix to be factored.
 *
 *  The return value of the function is an estimate of the reciprocal
 *  of the condition number of A.
 *
 */

{
	/* local variable declarations */
	double ek, t, wk, wkm, s, sm;
	double anorm, ynorm, rCond;
	int j, k, kp1, n, cholInfo;
	Vector Z;

	/*  allocate work areas */
	/*  Z is a work vector whose contents are usually unimportant. */

	if ( ( Z = newVector( A->rows )) == NULL )  return OTHER_ERROR;

	/*  (1)  calculate the L1 norm of matrix A */

	anorm = matL1Norm( A );
	n = A->rows;

	/*  (2)  Factor the matrix A into lower and upper triangular
	*  matrices using the choleski decomposition.  The choleski
	*  routine replaces the contents of matrix A with the lower
	*  triangular matrix L.
	*
	*  choleski returns 1 if it is successful.
	*
	*/

	cholInfo = choleskiDecomp( A );

	/*  (3)  If the Choleski decomposition was unsuccessful,
	*  return(0).
	*
	*  rCond = 1 / (norm( A ) * ( estimate of norm( inverse( A ))) ).
	*
	*  Estimate = norm( Z )/norm( Y ) where A*Z=Y and A*Y=E.
	*  The components of E are chosen to cause maximum local growth
	*  in the elements of W where trans( R ) * W = E.
	*
	*  The vectors are frequently rescaled to avoid overflow.
	*
	*  Solve Trans( R )*W = E.
	*
	*/

	if ( cholInfo != 1 ) return( 0 );

	ek = 1.0E0;

	for ( j=0; j<n; j++ ){
		vecAt( Z, j ) = 0.0E0;
	}

	for ( k=0; k<n; k++ ) {
		if ( vecAt( Z, k ) != 0.0E0){
			ek = vecAt( Z,k ) < 0 ? fabs(ek) : -fabs(ek);
		}
		if ((fabs ( ek-vecAt( Z, k ) )) > matAt( A, k, k )){
			s = matAt( A,k,k ) / fabs( ek - (vecAt( Z, k )));
			vecScale( Z, s );
			ek *= s;
		}

		wk = ek - vecAt( Z, k );
		wkm = -ek - vecAt( Z, k );
		s = fabs( wk );
		sm = fabs( wkm );
		wk /= matAt( A, k, k );
		wkm /= matAt( A, k, k );
		kp1 = k+1;

		if ( kp1 < n ){
			for ( j=kp1; j<n; j++ ){
				sm += fabs( vecAt( Z, j ) + wkm * matAt( A,k,j ) );
				vecAt( Z, j ) += wk * matAt( A, k, j );
				s += fabs( vecAt( Z, j ) );
			}
			if ( s < sm ){
				t = wkm - wk;
				wk = wkm;
				for ( j = kp1; j<n; j++ ){
					vecAt( Z, j ) += t * matAt( A, k, j );
				}
			}
		}
		vecAt( Z, k ) = wk;
	}

	/*  (4)  Solve R * Y = W */

	for (k=(n-1); k>=0; k--){
		if ((abs( vecAt( Z, k ) )) > matAt(A, k, k)){
			s=matAt(A, k, k)/ abs(vecAt( Z, k ));
			vecScale( Z, s );
		}
		vecAt( Z, k ) /= matAt(A, k, k);
		t = -vecAt( Z, k );
		vecWtSum2 ( A, k, Z, k-1, t );
	}

	s = 1.0E0/vecL1Norm( Z );
	vecScale( Z,s );
	ynorm = 1.0E0;

	/*  (5)  Solve trans(R) * V = Y */

	for (k=0; k<n; k++){
		vecAt( Z, k ) -=  matrixVectorDot ( A, k, Z, k-1 );
		if (abs(vecAt( Z, k )) > matAt(A, k, k)){
			s=matAt( A, k, k )/abs(vecAt( Z, k ));
			vecScale( Z, s );
			ynorm *= s;
		}
		vecAt( Z, k ) /= matAt( A, k, k );
	}
	s=1.0E0 / vecL1Norm( Z );
	vecScale( Z, s );
	ynorm *= s;

	/*  (6)  Solve R * Z = V */

	for (k=(n-1); k>=0;  k--){
		if ((abs(vecAt( Z, k ))) > matAt(A, k, k)){
			s = matAt(A, k, k) / abs(vecAt( Z, k ));
			vecScale( Z, s );
			ynorm *= s;
		}

		vecAt( Z, k ) /= matAt(A, k, k);
		t = -vecAt( Z, k );
		vecWtSum2 ( A, k, Z, k-1, t );
	}

	s = 1.0E0 / vecL1Norm( Z );
	vecScale( Z, s );
	ynorm *= s;

	/*  (7)  return rCond */

	if ( anorm != 0.0E0 ) rCond = ynorm / anorm;
	else rCond = 0.0E0;

	killVector( &Z );

	return rCond;

}