#include "gpk_std.h"
#include <stdio.h>
#include <assert.h>
#include <math.h>

#include "gpklib.h"


#ifdef THREADS
#include <thread.h>
#endif

inline int min(int a, int b) {return a<b ? a : b; }


BOOL
invert(double **ainv, int Np)		/* Invert matrix in place */
{
	int kx;
	C double eps = 1e-31;
	/* How many rows to consider for pivoting:: */
	C int nnp = (int)(1+sqrt(2*Np));
	int *im = int1alloc(Np);
	int *jm = int1alloc(Np);
	double *bx = double1alloc(Np);
	double *c = double1alloc(Np);

	assert(im && jm && bx && c);

/*
* zero out the indices marking the pos of the max for each param
* (finding the max helps precision of final ans)
*/
	{
	int j;
	for(j=0; j<Np; ++j)
		im[j] = jm[j] = 0;
	}

/* find location of max for each param- here called pivot */
	for(kx=0; kx<Np; ++kx) {
		/*determination of pivot element*/
		double pivot = 0;

		/* fprintf(stderr, "Gjr1.%d\n", kx); */
		/* start at 'kx' bec only have to do param's once */
		{
		int i;
		double fabs_pivot = 0;
		C int np = min(Np, kx+nnp);
		for(i=kx; i<np; ++i) {
			int j;

			for(j=kx; j < Np; ++j) {
				if(fabs(ainv[i][j]) > fabs_pivot) {
					pivot = ainv[i][j];
					fabs_pivot = fabs(pivot);
					jm[kx] = i;
					im[kx] = j;
					}
				}
			}
		}

		/* if max val is tiny,  something's wrong */
		if(fabs(pivot) < eps) {
			fprintf(stderr, "Singular Matrix, kx=%d, im,jm=%d,%d:\n",
									kx, im[kx], jm[kx]);
			int iq;
			for(iq=0;iq<Np;iq++)	{
				int jq;
				for(jq=0;jq<Np;jq++)
					fprintf(stderr, "%g ", ainv[iq][jq]);
				fprintf(stderr, "\n");
				}

			int1free(im);
			int1free(jm);
			double1free(bx);
			double1free(c);
			return FALSE;
			}

		/* if max is not on diag, move it there */
		if(jm[kx] != kx)	{	/* row swap */
			int j;

			for(j=0; j< Np; ++j) {
				C int l = jm[kx];
				C double z = ainv[l][j];

				ainv[l][j] = ainv[kx][j];
				ainv[kx][j] = z;
				}
			}

		if(im[kx] != kx)	{	/* column swap*/
			int ii;

			for(ii=0; ii<Np; ++ii) {
				C int l = im[kx];
				C double z = ainv[ii][l];

				ainv[ii][l] = ainv[ii][kx];
				ainv[ii][kx] = z;
				}
			}

		{
		int i;
		/*Gauss-Jordan method to manip matrix I/A to get invA */
		for(i=0; i<Np; ++i) {
			if(i == kx)	{	/* diag elem must be div by self2*/
				bx[i] = 1./ pivot;
				c[i] = 1.;
				}
			else	{	/* others by diag, then frac made up
						* of col elems of the 'kx' col(saved as bx)
						*sub'd from self (saved in 'c'*/
				bx[i] = (-ainv[kx][i])/pivot;
				c[i] = ainv[i][kx];
				}
			ainv[kx][i] = ainv[i][kx] = 0.;
			}

		/* fprintf(stderr, "Gjr6.%d\n", kx); */
		for(i=0; i<Np; ++i)	{
			int j;

			for(j=0; j<Np; ++j)
				ainv[i][j] += c[i]*bx[j];
			}
		}
	}

	/*reordering the matrix - undo the swap for max */
	{
	int m;
	/* fprintf(stderr, "Gjr7\n"); */
	for(m=0; m<Np; ++m) {
		kx = Np-m -1;	
		/* fprintf(stderr, "Gjr7.%d\n", kx); */
		if(jm[kx] != kx) {
			int i;

			for(i=0; i<Np; ++i) {
				C int l = jm[kx];
				C double z=ainv[i][l];

				ainv[i][l] = ainv[i][kx];
				ainv[i][kx] = z;
				}
			}
		if(im[kx] != kx) {
			int j;
			for(j=0; j<Np; ++j) {
				C int l = im[kx];
				C double z = ainv[l][j];

				ainv[l][j] = ainv[kx][j];
				ainv[kx][j] = z;
				}
			}
		}
	}
 int1free(im);
 int1free(jm);
 double1free(bx);
 double1free(c);
 return TRUE;
}

