// functions for Gauss Elimination without pivoting and with 
// scaled row partial pivoting. Procedural version. Students
// are encouraged to write object oriented version.

#include <iostream.h>
#include <math.h>
#include <stdlib.h>               // for rand()
#include <algorithm>               // for rand()

// **** Gauss Elimination without pivoting to solve A X = b ******
void GaussElim(int nn, double** mx, double* bb) {

// nn:  number of rows = number of columns
// mx:  on entry: coefficient matrix A[i][j], i,j = 0, 1, ..., nn -1.
//      on return: elements of L and U in LU decomposition A = LU
// bb:  on entry: right hand side vector bb[i], i = 0, 1, ..., nn -1.
//      on return: numerical solution x to A x = b

  // LU (Doolittle's) decomposition without pivoting
  for (int k = 0; k < nn - 1; k++) {
   for (int i = k + 1; i < nn; i++) {
     if (mx[k][k] == 0)  cout << "pivot is zero in Gausselim()\n";
     double mult = mx[i][k]/mx[k][k];
     mx[i][k] = mult;                      // entries of L is saved in mx
     for (int j = k + 1; j < nn; j++) 
       mx[i][j] -= mult*mx[k][j];          // entries of U is saved in mx
   }
  }

  // forwad substitution for L y = b. y is still stored in b
  for (int i = 1; i < nn; i++) 
    for (int j = 0; j < i; j++) 
      bb[i] -= mx[i][j]*bb[j];
  
  // back substitution for U x = y. x is still stored in b 
  for (int i = nn - 1; i >= 0; i--) {
    for (int j = i + 1; j < nn; j++) bb[i] -= mx[i][j]*bb[j];
    bb[i] /= mx[i][i];
  }
} // end GaussElim()


// **** Gauss Elim with scaled row partial pivoting for solving Ax = b
void GaussElimSRPP(int nn, double** mx, double* bb) {

// nn:  number of rows = number of columns
// mx:  on entry: coefficient matrix A[i][j], i,j = 0, 1, ..., nn -1.
//      on return: elements of L and U in LU decomposition PA = LU
// bb:  on entry: right hand side vector bb[i], i = 0, 1, ..., nn -1.
//      on return: numerical solution x to A x = b
// Note: pivoting information is stored in temperary vector pvt

  int* pvt = new int [nn];                      // pivoting vector 
  for (int k = 0; k < nn; k++) pvt[k] = k;

  double* scale  = new double [nn];             // find scale vector
  for (int k = 0; k < nn; k++) {
    scale[k] = 0;
    for (int j = 0; j < nn; j++) 
      if (fabs(scale[k]) < fabs(mx[k][j])) scale[k] = fabs(mx[k][j]);
  } 

  for (int k = 0; k < nn - 1; k++) {            // main elimination loop

    // find the pivot in column k in rows pvt[k], pvt[k+1], ..., pvt[n-1]
    int pc = k; 
    double aet = fabs(mx[pvt[k]][k]/scale[k]);
    for (int i = k + 1; i < nn; i++) {
      double tmp = fabs(mx[pvt[i]][k]/scale[pvt[i]]); 
      if (tmp > aet) {
        aet = tmp; 
        pc = i;
      }
    }
    if (aet == 0) cout << "pivot is zero in GaussElimSRPP()\n"; 
    if (pc != k) {                              // swap pvt[k] and pvt[pc]
      int ii = pvt[k];
      pvt[k] = pvt[pc];
      pvt[pc] = ii;
    }

    // now eliminate the column entries logically below mx[pvt[k]][k]
    int pvtk = pvt[k];                           // pivot row
    for (int i = k + 1; i < nn; i++) {
      int pvti = pvt[i];
      if (mx[pvti][k] != 0) {
        double mult = mx[pvti][k]/mx[pvtk][k]; 
        mx[pvti][k] = mult;
        for (int j = k + 1; j < nn; j++) mx[pvti][j] -= mult*mx[pvtk][j];
      }
    }
  }

  // forwad substitution for L y = Pb. Store y in b
  for (int i = 1; i < nn; i++)  
    for (int j = 0; j < i; j++) bb[pvt[i]] -= mx[pvt[i]][j]*bb[pvt[j]];

  // back substitution  for Ux = y
  double* xx = new double [nn];                // xx stores solution      
  for (int i = nn - 1; i >= 0; i--) {
    for (int j = i+1; j < nn; j++) bb[pvt[i]] -= mx[pvt[i]][j]*xx[j];
    xx[i] = bb[pvt[i]] / mx[pvt[i]][i];
  }

  for (int i = 0; i < nn; i++)  bb[i] = xx[i]; // put solution in bb 

  delete[] xx;                                 // free space
  delete[] scale;
  delete[] pvt;
} // end GaussElimSRPP()


