/* Gaussian Elimination with Partial Pivoting (GEPP) */

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

#define	N	5

#define SWAP(type,x,y)	{type temp; temp=x; x=y; y=temp;}

typedef float real;

/* Print a vector of reals as one row. */
static void
print_vector( 
	     const char *title,
	     real *x,
	     int n)
{
  int i;
  printf("%s (dim=%d): ", title, n);
  for(i=0; i<n; i++ ) printf("%9.4f ", x[i]);
  putchar('\n');
  return;
}

/* Print a list of ints as one row. */
static void
print_ints( 
	   const char *title,
	   int *x,
	   int n)
{
  int i;
  printf("%s (dim=%d): ", title, n);
  for(i=0; i<n; i++ ) printf("%3d ", x[i]);
  putchar('\n');
  return;
}


/* Gaussian Elimination with Partial Pivoting (GEPP):
[ 1] For k in 1,...,N-1, do [2] to [10]:
[ 2]   Find the largest value |A(m,k)| in column k, for m in k,...,N
[ 3]   If A(m,k)==0, then print "A is singular" and stop.
[ 4]   If m==k, then go to [7]; else, do [5] and [6]:
[ 5]     For j in k,...,N, interchange A(m,j) and A(k,j)
[ 6]     Interchange b(m) and b(k)
[ 7]   For i in k+1,...,N, do [8] to [10]:
[ 8]     Replace A(i,k) with A(i,k)/A(k,k)
[ 9]     For j in k+1,...,N, replace A(i,j) with A(i,j)-A(i,k)*A(k,j)
[10]     Replace  b(i)  with b(i) - A(i,k)*b(k)
[11] For k in N,...,1, do [12] and [13]:
[12]   For j in k+1,...,N, replace b(k) with b(k)-A(k,j)*b(j)
[13]   Replace b(k) with b(k)/A(k,k)
 */
void
gepp( real A[N][N], real b[N] )
{
  int i,j,k,m, mmax;
  real amax;

  /* FORWARD ELIMINATION */
  for(k=0; k<N-1; k++){
    /* Find the largest value |A(m,k)| in column k, for m in k,...,N */
    amax = fabs(A[k][k]);
    mmax = k;
    for(m=k+1; m<N; m++)
      if(amax<fabs(A[m][k])){
	amax=fabs(A[m][k]);
	mmax = m;
      }
    if(amax==0){
      fprintf(stderr,"Matrix A is singular (at k=%d).\n", k);
      exit(EXIT_FAILURE);
    }
    /* Pivot, or interchange, if necessary */
    if(mmax!=k){
      for(j=k; j<N; j++)
	SWAP(real, A[mmax][j], A[k][j] );
      SWAP(real, b[mmax], b[k] );
    }
    for(i=k+1; i<N; i++){
      A[i][k] /= A[k][k];
      for( j=k+1; j<N; j++ )
	A[i][j] -= A[i][k]*A[k][j];
      b[i] -= A[i][k]*b[k];
    }
  }
  /* BACK SUBSTITUTION */
  for(k=N-1; k>=0; k--){
    for(j=k+1; j<N; j++)
      b[k] -= A[k][j]*b[j];
    b[k] /= A[k][k];
  }
  return;
}

/* Matrix Inversion with Partial Pivoting (MIPP):
[ 0'] Initialize B(i,j)=0 except that B(i,i)=1 for i in 1,...,N 
[ 1 ] For k in 1,...,N-1, do [2] to [10']:
[ 2 ]   Find the largest value |A(m,k)| in column k, for m in k,...,N
[ 3 ]   If A(m,k)==0, then print "A is singular" and stop.
[ 4 ]   If m==k, then go to [7]; else, do [5] and [6]:
[ 5 ]     For j in k,...,N, interchange A(m,j) and A(k,j)
[ 6']     For j in k,...,N, interchange B(m,j) and B(k,j)
[ 7 ]   For i in k+1,...,N, do [8] to [10']:
[ 8 ]     Replace A(i,k) with A(i,k)/A(k,k)
[ 9 ]     For j in k+1,...,N, replace A(i,j) with A(i,j)-A(i,k)*A(k,j)
[10']     For j in 1,...,N, replace B(i,j) with B(i,j)-A(i,k)*B(k,j)
[11 ] For k in N,...,1, do [12'] to [14']:
[12']   For n in 1,...,N, do [13'] and [14']
[13']     For j in k+1,...,N, replace B(k,n) with B(k,n)-A(k,j)*B(j,n)
[14']     Replace B(k,n) with B(k,n)/A(k,k)
 */
void
mipp( real A[N][N], real B[N][N] )
{
  int i,j,k,m,n, mmax;
  real amax;

  /* INITIALIZE B */
  for(i=0; i<N; i++)
    for(j=0; j<N; j++)
      B[i][j] = (i==j?1:0);

  /* FORWARD ELIMINATION */
  for(k=0; k<N-1; k++){
    /* Find the largest value |A(m,k)| in column k, for m in k,...,N */
    amax = fabs(A[k][k]);
    mmax = k;
    for(m=k+1; m<N; m++)
      if(amax<fabs(A[m][k])){
	amax=fabs(A[m][k]);
	mmax = m;
      }
    if(amax==0){
      fprintf(stderr,"Matrix A is singular (at k=%d).\n", k);
      exit(EXIT_FAILURE);
    }
    /* Pivot, or interchange, if necessary */
    if(mmax!=k){
      for(j=k; j<N; j++)
	SWAP(real, A[mmax][j], A[k][j] );
      for(j=0; j<N; j++)
	SWAP(real, B[mmax][j], B[k][j] );
    }
    for(i=k+1; i<N; i++){
      A[i][k] /= A[k][k];
      for( j=k+1; j<N; j++ ) A[i][j] -= A[i][k]*A[k][j];
      for( j=0; j<N; j++ ) B[i][j] -= A[i][k]*B[k][j];
    }
  }
  /* BACK SUBSTITUTION */
  for( k=N-1; k>=0; k-- )
    for( n=0; n<N; n++ ){
      for( j=k+1; j<N; j++ )
	B[k][n] -= A[k][j]*B[j][n];
      B[k][n] /= A[k][k];
    }
  return;
}

