# include <ohana.h>

int dgaussj (double **a, int n, double **b, int m) {

  int *indexCol;
  int *indexRow;
  int *pivot;
  int i, col, row, j, k, l, ll;
  double big, temp;
  
  ALLOCATE (indexCol, int, n);
  ALLOCATE (indexRow, int, n);
  ALLOCATE (pivot, int, n);
  for (j = 0; j < n; j++) pivot[j] = 0;

  row = col = 0;
  big = fabs(a[0][0]);

  for (i = 0; i < n; i++) {
    big = 0.0;
    for (j = 0; j < n; j++) {
      if (!finite(a[i][j])) goto escape;
      if (pivot[j] != 1) {
	for (k = 0; k < n; k++) {
	  if (pivot[k] == 0) {
	    if (fabs (a[j][k]) >= big) {
	      big = fabs (a[j][k]);
	      row = j;
	      col = k;
	    }
	  } else {
	    if (pivot[k] > 1) goto escape;
	  }
	}
      }
    }
    pivot[col]++;
    if (row != col) {
      for (l = 0; l < n; l++) SWAP (a[row][l], a[col][l]);
      for (l = 0; l < m; l++) SWAP (b[row][l], b[col][l]);
    }
    indexRow[i] = row;
    indexCol[i] = col;
    if (a[col][col] == 0.0) goto escape;

    /* rescale by pivot reciprocal */
    temp = 1.0 / a[col][col];
    a[col][col] = 1.0;
    for (l = 0; l < n; l++) a[col][l] *= temp;
    for (l = 0; l < m; l++) b[col][l] *= temp;

    /* adjust the elements above the pivot */
    for (ll = 0; ll < n; ll++) {
      if (ll != col) {
	temp = a[ll][col];
	a[ll][col] = 0.0;
	for (l = 0; l < n; l++) a[ll][l] -= a[col][l]*temp;
	for (l = 0; l < m; l++) b[ll][l] -= b[col][l]*temp;
      }
    }
  }

  for (l = n - 1; l >= 0; l--) {
    if (indexRow[l] != indexCol[l]) {
      for (k = 0; k < n; k++) SWAP (a[k][indexRow[l]], a[k][indexCol[l]]);
    }
  }
  free (pivot);
  free (indexRow);
  free (indexCol);
  return (TRUE);

escape:
  free (pivot);
  free (indexRow);
  free (indexCol);
  return (FALSE);
}

int fgaussj (float **a, int n, float **b, int m) {

  int *indexCol;
  int *indexRow;
  int *pivot;
  int i, col, row, j, k, l, ll;
  float big, temp;
  
  ALLOCATE (indexCol, int, n);
  ALLOCATE (indexRow, int, n);
  ALLOCATE (pivot, int, n);
  for (j = 0; j < n; j++) pivot[j] = 0;

  row = col = 0;
  big = fabs(a[0][0]);

  for (i = 0; i < n; i++) {
    big = 0.0;
    for (j = 0; j < n; j++) {
      if (!finite(a[i][j])) goto escape;
      if (pivot[j] != 1) {
	for (k = 0; k < n; k++) {
	  if (pivot[k] == 0) {
	    if (fabs (a[j][k]) >= big) {
	      big  = fabs (a[j][k]);
	      row = j;
	      col = k;
	    }
	  } else {
	    if (pivot[k] > 1) goto escape;
	  }
	}
      }
    }
    pivot[col]++;
    if (row != col) {
      for (l = 0; l < n; l++) SWAP (a[row][l], a[col][l]);
      for (l = 0; l < m; l++) SWAP (b[row][l], b[col][l]);
    }
    indexRow[i] = row;
    indexCol[i] = col;
    if (a[col][col] == 0.0) goto escape;

    /* rescale by pivot reciprocal */
    temp = 1.0 / a[col][col];
    a[col][col] = 1.0;
    for (l = 0; l < n; l++) a[col][l] *= temp;
    for (l = 0; l < m; l++) b[col][l] *= temp;
 
    /* adjust the elements above the pivot */
    for (ll = 0; ll < n; ll++) {
      if (ll != col) {
	temp = a[ll][col];
	a[ll][col] = 0.0;
	for (l = 0; l < n; l++) a[ll][l] -= a[col][l]*temp;
	for (l = 0; l < m; l++) b[ll][l] -= b[col][l]*temp;
      }
    }
  }

  for (l = n - 1; l >= 0; l--) {
    if (indexRow[l] != indexCol[l]) {
      for (k = 0; k < n; k++) SWAP (a[k][indexRow[l]], a[k][indexCol[l]]);
    }
  }
  free (pivot);
  free (indexRow);
  free (indexCol);
  return (TRUE);

escape:
  free (pivot);
  free (indexRow);
  free (indexCol);
  return (FALSE);
}
