/* qinv.c - Code file for some quick matrix inverse routines.
 * (C) Copyright 1999 by John Halleck
 * All Rights Reserved
 */
/* Version of September 8th, 1999 */

#include "errors.h"
/* We use the package wide standard error codes. */

#include "vec.h"
/* We make use of the vector routines. */
#include "mat.h"
/* We make use of the matrix routines. */

/* #include "matdebug.h" */
/* DEBUG */

/* =============== Inverse without an attempt to pivot ====================== */
/* Non-pivoting inverse.  Clearly not a general inverse */

static error invnp (int size, matrix result, matrix working) {
   /* Result should either be the identity matrix, or should be
    * the result of pivoting the identity matrix to match 
    * whatever pivoting was done to the working array.
    */

   error status; /* Place to capture status of routines we call */

   int i, j; /* Indices for loops.  (Yes, I used to program in ForTran) */
   double *diag, dvalue; /* A diagonal element */
   double *leading, lvalue; /* leading non-zero value on a row */
   double *workrow, *resultrow; /* The row we are working on. */
   double *worksub, *resultsub; /* The other rows we are clearing out. */

   /* A stable way to invert a positive definite matrix
    * is to use elementary matrix transformations to reduce it
    * the identity, while using the same transformations to transform
    * the identity to the inverse.
    */
   /* Oddly enough, this routine should be much cleaner in Pascal */

   if (!result || !working) return ERRnil;  /* Arrays must exist */
   if (size<1)              return ERRsize; /* with reasonable sizes */
   if (result==working)     return ERRsame; /* And can't be the same arrays. */

   workrow = (double *) working; resultrow = (double *) result;
   diag = workrow;
   for (i=0; i<size; i++) {  /* clear up each row. */

       /* Scale the row by the leading non-zero entry. */
       if (*diag == 0.0) return ERRnumeric;
          /* This shouldn't be possible for a positive definite matrix. */

       else if (*diag != 1.0) {
          dvalue = 1.0 / *diag;
          if ((status = vecscl(size, workrow,   workrow,   dvalue)))
             return status;
          if ((status = vecscl(size, resultrow, resultrow, dvalue)))
             return status;
       }

       /* Add appropriate scaled version of this row to the other rows 
        * in order to zero items.
        */
       worksub = (double *) working;  resultsub = (double *) result;
       leading = i + (double *) working;
       for (j=0; j<size; j++) {
         if (i != j) { /* Don't do it to this row itself. */
            if (*leading != 0.0) {
               lvalue = - *leading;
               if ((status = vecadds (size, worksub, worksub, workrow, lvalue)))
                  return status;
               if ((status = vecadds (size, resultsub, resultsub, resultrow,
                                                                    lvalue)))
                  return status;
            }
         }
         leading = leading + size; worksub += size; resultsub += size;
       }

       workrow += size; resultrow += size; /* drop down a row */
       diag += size + 1; /* The diagonal is one row down and one item over */
   }
   return 0;
}

/* ==================== Inverse without need of pivoting ==================== */

/* Inverse of a positive definite matrix. result and working must be
 * distinct arrays.
 * Note that positive definite matricies do not require pivoting or
 * other rearrangement to form the inverse.
 *
 *  * This is NOT a GENERAL matrix inverse *
 *
 * This does not make use of the fact that all the positive definite
 * matrices in this package are symmetrical
 */

error invpd (int size, matrix result, matrix given, matrix working) {
   /* A stable way to invert a positive definite matrix
    * is to use elementary matrix transformations to reduce it
    * the identity, while using the same transformations to transform
    * the identity to the inverse.
    * Of course, that would distroy the original, which we probably
    * don't want to do...
    * So we ask for a matrix for working space.
    * Note that setting given and working to the same array means
    * we can trash given, and it can run a bit faster.
    */
   /* Oddly enough, this routine should be much cleaner in Pascal */

   error status; /* Place to capture status of routines we call */

   if (!result || !given || !working)
      return ERRnil;  /* Arrays must exist */
   if (size<1)
      return ERRsize; /* with reasonable sizes */
   if (result==working)
      return ERRsame; /* And result and working have to be distinct arrays */

   /* Put array into working memory, if necessary */
   if (working!=given) if ((status = matcpy (size, size, working, given)))
                          return status;

   /* Get us an identity array to play with. */
   if ((status = matidn (size, result))) return status;

   /* Return the non-pivoting inverse */
   return invnp (size, result, working);
}


/* ==================== Inverse with pivoting =============================== */
/* Invert non-singular array */

error invns (int size, matrix result, matrix given, matrix working) {
   /* One can increase the classes of matrix that one can invert
    * if one does matrix pivoting.
    * This tries the matrix pivot before trying it..
    */
   /* Oddly enough, this routine should be much cleaner in Pascal */

   /* This routine only does partial pivoting. */

   double *workrow, *testvalue, testmax, currentmax;
   double *resrow,  *rowofmax, *rowofres, *resrowofmax, *rowoftest;
   int i, j;
   error status;

   if (!result || !given || !working)
      return ERRnil;  /* Arrays must exist */
   if (size<1)
      return ERRsize; /* with reasonable sizes */
   if (result==working)
      return ERRsame; /* And can't be the same arrays. */

   /* Put array into working memory, if necessary */
   if (working!=given) if ((status = matcpy (size, size, working, given)))
                          return status;

   /* Get us an identity array to play with. */
   if ((status = matidn (size, result))) return status;

   /* Pivot the arrays. */
   /* We are doing row swaps to pivot, but we are doing them to
    * both arrays.
    */
   workrow = (double *) working;  resrow = (double *) result;
   for (i=0; i<size-1; i++) {
       rowofmax = workrow; rowofres = resrow;
         /* pointers to current rows in each array */
       rowoftest = workrow;   /* Row we are testing */
       testvalue = workrow + i;
       currentmax = *testvalue;  if (currentmax < 0) currentmax = -currentmax;
         /* The value of the i'th column of the test row is our guess for max */

       /* Find the row with the maximum value */
       for (j=i+1; j<size; j++) {
           rowoftest += size;   rowofres += size;
           testvalue += size;   testmax   = *testvalue;
           testmax = *testvalue; if (testmax<0) testmax = -testmax;
           /* If this row has a bigger value in this column than the prior
            * max, it is the new max.
            */
           if (testmax > currentmax) {
               currentmax = testmax; /* Max value so far */
               rowofmax = rowoftest; resrowofmax = rowofres;
                  /* Row (in each array) where we found the max values */
           }
       }

       /* (The max value had better be greater than 0) */
       if (currentmax == 0.0) return ERRnumeric;

       /* If the max value is not the current row, swap them. */
       if (rowofmax != workrow) { /* swap */
          if ((status = vecswp (size, workrow, rowofmax)))
             return status;
          if ((status = vecswp (size, resrow,  resrowofmax)))
             return status;
       }
       workrow += size;  resrow += size;
   }

   /* Return the non-pivoting inverse */
   return invnp (size, result, working);
}