
//	Title:		MatrixInverse.cpp
//	Purpose:	Performs matrix inversion using Gauss-Jordan elimination with full pivoting.
//	Author:		Bud Fox
//	Source:		Numerical Recipes in C: The Art of Scientific Computing (2nd Ed.), sections 2.0 - 2.1.
//	Date:		26-April-2009


#include <iostream>
#include <iomanip>
#include <math.h>
#include <time.h>
#include "MatrixInverse.h"
using namespace std;


int main(int argc, char **argv)
{
	cout << "\n main()\n";

	//SetupMatrix();		// dynamic hence more variable/general
	//PrepareMatrixMult();
	//PrepareSwap();
	PrepareMatrixInversion();
	//PrepareMatrixInversionLargeSystem();
		
	cout << endl;
	return 0;
}


int SetupMatrix(void)
{
	cout << " SetupMatrix()\n";

	int		i;
	int		j;
	int		cnt = 0;
	int		nrows = NROWS;
	int		ncols = NCOLS;
	double	*vector = NULL;			// vector is a ptr to double.
	double	**matrix = NULL;		// matrix 

	// MEMORY NEW
	// Allocate an array of doubles, and return the & to vector: hence vector is of type ptr.
	vector = new double[ncols];

	// Allocate an array of ptrs-to-double and return the & to matrix
	// Note: an array is of type ptr, hence an array of ptrs is of type ptr-ptr.
	matrix = new double *[nrows];		

	// Allocate an array of doubles for each of the entries (rows of the column vector) of matrix, 
	// and return the & to matrix[i].
	// Note: an array is of type ptr, and its & is stored at location i in the array matrix[]
	for(i=0;i<nrows;i++)
	{
		matrix[i] = new double[ncols];	// allocate an array of doubles, return & to matrix[i]
	}


	// SETUP MATRIX
	cout << endl;
	for(i=0;i<nrows;i++)
	{
		for(j=0;j<ncols;j++)
		{
			matrix[i][j] = cnt;
			cnt++;
		}
	}

	PrintMatrix(matrix, nrows, ncols);
	

	// SETUP VECTOR
	for(i=0;i<ncols;i++)
	{
		vector[i] = i+3;
	}

	PrintVector(vector, ncols);

	// MEMORY DELETE
	delete [] vector;

	for(i=0;i<nrows;i++)
	{
		delete [] matrix[i];
	}
	delete [] matrix;

	return 0;
}


int PrepareMatrixMult()
{
	int i;
	int j;
	int cnt = 0;
	int nrowsA = 2;
	int ncolsA = 3;
	int nrowsB = ncolsA;
	int ncolsB = 4;
	int nrowsC = nrowsA;
	int ncolsC = ncolsB;
	double **A = NULL;
	double **B = NULL;
	double **C = NULL;

	cout << "\n PrepareMatrixMult()\n";

	// MEMORY ALLOC
	A = new double *[nrowsA];		// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsA;i++)
	{
		A[i] = new double[ncolsA];	// allocate an array of doubles, return & to matrix[i]
	}

	B = new double *[nrowsB];		// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsB;i++)
	{
		B[i] = new double[ncolsB];	// allocate an array of doubles, return & to matrix[i]
	}

	C = new double *[nrowsC];		// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsC;i++)
	{
		C[i] = new double[ncolsC];	// allocate an array of doubles, return & to matrix[i]
	}

	// SETUP MATRICES
	for(i=0; i<nrowsA; i++)
	{
		for(j=0; j<ncolsA; j++)
		{
			cnt++;
			A[i][j] = cnt;
		}
	}

	cnt = -1;
	for(i=0; i<nrowsB; i++)
	{
		for(j=0; j<ncolsB; j++)
		{
			cnt++;
			B[i][j] = cnt;
		}
	}

	// Matrix mult
	MatrixMult(nrowsA, ncolsA, ncolsB, A, B, C);

	PrintMatrix(A, nrowsA, ncolsA);
	PrintMatrix(B, nrowsB, ncolsB);
	PrintMatrix(C, nrowsC, ncolsC);

	// -- FREM MEM
	// Delete A
	for(i=0;i<nrowsA;i++)
	{
		delete [] A[i];
	}
	delete [] A;

	// Delete B
	for(i=0;i<nrowsB;i++)
	{
		delete [] B[i];
	}
	delete [] B;

	// Delete C
	for(i=0;i<nrowsC;i++)
	{
		delete [] C[i];
	}
	delete [] C;

	return 0;
}


