/*
**  GE1bs
**
**  Purpose:
**    Gauss elimination, find upper triangular matrix, do
**    backsubstitution
**
**  Date:
**    20/8/2012
**
**  Version:
**    0     Start program, prepare the extended matrix
**    1     Start eliminating row 1
**    1bs   Perform backsubstitution
**
**  Author:
**    Charles Bos
*/
#include <oxstd.h>

/*
**  EliminateRow(const amAB, const j, const k)
**
**  Purpose:
**    Eliminate one row from the extended matrix mAB
**
**  Inputs:
**    amAB      address of the iK x iK+1 extended matrix
**    j         integer, row to eliminate
**    k         integer, pivot number, base row
**
**  Output:
**    amAB      address of iK x iK+1 extended matrix, with element [j][k] put to
**              zero
**
**  Return value:
**    none       
*/
EliminateRow(const amAB, const j, const k)
{
  decl i, iK, dMult;
  
  iK= rows(amAB[0]);
  dMult= amAB[0][j][k]/amAB[0][k][k];
  for (i= k; i < iK+1; ++i)
    amAB[0][j][i]= amAB[0][j][i] - dMult * amAB[0][k][i];
}

/*
**  EliminateColumn(const amAB, const k)
**
**  Purpose:
**    Eliminate column k of the extended matrix
**
**  Inputs:
**    amAB      address of the iK x iK+1 extended matrix
**    k         integer, pivot number, base row
**
**  Output:
**    amAB      address of iK x iK+1 extended matrix, with elements
**              below [k][k] all put to zero
**
**  Return value:
**    none
*/
EliminateColumn(const amAB, const k)
{
  decl iK, j;
  
  iK= rows(amAB[0]);
  for (j= k+1; j < iK; ++j)
    EliminateRow(amAB, j, k);
}

/*
**  EliminateMatrix(const amAB)
**
**  Purpose:
**    Eliminate extended matrix
**
**  Inputs:
**    amAB      address of the iK x iK+1 extended matrix
**
**  Output:
**    amAB      address of iK x iK+1 extended matrix, eliminated
**
**  Return value:
**    none
*/
EliminateMatrix(const amAB)
{
  decl iK, k;
  
  iK= rows(amAB[0]);
  for (k= 0; k < iK; ++k)
    EliminateColumn(amAB, k);
}

/*
**  BackSubstitution(const mAB)
**
**  Purpose:
**    Perform backsubstitution on the extended mAB matrix
**
**  Inputs:
**    mAB       iK x iK+1 matrix, upper diagonal
**
**  Return value:
**    vX        iK x 1 vector, outcome of Ax=b
*/
BackSubstitution(const mAB)
{
  decl iK, vX, i, j, dS;
  
  // Do backsubstitution
  iK= rows(mAB);
  vX= zeros(iK, 1);  
  for (i= iK-1; i >= 0; --i)
    {
      dS= 0; 
      for (j= i+1; j < iK; ++j)
        dS= dS + mAB[i][j] * vX[j];
      vX[i]= (mAB[i][iK] - dS)/mAB[i][i];
    }
  return vX;
}



main()
{
  decl mA, vB, mAB, vX, iK;
  
  // Magic numbers
  mA= < 6,  -2, 2,   4; 
       12,  -8, 6,  10; 
        3, -13, 9,   3; 
       -6,   4, 1, -18>; 
  vB= <16; 26; -19; -34>;
  
  // Initialisation
  mAB= mA~vB;

  // Estimation  
  EliminateMatrix(&mAB);
  vX= BackSubstitution(mAB);  
  
  // Output
  iK= rows(mA);
  print ("Extended matrix mAB is, after: ", mAB);
  println ("The outcome would be ", vX);
  println ("Check: Ax: ", mA*vX~vB);
  println ("Check2, extended matrix : Ax: ", mAB[][:iK-1]*vX~mAB[][iK]);
  
}