// Gauss Elimination routine
// modified version based on:
//
//   gauselim.cpp : Gaussian Elimination / Backward Substitution / FULL Pivoting
//   by Kenny Hoff : 2/6/97 : VERSION 4
//   http://www.cs.unc.edu/~hoff/projects/comp205/proj3/gauselim/gauseli4.cpp

#include "Error.h"

#define ABS(x) ((x) >= 0 ? (x) : (-(x)))

void GaussElimFullPivot(double M[8][9], int n, int m, double* X)
{
    int k,i,j,iPivot,jPivot;
    double SwapColTmp, SwapRowTmp, NullFactor;
	
    // REQUIRES A EXTRA STORAGE TO RECORD COLUMN SWAPS (FULL PIVOTING SWAPS
    // BOTH ROWS AND COLUMNS SINCE THE PIVOT ELEMENT IS CHOSEN FROM THE ENTIRE
    // SUBMATRIX, RATHER THAN JUST THE COL AS IN PARTIAL PIVOTING).
    // EACH ELT j IN SWAP STORES THE NEW LOCATION OF THE jth COLUMN.
    int *Swap = new int[n];         // DOES NOT INCLUDE LAST COLUMN
    for (j=0; j<n; j++) Swap[j]=j;  // INIT TO INDICATE NO SWAPPING HAS OCCURRED
    int tSwap;                      // TEMP SWAPPING VARIABLE FOR SWAP ELEMENTS
	
    //------- PART 1: CONVERSION OF MATRIX M INTO ROW-ECHELON FORM -------
    for (k=0; k<n; k++)  // FOR EACH SUBMATRIX SIZE (n-k) x (n-m)
	{
	    
	    // FIND THE ELEMENT IN THE KTH SUBMATRIX WITH THE LARGEST ABS VAL (PIVOT ELEMENT)
	    // EXCLUDE THE LAST COLUMN (NOT A PART OF THE COEFFICIENT MATRIX).
	    iPivot = jPivot = k;
	    for (i=k; i<n; i++) {
		for (j=k; j<n; j++) {
		    if (ABS(M[i][j]) > ABS(M[iPivot][jPivot]))
			{ iPivot=i; jPivot=j; }
		}
	    }
		
	    // IF THERE IS NO NONZERO ELEMENT AS A PIVOT, QUIT!
	    if (M[iPivot][jPivot] == 0)
		throw CError("GaussElim: singular matrix!");
		
	    // SWAP THE PIVOT ROW WITH THE KTH ROW
	    for (j=0; j<m; j++)
		{			
		    SwapRowTmp = M[iPivot][j];
		    M[iPivot][j] = M[k][j];
		    M[k][j] = SwapRowTmp;
		}
	    // SWAP THE PIVOT COL WITH THE KTH COL. BE SURE TO STORE THE SWAP.
	    for (i=0; i<n; i++)
		{
		    SwapColTmp = M[i][jPivot];
		    M[i][jPivot] = M[i][k];
		    M[i][k] = SwapColTmp;
		}
	    tSwap = Swap[jPivot];
	    Swap[jPivot] = Swap[k];
	    Swap[k] = tSwap;
		
	    // NULLIFY (TURN INTO ZERO) ALL ELEMENT IN COL BELOW TOP ROW BY
	    // SUBTRACTING SOME FACTOR TIMES THE TOP ROW.
	    for (i=k+1; i<n; i++)
		{
		    NullFactor = -(M[i][k]/M[k][k]);
		    M[i][k]=0;
		    for (j=k+1; j<m; j++)
			M[i][j] += (M[k][j]*NullFactor);
		}
	    if (0) {
		printf("GaussElim: k = %d, iPivot=%d, jPivot=%d, val=%g\n",
		       k, iPivot, jPivot, M[iPivot][jPivot]);
		for (int i=0; i<n; i++) {
		    for (int j=0; j < m; j++)
			printf(j<6? "%5d " : "%9d ", (int)M[i][j]);
		    printf("\n");
		}
		printf("\n");
	    }
		
	    // IF THEIR IS A ROW CONTAINING ALL ZEROS (EXCEPT FOR LAST ELT), QUIT!
	    // DS: yes, but the following is not checking this!!!
	    //if (M[n-1][n-1] == 0) 
	    //throw CError("GaussElim: singular matrix!");
	}
	
    //------- PART 2: BACKWARDS SUBSTITUTION -------
    for (k=(n-1); k>=0; k--)  // FOR EACH ROW STARTING AT THE BOTTOM
	{
	    X[k] = M[k][m-1];      // SOLVE FOR CURRENT X[k]
	    for (j=(k+1); j<(m-1); j++)
		X[k] -= (M[k][j]*X[j]);
	    X[k] /= M[k][k];
	}
	
    // MUST REARRANGE THE SOLUTION VECTOR X BASED ON SWAP ARRAY
    double *Xcopy = new double[n];
    for (i=0; i<n; i++) Xcopy[i] = X[i];
    for (i=0; i<n; i++) X[ Swap[i] ] = Xcopy[i];
    delete[] Xcopy; // DELETE COPY OF X SOLUTION COLUMN VECTOR
	
    delete[] Swap;  // FREE COLUMN SWAP SPACE
}