void MatrixMult(int nrowsA, int ncolsA, int ncolsB, double **A, double **B, double **C)
{
	int i;
	int j;
	int k;
	
	// Matrix multiplication
	// A: nrowsA x ncolsA
	// B: nrowsB x ncolsB
	// C: nrowsA x ncolsB
	// C = A*B

	cout << "\n MatrixMult()\n\n";

	// Nullify C
	for(i=0; i<nrowsA; i++)
	{
		for(j=0; j<ncolsB; j++)
		{
			C[i][j] = 0.0;
		}
	}

	// Matrix multiplication
	for(k=0; k<ncolsB; k++)
	{
		for(i=0; i<nrowsA; i++)
		{
			for(j=0; j<ncolsA; j++)
			{
				C[i][k] = C[i][k] + A[i][j]*B[j][k];
				//printf(" C[%d][%d] = %lf\n", i, k, C[i][k]);
			}
		}
	}

	return;
}

int PrepareSwap(void)
{
	double a = 1;
	double b = 2;

	cout << "\n PrepareSwap()\n";
	cout << " a = " << a << " b = " << b << endl;

	Swap(a,b);

	cout << " a = " << a << " b = " << b << endl; 

	return 0;
}

void Swap(double &a, double &b)
{
	// NOTE: pass by reference uses syntactically clean pass-by-reference mechanism, i.e. without requiring
	// client of the fn to pass an address, which would then be reflected by a ptr-to-double in the arg. list.
	double temp;
	
	// Swap a and b
	temp = a;
	a = b;
	b = temp;
}

int PrepareMatrixInversion(void)
{
	int i;
	int j;
	int nrowsA = 3;
	int ncolsA = 3;
	int nrowsB = 3;
	int ncolsB = 1;
	double norm;			// matrix Euclidean norm of matrix difference
	double sum = 0.0;		// sum of squares of error
	double **A = NULL;		// leading matrix in A.x = b
	double **A_copy = NULL;	// copy of leading matrix in A.x = b
	double **B = NULL;		// r.h.s. vector in A.x = b
	double **B_copy = NULL;	// copy of r.h.s. vector in A.x = b
	double **C = NULL;		// matrix used to check that A.A^-1 = I
	double **D = NULL;		// matrix used to check that A.A^-1.b = b
	double **E = NULL;		// Error matrix = B_copy - D. Here D = A.A^-1.b

	cout << "\n PrepareMatrixInversion()\n";

	// MEMORY ALLOC
	A = new double *[nrowsA];			// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsA;i++)
	{
		A[i] = new double[ncolsA];		// allocate an array of doubles, return & to matrix[i]
	}

	A_copy = new double *[nrowsA];		// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsA;i++)
	{
		A_copy[i] = new double[ncolsA];	// allocate an array of doubles, return & to matrix[i]
	}

	B = new double *[nrowsB];			// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsB;i++)
	{
		B[i] = new double[ncolsB];		// allocate an array of doubles, return & to matrix[i]
	}

	B_copy = new double *[nrowsB];		// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsB;i++)
	{
		B_copy[i] = new double[ncolsB];	// allocate an array of doubles, return & to matrix[i]
	}

	C = new double *[nrowsA];			// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsA;i++)
	{
		C[i] = new double[ncolsA];		// allocate an array of doubles, return & to matrix[i]
	}

	D = new double *[nrowsB];			// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsB;i++)
	{
		D[i] = new double[ncolsB];		// allocate an array of doubles, return & to matrix[i]
	}

	E = new double *[nrowsB];			// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsB;i++)
	{
		E[i] = new double[ncolsB];		// allocate an array of doubles, return & to matrix[i]
	}


	// Setup A and B
	A[0][0] = 1.0;
	A[1][0] = 2.0;
	A[2][0] = 1.0;
	A[0][1] = 2.0;
	A[1][1] = 5.0;
	A[2][1] = 0.0;
	A[0][2] = 3.0;
	A[1][2] = 3.0;
	A[2][2] = 8.0;

	B[0][0] = 1.0;
	B[1][0] = 1.0;
	B[2][0] = 1.0;

	// Make a copy of A
	for(i=0; i<nrowsA; i++)
	{
		for(j=0; j<ncolsA; j++)
		{
			A_copy[i][j] = A[i][j];
		}
	}

	// Make a copy of B
	for(i=0; i<nrowsB; i++)
	{
		for(j=0; j<ncolsB; j++)
		{
			B_copy[i][j] = B[i][j];
		}
	}

	// Print A and B
	cout << "\n A:\n";	
	PrintMatrix(A, nrowsA, ncolsA);

	cout << "\n B:\n";
	PrintMatrix(B, nrowsB, ncolsB);


	// -- COMPUTE THE INVERSE AND THE SOLN VECTOR
	GaussJordanEliminationFullPivoting(A, nrowsA, B, ncolsB);


	// Print A^-1 and the soln vec. x = A^-1.b
	cout << "\n A^-1:\n";	
	PrintMatrix(A, nrowsA, ncolsA);

	cout << "\n x = A^-1.B\n";	
	PrintMatrix(B, nrowsB, ncolsB);


	// -- COMPUTE THE ERRORS IN THE MATRIX AND R.H.S. VECTOR

	// Check the inversion: A.A^-1
	MatrixMult(nrowsA, ncolsA, ncolsA, A_copy, A, C);		// A.A^-1 = C

	cout << "\n I = A.A^-1\n";	
	PrintMatrix(C, nrowsA, ncolsA);

	// Check the soln vector: x = A^-1.b => b = A.A^-1.b
	MatrixMult(nrowsA, ncolsA, ncolsB, A_copy, B, D);		// A.A^-1.b = b

	cout << "\n b = A.A^-1.b\n";	
	PrintMatrix(D, nrowsB, ncolsB);

	// Determine the differences bw. the original B and the recovered B, i.e. A.A^-1.b = D
	for(i=0; i<nrowsB; i++)
	{
		for(j=0; j<ncolsB; j++)
		{
			E[i][j] = B_copy[i][j] - D[i][j];
		}
	}

	// Check the matrix norm of the difference bw. original and recovered B matrices
	norm = MatrixNorm(nrowsB, ncolsB, E);
	//printf("\n norm = %7.4lf\n", norm);

	
	// MEMORY DELETE
	// Delete A
	for(i=0;i<nrowsA;i++)
	{
		delete [] A[i];
	}
	delete [] A;

	// Delete A_copy
	for(i=0;i<nrowsA;i++)
	{
		delete [] A_copy[i];
	}
	delete [] A_copy;

	// Delete B
	for(i=0;i<nrowsB;i++)
	{
		delete [] B[i];
	}
	delete [] B;

	// Delete B_copy
	for(i=0;i<nrowsB;i++)
	{
		delete [] B_copy[i];
	}
	delete [] B_copy;

	// Delete C
	for(i=0;i<nrowsA;i++)
	{
		delete [] C[i];
	}
	delete [] C;

	// Delete D
	for(i=0;i<nrowsB;i++)
	{
		delete [] D[i];
	}
	delete [] D;

	// Delete E
	for(i=0;i<nrowsB;i++)
	{
		delete [] E[i];
	}
	delete [] E;

	return 0;
}


