/* dgeco.c -- double precision solution of general matrix by
 *	      Gaussian elimination with condition estimate.
 *
 *	From "Linpack Users' Guide", Dongarra, et al.,
 */
#include <stdio.h>
#include <math.h>
#include "func.h"

#define max(a,b)	(((a) > (b)) ? (a) : (b))
#define min(a,b)	(((a) < (b)) ? (a) : (b))
#define signum(a,b)	(((b) < (0)) ? (-a) : (a)) 
 
void dgeco(double **a,int n, int *ipvt, double *rcond,double *z)
{
	double anorm,ek,s,sm,t,vecdot,vecsum,wk,wkm,ynorm;
	int i,info,j,k,kb,kp1,l;
	
/* Compute 1-norm of a */
	anorm = 0.0;
	for (j = 0; j < n; j++) {
		vecsum = 0.0;
		for (i = 0;i < n; i++)
			vecsum += fabs(a[i][j]);
		anorm = max(anorm,vecsum);
	}
	
/* Factor. */
	dgefa(a,n,ipvt,&info);	

/* rcond = 1/(norm(a) * (estimate of norm(inverse(a)))).
 * estimate = norm(z)/norm(y), where a*z=y and trans(a)*y=e.
 * trans(a) is the transpose of a. The components of e are
 * chosen to cause maximum local growth in the elements of
 * w, where trans(u)*w=e. The vectors are frequently rescaled
 * to avoid overflow.
 */
 	ek = 1.0;
 	for (j = 0; j < n; j++)
 		z[j] = 0.0;
 	for (k = 0; k < n; k++) {
 		if (z[k] != 0.0)
 			ek = signum(ek,-z[k]);
 		if (fabs(ek-z[k]) <= fabs(a[k][k])) goto _30;
 		s = fabs(a[k][k])/fabs(ek-z[k]);
		/* dscal(n,s,z,1) */
		for (i = 0; i < n; i++)
			z[i] *= s; 
		ek *= s;
_30:
		wk = ek - z[k];
		wkm = -ek - z[k];
		s = fabs(wk);
		sm = fabs(wkm);
		if (a[k][k] == 0.0) goto _40;
		wk /= a[k][k];
		wkm /= a[k][k];
		goto _50; 
_40:
		wk = 1.0;
		wkm = 1.0;
_50:
		kp1 = k + 1;
		if (kp1 > (n-1)) goto _90;
		for (j = kp1; j < n; j++) {
			sm += (fabs(z[j] + wkm * a[k][j]));
			z[j] += (wk * a[k][j]);
			s += fabs(z[j]);
		}
		if (s >= sm) goto _80;
		t = wkm -wk;
		wk = wkm;
		for (j = kp1; j < n; j++)
			z[j] += (t * a[k][j]);
_80:
_90:
		z[k] = wk;
	}
	/* dasum(n,s,z,1) */
	vecsum = 0.0;
	for (i = 0;i < n; i++)
		vecsum += fabs(z[i]);
	s = 1.0/vecsum;
	/* dscal(n,s,z) */
	for (i = 0; i < n; i++)
		z[i] *= s;	

/* Solve trans(l)*y= w
 */
 	for (kb = 0; kb < n; kb++) {
 		k = n - kb - 1;
 		if (k < (n-1)) {
			/* sdot(n-k,a(k+1,k),1,z(k+1),1) */
			vecdot = 0.0;
			for (i = k+1;i < n; i++)
				vecdot += (a[i][k] * z[i]);
			z[k] += vecdot;
		}
		if (fabs(z[k]) <= 1.0) goto _110;
		s = 1.0/fabs(z[k]);
		/* dscal(n,s,z) */
		for (i = 0; i < n; i++)
			z[i] *= s;
_110:
			l = ipvt[k];
			t = z[l];
			z[l] = z[k];
			z[k] = t;
	} /* endfor kb */
	/* dasum(n,z,1) */
	vecsum = 0.0;
	for (i = 0; i < n; i++)
		vecsum += fabs(z[i]);
	s = 1.0/vecsum;
	/* dscal(n,s,z) */
	for (i = 0; i < n; i++)
		z[i] *= s;	

	ynorm = 1.0; 
/*
 * Solve l * v = y
 */
		for (k = 0; k < n; k++) {
			l = ipvt[k];
			t = z[l];
			z[l] = z[k];
			z[k] = t;
			if (k < (n-1)) {
				/* daxpy(n-k,t,a[k+1][k],1,z[k+1],1) */
				for (i = k+1;i < n; i++)
					z[i] += (t * a[i][k]);
			}
			if (fabs(z[k]) <= 1.0) goto _130;
			s = 1.0/fabs(z[k]);
			/* dscal(n,s,z,1) */
			for (i = 0; i < n; i++)
				z[i] *= s;
			ynorm *= s;
_130:
		}
		/* dasum(n,z,1) */
		vecsum = 0.0;
		for (i = 0; i < n; i++)
			vecsum += fabs(z[i]);
		s = 1.0/vecsum;
		/* dscal(n,s,z,1) */
		for (i = 0; i < n; i++)
			z[i] *= s;
 		ynorm *= s;

/* Solve u * z = v */
		for (kb = 0; kb < n; kb++) {
			k = n - kb - 1;
			if (fabs(z[k]) <= fabs(a[k][k])) goto _150;
			s = fabs(a[k][k])/fabs(z[k]);
			/* dscal(n,s,z,1) */
			for (i = 0; i < n; i++)
				z[i] *= s;
			ynorm *= s;
_150:
			if (a[k][k] != 0.0)
				z[k] /= a[k][k];
			if (a[k][k] == 0.0)
				z[k] = 1.0;
			t = -z[k];
			/* daxpy(k-1,t,a[1][k],1,z[1],1) */
			for (i = 0; i < k; i++)
				z[i] += (t * a[i][k]);
		}
/* Make znorm = 1.0 */
		/* dasum(n,z,1) */
		vecsum = 0.0;
		for (i = 0; i < n; i++)
			vecsum += fabs(z[i]); 
		s = 1.0/vecsum;
		/* dscal(n,s,z,1) */
		for (i = 0; i < n; i++)
			z[i] *= s;
		ynorm *= s;
		if (anorm != 0.0) *rcond = ynorm/anorm;
		if (anorm == 0.0) *rcond = 0.0;
}