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

#include "CovMatrices/CovMatrix4.h"
#include "CovMatrixElement.h"

// Invert a positive definite symmetric 4x4 matrix -  using L*D*L^T decomposition
//
// Base on algorithm in Golub & Van Loan "Matrix Computations" 3rd Ed. pp 138-139
// The exact formulas used were determined by hand.
// 

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

// Invert by 
//
// a) decomposing M = G*D*G^T 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^T * 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^T algoritm should always work for posive definite symmetric matrices.
// It will also work for many, but not all, non-positive-definite symmetric matrice,
// but may be unstable.  In particular it will fail if M[0,0]  is zero, which
// is quite possible for a nonsingular symmetric matrix.

  // double h10;				// below-diagonal elements of H
  // double h20, h21;				// will be defined later
  // double h30, h31, h32;
	
  double d00, d11, d22, d33;		// 1/diagonal elements of D = 
					// diagonal elements of D^-1

  double g10;				// below-diagonal elements of G
  double g20, g21;
  double g30, g31, g32;

  double G10;				// UNSCALED below-diagonal elements of G
  double G20, G21;			
  double G30, G31, G32;


// Form G and D -- compute diagonal members of D^-1 directly 
//-------

// Scale first column by 1/M00

  d00 = M[M00]; 
  if (d00 <= 0.) return false;
  d00 = 1.0 / d00;

  G10 = M[M10];
  g10 = M[M10] * d00;
  G20 = M[M20];
  g20 = M[M20] * d00;
  G30 = M[M30];
  g30 = M[M30] * d00;

// Form d11

  d11 = M[M11] - (G10 * g10);
  if (d11 <= 0.) return false;
  d11 = 1.0 / d11;

// Subtract inter-column column dot products from rest of column 1 and
// scale to get column 1 of G 

  G21 = (M[M21] - (G10 * g20));
  g21 = G21 * d11;
  G31 = (M[M31] - (G10 * g30));
  g31 = G31 * d11;

// Form d22

  d22 = M[M22] - (G20 * g20) - (G21 * g21);
  if (d22 <= 0.) return false;
  d22 = 1.0 / d22;

// Subtract inter-column column dot products from rest of column 2 and
// scale to get column 2 of G 

  G32 = (M[M32] - (G20 * g30) - (G21 * g31));
  g32 = G32 * d22;

// Form  d33

  d33 = M[M33] - (G30 * g30) - (G31 * g31) - (G32 * g32);
  if (d33 <= 0.) return false;
  d33 = 1.0 / d33;

// Form H = 1/G  -- Diagonal elements of G and H are 1.0
//-------------

// Because the unscaled matrix elements  of G are no longer needed
// define the elements of H to occupy the same memory
// Because of their simple forms, h10, h21, h32 and h43 are defined by their formulas
#define h10 (-g10)
#define h20 G20
#define h30 G30
#define h21 (-g21)
#define h31 G31
#define h32 (-g32)


  // h10 = -g10;
  // h21 = -g21;
     h20 =  g10 * g21 - g20;
  // h32 = -g32;
     h31 =  g21 * g32 - g31;
     h30 =  g32 * g20 - h31 * g10 - g30;

// Change this to its inverse = H^T * D^-1 * H
//------------------------------------
// NOTE: Common subexpressions are parenthesized to provide proper ordering 
// for optimizing C++ compilers

  M[M00] = d00 + h10 * (h10*d11) + h20 * (h20*d22) + h30 * (h30*d33);
  M[M01] = (h10*d11) + h20 * (h21*d22) + h30 * (h31*d33);
  M[M11] = d11 + h21 * (h21*d22) + h31 * (h31*d33);
  M[M02] = (h20*d22) + h30 * (h32*d33);
  M[M12] = (h21*d22) + h31 * (h32*d33);
  M[M22] = d22 + h32 * (h32*d33);
  M[M03] = (h30*d33);
  M[M13] = (h31*d33);
  M[M23] = (h32*d33);
  M[M33] = d33;

  return true;

}