// -----------------------------------------------------------------------
/* LU Decomposition
*  with partial implicit pivoting
*  partial means that the pivoting only happems by row
*  implicit means that the pivots are scaled by the maximum value in the row
*/

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

#define   SWAP
/* #define DEBUG */
/* #define SWAP_TEST */

// -----------------------------------------------------------------------
void LUsubstitute(float **A,int n,int *Rowswaps,float *b);
int LUdecomp(float **A,int n,int *Rowswaps,int *val);
void InvrsMatrix(float **A,int n);

// -----------------------------------------------------------------------
/* LUdecomp 
*  inputs: A matrix of coefficients 
*               n - dimension of A 
* outputs: A matrix replaced with L and U diagonal matrices (diagonal values of L == 1)
*                 Rowswaps - vector to keep track of row swaps
*                 Val - indicator of odd/even number of rowswaps
*/
int LUdecomp(float **A,int n,int *rowswaps,int *val)
{
	float		epsilon,*rowscale, temp;
	float		sum;
	float		pvt;
	int			ipvt;
	int			i,j,k;
	
	/* #ifdef SWAP
	int			itemp;
	#endif */
	
	rowscale = (float *)malloc(sizeof(float)*n);
	
	epsilon = 0.00000000001;				/* small value to avoid division by zero */
	*val = 1;								/* even/odd indicator (valence) */
	
	/* initialize the rowswap vector to indicate no swaps */
	for (i=0; i<n; i++) rowswaps[i] = i;
	
	/* for each row, find largest (in absolute value sense) element and record in rowscale */
	for (i=0; i<n; i++) {
		temp = fabs(A[i][0]);
		for (j=1; j<n; j++)
			if (fabs(A[i][j]) > temp) temp = fabs(A[i][j]);
		if (temp == 0) return(-1);			/* got a row of all zeros - can't deal with that */
		rowscale[i] = 1/temp;				/* later we need to divide by largest element */
	}
	
	#ifdef DEBUG
		printf("\nlargest value in each row\n");
		for (i=0; i<n; i++) printf("%f\n",rowscale[i]);
	#endif
	
	/*  loop through the columns  of A (and U) */
	for (j=0; j<n; j++) {
		/* do the rows down to the diagonal  - these don't need a division so no swap */
		for (i=0; i<j; i++) {
			sum = A[i][j];
			for (k=0; k<i; k++) sum = sum - A[i][k]*A[k][j];
			A[i][j] = sum;
		}
		/* do the rows from the diagonal down */
		#ifdef SWAP
			pvt = 0.0;
			ipvt = -1;
		#endif
		for (i=j; i<n; i++) {
			sum = A[i][j];
			for (k=0; k<j; k++) sum = sum - A[i][k]*A[k][j];
			A[i][j] = sum;
			/* calculate the scaled value for pivoting consideration */
			#ifdef SWAP
				temp = rowscale[i]*fabs(sum);
				if (temp >= pvt) {ipvt = i; pvt = temp;}
			#endif
		}
		
		#ifdef SWAP
			#ifdef DEBUG
				printf("performing row swaps in LUdecom\n");
			#endif
			/* if a better pivot value is found, interchange the rows */ 
			if (j != ipvt) {
				for (k=0; k<n; k++) {
					temp = A[ipvt][k];
					A[ipvt][k] = A[j][k];
					A[j][k] = temp;
				}
				*val = -(*val);						/* keep track of even/odd number interchanges  */ 
				rowscale[ipvt] = rowscale[j];		/* and record which was swapped  */ 
				/*
				itemp = rowswaps[ipvt];
				rowswaps[ipvt] = rowswaps[j];
				rowswaps[j] = itemp;
				*/
			}
		rowswaps[j] = ipvt;
		#endif
		
		if (A[j][j] == 0.0) A[j][j] = epsilon;		/* to guard against divisions by zero */
		/* now the row is ready for division */
		for (i=j+1; i<n; i++)  A[i][j] = A[i][j]/A[j][j];
	}
	return 1;
}

