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

#include "linsubs.h" 

void pdinv(double *cinv, double *coeff, int n)   {
  double *tt;
  double *p ;
  double t, sum ;
  int i,j, k ;

  p = (double *)malloc(n*sizeof(double));
  tt = (double *)malloc(n*n*sizeof(double));

  for(i=0;i<n;i++)  {
    p[i] = 0.0;
    for(j=0;j<n;j++)  {
      tt[i*n+j] = 0.0;
    }
  }
   
  for(i=0;i<n*n;i++)  {
    tt[i] = coeff[i];
  }

  choldc (tt, n, p) ;
 
  for (i=0; i<n; i++) {
    tt[i*n+i] = 1.0/p[i] ;
    for (j=i+1; j<n; j++) {
      sum=0.0 ;
      for (k=i; k<j; k++) {
        sum -= tt[j*n+k]*tt[k*n+i] ;
      }
      tt[j*n+i] = sum/p[j] ;
    }
  }

  for (i=0; i<n; i++)   {
    for (j=i; j<n; j++) {
     sum=0.0 ;
     for (k=j; k<n; k++) {
       sum += tt[k*n+j]*tt[k*n+i] ;
     }
     cinv[i*n+j] = cinv[j*n+i] = sum ;
    }
  }

  free(tt) ;
  free(p) ;

}

void 
cholsl (double *a, int n, double p[], double b[], double x[])

/** 
 Numerical Recipes.  Must change 
*/

{
  /*AT: Changing the code*/

 int i, k; 
  double sum; 
     
 
  for (i = 0; i < n; i++) 
    { 
      sum = b[i]; 
      for (k = i - 1; k >= 0; k--) 
        sum -= a[i*n+k] * x[k]; 
      x[i] = sum / p[i]; 
    } 
 
  for (i = (n-1); i >= 0; i--) 
    { 
      sum = x[i]; 
      for (k = i + 1; k < n; k++) 
        sum -= a[k*n+i]* x[k]; 
      x[i] = sum / p[i]; 
    }        


}

int 
choldc (double *a, int n, double p[])
{
   int i, j,k;         
  double sum; 
     
  
  for (i = 0; i < n; i++) 
    {           
      for (j = i; j < n; j++) 
        {                 
          sum = a[i*n+j]; 
          for  (k = i - 1; k >= 0; k--) 
            sum -= a[i*n+k] * a[j*n+k]; 
          if (i == j) 
            {          
	      /**                                       
              printf("zzchol %d %20.10f %9.3f\n",i, sum, a[i][i]) ; 
	      */                                  
              if (sum <= 0.0) {                  
                return -1 ; // not pos def
              }  
              p[i] = sqrt (sum); 
            }   
          else  
            { 
              a[j*n+i] = sum / p[i]; 
               
            } 
        }                                    
    } 
  
    return 1 ;
                
}

void cholesky(double *cf, double *a, int n) 
{  
  int i, j, k ;
  double *tt ;
  double *p ;
  
  p = (double *)malloc(n*sizeof(double));
  tt = (double *)malloc(n*n*sizeof(double));

  for(i=0;i<n;i++)  {
    p[i] = 0.0;
    for(j=0;j<n;j++)  {
      tt[i*n+j] = 0.0;
    }
  }

  for(i=0;i<n*n;i++)  {
    tt[i] = a[i];
  }

  choldc(tt, n, p ) ;

  for(i=0;i<n*n;i++)  {
    cf[i] = a[i];
  }

  for (i = 0; i < n; i++) {
    tt[i*n+i] = p[i] ;
    for (j=0; j <= i ; j++) {  
      k = (i)*n+(j) ;
      cf[k] = tt[i*n+j] ;
    }
  }
  
  free(tt) ; 
  free(p) ;
}