// **** Gauss Elim with complete pivoting for solving Ax = b
void GaussElimCP(int nn, double** mx, double* bb) {

// Gauss elimination with complete pivoting (without scaling)
// nn:  number of rows = number of columns
// mx:  on entry: coefficient matrix A[i][j], i,j = 0, 1, ..., nn -1.
//      on return: elements of L and U in LU decomposition PAQ = LU
// bb:  on entry: right hand side vector bb[i], i = 0, 1, ..., nn -1.
//      on return: numerical solution x to A x = b
// Note: pivoting information is stored in temperary vectors px, qy

  int* px = new int [nn];                      // pivoting vector 
  int* qy = new int [nn];                      // pivoting vector 
  for (int k = 0; k < nn; k++) px[k] = qy[k] = k;

  for (int k = 0; k < nn - 1; k++) {            // main elimination loop

    // find the pivot in column qy[k] in rows px[k], px[k+1], ..., px[n-1]
    int pct = k, qdt = k; 
    double aet = 0;
    for (int i = k; i < nn; i++) {
      for (int j = k; j < nn; j++) {
        double tmp = fabs(mx[px[i]][qy[j]]); 
        if (tmp > aet) { aet = tmp; pct = i; qdt = j; }
      }
    }
    if (aet == 0) cout << "pivot is zero in GaussElimCP()\n"; 
    swap(px[k], px[pct]);                        // swap px[k] and px[pct]
    swap(qy[k], qy[qdt]);

    // now eliminate the column entries logically below mx[px[k]][qy[k]]
    for (int i = k + 1; i < nn; i++) {
      if (mx[px[i]][qy[k]] != 0) {
        double mult = mx[px[i]][qy[k]]/mx[px[k]][qy[k]]; 
        mx[px[i]][qy[k]] = mult;
        for (int j = k + 1; j < nn; j++) 
	  mx[px[i]][qy[j]] -= mult*mx[px[k]][qy[j]];
      }
    }
  }

  // forwad substitution for L y = Pb. Store y in b
  for (int i = 1; i < nn; i++)  
    for (int j = 0; j < i; j++) bb[px[i]] -= mx[px[i]][qy[j]]*bb[px[j]];

  // back substitution  for Uz = y and x = Q z
  double* xx = new double [nn];                // xx stores solution      
  for (int i = nn - 1; i >= 0; i--) {
    for (int j = i+1; j < nn; j++) bb[px[i]] -= mx[px[i]][qy[j]]*xx[qy[j]];
    xx[qy[i]] = bb[px[i]] / mx[px[i]][qy[i]];
  }

  for (int i = 0; i < nn; i++)  bb[i] = xx[i]; // put solution in bb 

  delete[] xx;                                 // free space
  delete[] px;
  delete[] qy;
} // end GaussCP()


// a main program to test the Gauss elimination functions above