// -----------------------------------------------------------------------
/* LUsubstitute
*  inputs: A - matrix holding the L and U matrix values as a result of LUdecomp
*               n - dimension of A
*               Rowswaps - vector holding a record of the row swaps performed in LUdecomp
*               b - vector of right-hand values as in Ax = b
*/
void LUsubstitute(float **A,int n,int *rowswaps,float *b)
{
	int		i,j,ib;
	float		sum;
	#ifdef SWAP
	int		m;
	#endif

	#ifdef SWAP
		#ifdef DEBUG
			printf("performing row swaps in LUsubstitute");
		#endif
		/* row swap version */
		ib = -1;
		for (i=0; i<n; i++) {
			m = rowswaps[i];
			sum = b[m];
			b[m]=b[i];
			if (ib != -1) {
				for (j=ib; j<i; j++) sum = sum-A[i][j]*b[j];
			}
			else {
				if (sum != 0.0) ib = i;
			}
			b[i] = sum;
		}
		
		for (i=n-1; i>=0; i--) {
			sum = b[i];
			for (j=i+1; j<n; j++) sum = sum - A[i][j]*b[j];
			b[i] = sum/A[i][i];
		}
	#endif
	
	#ifndef SWAP
		#ifdef DEBUG
			printf("not performing row swaps in LUsubstitute");
		#endif
		/* Ly = b */
		/* solve with row by row substitution */
		/* remember diagonal elements of 'L' are 1.0 */
		/* result 'y' goes back into 'b' */
		for (i=0; i<n; i++) {
			sum = b[i];
			for (j=0; j<i; j++) sum = sum - A[i][j]*b[j];
			b[i] = sum;
			
			#ifdef DEBUG
			printf("m=%d\n",m);
			printf("b[m]=%f\n",b[m]);
			printf("sum=%f\n",sum);
			scanf("%f",&sum);
			#endif
		}

		/* Ux = b */
		/* solve with row by row substitution from bottom up */
		for (i=n-1; i>=0; i--) {
			sum = b[i];
			for (j=i+1; j<n; j++) sum = sum - A[i][j]*b[j];
			b[i] = sum/A[i][i];
		}
	#endif
	
	#ifdef SWAP_TEST
		for (i=n-1;i>=0; i--) {
			m = rowswaps[i];
			if (m != i) {
				sum = b[i];
				b[i] = b[m];
				b[m] = sum;
			}
		}
	#endif

	return;
}

// -----------------------------------------------------------------------
/* INVERSEMATRIXC
*   compute the inverse of a matrix
*   by repeated calls to the linear solver, one call for each column of an identity matrix
*/
void InvrsMatrix(float **A,int n)
{
	int		i,j;
	float	**Ainverse;
	int		*rowswap;
	float	temp;
	int     val;
	
	rowswap = (int *)malloc(sizeof(int)*n);
	Ainverse = (float **)malloc(sizeof(float *)*n);
	for (i=0; i<n; i++) Ainverse[i] = (float *)malloc(sizeof(float)*n);
	
	/* initialize the Ainverse matrix to be an identity matrix */
	for (i=0; i<n; i++) {
		for (j=0; j<n; j++) Ainverse[i][j] = (i==j) ? 1.0 : 0.0;
	}
	
	/* compute the L and U matrices for the given A matrix */
	LUdecomp(A,n,rowswap,&val);
	
	/* now solve the system Ax=b with b being successive rows of the identity matrix */
	/* in this implementation, use rows of Ainverse instead of columns*/
	/* because thatŐs the way matrices are stored */
	/* and then it will be transposed later */
	for (i=0; i<n; i++) {
		LUsubstitute(A,n,rowswap,Ainverse[i]);
	}
	
	/* now transpose Ainverse to get it in the correct form */
	for (i=0; i<n; i++) {
		for (j=0; j<n; j++) {
			A[j][i] = Ainverse[i][j];
		}
	}
	return;
}