/* Invert the nxn matrix A by Gauss elimination and pivoting */

/* Using row swaps rather than permutation indices */

#define PROG_DATE "February 7, 1998"
#define MYNAME "<Myname>"

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

/* Do an example for a 5x5 matrix */

#define NN 5

int nn=NN;

/* The matrix AA is assumed to be stored as a   */
/*   one-dimensional array of length nn*nn      */
/*   with the rows of AA stored in sequence.     */

/* Storing AA as a long vector makes it easier */
/*   to work with matrices whose dimensions are */
/*   only known at run time */

/* A sample matrix A and vector Y: */

double aa[NN*NN] = {  4,   0,   4,   3,   3,
                      1,   2,   3,   4,   5,
                     -1,   2,  -1,   2,  -1,
                      3,   1,   4,   1,   6,
                      2,   7,   1,   8,   3 };

double ainv[NN*NN];

/* Save the original matrix here */
/* (The algorithm destroys the original copies.) */

double asave[NN*NN], acheck[NN*NN];

void showmat(double *vv, int mm, int nn);

/* Put the solution procedure in its own subroutine. */

/* The function */
/*                                                */
/*     int mat_GaussPiv_inv(aa,ainv,nn);          */
/*                                                */
/* finds the inverse of the nxn matrix A.
/* If A is invertible, the inverse matrix is put in ainv[].
/* The matrix A is overwritten  */
/*                                                */
/* The function returns */
/*       0   if the matrix A is invertible (and AX=Y is solved ) */
/*      -1   if A is not invertible (and AX=Y is NOT solved) */
/*                                              */


int mat_GaussPiv_inv(double *aa, double *ainv, int nn);


void main(void)
 { int i,j,k;  double csum;
   printf ("Find the inverse of the nxn matrix A\n"
     "  by Gauss elimination and pivoting.\n");
   printf ("Vs. %s - %s\n", PROG_DATE, MYNAME);

   /* Show the sample matrix and forcing vector */
   printf ("The %dx%d matrix A (to 5 significant figures) is:\n", nn,nn);
   showmat(aa,nn,nn);

   /* Save a copy of aa[] since the solution process */
   /*   destroys aa[] */

   for (i=0; i<nn; i++)
     for (j=0; j<nn; j++)  asave[i*nn+j] = aa[i*nn+j];

   /* We put the Gauss elimination process in a subroutine */
   /*   to make it easier to use in other programs */
   /* The function returns -1 if aa[] is singular */
   /*   and returns 0 if aa[] is nonsingular */

   if (mat_GaussPiv_inv(aa,ainv,nn))
    { printf ("THE MATRIX AA IS SINGULAR!!!\n");  exit(1); }

   /* We must have succeeded if we got to this point, */
   /*   so show the solution: */
   printf ("The inverse matrix AINV (to 5 significant figures) is:\n");
   showmat(ainv,nn,nn);

   printf ("As a check for AINV:\n");
   /* Compare A*AINV with the identity matrix I */
   /* Since we have destroyed aa[], we must use asave[] */
   /* Keep a running score of the sum of the absolute differences */
   /*   between A*AINV and I */
   csum=0.0;
   for (i=0; i<nn; i++)  for (k=0; k<nn; k++)
    { double sum=0.0;
      for (j=0; j<nn; j++)  sum += asave[i*nn+j] * ainv[j*nn+k];
      acheck[i*nn+k]=sum;
      csum += fabs(sum - ( (i==k) ? 1.0 : 0.0) ); }
   printf ("The matrix A*AINV is:\n");
   showmat(acheck,nn,nn);
   printf ("The sum of the absolute errors is: %g\n", csum); }


/* Display the vector in vv[] */

void showvec(double *vv, int nn)
 { int j;  printf ("   ");
   for (j=0; j<nn; j++)  printf("  %9.5f", vv[j]);
   printf ("\n"); }


/* Display the matrix at aa[], */
/*   which is assumed to be stored by rows */

void showmat(double *vv, int mm, int nn)
 { int i;
   for (i=0; i<mm; i++)  showvec(vv+i*nn,nn); }


/* The rest of the file is the routine   */
/*    int mat_GaussPiv_inv(double *aa, double *ainv, int nn);   */

