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

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

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

int ldlt_n_dimensions = 0;
	/* Actual number of dimensions in each point. */
int ldlt_n_b_vectors = 0;
	/* Actual number of B vectors to be solved simultaneously. */

double **ldlt_ata;
	/* The left hand side of the normal equations A^T*A*x=A^T*b. */
double **ldlt_atb;
	/* The right hand side of the normal equations A^T*A*x=A^T*b. */

double **ldlt_l;
	/* Part of the LDL^T decomposition of ATA. */
double *ldlt_d;
	/* Part of the LDL^T decomposition of ATA. */
double *ldlt_r;
	/* A temporary vector used in the LDL^T decomposition of ATA. */

double **ldlt_z;
	/* An intermediate solution vector Lz = b. */
double **ldlt_y;
	/* An intermediate solution vector Dy = z. */
double **ldlt_x;
	/* The solution vector L^T*x = y. */

double *malloc_double_array();
double **malloc_double_matrix();

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


/*

#define N_POINTS 69
#define N_INPUTS 7
#define N_OUTPUTS 2

double input_array[ N_INPUTS ];
double output_array[ N_OUTPUTS ];

main()
{
  int i, j, k;

  init_ldlt( N_INPUTS, N_OUTPUTS );
  zero_ata_atb();
  for ( i = 0; i < N_POINTS; i++ )
    {
      for ( j = 0; j < ldlt_n_dimensions; j++ )
	{
	  scanf( "%lf", &(input_array[j]) );
	  printf( "%lf ", input_array[j] );
	}
      printf( "\n" );

      for ( k = 0; k < ldlt_n_b_vectors; k++ )
	{
	  scanf( "%lf", &(output_array[k]) );
	  printf( "%lf ", output_array[k] );
	}
      printf( "\n" );

      add_point( input_array, output_array );
    }

  for( i = 0; i < ldlt_n_dimensions; i++ )
    {
      for( j = 0; j < ldlt_n_dimensions; j++ )
	{
	  printf( "%lf ", ldlt_ata[i][j] );
	}
      printf( "\n" );
    }

  for( i = 0; i < ldlt_n_dimensions; i++ )
    {
      for( j = 0; j < ldlt_n_b_vectors; j++ )
	{
	  printf( "%lf ", ldlt_atb[i][j] );
	}
      printf( "\n" );
    }

  ldlt_decompose();

  for( i = 0; i < ldlt_n_dimensions; i++ )
    {
      for( j = 0; j < ldlt_n_dimensions; j++ )
	{
	  printf( "%lf ", ldlt_l[i][j] );
	}
      printf( "\n" );
    }

  for( i = 0; i < ldlt_n_dimensions; i++ )
    {
      printf( "%lf ", ldlt_d[i] );
    }
  printf( "\n" );

  for( i = 0; i < ldlt_n_dimensions; i++ )
    {
      printf( "%lf ", ldlt_r[i] );
    }
  printf( "\n" );

  ldlt_solve_1();
  ldlt_solve_2();

  for( i = 0; i < ldlt_n_dimensions; i++ )
    {
      for( j = 0; j < ldlt_n_b_vectors; j++ )
	{
	  printf( "%lf ", ldlt_z[i][j] );
	}
      printf( "\n" );
    }

  for( i = 0; i < ldlt_n_dimensions; i++ )
    {
      for( j = 0; j < ldlt_n_b_vectors; j++ )
	{
	  printf( "%lf ", ldlt_y[i][j] );
	}
      printf( "\n" );
    }

  for( i = 0; i < ldlt_n_dimensions; i++ )
    {
      for( j = 0; j < ldlt_n_b_vectors; j++ )
	{
	  printf( "%lf ", ldlt_x[i][j] );
	}
      printf( "\n" );
    }
}

*/

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

init_ldlt( n_dim, n_b )
     int n_dim, n_b;
{
  ldlt_n_dimensions = n_dim;
  ldlt_n_b_vectors = n_b;

  ldlt_ata = malloc_double_matrix( n_dim, n_dim );
  ldlt_atb = malloc_double_matrix( n_dim, n_b );
  ldlt_l = malloc_double_matrix( n_dim, n_dim );
  ldlt_d = malloc_double_array( n_dim );
  ldlt_r = malloc_double_array( n_dim );
  ldlt_x = malloc_double_matrix( n_dim, n_b );
  ldlt_y = malloc_double_matrix( n_dim, n_b );
  ldlt_z = malloc_double_matrix( n_dim, n_b );
}

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

zero_ata_atb()
{
  int i, j;

  for ( i = 0; i < ldlt_n_dimensions; ++i )
    {
      for ( j = 0; j < ldlt_n_dimensions; ++j )
	{
	  ldlt_ata[i][j] = 0.0;
	}
      for ( j = 0; j < ldlt_n_b_vectors; ++j )
	{
	  ldlt_atb[i][j] = 0.0;
	}
    }
}

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

add_point( input_array, output_array )
     double *input_array, *output_array;
{
  int i, j;

  for( i = 0; i < ldlt_n_dimensions; ++i )
    {
      for( j = i; j < ldlt_n_dimensions; ++j )
	{
	  ldlt_ata[i][j] += input_array[i] * input_array[j];
	}
      for( j = 0; j < ldlt_n_b_vectors; ++j )
	{
	  ldlt_atb[i][j] += input_array[i] * output_array[j];
	}
    }
}

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

double *malloc_double_array( size )
     int size;
{
  double *p;

  p = (double *) malloc( size * sizeof(double) );
  if ( p == NULL )
    {
      fprintf( stderr, "Memory allocation failed for double array of size %d",
	      size );
      exit( -1 );
    }
  return p;
}

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

double **malloc_double_matrix( dimension_1, dimension_2 )
     int dimension_1, dimension_2;
{
  double **p;
  int i;

  p = (double **) malloc( dimension_1 * sizeof(double *) );
  if ( p == NULL )
    {
      fprintf( stderr, "Memory allocation failed for double matrix of size %d",
	      dimension_1 );
      exit( -1 );
    }
  for ( i = 0; i < dimension_1; ++i )
    p[i] = malloc_double_array( dimension_2 );

  return p;
}

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