/*
Name          dsweep - Inverts a positive definite matrix in place by a 
                       diagonal sweep.

Usage         #include "usual.h"
              #include "tools.h"
              INTEGER dsweep(REAL *a, INTEGER n, REAL eps)

Prototype in  tools.h

Description   a is a symmetric, positive definite, n by n matrix stored 
              columnwise with no unused space and first element in a[0]; i.e.,
              for (j=1; j<=n; j++) for (i=1; i<=n; i++) aij=a[n*(j-1)+(i-1)];
              will traverse the matrix with aij being the element in the 
              i-th row and j-th column.  On return a contains the inverse of a
              stored columnwise.  eps is an input constant used as a relative 
              tolerance in testing for degenerate rank.  A reasonable value 
              for eps is 1.e-13. 

Remark        dsweep.cc is dsweep.f translated to C++.

Reference     Schatzoff, M. et al.  Efficient Calculation of all Possible 
              Regressions. Technometrics, 10. 769-79 (November 1968)

Return value  The return value ier is an error parameter coded as follows:
              ier=0, no error, rank of a is n;  ier > 0, a is singular, rank 
              of a is n-ier.  If ier > 0 then dsweep returns a g-inverse. 

Functions     Library: (none)
called        Sublib:  (none)
*/

#include "usual.h"
#include "tools.h"


INTEGER dsweep(REAL *a, INTEGER n, REAL eps)
{
  INTEGER i, j, k, kk, ier;
  REAL    test, d, aik, akj, akk, tol;

  a--;   // Adjust offset to allow Fortran style indexing.

  tol=0.0;

  for (i=1; i<=n; i++) {
    test=a[n*(i-1)+i];
    if(test > tol) tol=test;
  }

  tol=tol*eps;
  ier=0;

  for (k=1; k<=n; k++) {

    kk=n*(k-1)+k;

    akk=a[kk];

    if (akk < tol) {

      ier=ier+1;

      for (j=k; j<=n; j++)     // zero out row k of upper triangle
        a[n*(j-1)+k]=0.0;

      for (i=1; i<=k-1; i++)  // zero out column k of upper triangle
        a[kk-i]=0.0;

    } 
    else {  // sweep

      d=1.0/akk;

      for (i=1; i <= n; i++) {
      for (j=i; j <= n; j++) {
        if (i != k && j != k)  {

          if (i<k) 
            aik=a[n*(k-1)+i];
          else 
            aik=a[n*(i-1)+k];

          if (k<j) 
            akj=a[n*(j-1)+k];
          else 
            akj=-a[n*(k-1)+j];

          a[n*(j-1)+i] -= aik*akj*d;
        }
      }
      }

      for (j=k; j<=n; j++)
        a[n*(j-1)+k] *= d;

      for (i=1; i<=k-1; i++)
        a[kk-i] = -a[kk-i]*d;

      a[kk]=d;

    }
  }

  // copy the upper triangle to the lower

  for (i=1; i <= n; i++)
  for (j=i; j <= n; j++)
    a[n*(i-1)+j]=a[n*(j-1)+i];

  return ier;
}