/* This functions finds the inverse matrix  AINV  */
/*    by Gauss elimination and pivoting */
/* Assumptions: */
/*     nn  is an integer   */
/*     aa  points to an array of length nn*nn*sizeof(double), */
/*        viewed as an  nnxnn  double-precision matrix      */
/*        stored by rows */
/*     ainv  points to another array of nnxnn doubles. */
/*        The solution will be stored here. */
/*                                   */
/* Returns:     */
/*      0  if the matrix A is nonsingular and AX=Y is solved. */
/*     -1  if A is singular.  The vector xx[] is undisturbed. */
/*                                   */
/* WARNING:     */
/*      The solution process destroys the matrix A */
/*                                   */


/* The macro  A(i,j)  refers to the (i,j)-th entry of A, */
/* The macro  AINV(i,j)  does the same for the augmented columns AINV */
/*                                         */

#define A(i,j)     aa[(i)*nn+(j)]
#define AINV(i,j)  ainv[(i)*nn+(j)]

int mat_GaussPiv_inv(double *aa, double *ainv, int nn)
 { int i,j,k,j2;

   /* Initialize AINV at the identity matrix. */
   for (i=0; i<nn; i++)
    { for (j=0; j<nn; j++)  ainv[i*nn+j]=0.0;
      ainv[i*nn+i]=1.0; }

   /* Conventions about indices: */
   /*   i  sums over rows */
   /*   k  sums over rows above or below i */
   /*   j  sums over columns */
   /*   j2  sums over columns in the augmented tableau */

   /* Do an upper triangularization */
   /*   of A in the augmented tableau (A,AINV): */

   /* Sum over rows up to (but not including) the last row */
   /* This is a sum over rows, so use i */
   for (i=0; i<nn-1; i++)
    { /* Find the ith pivot element */
      int imax=i;  double z, mpiv=fabs(A(i,i));
      /* This is a sum over rows (below row i), so use k */
      for (k=i+1; k<nn; k++)
        if ( (z=fabs(A(k,i))) > mpiv)  { mpiv=z;  imax=k; }
      /* If the best diagonal element is 0.0, */
      /*   then the matrix is singular, so return  -1 */
      if (mpiv==0.0)  return -1;
      if (imax>i)
       { /* Found a better pivot, so permute the two rows */
         /* Physically permute the two rows, since we need */
         /*   the rows of AINV in the proper order at the end */
         for (j=0; j<nn; j++)
          { double term=A(i,j);  A(i,j)=A(imax,j);  A(imax,j)=term;
            term=AINV(i,j); AINV(i,j)=AINV(imax,j); AINV(imax,j)=term; }}
      /* Clear the column below the pivot */
      /* Note that A(i,...) now refers to the old row[imax] */
      mpiv=A(i,i);    /* The pivot without the absolute value */
      /* Sum over the rows below i, so use k */
      for (k=i+1; k<nn; k++)
       { /* Carry out  (Matrix)Row(k) = Row(k) - mm*Row(i) */
         /*   where  mm = A(k,i)/A(i,i) */
         double mm=A(k,i)/mpiv;
         /* We know what the value at (k,i) will be, so set it directly */
         /*   equal to zero without bothering to subtract */
         A(k,i)=0.0;
         /* The following is a sum over columns, so use j */
         for (j=i+1; j<nn; j++)
            A(k,j) -= A(i,j)*mm;
         /* Don't forget the augmented AINV columns: */
         for (j2=0; j2<nn; j2++)
            AINV(k,j2) -= AINV(i,j2)*mm; }}
   /* Check the last row: */
   /* Return -1 if the diagonal element is 0 */
   if (A(nn-1,nn-1)==0)  return -1;

   /* The matrix A in the tableau is now in upper triangular form. */
   /* Continue until the matrix A is the identity */
   /* This loop goes over nn-1, nn-2, ...., 1, 0  */

   for (i=nn; --i>=0; )
    { if (A(i,i)!=1.0)
       { double fac=A(i,i);
         for (j=0; j<nn; j++)
          { A(i,j) /= fac;  AINV(i,j) /= fac; }}
      /* This loop goes over the rows above row i: */
      /*   that is, i-1, i-2, ..., 1, 0  */
      for (k=i; --k>=0; )
       { double fac=A(k,i);
         A(k,i)=0.0;
         /* Don't forget the augmented AINV columns: */
         for (j2=0; j2<nn; j2++)
           AINV(k,j2) -= AINV(i,j2)*fac; }}

   /* Everything is OK, so return 0 for success */
   return 0; }