/* sole.h: Systems Of Linear Equations */

#include "sole.h"

#include <stdio.h>
#include <math.h>

#define NMAX	BASISMAXSIZE
#define TINY	1e-20

/* LU decomposition, Numerical Recipes II */
static double ludcmp(double a[BASISMAXSIZE][BASISMAXSIZE*2],
		     int n, int indx[BASISMAXSIZE])
{
	int i, imax, j, k;
	double d, aamax, dum, sum, vv[NMAX];

	d = 1.;
	for (i=0; i<n; i++) {
		aamax = 0.;
		for (j=0; j<n; j++) {
			if (fabs(a[i][j]) > aamax)
				aamax = fabs(a[i][j]);
		}
		if (aamax == 0.) 
			return 0.;
		else
			vv[i] = 1./aamax;
	}

	for (j=0; j<n; j++) {
		for (i=0; i<j; i++) {
			sum = a[i][j];
			for (k=0; k<i; k++)
				sum -= a[i][k]*a[k][j];
			a[i][j] = sum;
		}

		aamax = 0.; imax=j;
		for (i=j; i<n; i++) {
			sum = a[i][j];
			for (k=0; k<j; k++)
				sum -= a[i][k] * a[k][j];
			a[i][j] = sum;
			dum = vv[i] * fabs(sum);
			if (dum > aamax) {
				imax = i;
				aamax = dum;
			} 
		}

		if (j != imax) {
			for (k=0; k<n; k++) {
				dum = a[imax][k];
				a[imax][k] = a[j][k];
				a[j][k] = dum;
			}
			d = -d;
			vv[imax] = vv[j];
		}
		indx[j] = imax;
		
		if (a[j][j] == 0.)
			a[j][j] = TINY;
		if (j != n-1) {
			dum = 1./a[j][j];
			for (i=j+1; i<n; i++) 
				a[i][j] *= dum;
		}
	}

	return d;
}

static void lubksb(double a[BASISMAXSIZE][BASISMAXSIZE*2], 
		   int n, int indx[BASISMAXSIZE],
		   double b[BASISMAXSIZE])
{
	int i, ii, j, ll;
	double sum;

	ii = -1;
	for (i=0; i<n; i++) {
		ll = indx[i];
		sum = b[ll];
		b[ll] = b[i];
		if (ii != -1) {
			for (j=ii; j<i; j++) 
				sum -= a[i][j] * b[j];			
		} else if (sum != 0.) {
			ii = i;
		}
		b[i] = sum;
	}

	for (i=n-1; i>=0; i--) {
		sum = b[i];
		for (j=i+1; j<n; j++) {
			sum -= a[i][j] * b[j];			
		}
		b[i] = sum / a[i][i];
	}
}

/*
 * solves a set of simultaneous linear equations: dimension (=nr of
 * equations = nr of unknowns) is N, M is the number of simultaneous
 * right hand sides. The M solutions replace the M rights hand
 * side vectors in the matrix c with the following layout:
 * - first N columns: the coefficients of the set
 * - next M columns: on entry: the rights hand sides of the simultaneous sets
 *                   on return: the corresponding solutions
 * Returns the determinant of the coefficients matrix ... the system is
 * not solved if this determinant is too close to zero.
 */
double SolveSystem(double a[BASISMAXSIZE][BASISMAXSIZE*2], int n, int m)
{
	int i, j, k, indx[BASISMAXSIZE];
	double d, b[BASISMAXSIZE];

/* LU decomposition */
	d = ludcmp(a, n, indx);

/* compute determinant */
	for (j=0; j<n; j++)
		if (a[j][j] == 0.)
			return 0.; /* singular matrix, don't solve it */

/* forward substitution and backsubstitution */
	for (k=0; k<m; k++) {
		for (i=0; i<n; i++)
			b[i] = a[i][n+k];
		lubksb(a, n, indx, b);
		for (i=0; i<n; i++)
			a[i][n+k] = b[i];
	}

	return d;
}

/* invert matrix in into out. n is the dimension (max BASISMAXSIZE) */
double InvertMatrix(double in[BASISMAXSIZE][BASISMAXSIZE], 
		    double out[BASISMAXSIZE][BASISMAXSIZE], int n)
{
	double a[BASISMAXSIZE][BASISMAXSIZE*2];
	int i, j;
	double d;

	for (i=0; i<n; i++) {
		for (j=0; j<n; j++)
			a[i][j] = in[i][j];

		for (j=n; j<n+i; j++)
			a[i][j] = 0.;
		a[i][n+i] = 1.;
		for (j=n+i+1; j<n+n; j++)
			a[i][j] = 0.;
	}

	d = SolveSystem(a, n, n);

	for (i=0; i<n; i++) 
		for (j=0; j<n; j++)
			out[i][j] = a[i][n+j];

	return d;
}


#ifdef TEST
int main (int argc, char **argv)
{
	double m[BASISMAXSIZE][BASISMAXSIZE], inv[BASISMAXSIZE][BASISMAXSIZE], x;
	int i, j, k;
	extern double drand48(void);

	for (i=0; i<BASISMAXSIZE; i++)
		for (j=0; j<BASISMAXSIZE; j++)
			m[i][j] = drand48();

	InvertMatrix(m, inv, BASISMAXSIZE);

/* write the product of the two random matrices */
	fprintf(stderr, "matrix * inverse:\n");
	for (i=0; i<BASISMAXSIZE; i++) {
		for (j=0; j<BASISMAXSIZE; j++) {
			x = 0.;
			for (k=0; k<BASISMAXSIZE; k++)
				x += m[i][k] * inv[k][j];
			fprintf(stderr, "%8.8f ", x);
		}
		fprintf(stderr, "\n");
	}

	fprintf(stderr, "inverse * matrix:\n");
	for (i=0; i<BASISMAXSIZE; i++) {
		for (j=0; j<BASISMAXSIZE; j++) {
			x = 0.;
			for (k=0; k<BASISMAXSIZE; k++)
				x += inv[i][k] * m[k][j];
			fprintf(stderr, "%8.8f ", x);
		}
		fprintf(stderr, "\n");
	}
	
	return 0;
}
#endif /*TEST*/