/* LU Decomposition with Partial Pivoting (LUPP)
[0"] For k in 1,...,N, let pi(k)=k
[1 ] For k in 1,...,N-1, do [2] to [9]:
[2 ]   Find the largest value |A(m,k)| in column k, for m in k,...,N
[3 ]   If A(m,k)==0, then print "A is singular" and stop.
[4 ]   If m==k, then go to [7]; else, do [5"] and [6"]:
[5"]     For j in 1,...,N, interchange A(m,j) and A(k,j)
[6"]     Interchange pi(m) and pi(k)
[7 ]   For i in k+1,...,N, do [8] and [9]:
[8 ]     Replace A(i,k) with A(i,k)/A(k,k)
[9 ]     For j in k+1,...,N, replace A(i,j) with A(i,j)-A(i,k)*A(k,j)
*/
void
lupp( real A[N][N], int pi[N] )
{
  int i,j,k,m,mmax;
  real amax;

  /* PERMUTATION INITIALIZATION: */
  for(k=0; k<N; k++)
    pi[k]=k;

  /* FORWARD ELIMINATION */
  for(k=0; k<N-1; k++){
    /* Find the largest value |A(m,k)| in column k, for m in k,...,N */
    amax = fabs(A[k][k]);
    mmax = k;
    for(m=k+1; m<N; m++)
      if(amax<fabs(A[m][k])){
	amax=fabs(A[m][k]);
	mmax = m;
      }
    if(amax==0){
      fprintf(stderr,"Matrix A is singular (at k=%d).\n", k);
      exit(EXIT_FAILURE);
    }
    /* Pivot, or interchange, if necessary */
    if(mmax!=k){
      for(j=0; j<N; j++)
	SWAP(real, A[mmax][j], A[k][j] );
      SWAP(int, pi[mmax], pi[k] );
    }
    for(i=k+1; i<N; i++){
      A[i][k] /= A[k][k];
      for( j=k+1; j<N; j++ )
	A[i][j] -= A[i][k]*A[k][j];
    }
  }
  return;
}

/* Linear System Solution from LU and Pivoting Data (SSLU)
[10] Permute b(1),...,b(N) to the order b(pi(1)),...,b(pi(N)) 
[11] For k in 1,...,N-1, do [12]:
[12]   For i in k+1,...,N, replace b(i) with b(i)-A(i,k)*b(k)
[13] For k in N,...,1, do [14] and [15]:
[14]   For j in k+1,...,N, replace b(k) with b(k)-A(k,j)*b(j)
[15]   Replace b(k) with b(k)/A(k,k)
*/
void
sslu( const real A[N][N], const int pi[N], real b[N] )
{
  int i,j,k;
  real bp[N];

  /* PRE-PIVOTING */
  for(k=0; k<N; k++)
    bp[k] = b[pi[k]];
  for(k=0; k<N; k++)
    b[k] = bp[k];
  
  /* FORWARD REDUCTION */
  for(k=0; k<N-1; k++)
    for(i=k+1; i<N; i++)
      b[i] -= A[i][k]*b[k];

  /* BACK SUBSTITUTION */
  for(k=N-1; k>=0; k--){
    for(j=k+1; j<N; j++)
      b[k] -= A[k][j]*b[j];
    b[k] /= A[k][k];
  }
  return;
}



int
main(void)
{
  real M[N][N]={{0}}, M1[N][N]={{0}}, M2[N][N]={{0}}, M3[N][N]={{0}};
  real iM[N][N]={{0}}, MiM[N][N]={{0}};
  real x[N]={0}, x1[N]={0}, x2[N]={0}, y[N]={0};
  int p[N];
  int m,n,k;

  /* Fill M and x=e1; put copies of M in M1,M2,M3; of x in x1,x2: */
  for(m=0; m<N; m++)
    for(n=0; n<N; n++)
      M[m][n] = M1[m][n] = M2[m][n] = M3[m][n] = 1/(0.5 + m + n);
  x[0] = x1[0] = x2[1] = 1.0;
  for(m=0; m<N; m++) print_vector("Row of M", M[m], N);

  gepp(M1,x1);
  print_vector("Solution to M*x=e1", x1, N);

  /*  M*x = y should be e1: */
  for(m=0; m<N; m++) {
    y[m] = 0;
    for(n=0; n<N; n++)
      y[m] += M[m][n]*x1[n];
  }
  print_vector("M*x=y should be e1", y, N);

  mipp(M2,iM);
  for(m=0; m<N; m++) print_vector("Row of iM", iM[m], N);
  for(m=0; m<N; m++)
    for(n=0; n<N; n++)
      for(k=0; k<N; k++)
	MiM[m][n] += M[m][k]*iM[k][n];
  for(m=0; m<N; m++) print_vector("Row of MiM", MiM[m], N);

  lupp(M3,p);
  sslu(M3,p,x2);
  print_ints("Pivots", p,N);
  print_vector("LU solve M*x=e2", x2, N);
  /*  M*x2 = y should be e2: */
  for(m=0; m<N; m++) {
    y[m] = 0;
    for(n=0; n<N; n++)
      y[m] += M[m][n]*x2[n];
  }
  print_vector("M*x restores e2", y, N);
  
  exit(EXIT_SUCCESS);
}