// $Id: CovMatrix5Invert.cc,v 1.9 2002/09/19 16:45:18 sachs Exp $
//
// Invert a positive definite symmetric 5x5 matrix 
// David Sachs, Fermilab - March 2002

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

// Define CHOLESKY to use Cholesky LU decomposition.
// The L*D*l^T method appears to be faster for small matrices

#undef CHOLESKY
#ifndef CHOLESKY
// Invert a positive definite symmetric 5x5 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::CovMatrix5::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 h40, h41, h42, h43;
	
  double d00, d11, d22, d33, d44;	// 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 g40, g41, g42, g43;

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


// 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;
  G40 = M[M40];
  g40 = M[M40] * 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;
  G41 = (M[M41] - (G10 * g40));
  g41 = G41 * 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;
  G42 = (M[M42] - (G20 * g40) - (G21 * g41));
  g42 = G42 * d22;

// Form  d33

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

// Subtract inter-column column dot product from A43 and scale to get G43

  G43 = (M[M43] - (G30 * g40) - (G31 * g41) - (G32 * g42));
  g43 = G43 * d33;

// Finally form d44 - if this is possible inversion succeeds

  d44 = M[M44] - (G40 * g40) - (G41 * g41) - (G42 * g42) - (G43 * g43);
  if (d44 <= 0.) return false;
  d44 = 1.0 / d44;

// 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 h40 G40
#define h21 (-g21)
#define h31 G31
#define h41 G41
#define h32 (-g32)
#define h42 G42
#define h43 (-g43) 


  // h10 = -g10;
  // h21 = -g21;
     h20 =  g10 * g21 - g20;
  // h32 = -g32;
     h31 =  g21 * g32 - g31;
     h30 =  g32 * g20 - h31 * g10 - g30;
  // h43 = -g43;
     h42 =  g32 * g43 - g42;
     h41 =  g31 * g43 - g21 * h42 - g41;
     h40 =  g30 * g43 - g10 * h41 - g20 * h42 - g40;

// 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) + h40 * (h40*d44);
  M[M01] = (h10*d11) + h20 * (h21*d22) + h30 * (h31*d33) + h40 * (h41*d44);
  M[M11] = d11 + h21 * (h21*d22) + h31 * (h31*d33) + h41 * (h41*d44);
  M[M02] = (h20*d22) + h30 * (h32*d33) + h40 * (h42*d44);
  M[M12] = (h21*d22) + h31 * (h32*d33) + h41 * (h42*d44);
  M[M22] = d22 + h32 * (h32*d33) + h42 * (h42*d44);
  M[M03] = (h30*d33) + h40 * (h43*d44);
  M[M13] = (h31*d33) + h41 * (h43*d44);
  M[M23] = (h32*d33) + h42 * (h43*d44);
  M[M33] = d33 + h43 * (h43*d44);
  M[M04] = (h40*d44);
  M[M14] = (h41*d44);
  M[M24] = (h42*d44);
  M[M34] = (h43*d44);
  M[M44] = d44;

  return true;

}

#else /* Not CHOLESKY */

// Invert a positive definite symmetric 5x5 matrix using Cholesky decomposition
// This is a modified version of software written by Mark Fischler and Steven Haywood
// 

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

// Invert by 
//
// a) decomposing M = G*G^T with G lower triangular
//	(if M is not positive definite this will fail, leaving this unchanged)
// b) inverting G to form H
// c) multiplying H^T * 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.

  double h10;				// below-diagonal elements of H
  double h20, h21;
  double h30, h31, h32;
  double h40, h41, h42, h43;
	
  double h00, h11, h22, h33, h44;	// 1/diagonal elements of G = 
					// diagonal elements of H

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


// Form G -- compute diagonal members of H directly rather than of G
//-------

// Scale first column by 1/sqrt(A00)

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

  g10 = M[M10] * h00;
  g20 = M[M20] * h00;
  g30 = M[M30] * h00;
  g40 = M[M40] * h00;

// Form G11 (actually, just h11)

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

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

  g21 = (M[M21] - (g10 * g20)) * h11;
  g31 = (M[M31] - (g10 * g30)) * h11;
  g41 = (M[M41] - (g10 * g40)) * h11;

// Form G22 (actually, just h22)

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

// 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)) * h22;
  g42 = (M[M42] - (g20 * g40) - (g21 * g41)) * h22;

// Form G33 (actually, just h33)

  h33 = M[M33] - (g30 * g30) - (g31 * g31) - (g32 * g32);
  if (h33 <= 0.) return false;
  h33 = 1.0 / sqrt(h33);

// Subtract inter-column column dot product from A43 and scale to get G43

  g43 = (M[M43] - (g30 * g40) - (g31 * g41) - (g32 * g42)) * h33;

// Finally form h44 - if this is possible inversion succeeds

  h44 = M[M44] - (g40 * g40) - (g41 * g41) - (g42 * g42) - (g43 * g43);
  if (h44 <= 0.) return false;
  h44 = 1.0 / sqrt(h44);

// Form H = 1/G -- diagonal members of H are already correct
//-------------

// The order here is dictated by speed considerations

  h43 = -h33 *  g43 * h44;
  h32 = -h22 *  g32 * h33;
  h42 = -h22 * (g32 * h43 + g42 * h44);
  h21 = -h11 *  g21 * h22;
  h31 = -h11 * (g21 * h32 + g31 * h33);
  h41 = -h11 * (g21 * h42 + g31 * h43 + g41 * h44);
  h10 = -h00 *  g10 * h11;
  h20 = -h00 * (g10 * h21 + g20 * h22);
  h30 = -h00 * (g10 * h31 + g20 * h32 + g30 * h33);
  h40 = -h00 * (g10 * h41 + g20 * h42 + g30 * h43 + g40 * h44);

// Change this to its inverse = H^T*H
//------------------------------------

  M[M00] = h00 * h00 + h10 * h10 + h20 * h20 + h30 * h30 + h40 * h40;
  M[M01] = h10 * h11 + h20 * h21 + h30 * h31 + h40 * h41;
  M[M11] = h11 * h11 + h21 * h21 + h31 * h31 + h41 * h41;
  M[M02] = h20 * h22 + h30 * h32 + h40 * h42;
  M[M12] = h21 * h22 + h31 * h32 + h41 * h42;
  M[M22] = h22 * h22 + h32 * h32 + h42 * h42;
  M[M03] = h30 * h33 + h40 * h43;
  M[M13] = h31 * h33 + h41 * h43;
  M[M23] = h32 * h33 + h42 * h43;
  M[M33] = h33 * h33 + h43 * h43;
  M[M04] = h40 * h44;
  M[M14] = h41 * h44;
  M[M24] = h42 * h44;
  M[M34] = h43 * h44;
  M[M44] = h44 * h44;

  return true;

}
#endif  /* CHOLESKY */