int main() {
  
  int n = 3;
  double** m = new double* [n];   // allocate space for a 2 by 2 matrix
  for (int i = 0; i < n; i++) m[i] = new double [n];

  double epsn = 1.0e-5;
  m[0][0] = epsn;
  m[0][1] = 1;
  m[0][2] = 3;
  m[1][0] = 2;
  m[1][1] = 1;
  m[1][2] = 4;
  m[2][0] = 3;
  m[2][1] = 1;
  m[2][2] = 0;

  double* b = new double [n];    // allocate space for right vector
  b[0] = 100;
  b[1] = 10;
  b[2] = 5;

  double** a = new double* [n];  // to store coefficient matrix 
  for (int i = 0; i < n; i++) a[i] = new double [n];
  for (int i = 0; i < n; i++) 
    for (int j = 0; j < n; j++) a[i][j] = m[i][j];

  double* c = new double [n];    // to store right vector
  for (int i = 0; i < n; i++) c[i] = b[i];

  GaussElim(n, m, b);            // m, b are modified by the function call

  cout.precision(15);
  cout << "solution by Gauss elim without pivoting is: \n";
  for (int i = 0; i < n; i++) cout << b[i] << "  ";

  cout << "\nresidual vector is: \n";            // c stores right vector
  for (int i = 0; i < n; i++) {                  // a stores coeff matrix
    double prod= 0;
    for (int j = 0; j < n; j++) prod += a[i][j]*b[j];  
    cout << c[i] - prod << "  ";
  }


  // restore matrix m and right vector b for Gauss elim with SRPP
  for (int i = 0; i < n; i++) {
    b[i] = c[i];
    for (int j = 0; j < n; j++) m[i][j] = a[i][j];
  }

  GaussElimSRPP(n, m, b);        // m, b are modified by the function call

  cout << "\n\nsolution by Gauss elim with scaled row partial pivoting is:\n";
  for (int i = 0; i < n; i++) cout << b[i] << "  ";

  cout << "\nresidual vector is: \n";            // c stores right vector
  for (int i = 0; i < n; i++) {                  // a stores coeff matrix
    double prod= 0;
    for (int j = 0; j < n; j++) prod += a[i][j]*b[j];  
    cout << c[i] - prod << "  ";
  }
  cout << "\n";

  // restore matrix m and right vector b for Gauss elim with CP
  for (int i = 0; i < n; i++) {
    b[i] = c[i];
    for (int j = 0; j < n; j++) m[i][j] = a[i][j];
  }
  GaussElimCP(n, m, b);        // m, b are modified by the function call
  cout << "\n\nsolution by Gauss elim with complete pivoting is:\n";
  for (int i = 0; i < n; i++) cout << b[i] << "  ";

  cout << "\nresidual vector is: \n";            // c stores right vector
  for (int i = 0; i < n; i++) {                  // a stores coeff matrix
    double prod= 0;
    for (int j = 0; j < n; j++) prod += a[i][j]*b[j];  
    cout << c[i] - prod << "  ";
  }
  cout << "\n";

  // free space
  for (int i = 0; i < n; i++) {
    delete[] a[i];
    delete[] m[i];
  }
  delete[] a;
  delete[] m;
  delete[] b;
  delete[] c;

  // test on using Hilbert matrix, a notoriously ill-conditioned matrix
  cout.precision(4);
  for (int k = 2; k < 2000; k *= 2) {

    //create a Hilbert matrix of k rows and k columns
    double** hilbert = new double* [k];
    double** hilbm = new double* [k];
    for (int i = 0; i < k; i++) {
      hilbert[i] = new double [k];
      hilbm[i] = new double [k];
    }
    for (int i = 0; i < k; i++) 
      for (int j = 0; j < k; j++) {
	 hilbm[i][j] = 1.0/(i + j + 1.0);
	 hilbert[i][j] = hilbm[i][j];
      }

    // create a column vector of k components and assign random numbers
    // there is a big difference when the right hand is generated  by
    // random numbers and by simply being i for component i
    double* b = new double [k];
    double* bm = new double [k];

    // compare the difference of results between the following two lines
//    for (int i = 0; i < k; i++) b[i] = bm[i] = rand()%100; 
    for (int i = 0; i < k; i++) b[i] = bm[i] = 10 - i;

    double maxbm = 0;             // max norm of right vector
    for (int i = 0; i < k; i++) {
      if (maxbm < fabs(bm[i])) maxbm = fabs(bm[i]);
    }

    // call gauss elimation without pivoting. Solution is stored in b
    GaussElim(k, hilbert, b);            // hilbert, b are modified 

    // form the residual vector and evaluate its max norm 
    double* resid = new double [k];
    for (int i = 0; i < k; i++) {
      resid[i] = bm[i];
      for (int j = 0; j < k; j++) resid[i] -= hilbm[i][j]*b[j];
    }
    double maxerr = 0;
    for (int i = 0; i < k; i++) {
      if (maxerr < fabs(resid[i])) maxerr = fabs(resid[i]);
    }
    cout <<"Gauss elim on Hilbert matrix of order k = " << k << "\n";
    cout <<"max norm of relative residual without pivoting = " 
	 << maxerr/maxbm << "\n";

    // call gauss elimation with paritial pivoting. Solution is stored in b
    for (int i = 0; i < k; i++) {
      b[i] = bm[i];
      for (int j = 0; j < k; j++) hilbert[i][j] = hilbm[i][j];
    }
    GaussElimSRPP(k, hilbert, b);            // hilbert, b are modified 
    for (int i = 0; i < k; i++) {
      resid[i] = bm[i];
      for (int j = 0; j < k; j++) resid[i] -= hilbm[i][j]*b[j];
    }
    maxerr = 0;
    for (int i = 0; i < k; i++) {
      if (maxerr < fabs(resid[i])) maxerr  = fabs(resid[i]);
    }
    cout <<"max norm of relative residual with scaled row partial pivoting = " 
	 << maxerr/maxbm << "\n\n";

    // call gauss elimation with complete pivoting. Solution is stored in b
    for (int i = 0; i < k; i++) {
      b[i] = bm[i];
      for (int j = 0; j < k; j++) hilbert[i][j] = hilbm[i][j];
    }
    GaussElimCP(k, hilbert, b);            // hilbert, b are modified 
    for (int i = 0; i < k; i++) {
      resid[i] = bm[i];
      for (int j = 0; j < k; j++) resid[i] -= hilbm[i][j]*b[j];
    }
    maxerr = 0;
    for (int i = 0; i < k; i++) {
      if (maxerr < fabs(resid[i])) maxerr  = fabs(resid[i]);
    }
    cout <<"max norm of relative residual with complete pivoting = " 
	 << maxerr/maxbm << "\n\n";
    
    // free space
    delete[] b;
    delete[] bm;
    delete[] resid;
    for (int i = 0; i < k; i++) {
      delete[] hilbert;
      delete[] hilbm;
      
    }
  }

}  // end main()