/* program to calculate -2*log(Likelhiood) from real-space power
   spectrum (with no window) using fitted likelhiood surface to
   spherical harmonics likelihood */

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

void err_handler(char*);
double *dvector(long,long);
double **dmatrix(long,long,long,long);
double Pk(double); 

int main() {

  FILE *fp_data, *fp_cov;
  int i,j,Nparam=0;
  double *apar, *data, *err, *diff, *model, **cov, **icov;
  double *k_min, *k_max, *bp_min, *bp_av, *bp_max;
  double logdet=0.0, m2ll=0.0;

  const int bsz=120; char buf[bsz];

  if((fp_data=fopen("2dF_realpk_data.txt","r"))==NULL) err_handler("cannot open data output file");
  fscanf(fp_data,"# %d marginalised band powers\n",&Nparam);

  k_min   = dvector(1,Nparam);
  k_max   = dvector(1,Nparam);
  bp_min  = dvector(1,Nparam);
  bp_max  = dvector(1,Nparam);
  bp_av   = dvector(1,Nparam);
  apar    = dvector(1,Nparam);
  data    = dvector(1,Nparam);
  err     = dvector(1,Nparam);
  model   = dvector(1,Nparam);
  diff    = dvector(1,Nparam);
  cov     = dmatrix(1,Nparam,1,Nparam);
  icov    = dmatrix(1,Nparam,1,Nparam);
  
  fgets(buf,bsz,fp_data);
  for(i=1;i<=Nparam;i++) 
    if(fscanf(fp_data,"%lf %lf %lf %lf %lf\n",&k_min[i],&k_max[i],&bp_min[i],&bp_av[i],&bp_max[i])!=5)
      err_handler("data input 1");
  fgets(buf,bsz,fp_data);
  fgets(buf,bsz,fp_data);
  for(i=1;i<=Nparam;i++) if(fscanf(fp_data,"%lf %lf %lf\n",&apar[i],&data[i],&err[i])!=3)
    err_handler("data input 2");
  fclose(fp_data);
  
  if((fp_cov=fopen("2dF_realpk_cov.txt","r"))==NULL) err_handler("cannot open cov output file");
  fgets(buf,bsz,fp_cov);
  for(i=1;i<=Nparam;i++) for(j=1;j<=Nparam;j++) 
    if(fscanf(fp_cov,"%lf",&cov[i][j])!=1) err_handler("cov input 1");
  fgets(buf,bsz,fp_cov);
  fgets(buf,bsz,fp_cov);
  for(i=1;i<=Nparam;i++) for(j=1;j<=Nparam;j++) 
    if(fscanf(fp_cov,"%lf",&icov[i][j])!=1) err_handler("cov input 2");
  fclose(fp_cov);

  /* set up models to be tested */
  for(i=1;i<=(Nparam-1);i++) model[i]=Pk(0.5*(k_min[i]+k_max[i]));
  model[Nparam]=0.55;

  /* set up difference vectors - N_NODE k-values and beta */
  for(i=1;i<=Nparam;i++) diff[i] = apar[i]*(model[i]-data[i])+(1.-apar[i])*data[i]*(log(model[i])-log(data[i]));
	      
  /* calculate -2log(likelihood) */
  for(i=1;i<=Nparam;i++) for(j=1;j<=Nparam;j++)
    m2ll += diff[i]*icov[i][j]*diff[j];

  printf("%g\n",m2ll);

  return 1;
}

/* example Power spectrum */
double Pk(double k)  
{  
  double gam,pow_n;
  double q, pn, tf, tfsq;

  if(k==0.) return 0.;

  gam   = 0.2;
  pow_n = 1.0;
  q     = k/gam;
  tf    = log(1.+2.34*q)/(2.34*q);
  tfsq  = tf*tf/sqrt(1.0+q*(3.89+q*(259.21+q*(162.77+q*2027.17))));
  pn    = pow(k,pow_n)*tfsq;

  return 2.78695e+06*pn;
}

/* std error handler */
void err_handler(char *error_text)
{
  printf("Run-time error...\n%s\nnow exiting\n",error_text);
  exit(1);
}

/* NR: allocate a double vector with subscript range v[nl..nh] */
double *dvector(long nl, long nh)
{
  double *v;

  v=(double *)malloc((size_t)((nh-nl+2)*sizeof(double)));
  if (!v) err_handler("allocation failure in dvector()");
  return v-nl+1;
}

/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
double **dmatrix(long nrl, long nrh, long ncl, long nch)
{
  long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
  double **m;
  
  /* allocate pointers to rows */
  m=(double **) malloc((size_t)((nrow+1)*sizeof(double*)));
  if (!m) err_handler("allocation failure 1 in matrix()");
  m += 1;
  m -= nrl;
  
  /* allocate rows and set pointers to them */
  m[nrl]=(double *) malloc((size_t)((nrow*ncol+1)*sizeof(double)));
  if (!m[nrl]) err_handler("allocation failure 2 in matrix()");
  m[nrl] += 1;
  m[nrl] -= ncl;
  
  for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;
  
  /* return pointer to array of pointers to rows */
  return m;
}