/*****************************************************************************
LDL^t decomposition of a symmetric matrix.
This version requires the matrix to be positive definite.
*****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/*****************************************************************************/
/*****************************************************************************/

double ldlt_tolerance = 1e-10;

/*****************************************************************************/

/* Compute the L matrix and D vector */

/* Return FALSE if solution failure, TRUE otherwise */

int ldlt_decompose( double **input, // the input matrix
		    double **l, // the lower triangular matrix of the decomposition
		    double *d, // the vector of diagonal elements
		    double *tmp, // a temporary vector
		    int n ) // the dimensionality of everything.
{
  int i, j, k, p;
  double dk, lik;

  /* copy input matrix to l matrix */
  for ( k = 0; k < n; ++k )
    {
      l[k][k] = input[k][k];
      for ( j = k+1; j < n; ++j )
	{
	  l[j][k] = l[k][j] = input[k][j];
	}
    }

  for ( k = 0; k < n; ++k )
    {
      for ( p = 0; p < k; ++p )
	{
	  tmp[p] = d[p] * l[k][p];
	}
      dk = l[k][k];
      for ( p = 0; p < k; ++p )
	{
	  dk -= l[k][p] * tmp[p];
	}
      d[k] = dk;
      if ( dk <= ldlt_tolerance )
	{
	  /*
	  fprintf( stderr, "ERROR: d(%d) = %f\n", k, dk );
	  exit( -1 );
	  */
	  fprintf( stderr, "\nldlt_decompose() ERROR: d(%d) = %g\n", k, dk );
	  fprintf( stderr, "input matrix ill conditioned\n" );
	  return 0;
	}
      for ( i = k + 1; i < n; ++i )
	{
	  lik = l[i][k];
	  for ( p = 0; p < k; ++p )
	    {
	      lik -= l[i][p] * tmp[p];
	    }
	  l[i][k] = lik/dk;
	}
    }

  /* clean up l matrix */
  for ( k = 0; k < n; k++ )
    {
      l[k][k] = 1.0;
      for ( j = 0; j < k; j++ )
	{
	  l[j][k] = 0.0;
	}
    }
  return 1;
}

/*****************************************************************************/
/*****************************************************************************/

/* Compute Lz = vector, Dy = z, and L^T*x = y. */

int ldlt_solve_vector( double **l, // the lower triangular matrix of the decomposition
		       double *d, // the vector of diagonal elements
		       double *input, // the input vector
		       double *output, // the output vector
		       double *tmp, // a temporary vector
		       int n ) // the dimensionality of everything.
{
  double zi, xi;
  int i, j;

  for ( i = 0; i < n; i++ )
    {
      zi = input[i];
      for ( j = 0; j < i; j++ )
	{
	  zi -= output[j] * l[i][j]; // output is used as temporary storage
	}
      output[i] = zi;  // output is used as temporary storage
    }

  for ( i = 0; i < n; i++ )
    {
      tmp[i] = output[i] / d[i];  // output is used as temporary storage
    }

  for ( i = n - 1; i >= 0; i-- )
    {
      xi = tmp[i];
      for ( j = i + 1; j < n; ++j )
	{
	  xi -= output[j] * l[j][i];
	}
      output[i] = xi;
    }
  return 1;
}

/*****************************************************************************/
/*****************************************************************************/
/* This routine exists because of the strange way C represents 2D matrixes ... */

/* Compute Lz = matrix, Dy = z, and L^T*x = y. */

int ldlt_solve_matrix( double **l, // the lower triangular matrix of the decomposition
		       double *d, // the vector of diagonal elements
		       double **input, // the input matrix
		       double **output, // the output matrix
		       double *tmp, // a temporary vector
		       int n, // the dimensionality of everything except ....
		       int n_columns ) // the number of columns in the input matrix.
{
  double zi, xi;
  int i, j, column;

  for ( column = 0; column < n_columns; ++column )
    {
      for ( i = 0; i < n; i++ )
	{
	  zi = input[i][column];
	  for ( j = 0; j < i; j++ )
	    {
	      zi -= output[j][column] * l[i][j];  // output is used as temporary storage
	    }
	  output[i][column] = zi;  // output is used as temporary storage
	}

      for ( i = 0; i < n; i++ )
	{
	  tmp[i] = output[i][column] / d[i];  // output is used as temporary storage
	}

      for ( i = n - 1; i >= 0; i-- )
	{
	  xi = tmp[i];
	  for ( j = i + 1; j < n; ++j )
	    {
	      xi -= output[j][column] * l[j][i];
	    }
	  output[i][column] = xi;
	}
    }
  return 1;
}

/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