int PrepareMatrixInversionLargeSystem(void)
{
	int i;
	int j;			
	int nrowsA = 100;
	int ncolsA = 100;
	int nrowsB = 100;
	int ncolsB = 1;
	double max = 0.0;			
	double min = 0.0;
	double norm;			// matrix Euclidean norm of matrix difference
	double random_no;		// random no.
	double sum = 0.0;		// sum of squares of error
	double uniformly_rand;	// uniformly random no.
	double **A = NULL;		// leading matrix in A.x = b
	double **A_copy = NULL;	// copy of leading matrix A
	double **B = NULL;		// r.h.s. vector in A.x = b
	double **C = NULL;		// product of A.A^-1 = I
	double **E = NULL;		// error matrix

	cout << "\n PrepareMatrixInversionLargeSystem()\n";

	// MEMORY ALLOC
	A = new double *[nrowsA];			// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsA;i++)
	{
		A[i] = new double[ncolsA];		// allocate an array of doubles, return & to matrix[i]
	}

	A_copy = new double *[nrowsA];			// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsA;i++)
	{
		A_copy[i] = new double[ncolsA];		// allocate an array of doubles, return & to matrix[i]
	}

	B = new double *[nrowsB];			// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsB;i++)
	{
		B[i] = new double[ncolsB];		// allocate an array of doubles, return & to matrix[i]
	}

	C = new double *[nrowsA];			// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsA;i++)
	{
		C[i] = new double[ncolsA];		// allocate an array of doubles, return & to matrix[i]
	}
	
	E = new double *[nrowsA];			// allocate an array of ptrs-to-double	
	for(i=0;i<nrowsA;i++)
	{
		E[i] = new double[ncolsA];		// allocate an array of doubles, return & to matrix[i]
	}

	// Seed the random no. generator with time value to get truly random nos.
	srand((unsigned)time(NULL));
	
	// Setup A and B
	for(i=0; i<nrowsA; i++)
	{
		for(j=0; j<ncolsA; j++)
		{
			random_no = rand();			
			uniformly_rand = double(random_no)/double(RAND_MAX);

			A[i][j] = uniformly_rand;
			A_copy[i][j] = A[i][j];
		}
	}

	for(i=0; i<nrowsB; i++)
	{
		random_no = rand();	
		uniformly_rand = double(random_no)/double(RAND_MAX);
		B[i][0] = uniformly_rand;
	}

	// Print A and B
	cout << "\n A:\n";	
	PrintMatrix(A, nrowsA, ncolsA);

	cout << "\n B:\n";
	PrintMatrix(B, nrowsB, ncolsB);


	// -- COMPUTE THE INVERSE AND THE SOLN VECTOR
	GaussJordanEliminationFullPivoting(A, nrowsA, B, ncolsB);


	// Print A^-1 and the soln vec. x = A^-1.b
	cout << "\n A^-1:\n";	
	PrintMatrix(A, nrowsA, ncolsA);

	cout << "\n x = A^-1.B\n";	
	PrintMatrix(B, nrowsB, ncolsB);


	// -- COMPUTE THE ERRORS

	// Check the inversion: A.A^-1
	MatrixMult(nrowsA, ncolsA, ncolsA, A_copy, A, C);		// A.A^-1 = C

	cout << "\n I = A.A^-1\n";	
	PrintMatrix(C, nrowsA, ncolsA);


	// Determine the differences bw. the identity I, and the recovered C, i.e. A.A^-1 = C
	for(i=0; i<nrowsA; i++)
	{
		for(j=0; j<ncolsA; j++)
		{
			if(i == j)
			{
				E[i][j] = 1.0 - C[i][j];	// I - C = 1 - C
			}
			else
			{
				E[i][j] = 0.0 - C[i][j];	// I - C = 0 - C
			}
		}
	}

	// Check the matrix norm of the difference bw. the identity and C
	norm = MatrixNorm(nrowsA, ncolsA, E);
	//printf("\n norm = %7.4lf\n", norm);

	
	// MEMORY DELETE
	// Delete A
	for(i=0;i<nrowsA;i++)
	{
		delete [] A[i];
	}
	delete [] A;

	// Delete A_copy
	for(i=0;i<nrowsA;i++)
	{
		delete [] A_copy[i];
	}
	delete [] A_copy;

	// Delete B
	for(i=0;i<nrowsB;i++)
	{
		delete [] B[i];
	}
	delete [] B;

	// Delete C
	for(i=0;i<nrowsA;i++)
	{
		delete [] C[i];
	}
	delete [] C;

	// Delete E
	for(i=0;i<nrowsB;i++)
	{
		delete [] E[i];
	}
	delete [] E;

	return 0;
}

