// $Id: CovMatrixInvert.cc,v 1.7 2002/09/19 16:45:18 sachs Exp $
//
// Invert a positive definite symmetric  matrix 
// David Sachs, Fermilab - April 2002

#include "CovMatrices/CovMatrix.h"

// Invert a positive definite symmetric matrix -  using LDL` decomposition
// Returns true if the matrix could be inverted.
// Returns false if the matrix is not positive definite
//
// Base on algorithm in Golub & Van Loan "Matrix Computations" 3rd Ed. pp 138-139
// The exact formulas used were determined by hand.
// 

bool CovMatrices::CovMatrix::invert(void)
{

  // First test for special cases wiht low values of n
  if (n < 3)
  {
    if (n==0) return false;	// Operation undefined for empty Matrix

    if (n==1)  			// 1x1 Matrix is trivial
    {
      if (M[0] <= 0.) return false;
      M[0] = 1./M[0];
      return true;
    }

    if (n==2)			// Use simple direct formula for 2x2 matrix
    {
      double det = M[0]*M[2] - M[1]*M[1];
      double t = M[0];
      if ((t <= 0.) || (det <= 0.)) return false; // Not positive definite
      M[0] = M[2]/det;
      M[1] = -M[1]/det;
      M[2] = t/det;
      return true;
    }
  }
  
  // Invert by 
  //
  // a) decomposing M = G*D*G' with G unit lower triangular and D positive diagonal
  //	(if M is not positive definite this will fail, leaving this unchanged)
  // b) inverting G to form H
  // c) multiplying H' * D^-1 * H to get M^-1.
  //
  // If the matrix is pos. def. it is inverted and 1 is returned.
  // If the matrix is not pos. def. it remains unaltered and 0 is returned.
  
  // NOTE: The LDL' algoritm should always work for posive definite symmetric matrices.
  // It will also work for many non-positive-definite symmetric matrice,
  // but may be unstable, and require pivoting.
  //
  // 

  std::vector<double> vec(n*(n+1));		// Storage for temporaries
  double* la = &vec[0];			// Size n-1 Triangular Matrix
  double* lb = &vec[(n*(n-1))/2];	// Size n-1 Triangular Matrix
  double* e  = &vec[n*(n-1)];		// Size n vector
  double* d  = &vec[n*n];		// Size n vector
  double t;
  double u;
  int i;
  int j;
  int k;
  int kk;
  int m;

  // PHASE 1; Copy off diagonal elemetns to la and diagonal elemeRnttns to d
  double* p1 = &M[0];
  double* p2 = la;
  double* p3 = d;

  *p3++ = *p1++;	// Copy element (0,0)

  for (j = 1; j < n; j++)
  {
    for (i = 0; i < j; i++)
    {
      *p2++ = *p1++;	// Copy element (i,j)
    }
    *p3++ = *p1++;	// Copy element (j,j)
  }

  // PHASE 2: Form factors in lb and d, la will contain unscaled elements

  k = 0;	// Index for G(j,0)
  if (d[0] <= 0.) return false;	// NOT positive definite
  t = 1./d[0];
  e[0] = t;

  for (j = 1; j < n; j++)
  {
    lb[k] = la[k]*t;	// g(1:n-1,0) = G(1:n-1,0)/d[0]
    k += j;
  }

  k = 0;	// Index for G(j,0) or g(j,0)
  for (j = 1; j < n; j++)
  {
    u = la[k]*lb[k];
    for ( i = 1; i < j; i++)
    {
      u += la[k+i]*lb[k+i];	// d[j] -= sum G(j,i)*g(j,i)
    }
    d[j] -= u;	// d[j] -= sum G(j,i)*g(j,i)
    if (d[j] <= 0.) return false;	// NOT positive definite
    t = 1./d[j];
    e[j] = t;
    kk = k+j;	// Index for G(i,0)
    for ( i = j+1; i < n; i++)
    {
      u = 0.;
      for ( m = 0; m < j; m++)
      {
	u += la[kk+m]*lb[k+m];
      }
      la[kk+j] -= u;		// G(i,j) -= G[j,0:j-1]*g[i,0:j-1]
      lb[kk+j] = la[kk+j]*t;	// g(i,j) = G(i,j)/d(j)
      kk += i;
    }
    k += j;
  }

  // PHASE 3: Invert g to form h in la
  // h(i,j) = -g(i,j) - sum(h(i,m)*g(m,j))  ( i > m > j )

  la[0] = -lb[0];	// No loops needed for index(1,0)
  k = 1;	// Index for g(i,0) and h(i,0)
  for (i = 2; i < n; i++)
  {
    la[k+i-1] = -lb[k+i-1];	// No inner loop needed for index (i,i-1)
    for ( j = i-2; j >= 0; j--)
    {
      kk = (j*(j+3))/2;	// Index for g(m,j)
      u = lb[k+j];
      for ( m = j+1; m < i; m++)
      {
	u += la[k+m]*lb[kk];
	kk +=m;
      }
      la[k+j] = -u;
    }
    k += i;
  }

  // PHASE 4: Form inverse M^-1 = h' * d * h
  // M(i,j) = sum h(k,i)*h(k,j)*d(k)	(( h(n,n)=1) )) (( k >= i >= j )) 

  // form lb(i,j) = la(i,j)/d(i)
  k = 0;
  for (i = 1; i < n; i++)
  {
    t = e[i];
    for ( j = 0; j < i; j++)
    {
      lb[k+j] = la[k+j]*t;
    }
    k += i;
  }

  // Form Inverse
  p1 = &M[0];
  kk = 0;	// Index to h(j,0)
  for (j = 0; j < n; j++)
  {
    for (i = 0; i < j; i++)
    {
      u = lb[kk+i];
      m = kk + j;	// Index to h(k,0)
      for (k = j+1; k < n; k++)
      {
	u += la[m+i]*lb[m+j];
	m += k;
      }
      *p1++ = u;	// I(i,j) =  h(j,i)*e(j) + sum(h(k,i)*h(k,j)*d(k))
    }
    u = e[j];
    m = kk + j;		// Index to h(k,0)
    for (k = j+1; k < n; k++)
    {
      u += la[m+j]*lb[m+j];
      m += k;
    }
    *p1++ = u;;		// I(j,j) = e(j) + sum(e(k)*h(k,j)^2)
    kk += j;		
  }

  return true;
}