/* Gauss-Jordan elimination with full pivoting,
   based on code in Numerical Recipes in C. */

#include <math.h>
#include "gauss-jordan.h"

static inline void swap(REAL &x, REAL &y) { REAL tmp = x; x = y; y = tmp; }

/* We're trying to solve Ax = b.  
   a[1..n][1..n] is the matrix, b[1..n][1..m] is the RHS.  
   If successful, A will be overwritten with its inverse and b will be
   overwritten with x.  If not successful, an exception may be thrown.
   In some cases, underflow or overflow may occur silently. 
   If product_of_pivots is non-NULL then the product of the pivots is
   stored in *product_of_pivots.
*/
void gauss_jordan(REAL **a, int n, REAL **b, int m,
		  double *product_of_pivots) 
{
  int *indxc, *indxr, *ipiv, i, icol, irow, j, k, l, ll;
  REAL big, dum, pivinv;

  indxc = new int [n + 1];
  indxr = new int [n + 1];
  ipiv = new int [n + 1];

  if (product_of_pivots)
    *product_of_pivots = 1.0;

  /* find pivot */
  for (j = 1; j <= n; j++) 
    ipiv[j] = 0;
  for (i = 1; i <= n; i++) {
    big = -1;
    irow = icol = 1;
    for (j = 1; j <= n; j++)
      if (ipiv[j] != 1)
	for (k = 1; k <= n; k++)
	  if (ipiv[k] == 0) {
	    if (fabs(a[j][k]) > big) {
	      big = fabs(a[j][k]);
	      irow = j;
	      icol = k;
	    }
	  } else
	    if (ipiv[k] > 1) goto singular;
    ++(ipiv[icol]);
    /* perform the pivot */
    if (irow != icol) {
      for (l = 1; l <= n; l++)
	swap(a[irow][l], a[icol][l]);
      for (l = 1; l <= m; l++)
	swap(b[irow][l], b[icol][l]);
    }
    indxr[i] = irow;
    indxc[i] = icol;
    /* a[icol][icol] is the pivot now */
    if (product_of_pivots)
      *product_of_pivots *= a[icol][icol];
    if (a[icol][icol] == 0.0) goto singular;
    pivinv = 1.0 / a[icol][icol];
    a[icol][icol] = 1.0;
    for (l = 1; l <= n; l++)
      a[icol][l] *= pivinv;
    for (l = 1; l <= m; l++)
      b[icol][l] *= pivinv;

    /* reduce rows, except the pivot row  */
    for (ll = 1; ll <= n; ll++)
      if (ll != icol) {
	dum = a[ll][icol];
	a[ll][icol] = 0.0;
	for (l = 1; l <= n; l++)
	  a[ll][l] -= a[icol][l] * dum;
	for (l = 1; l <= m; l++)
	  b[ll][l] -= b[icol][l] * dum;
      }
  }
  /* Interchange columns to undo permutation */
  for (l = n; l >= 1; l--)
    if (indxr[l] != indxc[l])
      for (k = 1; k <= n; k++)
	swap(a[k][indxr[l]], a[k][indxc[l]]);

  delete[] ipiv;
  delete[] indxr;
  delete[] indxc;
  return;
 singular:
  throw SingularMatrix();
}