void GaussJordanEliminationFullPivoting(double **A, int n, double **B, int m)
{
	//  THIS MATERIAL FOLLOWS CLOSELY THE WORK OF:
	//  Numerical Recipes in C: The Art of Scientific Computing (2nd Ed.), sections 2.0 - 2.1.
	//	by, W.H. Press, S.A. Teukolsky, W.T. Vetterling and B.P. Flannery.
	//  Only minor syntactic changes are made here to the original, e.g. array indexing 
	//  (0 to n-1 is used here, rather than the original 1 to n).

	/*  GaussJordanEliminationFullPivoting() solves the systems of equations represented by, e.g.,

		[A].[x_1, x_2, x_3, Y] = [b_1, b_2, b_3, I]

		where

		A.x_1 = b_1,
		A.x_2 = b_2,
		A.x_3 = b_3,

		and

		A.Y = I
	 => Y = A^-1

		A:nxn = input matrix. 
		This is overwritten with A^-1.

		B:nxm = input matrix containing the m r.h.s. vectors (of length n).
		This is overwritten with the corres. set of soln. vectors. That is,
		x_1 = A^-1.b_1,
		x_2 = A^-1.b_2,
		x_3 = A^-1.b_3.
	*/

	// Declaration
	int i;
	int icol;		// col index
	int irow;		// row index
	int j;
	int k;
	int u;
	int v;
	int *index_c = NULL;	// col index used for pivoting
	int *index_r = NULL;	// row index used for pivoting
	int *ipiv = NULL;		// pivot index array
	double big;
	double dum;
	double pivinv;
	double eps = 1.0e-6;	// ALTERATION: epsilon value to check for a "zero" on the diagonal

	// MEMORY NEW
	// Integer arrays used to manage the pivoting
	index_c = new int[n];	
	index_r = new int[n];
	ipiv = new int[n];

	// Nullify the ipiv array
	for(j=0; j<n; j++)
	{
		ipiv[j] = 0;
	}

	// Main loop over the n columns to be reduced.
	for(i=0; i<n; i++)
	{
		big = 0.0;											// init big == 0.0
		
		// Outer loop of the search for a pivot element
		for(j=0; j<n; j++)
		{
			if(ipiv[j] != 1)								// if the pivot array element != unity
			{
				for(k=0; k<n; k++)							
				{
					if(ipiv[k] == 0)						// if the pivot array element == nullity
					{
						if(fabs(A[j][k]) >= big)			// floating point absolute value
						{
							big = fabs(A[j][k]);			// update big with the double-type abs value of A[j][k]
							irow = j;
							icol = k;
						}
					}
				}// end for k					
			}// end if ipiv[j]
		}// end for j

		++(ipiv[icol]);

		// Now the pivot element is found, rows need to be intechanged, if rqd. to put the pivot element on the diag.
		// The cols are not physically interchanged, only relabelled:
		// index_c[i] the col of the ith pivot element is the ith col that is reduced
		// index_r[i] the row in which the pivot element was originally located.
		// If index_r[i] != index_c[i] there is an implied column interchange.
		// With this form of pivot management, the solutions x = A^-1.b which overwrite B, will end up in the correct order,
		// and the inverse matrix will be out of column alignment.

		if(irow != icol)
		{
			for(u=0; u<n; u++)
			{
				Swap(A[irow][u], A[icol][u]);
			}

			for(u=0; u<m; u++)
			{
				Swap(B[irow][u], B[icol][u]);
			}
		}

		// Pivot element indexing
		index_r[i] = irow;
		index_c[i] = icol;

		// Warn if there is a zero on the diag. implying a singular (non-invertible) matrix: was "if(A[icol][icol] == 0.0)"
		if((A[icol][icol] >= -eps) && (A[icol][icol] <= eps))
		{	
			cout << "\n GaussJordanEliminationFullPivoting(): Singular Matrix\n";
	
		}

		// Now the pivot row can be divided by the pivot element, located at irow and icol.
		pivinv = 1.0/A[icol][icol];
		A[icol][icol] = 1.0;

		for(u=0; u<n; u++)
		{
			A[icol][u] *= pivinv;
		}

		for(u=0; u<m; u++)
		{
			B[icol][u] *= pivinv;
		}

		// Now the rows are reduced, except for the pivot one.
		for(v=0; v<n; v++)
		{
			if(v != icol)
			{
				dum = A[v][icol];
				A[v][icol] = 0.0;

				for(u=0; u<n; u++)
				{
					A[v][u] -= A[icol][u]*dum;
				}

				for(u=0; u<m; u++)
				{
					B[v][u] -= B[icol][u]*dum;
				}
			}
		}
	}// end for i
	// The end of the main loop over columns of the reduction.

	// Realign the soln given the column interchanges. This is done by interchanging pairs of columns in the 
	// reverse order to that in which the permutation was built.
	for(u=(n-1); u>=0; u--)
	{
		if(index_r[u] != index_c[u])
		{
			for(k=0; k<n; k++)
			{
				Swap(A[k][index_r[u]], A[k][index_c[u]]);
			}
		}
	}
	
	// MEMORY DELETE
	delete [] index_c;
	delete [] index_r;
	delete [] ipiv;
}


