/**@file
 *
 * Simple Gauss elimination with total pivot search for non-degenerate
 * quadratic matrices.
 * 
 *      author:  Claus-Justus Heine
 *               Abteilung fuer Angewandte Mathematik                     
 *               Albert-Ludwigs-Universitaet Freiburg
 *               Hermann-Herder-Str. 10
 *               79104 Freiburg
 *               Germany
 *               claus@mathematik.uni-freiburg.de
 *
 *      Copyright (c) by Claus-Justus Heine (2007)
 */

#include <alberta_util.h>

/**@addtogroup MISCELLANEOUS
 * @{
 */

static inline void
swap_rows(REAL *M, int r1, int r2, int n, REAL *b, int nbcol);
static inline void
swap_cols(REAL *M, int c1, int c2, int *perm, int n);

/** Simple Gauss elimination with total pivot search for
 * non-degenerate quadratic matrices. BIG FAT NOTE: M and b are
 * destroyed by this function.
 *
 * @param[in,out] M Quadratic NxN matrix in row-major storage order.
 *
 * @param[in] n Dimension of @a M.
 *
 * @param[in,out] b Right hand side in column major order.
 *
 * @param[in] nbcol Number of columns of @a b and @a x.
 *
 * @param[in,out] x Storage for the result. @a x is in column major
 * order.
 */
void square_gauss(REAL *M, REAL *b, REAL *x, int n, int nbcol)
{
  int perm[n];

  for (int i = 0; i < n; i++) {
    perm[i] = i;
  }
  for (int i = 0; i < n; i++) {
    /* find the pivot element */
    REAL max = 0.0;
    int maxcol = i;
    int maxrow = i;
    for (int row = i; row < n; row++) {
      for (int col = i; col < n; col++) {
        if (fabs(M[row*n+col]) > max) {
          max = fabs(M[row*n+col]);
          maxrow = row;
          maxcol = col;
        }
      }
    }
    if (maxrow != i) {
      swap_rows(M, i, maxrow, n, b, nbcol);
    }
    if (maxcol != i) {
      swap_cols(M, i, maxcol, perm, n);
    }
    REAL pivot = M[i*n + i];

    /* clear i-th column of M */
    for (int row = i+1; row < n; row++) {
      REAL factor = M[row*n + i] / pivot;
      for (int col = i+1; col < n; col++) {
        M[row*n + col] -= M[i*n + col] * factor;
      }
      /* also perform that operation on b */
      for (int col = 0; col < nbcol; col++) {
        b[col*n + row] -= b[col*n + i] * factor;
      }
    }
  }

  /* at this stage the upper triangular part of M holds the upper
   * triangular factor of the LR decomposition; just perform
   * back-substitution to get the solution.
   */
  for (int row = n-1; row >= 0; --row) {
    for (int col = 0; col < nbcol; col++) {
      x[col*n + perm[row]] = b[col * n + row];
      for (int i = row+1; i < n; i++) {
        x[col*n + perm[row]] -= M[row*n+i]*x[col*n + perm[i]];
      }
      x[col*n + perm[row]] /= M[row*n+row];
    }
  }
}

static inline void
swap_rows(REAL *M, int r1, int r2, int n, REAL *b, int nbcol)
{
  REAL tmp;
  REAL *row1 = M+r1*n;
  REAL *row2 = M+r2*n;
  int col;

  for (col = 0; col < n; col++) {
    tmp     = *row1;
    *row1++ = *row2;
    *row2++ = tmp;
  }
  for (col = 0; col < nbcol; col++) {
    tmp           = b[col*n + r1];
    b[col*n + r1] = b[col*n + r2];
    b[col*n + r2] = tmp;
  }
}

static inline void
swap_cols(REAL *M, int c1, int c2, int *perm, int n)
{
  int tmpi;

  tmpi     = perm[c1];
  perm[c1] = perm[c2];
  perm[c2] = tmpi;
  for (int i = 0; i < n; i++) {
    REAL tmp    = M[i*n + c1];
    M[i*n + c1] = M[i*n + c2];
    M[i*n + c2] = tmp;
  }
}

/**@} MISCELLANEOUS */

#ifdef TESTING

#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#define DIM 5

static inline Mv(const REAL *M, int n, const REAL *x, REAL *result, int nres)
{
  for (int k = 0; k < nres; k++) {
    for (int i = 0; i < n; i++) {
      result[k*n+i] = 0.0;
      for (int j = 0; j < n; j++) {
        result[k*n+i] += M[i*n+j]*x[k*n+j];
      }
    }
  }
}

int main(int argc, const char *argv[])
{
  int i, j;
  REAL M[DIM*DIM];
  REAL MC[DIM*DIM];
  REAL b[3*DIM];
  REAL bC[3*DIM];
  REAL x[3*DIM];
  
  for (i = 0; i < 3; i++) {
    for (j = 0; j < DIM; j++) {
      b[i*DIM+j] = drand48()-0.5;
    }
  }
#if 1
  for (i = 0; i < DIM; i++) {
    for (j = 0; j < DIM; j++) {
      M[i*DIM+j] = drand48()-0.5;
    }
    M[i*DIM+i] += 1.0;
  }
#else
  memset(M, 0, sizeof(M));
  for (i = 0; i < DIM; i++) {
    M[i*DIM+i] = i+1;
#if 0    
    for (j = 0; j <= i; j++) {
      M[i*DIM+j] = j+1;
    }
#endif
  }
#endif
  memcpy(MC, M, sizeof(M));
  memcpy(bC, b, sizeof(b));
  
  gauss(M, b, x, DIM, 3);

  Mv(MC, DIM, x, b, 3);

  printf("Matrix:\n");
  for (i = 0; i < DIM; i++) {
    for (j = 0; j < DIM; j++) {
      printf("%e ", MC[i*DIM+j]);
    }
    printf("\n");
  }

  printf("RHS:\n");
  for (i = 0; i < 3; i++) {
    for (j = 0; j < DIM; j++) {
      printf("%e ", bC[i*DIM+j]);
    }
    printf("\n");
  }
  printf("\n");
  printf("Result:\n");
  for (i = 0; i < 3; i++) {
    for (j = 0; j < DIM; j++) {
      printf("%e ", b[i*DIM+j]);
    }
    printf("\n");
  }
}
#endif
/*
 * Local Variables: ***
 * mode: c ***
 * c-basic-offset: 2 ***
 * compile-command: "gcc -o gauss -DTESTING -O0 -ggdb3 gausselim.c -lm" ***
 * End: ***
 */
 