double MatrixNorm(int nrows, int ncols, double **M)
{
	int i;
	int j;
	double norm = 0.0;	// Matrix Euclidean norm
	double sum = 0.0;	// sum of square of matrix entries

	cout << "\n MatrixNorm()\n";

	// Matrix Norm
	for(i=0; i<nrows; i++)
	{
		for(j=0; j<ncols; j++)
		{
			sum = sum + pow(M[i][j],2);
		}
	}
	norm = sqrt(sum);

	printf("\n Euclidean norm = %-10.3e \n", norm);	// left adjusted, e-format, field width = 10, precision = 3"

	return norm;
}

// -- PRINTING FNS		

int PrintMatrix(double **M, int nrows, int ncols)
{
	cout << " PrintMatrix()\n";
	
	int	i;
	int	j;

	// Print Matrix

	cout << endl;
	for(i=0;i<nrows;i++)
	{
		for(j=0;j<ncols;j++)
		{
			cout << " " << setprecision(prec) << M[i][j];
		}
		cout << endl;
	}
	cout << endl;

	return 0;
}


int PrintVector(double *v, int ncols)
{
	cout << " print_vector()\n";
	
	int	i;

	// Print Vector

	cout << endl;
	for(i=0;i<ncols;i++)
	{
		cout << " " << setprecision(prec) << v[i];
	}
	cout << "\n\n";

	return 0;
}


//eof