/*
This files contains some generic utilities
 */

/* ----- System includes -----*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

/* ----- IFU and GSL includes -----*/
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort.h>
#include "IFU_io.h"
// #include "IFU_math.h"
/* ----- Specific IFU includes ----- */
#include "data_io.h"
#ifdef FITS
#include <fitsio.h>
#endif

/* Removing MIDAS by hand - not really clean, but should allow to compile with an IFU with --disable-midas enabled*/
#undef MIDAS

#ifdef MIDAS
#include "midas_defs.h"
#include "tbldef.h"
#include "fctext.h"
#include "ldbext.h"
#include "proto_st.h"

#endif
extern IO_Format InputIO, OutputIO;

/* ----- local includes ----- */
#include "utils.h"

/* ----- static functions ---------------------------------------- */
static int ascending(const void *a, const void*b) {
  if (*(double*)a==*(double*)b) return 0;
  return (*(double*)a > *(double*)b ) ? 1 : -1;
}

/* ----- ut_median ---------------------------------------- */
void ut_sort_ascending(double* values, int n)
     /* median of unsorted values */
{
  qsort(values,n,sizeof(double),&ascending);
}

/* ----- ut_median ---------------------------------------- */
double ut_median(double* values, int n)
     /* median of unsorted values */
{
  qsort(values,n,sizeof(double),&ascending);
  /* alternatively, I could have used  gsl_stats_median_from_sorted_data
     but it is fucking the flies ! */
  /* i saw also something usable in mathlib : the names might conflict !*/
  if (n%2)
    return values[n/2];
  else
    return (values[n/2-1]+values[n/2])/2;

}

/* ----- ut_min ---------------------------------------- */
double ut_min(double* values, int n)
     /* min of array */
{
  double min = values[0];
  int i;

  for (i=1;i<n;i++) {
    if (values[i]<min)
      min=values[i];
  }
  return min;
}

/* ----------------------------- ut_trunc_sigma_known -------------- */
void ut_trunc_sigma_known(double** values, int * nitems, double sigma, double sigcut){
     /* trunc the data set, removing any value outside +/- sigcut sigma
       if sigcut<0, does noting .
       it works iteratively by removing too far away values. => slow if
       the sigcut is too low !

       values : pointer to array of values - modified
       nitems : number of values -> modified
       sigcut : if <0, does nothing

       Beware : the number of remaining items might be 0 !

     */
  int ok=0;

  if (sigcut<0)
    return;

  qsort(*values,*nitems,sizeof(double),ascending);

  while (!ok && *nitems>1) {

    double mean=ut_mean(*values,*nitems);
    double valinf = (mean-(*values)[0])/sigma;
    double valsup = ((*values)[*nitems-1]-mean)/sigma;
    if (valsup>sigcut && valsup>=valinf) {
      double bound = MAX(sigcut,valinf);
      /* remove more than one for efficiency */
      while(valsup>=bound && *nitems>1) {
        (*nitems)--;
        valsup = ((*values)[*nitems-1]-mean)/sigma;
      }
    } else if (valinf>sigcut && valinf>=valsup) {
      double bound = MAX(sigcut,valinf);
      while(valinf>=bound && *nitems>1 ){
        (*nitems)--;
        (*values)++;
        valinf = (mean-(*values)[0])/sigma;
      }
    } else {
      ok=1;
    }
  }
}

/* ------------------------------- ut_trunc_sigma_unknown ------------------ */
void ut_trunc_sigma_unknown(double** values, int * nitems, double sigcut){
     /* trunc the data set, removing any value outside +/- sigcut sigma
       if sigcut<0, does noting .
       it works iteratively by removing too far away values. => slow if
       the sigcut is too low !
       Beware : sigma is computed from the distribution itself.
       Points far away will push the sigma towards greater values.

       values : pointer to array of values - modified
       nitems : number of values -> modified
       sigcut : if <0, does nothing

       Beware : the number of remaining items might be 0 !

     */
  int ok=0;

  if (sigcut<0)
    return;

  qsort(*values,*nitems,sizeof(double),ascending);

  while (!ok && *nitems>1) {
    double mean,sigma,valinf,valsup;

    mean = ut_mean((*values),*nitems);
    sigma = gsl_stats_sd_m(*values,1,*nitems,mean);
    valinf = (mean-(*values)[0])/sigma;
    valsup = ((*values)[*nitems-1]-mean)/sigma;
    if (valsup>sigcut && valsup>=valinf) {
      (*nitems)--;
    } else if (valinf>sigcut && valinf>=valsup) {
      (*nitems)--;
      (*values)++;
    } else {
      ok=1;
    }
  }
}

/* ------------------------------- ut_trunc_sigma_unknown ------------------ */
void ut_trunc_sigma_unknown_fast_sorted(double** values, int * nitems, double sigcut){
     /* trunc the data set, removing any value outside +/- sigcut sigma
       if sigcut<0, does noting .
       it works grossly, removing each time all what is outside the window,
       which is not accurate

       Beware : sigma is computed from the distribution itself.
       Points far away will push the sigma towards greater values.

       values : pointer to array of values - modified
       nitems : number of values -> modified
       sigcut : if <0, does nothing

       Beware : the number of remaining items might be 0 !

     */
  int ok=0;

  if (sigcut<0)
    return;

  /*  qsort(*values,*nitems,sizeof(double),ascending); */

  while (!ok && *nitems>1) {

    double mean,sigma,valinf,valsup;
    int nitems_old = *nitems;

    mean = ut_mean((*values),*nitems);
    sigma = gsl_stats_sd_m(*values,1,*nitems,mean);
    valinf = mean-sigma*sigcut;
    valsup = mean + sigma*sigcut;
    while ((*values)[0] < valinf && *nitems>1) {
      (*nitems)--;
      (*values)++;
    }
    while ((*values)[*nitems-1] > valsup && *nitems>1) {
      (*nitems)--;
    }
    if (nitems_old == (*nitems))
      ok=1;
  }
}

/*-------------------- ut_fraction_sigcut ----------------------*/
double ut_fraction_sigcut(double fraction) {
  /* given a distribution with fraction outliers, at infinite
   distance with respect to normal values,
  returns the standard deviation of the outliers, when the
  RMS is computed with it included.*/

  /* the computation is performed with fraction values at 1 and
   1-fraction at 0 */
  double sigcut,rms;
  /* mean = fraction */
  rms = sqrt(fraction*fraction*(1-fraction) + (1-fraction)*(1-fraction)*fraction);

  sigcut = (1-fraction)/rms;
  return sigcut;
}

/*-------------------- ut_mean ----------------------*/
double ut_mean(double* val,int n){

  int i;
  double sum = 0;
  for (i=0;i<n;i++)
    sum += val[i];
  return sum/n;
}

/*-------------------- ut_mean ----------------------*/
double ut_mode(double* val,int n){
  /* This function returns the highest probability density a set of
  datas. This only works reliabely if there is only 1 maximum,
  and in the limit where the data is sufficiently sampled.
  No error computation is performed ...*/
  /* The search is dyadic and iterative, each step looking for the part
  where the median is the narrower. The time is O(NlnN) (+ the time to run
  indexx)*/
  /* This is a fast and inaccurate algorithm. For a general algorithm, see the
  Parzenwindow method - the drawback is that it depends on a window size
  a priori*/

  size_t * index;
  double med;
  int nmin=0;
  int nmax=n; /*  out of the table */

  index = malloc(n*sizeof(size_t));

  /* test */

  gsl_sort_index(index,val,1,n);

  do {

    /* median computation */
    if ((nmax-nmin) % 2 == 0)
      med = (val[index[ (nmin + nmax )/2 ]] + val[index[ (nmin+nmax)/2 -1 ]])/2;
    else
      med = val[index[ (nmin + nmax)/2 ]];

    /* The density is higher for the upper part */
    if ( fabs(med - val[index[nmin]])  > fabs (med - val[index[nmax-1]]) )
      nmin =(nmin + nmax - 1 )/2;
    else if ( fabs(med - val[index[nmin]])  < fabs (med - val[index[nmax-1]]) )
      /* rounding in the good direction  in order to include the median place */
      nmax =  ( nmin + nmax)/2 + 1;
    else /* stop in case of equality */
      nmax = nmin;
  } while (nmax-nmin > 2);

  return med;
}

/*-------------------- ut_mean ----------------------*/
double ut_mode2(double* val,int n){
  /* This function returns the highest probability density a set of
  datas. This only works reliabely if there is only 1 maximum,
  and in the limit where the data is sufficiently sampled.
  No error computation is performed ...*/
  /* The search is dyadic and iterative, each step looking for the part
  which contains most of the data with respect to the highest value.
  The time is O(NlnN) (+ the time to run indexx)*/
  /* This is a fast and inaccurate algorithm. For a general algorithm, see the
  Parzenwindow method - the drawback is that it depends on a window size
  a priori*/

  size_t * index;
  double half;
  int nmin=0;
  int nmax=n; /*  out of the table */

  index = malloc(n*sizeof(size_t));

  /* test */

  gsl_sort_index(index,val,1,n);

  do {
    int i;
    /* half window computation */
    half=(val[index[nmin]]+val[index[nmax-1]])/2;
    for (i=nmin;i<nmax;i++) {
      if (val[index[i]]>half)
        break;
    }
    if (i==nmax)
      return half;
    else if (i<nmin+(nmax-nmin)/2)
      nmin=i;
    else if (i>nmin+(nmax-nmin)/2 || ((nmax-nmin)%2) )
      nmax=i;
    else {
      nmin++;
      nmax--;
    }
  } while (nmax-nmin>2);
  return half;
}

/*-------------------- ut_autocovariance ----------------------*/
void ut_autocovariance(double* data, double* autocorr, int n) {
/* data is of dim n, autocorr of dim n */
  int decal,i;
  double mean;
  /*  double var; */
  double *mdata = malloc(sizeof(double)*n);

  /* compute the mean */
  mean = ut_mean(data,n);

  /* substract it (n computations) */
  for (i=0;i<n;i++)
    mdata[i] = data[i]-mean;

  /* compute variance and what is needed for decal=0*/
  decal=0;
  autocorr[0]=0;
  for (i=0;i<n;i++) {
    autocorr[0]+=mdata[i]*mdata[i];
  }
  autocorr[0] /= n;
  /* var = autocorr[0]; */
  /* autocorr[0] = 1.0; */

  /* n^2 part */
  for (decal=1; decal<n; decal ++) {
    autocorr[decal]=0;
    for (i=0;i<n-decal;i++) {
      autocorr[decal]+=mdata[i]*mdata[i+decal];
    }
    autocorr[decal] /= (n-decal);
    /*    autocorr[decal] /= var; */
  }
  free(mdata);

}

/*-------------------- ut_varname_from_imname ----------------------*/
void ut_varname_from_imname(char* imname,char* varname){
  /* returns the name of the variance image from the name of
   the image */

  /* check standard name */
  char extname[lg_name+1];
  extname[0]='\0';
  varname[0]='\0';
  if (strstr(imname,"[chip00]"))
    strcpy(extname,"[var00]");
  if (strstr(imname,"[chip01]"))
    strcpy(extname,"[var01]");
  if (strstr(imname,"[chip02]"))
    strcpy(extname,"[var02]");
  if (strstr(imname,"[chip03]"))
    strcpy(extname,"[var03]");
  if (strstr(imname,"[image]"))
    strcpy(extname,"[variance]");

  if (extname[0]) {
    /* builds the variance name */
    char *ptName;
    strcpy(varname,imname);
    ptName = strchr(varname,'[');
    strcpy(ptName,extname);
  }
}

/*-------------------- ut_open_check_name ----------------------*/
char* ut_open_check_name(char* name){
  /* if name is a name without extension, tries to see if a
     [image] exists */

  if (!strchr(name,'[')) {
    char new_name[lg_name+1];
    sprintf(new_name,"%s[image]",name);
    if (exist(new_name))
      strcpy(name,new_name);
  }
  return name;
}

/*-------------------- ut_open_check_name ----------------------*/
char* ut_create_check_name(char* name){
  /* if name is a name without extension, appends an [image]
     by default */

  if (!strchr(name,'[')) {
    char new_name[lg_name+1];
    sprintf(new_name,"%s[image]",name);
    strcpy(name,new_name);
  }
  return name;
}

/*-------------------- ut_varname_from_imname ----------------------*/
int ut_is_bichip_detcom(char* filename){
  /* returns 1 if the name corresponds to a detcom image */

  char testName[lg_name+1];
  if (strstr(filename,"%d"))
    return 1;
  if (strrchr(filename,'['))
    return 0;
  // .fits file extension default
  if (strstr(filename,".fits"))
    sprintf(testName,"%s[chip00]",filename);
  else
    sprintf(testName,"%s.fits[chip00]",filename);
  if (exist(testName))
    return 1;
  else {
    if (strstr(filename,".fits"))
      sprintf(testName,"%s[CHIP00]",filename);
    else
      sprintf(testName,"%s.fits[CHIP00]",filename);
    if (exist(testName))
      return 1;
    else
      return 0;
  }
}

/*-------------------- ut_build_tmp_name ----------------------*/
void ut_build_tmp_name(char* filename, char* tmp_prefix){
  /* builds a temporary memory file name from the image name and a
  prefix */
  char imname[lg_name+1], *pt_name;

  if (!(pt_name = strrchr(filename,'/')))
    pt_name = filename;
  else
    pt_name = pt_name+1;
  sprintf(imname,"mem://%s_%s",tmp_prefix,pt_name);
  //  sprintf(imname,"%s_%s",tmp_prefix,pt_name);
  strcpy(filename,imname);
}

/*-------------------- ut_primary_header_name ----------------------*/
void ut_primary_header_name(char* full_name, char* primary_name){
  /* the primary header is the image name without any extension*/

  char* pt_name;
  strcpy(primary_name,full_name);

  if ((pt_name = strchr(primary_name,'[')))
    pt_name[0]='\0';

}

/* ===== IFU lib disagreement ======================================== */

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
!
!.func                        open_frame_fast()
!
!.purp          opens a 2D frame and updates the image structure items
!.desc
! open_frame(frame,name,mode)
!
! IMAGE2D *frame;       image structure
! char *name;           frame name
! char *mode;           open mode (Input,Ouput,IO)
!.ed
-------------------------------------------------------------------- */
int
open_frame_fast(IMAGE2D *frame, char *name, char *mode)
{
  char errtext[132], filename[lg_name+1];
  int status, nbaxes, iomode, int_datatype;
  float cuts[4];
  int info[5];
#ifdef IRAF
  int two_dim=2;
  int len;
#endif
#ifdef FITS
  fitsfile *fptr;
  int nbread;
  int npix;
  int group = 0;
  double pixref;
#endif

  memset(frame->ident,' ',lg_ident);
  frame->ident[lg_ident] = '\0';
  memset(frame->cunit,' ',lg_unit);
  frame->cunit[lg_unit] = '\0';
  memset(frame->history,' ',lg_hist);
  frame->history[lg_hist] = '\0';
  frame->external_info = NULL;
  frame->file_type = T_IMA2D;
  frame->data_format = InputIO.basic_io;

  strcpy(filename,name);
  first_blk(filename);
  strcpy(frame->name,filename);
  append_ima_extension(frame->name,InputIO.basic_io);

  strcpy(filename,frame->name);

  if (!exist(filename)) { /* check if fil exists */
    status = ERR_OPEN;
    sprintf(errtext,"open_frame: frame %s",filename);
    Handle_Error(errtext,status);
    return(status);
  }

  switch(mode[0]) {
  case 'I' :
    if (mode[1] == 'O')
      frame->iomode = (int)IO_MODE;
    else
      frame->iomode = (int)I_MODE;
    break;
  case 'O' : frame->iomode = (int)O_MODE;
    break;
  default  : frame->iomode = (int)I_MODE;
    break;
  }

  iomode = get_iomode_code(InputIO.basic_io,frame->iomode);

  switch (InputIO.basic_io) {

#ifdef MIDAS
  case MIDAS_FORMAT :
    status = SCFINF(filename,2,info);
    if (status == 0) {
      status = SCIGET(filename, info[1], iomode, F_IMA_TYPE, 2,
                      &nbaxes, &(frame->nx), &(frame->startx), &(frame->stepx),
                      frame->ident, frame->cunit, (char **)(&(frame->data)),
                      &(frame->imno));
      frame->data_type = info[1];
      frame->data_type = decode_datatype(InputIO.basic_io,frame->data_type);

      if (nbaxes!=2) /* We open a spectrum like an image, and that's not good */
        status = ERR_OPEN;

    }
    break;
#endif
#ifdef IRAF
  case IRAF_FORMAT :
  case STSDAS_FORMAT :
    len = strlen(filename);
    uimopn(filename,&iomode,&(frame->imno),&status,len);
    if (status != 0)
      break;
    uimgid(&(frame->imno),&int_datatype,&two_dim,&(frame->nx),&status);
    frame->data_type = decode_datatype(InputIO.basic_io,(short)(int_datatype));
    if (status != 0)
      break;
    alloc_frame_mem(frame, datatype);
    switch(frame->data_type) {
    case SHORT :
      uigs2s(&(frame->imno),&one,&(frame->nx),&one,&(frame->ny),
             frame->data.s_data,&status);
      break;
    case INT :
    case LONG :
      uigs2l(&(frame->imno),&one,&(frame->nx),&one,&(frame->ny),
             frame->data.l_data,&status);
      break;
    case FLOAT :
      uigs2r(&(frame->imno),&one,&(frame->nx),&one,&(frame->ny),
             frame->data.f_data,&status);
      break;
    case DOUBLE :
      uigs2d(&(frame->imno),&one,&(frame->nx),&one,&(frame->ny),
             frame->data.d_data,&status);
      break;
    }
    disable_user_warnings();
    RD_desc(frame,"IDENT",CHAR,lg_ident,frame->ident);
    restore_user_warnings();
    break;
#endif
#ifdef FITS
  case FITS_A_FORMAT :
  case FITS_B_FORMAT :
    status =0;
    if (fits_open_file(&fptr,filename,iomode,&status)) {
      status = ERR_ACCESS; break;
    }
    frame->external_info = (void *)fptr;
    if (fits_read_key(fptr, TINT,"NAXIS", &nbaxes,NULL, &status)) {
      status = ERR_READ; break;
    }
    if (nbaxes != 2) {
      status = ERR_IMA_HEAD; break;
    }
    if (fits_read_key(fptr, TINT, "NAXIS1",
                      &(frame->nx), NULL, &status)) {
      status = ERR_READ; break;
    }
    if (fits_read_key(fptr, TINT, "NAXIS2",
                      &(frame->ny), NULL, &status)) {
      status = ERR_READ; break;
    }
    if (status == 0) {
      pixref = 1.0;
      fits_read_key(fptr, TDOUBLE, "CRPIX1", &pixref, NULL, &status);
      if (status) { status = 0; pixref = 1; }
      fits_read_key(fptr, TDOUBLE, "CRVAL1", &(frame->startx), NULL, &status);
      if (status) { status = 0; frame->startx = (double)1; }
      fits_read_key(fptr, TDOUBLE, "CDELT1", &(frame->stepx), NULL, &status);
      if (status) { status = 0; frame->stepx = (double)1; }
      frame->startx -= (pixref-1)*frame->stepx;
      pixref = 1.0;
      fits_read_key(fptr, TDOUBLE, "CRPIX2", &pixref, NULL, &status);
      if (status) { status = 0; pixref = 1; }
      fits_read_key(fptr, TDOUBLE, "CRVAL2", &(frame->starty), NULL, &status);
      if (status) { status = 0; frame->starty = (double)1; }
      fits_read_key(fptr, TDOUBLE, "CDELT2", &(frame->stepy), NULL, &status);
      if (status) { status = 0; frame->stepy = (double)1; }
      frame->starty -= (pixref-1)*frame->stepy;
    }
    else
      break;

    int_datatype = (fptr->Fptr->tableptr)->tdatatype;
    frame->data_type = decode_datatype(InputIO.basic_io,(short)int_datatype);
    if (frame->data_type == SHORT) {
      if (fptr->Fptr->tableptr[1].tscale == 1 && fptr->Fptr->tableptr[1].tzero == 32768)
        /* unsigned short !!! */
        frame->data_type = LONG;
    }

    if (alloc_frame_mem(frame, frame->data_type) < 0) {
      fits_close_file(fptr,&status);
      status = ERR_ALLOC;
      break;
    }

    npix = frame->nx*frame->ny;
    switch (frame->data_type) {
    case SHORT :
      if (fits_read_img_sht(fptr,group,1L,npix,(short)0,
                            frame->data.s_data,&nbread,&status)) {
        status = ERR_READ; break;
      }
      break;
    case LONG :
    case INT :
      if (fits_read_img_lng(fptr,group,1L,npix,(int)0,
                            frame->data.l_data,&nbread,&status)) {
        status = ERR_READ; break;
      }
      break;
    case FLOAT :
      if (fits_read_img_flt(fptr,group,1L,npix,(float)0,
                            frame->data.f_data,&nbread,&status)) {
        status = ERR_READ; break;
      }
      break;
    case DOUBLE :
      if (fits_read_img_dbl(fptr,group,1L,npix,(double)0,
                            frame->data.d_data,&nbread,&status)) {
        status = ERR_READ; break;
      }
      break;
    }
    break;
#endif
  }

  if (status) {
    sprintf(errtext,"open_frame: frame %s",filename);
    status = get_tiger_errcode(frame->data_format,status);
    Handle_Error(errtext,status);
  }
  else {
    disable_user_warnings();
    status = RD_desc(frame,"LHCUTS",FLOAT,4,cuts);
    RD_desc(frame,"HISTORY",CHAR,lg_hist,frame->history);
    restore_user_warnings();

    frame->endx = frame->startx + (frame->nx -1)*frame->stepx;
    frame->endy = frame->starty + (frame->ny -1)*frame->stepy;
    if (status <= 0) {
      /* image_minmax is a really slow routine, and most of the time useless */
      frame->min = -ut_big_value;
      frame->max = +ut_big_value;
      /*			image_minmax(frame); */
    }
    else {
      frame->min = cuts[2];
      frame->max = cuts[3];
    }
    status = 0;
    /* parse wcs if contained in file */
    status = parse_wcs(frame);
  }
  return(status);
}

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
!
!.func                       close_frame_fast()
!
!.purp             closes a currently active 2D frame
!.desc
! close_frame(frame)
!
! IMAGE2D *frame;       image structure
!.ed
-------------------------------------------------------------------- */
int
close_frame_fast(IMAGE2D *frame)			/* close active frame */
{
  char  errtext[132], filename[lg_name+1];
  int   stat, int_datatype;
  float cuts[4];
#ifdef IRAF
  int one=1;
#endif
#ifdef FITS
  fitsfile *fptr;
  int npix;
#endif

  strcpy(filename,frame->name);

  if (frame->iomode == (int)I_MODE) {
    switch (frame->data_format) {
#ifdef MIDAS
    case MIDAS_FORMAT :
      stat = SCFCLO(frame->imno);
      break;
#endif
#ifdef IRAF
    case IRAF_FORMAT :
    case STSDAS_FORMAT :
      uimclo(&(frame->imno),&stat);
      break;
#endif
#ifdef FITS
    case FITS_A_FORMAT :
    case FITS_B_FORMAT :
      stat =0;
      fptr = (fitsfile *)frame->external_info;
      fits_close_file(fptr,&stat);
      free_frame_mem(frame);
      frame->external_info = NULL;
      break;
#endif
    }
    if (stat) {
      sprintf(errtext,"close_frame: frame %s",filename);
      stat = get_tiger_errcode(frame->data_format,stat);
      Handle_Error(errtext,stat);
    }
    return(stat);
  }

  /*
  if (frame->data.d_data != NULL) {
    image_minmax(frame);

    cuts[0]=(float)frame->min; cuts[2]=(float)frame->min;
    cuts[1]=(float)frame->max; cuts[3]=(float)frame->max;
    stat = WR_desc(frame,"LHCUTS",FLOAT,4,cuts);
  }
  */

  WR_history(frame, (Anyfile *)0);

  switch (frame->data_format) {
#ifdef MIDAS
  case MIDAS_FORMAT :
    stat = SCFCLO(frame->imno);
    break;
#endif
#ifdef IRAF
  case IRAF_FORMAT :
  case STSDAS_FORMAT :
    switch(frame->data_type) {
    case SHORT :
      uips2s(&(frame->imno),&one,&(frame->nx),&one,&(frame->ny),
             frame->data.s_data,&stat);
      break;
    case INT :
    case LONG :
      uips2l(&(frame->imno),&one,&(frame->nx),&one,&(frame->ny),
             frame->data.l_data,&stat);
      break;
    case FLOAT :
      uips2r(&(frame->imno),&one,&(frame->nx),&one,&(frame->ny),
             frame->data.f_data,&stat);
      break;
    case DOUBLE :
      uips2d(&(frame->imno),&one,&(frame->nx),&one,&(frame->ny),
             frame->data.d_data,&stat);
      break;
    }
    if (stat == 0)
      uimclo(&(frame->imno),&stat);
    free_frame_mem(frame);
    break;
#endif
#ifdef FITS
  case FITS_A_FORMAT :
  case FITS_B_FORMAT :
    stat = 0;
    fptr = (fitsfile *)frame->external_info;
    if (frame->iomode != (int)I_MODE) {
      if (frame->data.d_data != NULL) {
        int_datatype = get_datatype_code(OutputIO.basic_io,frame->data_type);
        npix = frame->nx*frame->ny;
        if (fits_write_img(fptr,int_datatype,1L,npix,
                           frame->data.s_data,&stat)) {
          stat = ERR_WRIT;
        }
      }
    }
    if (! stat) {
      fits_close_file(fptr,&stat);
      stat = wcs_free(frame);
    }
    free_frame_mem(frame);
    frame->external_info = NULL;
    break;
#endif
  }
  if (stat) {
    sprintf(errtext,"close_frame: frame %s",filename);
    stat = get_tiger_errcode(frame->data_format,stat);
    Handle_Error(errtext,stat);
  } else {
    if (TK && (frame->iomode == O_MODE || frame->iomode == IO_MODE))
      {
        printf("@ N {%s}\n",filename);
      }
  }

  return(stat);
}

/* ----- juldat --------------------------------------------------------- */
/*C
C       FUNCTION JULDAT RETURNS THE JULIAN DATE AS A DOUBLE
C       PRECISION NUMBER.
C       ALGORITHM FROM "ALAMANAC FOR COMPUTERS" p.B2
C*/
double juldat( int year, int month, int day, double ut)
     /* year, month, day, ut are the UTC ones
      ut is the fractional hour */
{
        double tmp;
        tmp = 367.0*year + 0.5 + ut/24.0;
        tmp = tmp - ((7*(year+((month+9)/12)))/4) + ((275*month)/9) + day + 1721013;
        return tmp;
}

/* =====  string utilities ===== */

int ut_parse_line(char* line, char** tokens) {
  /* the token strings will contain the line elements separated by spaces */
  /* the original line will be modified (some '/0' char added)*/
  char* pt_line = line;
  int itoken=0;

  while (pt_line && pt_line[0]!='\0') {
    tokens[itoken++] = pt_line;
    pt_line = strchr(pt_line,' ');
    if (pt_line)
      while((pt_line)[0]==' ') {
        pt_line[0]='\0';
        pt_line++;
      }
  }
  return itoken;
}


#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv2.h>

typedef struct {
    double l, g;
} Params;

int func (double t, const double y[], double f[], void *params) {
    Params *p = (Params *) params;
    double l = p->l;
    double g = p->g;
    f[0] = y[1];
    f[1] = -(g/l)*y[0];
    return GSL_SUCCESS;
}

int jac (double t, const double y[], double *dfdy, double dfdt[],
         void *params) {
    Params *p = (Params *) params;
    double l = p->l;
    double g = p->g;
    gsl_matrix_view dfdy_mat = gsl_matrix_view_array (dfdy, 2, 2);
    gsl_matrix * m = &(dfdy_mat.matrix);
    gsl_matrix_set (m, 0, 0, 0.0);
    gsl_matrix_set (m, 0, 1, 1.0);
    gsl_matrix_set (m, 1, 0, -(g/l));
    gsl_matrix_set (m, 1, 1, 0.0);
    dfdt[0] = 0.0;
    dfdt[1] = 0.0;
    return GSL_SUCCESS;
}

int main (void) {
    Params params;
    params.l = 1.0;
    params.g = 9.81;
    gsl_odeiv2_system sys = {func, jac, 2, &params};

    gsl_odeiv2_driver * d = gsl_odeiv2_driver_alloc_y_new (
            &sys, gsl_odeiv2_step_rk8pd, 1e-6, 1e-6, 0.0
    );
    int i;
    double t = 0.0, t1 = 100.0;
    double y[2] = { 0.1, 0.0 };

    for (i = 1; i <= 1000; i++) {
        double ti = i*t1/1000.0;
        int status = gsl_odeiv2_driver_apply (d, &t, ti, y);

        if (status != GSL_SUCCESS) {
            fprintf(stderr, "error, return value=%d\n", status);
            break;
        }

        printf ("%.5le %.5le %.5le\n", t, y[0], y[1]);
    }

    gsl_odeiv2_driver_free (d);
    return EXIT_SUCCESS;
}


/* interpolation/cspline.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2001, 2002, 2004 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_vector.h>
#include "integ_eval.h"
#include <gsl/gsl_interp.h>

typedef struct
{
  double * c;
  double * g;
  double * diag;
  double * offdiag;
} cspline_state_t;

/* common initialization */
static void *
cspline_alloc (size_t size)
{
  cspline_state_t * state = (cspline_state_t *) malloc (sizeof (cspline_state_t));

  if (state == NULL)
    {
      GSL_ERROR_NULL("failed to allocate space for state", GSL_ENOMEM);
    }

  state->c = (double *) malloc (size * sizeof (double));

  if (state->c == NULL)
    {
      free (state);
      GSL_ERROR_NULL("failed to allocate space for c", GSL_ENOMEM);
    }

  state->g = (double *) malloc (size * sizeof (double));

  if (state->g == NULL)
    {
      free (state->c);
      free (state);
      GSL_ERROR_NULL("failed to allocate space for g", GSL_ENOMEM);
    }

  state->diag = (double *) malloc (size * sizeof (double));

  if (state->diag == NULL)
    {
      free (state->g);
      free (state->c);
      free (state);
      GSL_ERROR_NULL("failed to allocate space for diag", GSL_ENOMEM);
    }

  state->offdiag = (double *) malloc (size * sizeof (double));

  if (state->offdiag == NULL)
    {
      free (state->diag);
      free (state->g);
      free (state->c);
      free (state);
      GSL_ERROR_NULL("failed to allocate space for offdiag", GSL_ENOMEM);
    }

  return state;
}

/* natural spline calculation
 * see [Engeln-Mullges + Uhlig, p. 254]
 */
static int
cspline_init (void * vstate, const double xa[], const double ya[],
              size_t size)
{
  cspline_state_t *state = (cspline_state_t *) vstate;

  size_t i;
  size_t num_points = size;
  size_t max_index = num_points - 1;  /* Engeln-Mullges + Uhlig "n" */
  size_t sys_size = max_index - 1;    /* linear system is sys_size x sys_size */

  state->c[0] = 0.0;
  state->c[max_index] = 0.0;

  for (i = 0; i < sys_size; i++)
    {
      const double h_i   = xa[i + 1] - xa[i];
      const double h_ip1 = xa[i + 2] - xa[i + 1];
      const double ydiff_i   = ya[i + 1] - ya[i];
      const double ydiff_ip1 = ya[i + 2] - ya[i + 1];
      const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
      const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
      state->offdiag[i] = h_ip1;
      state->diag[i] = 2.0 * (h_ip1 + h_i);
      state->g[i] = 3.0 * (ydiff_ip1 * g_ip1 -  ydiff_i * g_i);
    }

  if (sys_size == 1)
    {
      state->c[1] = state->g[0] / state->diag[0];
      return GSL_SUCCESS;
    }
  else
    {
      gsl_vector_view g_vec = gsl_vector_view_array(state->g, sys_size);
      gsl_vector_view diag_vec = gsl_vector_view_array(state->diag, sys_size);
      gsl_vector_view offdiag_vec = gsl_vector_view_array(state->offdiag, sys_size - 1);
      gsl_vector_view solution_vec = gsl_vector_view_array ((state->c) + 1, sys_size);

      int status = gsl_linalg_solve_symm_tridiag(&diag_vec.vector,
                                                 &offdiag_vec.vector,
                                                 &g_vec.vector,
                                                 &solution_vec.vector);
      return status;
    }
}

/* periodic spline calculation
 * see [Engeln-Mullges + Uhlig, p. 256]
 */
static int
cspline_init_periodic (void * vstate, const double xa[], const double ya[],
                       size_t size)
{
  cspline_state_t *state = (cspline_state_t *) vstate;

  size_t i;
  size_t num_points = size;
  size_t max_index = num_points - 1;  /* Engeln-Mullges + Uhlig "n" */
  size_t sys_size = max_index;    /* linear system is sys_size x sys_size */

  if (sys_size == 2) {
    /* solve 2x2 system */

    const double h0 = xa[1] - xa[0];
    const double h1 = xa[2] - xa[1];

    const double A = 2.0*(h0 + h1);
    const double B = h0 + h1;
    double g[2];
    double det;

    g[0] = 3.0 * ((ya[2] - ya[1]) / h1 - (ya[1] - ya[0]) / h0);
    g[1] = 3.0 * ((ya[1] - ya[2]) / h0 - (ya[2] - ya[1]) / h1);

    det = 3.0 * (h0 + h1) * (h0 + h1);
    state->c[1] = ( A * g[0] - B * g[1])/det;
    state->c[2] = (-B * g[0] + A * g[1])/det;
    state->c[0] = state->c[2];

    return GSL_SUCCESS;
  } else {

    for (i = 0; i < sys_size-1; i++) {
      const double h_i       = xa[i + 1] - xa[i];
      const double h_ip1     = xa[i + 2] - xa[i + 1];
      const double ydiff_i   = ya[i + 1] - ya[i];
      const double ydiff_ip1 = ya[i + 2] - ya[i + 1];
      const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
      const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
      state->offdiag[i] = h_ip1;
      state->diag[i] = 2.0 * (h_ip1 + h_i);
      state->g[i] = 3.0 * (ydiff_ip1 * g_ip1 - ydiff_i * g_i);
    }

    i = sys_size - 1;

    {
      const double h_i       = xa[i + 1] - xa[i];
      const double h_ip1     = xa[1] - xa[0];
      const double ydiff_i   = ya[i + 1] - ya[i];
      const double ydiff_ip1 = ya[1] - ya[0];
      const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
      const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
      state->offdiag[i] = h_ip1;
      state->diag[i] = 2.0 * (h_ip1 + h_i);
      state->g[i] = 3.0 * (ydiff_ip1 * g_ip1 - ydiff_i * g_i);
    }

    {
      gsl_vector_view g_vec = gsl_vector_view_array(state->g, sys_size);
      gsl_vector_view diag_vec = gsl_vector_view_array(state->diag, sys_size);
      gsl_vector_view offdiag_vec = gsl_vector_view_array(state->offdiag, sys_size);
      gsl_vector_view solution_vec = gsl_vector_view_array ((state->c) + 1, sys_size);

      int status = gsl_linalg_solve_symm_cyc_tridiag(&diag_vec.vector,
                                                     &offdiag_vec.vector,
                                                     &g_vec.vector,
                                                     &solution_vec.vector);
      state->c[0] = state->c[max_index];

      return status;
    }
  }
}

static
void
cspline_free (void * vstate)
{
  cspline_state_t *state = (cspline_state_t *) vstate;

  free (state->c);
  free (state->g);
  free (state->diag);
  free (state->offdiag);
  free (state);
}

/* function for common coefficient determination
 */
static inline void
coeff_calc (const double c_array[], double dy, double dx, size_t index,
            double * b, double * c, double * d)
{
  const double c_i = c_array[index];
  const double c_ip1 = c_array[index + 1];
  *b = (dy / dx) - dx * (c_ip1 + 2.0 * c_i) / 3.0;
  *c = c_i;
  *d = (c_ip1 - c_i) / (3.0 * dx);
}

static
int
cspline_eval (const void * vstate,
              const double x_array[], const double y_array[], size_t size,
              double x,
              gsl_interp_accel * a,
              double *y)
{
  const cspline_state_t *state = (const cspline_state_t *) vstate;

  double x_lo, x_hi;
  double dx;
  size_t index;

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  x_hi = x_array[index + 1];
  x_lo = x_array[index];
  dx = x_hi - x_lo;
  if (dx > 0.0)
    {
      const double y_lo = y_array[index];
      const double y_hi = y_array[index + 1];
      const double dy = y_hi - y_lo;
      double delx = x - x_lo;
      double b_i, c_i, d_i;
      coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
      *y = y_lo + delx * (b_i + delx * (c_i + delx * d_i));
      return GSL_SUCCESS;
    }
  else
    {
      *y = 0.0;
      return GSL_EINVAL;
    }
}

static
int
cspline_eval_deriv (const void * vstate,
                    const double x_array[], const double y_array[], size_t size,
                    double x,
                    gsl_interp_accel * a,
                    double *dydx)
{
  const cspline_state_t *state = (const cspline_state_t *) vstate;

  double x_lo, x_hi;
  double dx;
  size_t index;

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  x_hi = x_array[index + 1];
  x_lo = x_array[index];
  dx = x_hi - x_lo;
  if (dx > 0.0)
    {
      const double y_lo = y_array[index];
      const double y_hi = y_array[index + 1];
      const double dy = y_hi - y_lo;
      double delx = x - x_lo;
      double b_i, c_i, d_i;
      coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
      *dydx = b_i + delx * (2.0 * c_i + 3.0 * d_i * delx);
      return GSL_SUCCESS;
    }
  else
    {
      *dydx = 0.0;
      return GSL_EINVAL;
    }
}

static
int
cspline_eval_deriv2 (const void * vstate,
                     const double x_array[], const double y_array[], size_t size,
                     double x,
                     gsl_interp_accel * a,
                     double * y_pp)
{
  const cspline_state_t *state = (const cspline_state_t *) vstate;

  double x_lo, x_hi;
  double dx;
  size_t index;

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  x_hi = x_array[index + 1];
  x_lo = x_array[index];
  dx = x_hi - x_lo;
  if (dx > 0.0)
    {
      const double y_lo = y_array[index];
      const double y_hi = y_array[index + 1];
      const double dy = y_hi - y_lo;
      double delx = x - x_lo;
      double b_i, c_i, d_i;
      coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
      *y_pp = 2.0 * c_i + 6.0 * d_i * delx;
      return GSL_SUCCESS;
    }
  else
    {
      *y_pp = 0.0;
      return GSL_EINVAL;
    }
}

static
int
cspline_eval_integ (const void * vstate,
                    const double x_array[], const double y_array[], size_t size,
                    gsl_interp_accel * acc,
                    double a, double b,
                    double * result)
{
  const cspline_state_t *state = (const cspline_state_t *) vstate;

  size_t i, index_a, index_b;

  if (acc != 0)
    {
      index_a = gsl_interp_accel_find (acc, x_array, size, a);
      index_b = gsl_interp_accel_find (acc, x_array, size, b);
    }
  else
    {
      index_a = gsl_interp_bsearch (x_array, a, 0, size - 1);
      index_b = gsl_interp_bsearch (x_array, b, 0, size - 1);
    }

  *result = 0.0;

  /* interior intervals */
  for(i=index_a; i<=index_b; i++) {
    const double x_hi = x_array[i + 1];
    const double x_lo = x_array[i];
    const double y_lo = y_array[i];
    const double y_hi = y_array[i + 1];
    const double dx = x_hi - x_lo;
    const double dy = y_hi - y_lo;
    if(dx != 0.0) {
      double b_i, c_i, d_i;
      coeff_calc(state->c, dy, dx, i,  &b_i, &c_i, &d_i);

      if (i == index_a || i == index_b)
        {
          double x1 = (i == index_a) ? a : x_lo;
          double x2 = (i == index_b) ? b : x_hi;
          *result += integ_eval(y_lo, b_i, c_i, d_i, x_lo, x1, x2);
        }
      else
        {
          *result += dx * (y_lo + dx*(0.5*b_i + dx*(c_i/3.0 + 0.25*d_i*dx)));
        }
    }
    else {
      *result = 0.0;
      return GSL_EINVAL;
    }
  }

  return GSL_SUCCESS;
}

static const gsl_interp_type cspline_type =
{
  "cspline",
  3,
  &cspline_alloc,
  &cspline_init,
  &cspline_eval,
  &cspline_eval_deriv,
  &cspline_eval_deriv2,
  &cspline_eval_integ,
  &cspline_free
};

const gsl_interp_type * gsl_interp_cspline = &cspline_type;

static const gsl_interp_type cspline_periodic_type =
{
  "cspline-periodic",
  2,
  &cspline_alloc,
  &cspline_init_periodic,
  &cspline_eval,
  &cspline_eval_deriv,
  &cspline_eval_deriv2,
  &cspline_eval_integ,
  &cspline_free
};

const gsl_interp_type * gsl_interp_cspline_periodic = &cspline_periodic_type;


// my_gsl.h is included in bayesMLR.c
// Contains wrapper funtions
// References:
// https://www.gnu.org/software/gsl/manual/html_node/Matrices.html
// https://github.com/Blei-Lab/diln/blob/master/gsl_wrapper.c
#include <stdio.h>             // standard input/output
#include <stdlib.h>            // malloc
#include <math.h>              // fabs, sqrt, etc.
#include <time.h>              // time / set seed
#include <gsl/gsl_rng.h>       // GNU Scientific Library
#include <gsl/gsl_cdf.h>       // GNU Scientific Library
#include <gsl/gsl_randist.h>   // GNU Scientific Library
#include <gsl/gsl_linalg.h>    // GNU Scientific Library
#include <gsl/gsl_matrix.h>    // GNU Scientific Library
#include <gsl/gsl_statistics.h>// GNU Scientific Library
#include <gsl/gsl_blas.h>      // GSL Basic Linear Algebra
#define pi 3.14159265358979323846

// Reading Matrices ////////////////////////////////////////////////
int countFileRows(char* filename) {
  // Count number of rows in file "filename"
  FILE *fp;
  int count = 0;  // Line counter (result)
  char c;  // To store a character read from file

  fp = fopen(filename, "r");
  // Extract characters from file and store in character c
  if (fp) { // check file existence
  for (c = getc(fp); c != EOF; c = getc(fp))
      if (c == '\n') // Increment count if this character is newline
          count = count + 1;
  }
  fclose(fp);

  return count;
}

int countFileCols(char* filename, char delim) {
  // Count number of columns in file "filename" (with delimiter delim)
  FILE *fp = fopen(filename, "r");
  char c;
  int count = 0;

  while( c=fgetc(fp) ) {
    if(c == EOF) {
      break; /* break if end of file */
    } else if (c == delim) {
      count += 1; /* otherwise add one to the count */
    } else if (c == '\n') {
      count += 1;
      break;
    }
  }
  fclose(fp);

  return count;
}

void read_csv(char* filename, gsl_matrix* m) {
  // Read a csv into matrix m
  FILE* f = fopen(filename, "r");
  gsl_matrix_fscanf(f, m);
  fclose(f);
}

// Printing Matrix Info ////////////////////////////
void print_vector(gsl_matrix* v, char* filename) {
  FILE* f;
  if (filename == "") {
    f = stdout;
    printf("\n");
  } else {
    f = fopen(filename, "w");
  }

  gsl_vector_fprintf(f,v,"%g");
}

void print_matrix(gsl_matrix* m, char* filename) {
  // print matrix to filename or stdout if filename == ""
  FILE* f;
  if (filename == "") {
    f = stdout;
    printf("\n");
  } else {
    f = fopen(filename, "w");
  }
  int n = m->size1;
  int k = m->size2;

  for (int i = 0; i < n; i++) {
    for (int j = 0; j < k; j++) {
      fprintf (f, "%g\t", gsl_matrix_get (m, i, j));
    }
    fprintf (f, "\n");
  }

  if (filename == "") {
    printf("\n");
  } else {
    fclose(f);
  }
}

void print_matrix_dims(gsl_matrix* m) {
  // print dimensions of matrix m
  printf("%d%s%d\n",m->size1,"x",m->size2);
}

// Access Matrix Elements: //////////////////////////////////
void mat_sub(gsl_matrix* X, size_t r1, size_t r2, size_t c1, size_t c2, gsl_matrix* X_new) {
  gsl_matrix_view x = gsl_matrix_submatrix(X,r1,c1,r2-r1+1,c2-c1+1);
  gsl_matrix_memcpy(X_new,&x.matrix);
}

// Random Number Functions: /////////////////////////////////
double runif() {
  // Returns a random number b/w 0 & 1
  return (double) rand() / (double) RAND_MAX;
}

void mvrnorm(gsl_vector* m, gsl_matrix* cholS, gsl_rng* r, gsl_vector* out) {
  // vector addition, vector prod
  // m + cholS * e

  int n = m->size;
  double tmp;
  gsl_vector* e = gsl_vector_alloc(n);

  for (int i=0; i<n; i++) {
    tmp = 0;
    for (int j=0; j<n; j++) {
      if (i==0) gsl_vector_set(e,j,gsl_ran_gaussian(r,1.0));
      tmp += gsl_matrix_get(cholS,i,j) * gsl_vector_get(e,j);
    }
    gsl_vector_set(out,i,gsl_vector_get(m,i) + tmp);
  }

  gsl_vector_free(e);
}

// Linear Algebra Wrappers //////////////////////////////////
// mm: matrix-matrix
// mv: matrix-vector
// ms: matrix-scalar
//  d: double real
// ge: general matrix
void mm_prod(gsl_matrix* A, gsl_matrix* B, double a, double b, gsl_matrix* out) {
}

void mm_add(gsl_matrix* A, gsl_matrix* B, double a, double b, gsl_matrix* out) {
}

void mv_prod(gsl_matrix* A, gsl_vector* x, double a, gsl_vector* out) {
  gsl_blas_dgemv(CblasNoTrans,a,A,x,0.0,out);
}

void vv_add(gsl_vector* x, gsl_vector* y, gsl_vector *out) {
  // sum two vectors
  if (x->size != y->size)
    error("Error in vv_add(): input vectors must have the same dimensionality");
  else if (x->size != out->size || y->size != out->size)
    error("Error in vv_add(): output vector has incorrect dimensions");

  double tmp;
  int n = x->size;

  for (int i=0; i<n; i++) {
    tmp = gsl_vector_get(x,i) + gsl_vector_get(y,i);
    gsl_vector_set(out,i,tmp);
  }
}

double vv_prod(gsl_vector* x, gsl_vector* y) {
  // dot product
  if (x->size != y->size)
    error("Error in vv_prod(): input vectors must have the same dimensionality");

  double out = 0;
  int n = x->size;

  for (int i=0; i<n; i++) {
    out += gsl_vector_get(x,i) * gsl_vector_get(y,i);
  }

  return out;
}

void mat_inv(gsl_matrix * X, gsl_matrix * invX) {
// invert the matrix X
    int signum;
    gsl_matrix * LU = gsl_matrix_calloc(X->size1,X->size2);
    gsl_permutation * p = gsl_permutation_calloc(X->size1);
    gsl_matrix_memcpy(LU,X);

    gsl_linalg_LU_decomp (LU,p,&signum);
    gsl_linalg_LU_invert(LU,p,invX);

    gsl_matrix_free(LU);
    gsl_permutation_free(p);
}

void xxi_m(gsl_matrix* X, gsl_matrix* xxi) {
  int n = X->size1;
  int k = X->size2;
  double dot;
  gsl_matrix* xx = gsl_matrix_alloc(k,k);

  for (int i=0; i<k; i++) {
    for (int j=0; j<=i; j++) {
      dot = 0;
      for (int z=0; z<n; z++) {
        dot += gsl_matrix_get(X,z,i) * gsl_matrix_get(X,z,j);
      }
      gsl_matrix_set(xx,j,i,dot);
      gsl_matrix_set(xx,i,j,dot);
    }
  }

  mat_inv(xx,xxi);
  gsl_matrix_free(xx);
}

int my_gsl_linalg_cholesky_decomp (gsl_matrix * A)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (M != N)
    {
      GSL_ERROR("cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      size_t i,j,k;
      int status = 0;

      /* Do the first 2 rows explicitly.  It is simple, and faster.  And
       * one can return if the matrix has only 1 or 2 rows.
       */

      double A_00 = gsl_matrix_get (A, 0, 0);

      double L_00 = sqrt(A_00);

      if (A_00 <= 0)
        {
          status = GSL_EDOM ;
        }

      gsl_matrix_set (A, 0, 0, L_00);

      if (M > 1)
        {
          double A_10 = gsl_matrix_get (A, 1, 0);
          double A_11 = gsl_matrix_get (A, 1, 1);

          double L_10 = A_10 / L_00;
          double diag = A_11 - L_10 * L_10;
          double L_11 = sqrt(diag);

          if (diag <= 0)
            {
              status = GSL_EDOM;
            }

          gsl_matrix_set (A, 1, 0, L_10);
          gsl_matrix_set (A, 1, 1, L_11);
        }

      for (k = 2; k < M; k++)
        {
          double A_kk = gsl_matrix_get (A, k, k);

          for (i = 0; i < k; i++)
            {
              double sum = 0;

              double A_ki = gsl_matrix_get (A, k, i);
              double A_ii = gsl_matrix_get (A, i, i);

              gsl_vector_view ci = gsl_matrix_row (A, i);
              gsl_vector_view ck = gsl_matrix_row (A, k);

              if (i > 0) {
                gsl_vector_view di = gsl_vector_subvector(&ci.vector, 0, i);
                gsl_vector_view dk = gsl_vector_subvector(&ck.vector, 0, i);

                gsl_blas_ddot (&di.vector, &dk.vector, &sum);
              }

              A_ki = (A_ki - sum) / A_ii;
              gsl_matrix_set (A, k, i, A_ki);
            }

          {
            gsl_vector_view ck = gsl_matrix_row (A, k);
            gsl_vector_view dk = gsl_vector_subvector (&ck.vector, 0, k);

            double sum = gsl_blas_dnrm2 (&dk.vector);
            double diag = A_kk - sum * sum;

            double L_kk = sqrt(diag);

            if (diag <= 0)
              {
                status = GSL_EDOM;
              }

            gsl_matrix_set (A, k, k, L_kk);
          }
        }

      /* Now copy the transposed lower triangle to the upper triangle,
       * the diagonal is common.
       */

      //for (i = 1; i < M; i++)
      //  {
      //    for (j = 0; j < i; j++)
      //      {
      //        double A_ij = 0;//gsl_matrix_get (A, i, j);
      //        gsl_matrix_set (A, j, i, A_ij);
      //      }
      //  }

      if (status == GSL_EDOM)
        {
          GSL_ERROR ("matrix must be positive definite", GSL_EDOM);
        }

      return GSL_SUCCESS;
    }
}

void chol(gsl_matrix* X, gsl_matrix* cholS) {
  int n = X->size1;
  gsl_matrix_memcpy(cholS,X);
  my_gsl_linalg_cholesky_decomp(cholS);

  // lower tri stored in cholS.
  // So: mvrnorm <- function(M,S,n=nrow(S)) M + chol(S) %*% rnorm(n)
}


/* randist/logarithmic.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* Logarithmic distribution

   prob(n) =   p^n / (n log(1/(1-p)) for n = 1, 2, 3, ...

   We use Kemp's second accelerated generator, from Luc Devroye's book
   on "Non-Uniform Random Variate Generation", Springer */

unsigned int
gsl_ran_logarithmic (const gsl_rng * r, const double p)
{
  double c = log (1-p) ;

  double v = gsl_rng_uniform_pos (r);

  if (v >= p)
    {
      return 1 ;
    }
  else
    {
      double u = gsl_rng_uniform_pos (r);
      double q = 1 - exp (c * u);

      if (v <= q*q)
	{
	  double x = 1 + log(v)/log(q) ;
	  return x ;
	}
      else if (v <= q)
	{
	  return 2;
	}
      else
	{
	  return 1 ;
	}
    }
}

double
gsl_ran_logarithmic_pdf (const unsigned int k, const double p)
{
  if (k == 0)
    {
      return 0 ;
    }
  else
    {
      double P = pow(p, (double)k) / (double) k / log(1/(1-p)) ;
      return P;
    }
}


/* integration/qcheb.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>

/* This function computes the 12-th order and 24-th order Chebyshev
   approximations to f(x) on [a,b] */

void
gsl_integration_qcheb (gsl_function * f, double a, double b, double *cheb12, double *cheb24)
{
  size_t i;
  double fval[25], v[12];

  /* These are the values of cos(pi*k/24) for k=1..11 needed for the
     Chebyshev expansion of f(x) */

  const double x[11] = { 0.9914448613738104,
                         0.9659258262890683,
                         0.9238795325112868,
                         0.8660254037844386,
                         0.7933533402912352,
                         0.7071067811865475,
                         0.6087614290087206,
                         0.5000000000000000,
                         0.3826834323650898,
                         0.2588190451025208,
                         0.1305261922200516 };

  const double center = 0.5 * (b + a);
  const double half_length =  0.5 * (b - a);

  fval[0] = 0.5 * GSL_FN_EVAL (f, b);
  fval[12] = GSL_FN_EVAL (f, center);
  fval[24] = 0.5 * GSL_FN_EVAL (f, a);

  for (i = 1; i < 12; i++)
    {
      const size_t j = 24 - i;
      const double u = half_length * x[i-1];
      fval[i] = GSL_FN_EVAL(f, center + u);
      fval[j] = GSL_FN_EVAL(f, center - u);
    }

  for (i = 0; i < 12; i++)
    {
      const size_t j = 24 - i;
      v[i] = fval[i] - fval[j];
      fval[i] = fval[i] + fval[j];
    }

  {
    const double alam1 = v[0] - v[8];
    const double alam2 = x[5] * (v[2] - v[6] - v[10]);

    cheb12[3] = alam1 + alam2;
    cheb12[9] = alam1 - alam2;
  }

  {
    const double alam1 = v[1] - v[7] - v[9];
    const double alam2 = v[3] - v[5] - v[11];
    {
      const double alam = x[2] * alam1 + x[8] * alam2;

      cheb24[3] = cheb12[3] + alam;
      cheb24[21] = cheb12[3] - alam;
    }

    {
      const double alam = x[8] * alam1 - x[2] * alam2;
      cheb24[9] = cheb12[9] + alam;
      cheb24[15] = cheb12[9] - alam;
    }
  }

  {
    const double part1 = x[3] * v[4];
    const double part2 = x[7] * v[8];
    const double part3 = x[5] * v[6];

    {
      const double alam1 = v[0] + part1 + part2;
      const double alam2 = x[1] * v[2] + part3 + x[9] * v[10];

      cheb12[1] = alam1 + alam2;
      cheb12[11] = alam1 - alam2;
    }

    {
      const double alam1 = v[0] - part1 + part2;
      const double alam2 = x[9] * v[2] - part3 + x[1] * v[10];
      cheb12[5] = alam1 + alam2;
      cheb12[7] = alam1 - alam2;
    }
  }

  {
    const double alam = (x[0] * v[1] + x[2] * v[3] + x[4] * v[5]
                + x[6] * v[7] + x[8] * v[9] + x[10] * v[11]);
    cheb24[1] = cheb12[1] + alam;
    cheb24[23] = cheb12[1] - alam;
  }

  {
    const double alam = (x[10] * v[1] - x[8] * v[3] + x[6] * v[5]
                - x[4] * v[7] + x[2] * v[9] - x[0] * v[11]);
    cheb24[11] = cheb12[11] + alam;
    cheb24[13] = cheb12[11] - alam;
  }

  {
    const double alam = (x[4] * v[1] - x[8] * v[3] - x[0] * v[5]
                - x[10] * v[7] + x[2] * v[9] + x[6] * v[11]);
    cheb24[5] = cheb12[5] + alam;
    cheb24[19] = cheb12[5] - alam;
  }

  {
    const double alam = (x[6] * v[1] - x[2] * v[3] - x[10] * v[5]
                + x[0] * v[7] - x[8] * v[9] - x[4] * v[11]);
    cheb24[7] = cheb12[7] + alam;
    cheb24[17] = cheb12[7] - alam;
  }

  for (i = 0; i < 6; i++)
    {
      const size_t j = 12 - i;
      v[i] = fval[i] - fval[j];
      fval[i] = fval[i] + fval[j];
    }

  {
    const double alam1 = v[0] + x[7] * v[4];
    const double alam2 = x[3] * v[2];

    cheb12[2] = alam1 + alam2;
    cheb12[10] = alam1 - alam2;
  }

  cheb12[6] = v[0] - v[4];

  {
    const double alam = x[1] * v[1] + x[5] * v[3] + x[9] * v[5];
    cheb24[2] = cheb12[2] + alam;
    cheb24[22] = cheb12[2] - alam;
  }

  {
    const double alam = x[5] * (v[1] - v[3] - v[5]);
    cheb24[6] = cheb12[6] + alam;
    cheb24[18] = cheb12[6] - alam;
  }

  {
    const double alam = x[9] * v[1] - x[5] * v[3] + x[1] * v[5];
    cheb24[10] = cheb12[10] + alam;
    cheb24[14] = cheb12[10] - alam;
  }

  for (i = 0; i < 3; i++)
    {
      const size_t j = 6 - i;
      v[i] = fval[i] - fval[j];
      fval[i] = fval[i] + fval[j];
    }

  cheb12[4] = v[0] + x[7] * v[2];
  cheb12[8] = fval[0] - x[7] * fval[2];

  {
    const double alam = x[3] * v[1];
    cheb24[4] = cheb12[4] + alam;
    cheb24[20] = cheb12[4] - alam;
  }

  {
    const double alam = x[7] * fval[1] - fval[3];
    cheb24[8] = cheb12[8] + alam;
    cheb24[16] = cheb12[8] - alam;
  }

  cheb12[0] = fval[0] + fval[2];

  {
    const double alam = fval[1] + fval[3];
    cheb24[0] = cheb12[0] + alam;
    cheb24[24] = cheb12[0] - alam;
  }

  cheb12[12] = v[0] - v[2];
  cheb24[12] = cheb12[12];

  for (i = 1; i < 12; i++)
    {
      cheb12[i] *= 1.0 / 6.0;
    }

  cheb12[0] *= 1.0 / 12.0;
  cheb12[12] *= 1.0 / 12.0;

  for (i = 1; i < 24; i++)
    {
      cheb24[i] *= 1.0 / 12.0;
    }

  cheb24[0] *= 1.0 / 24.0;
  cheb24[24] *= 1.0 / 24.0;
}


/*
 * ParticleFilter.h
 *
 *  Created on: Mar 21, 2012
 *      Author: nair
 */

#ifndef PARTICLEFILTER_H_
#define PARTICLEFILTER_H_

#include <gsl/gsl_rng.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

namespace tracking1
{

class ParticleFilterParams
{
public:
    ParticleFilterParams();
    ~ParticleFilterParams();

    void parse(std::string filename);

public:

    int NUMBER_OF_PARTICLES;

    /* standard deviations for gaussian sampling in transition model */
    float TRANS_X_STD;
    float TRANS_Y_STD;
    float TRANS_SCALE_STD;

    /* autoregressive dynamics parameters for transition model */
    float DYNAMICS_A1;
    float DYNAMICS_A2;
    float DYNAMICS_B0;

    double LAMBDA;

    bool DYNAMIC_MODEL_CV;
};

struct Particle
{
    float x;          /**< current x coordinate */
    float y;          /**< current y coordinate */
    float s;          /**< current z coordinate */
    float xp;         /**< previous x coordinate */
    float yp;         /**< previous y coordinate */
    float sp;         /**< previous z coordinate */
    float x0;         /**< original x coordinate */
    float y0;         /**< original y coordinate */
    float s0;			/**< original z coordinate */
    float x_velocity; /**< current x_velocity coordinate */
    float y_velocity; /**< current y_velocity coordinate */
    float s_velocity; /**< current z_velocity coordinate */

    float w;          /**< weight */
};

class ParticleFilter
{
public:

    ParticleFilter();
    ~ParticleFilter();

    void initialize(const cv::Vec3d& initPose);

    void predict();

    void predict_with_simple_model();

    void predict_with_cv_model();

    void correct();

    void resizeParticleSet(int n);
    void setParticleWeight(int id, float w);

    double getMaxLikelihood()
    {
        double max=0.0;
        for(int i=0;i<mParams.NUMBER_OF_PARTICLES;++i)
        {
            if(max<mParticles[i].w)
            {
                max=mParticles[i].w;
            }
        }
        return max;
    }

    double getAvgLikelihood()
    {
        double avg=0.0;
        for(int i=0;i<mParams.NUMBER_OF_PARTICLES;++i)
        {

            avg+=mParticles[i].w;
        }
        return avg/mParams.NUMBER_OF_PARTICLES;
    }

    int nOfParticles()
    {
        return mParams.NUMBER_OF_PARTICLES;
    }

    static int particle_cmp(const void* p1,const void* p2 )
    {
        Particle* _p1 = (Particle*)p1;
        Particle* _p2 = (Particle*)p2;

        if( _p1->w > _p2->w )
            return -1;
        if( _p1->w < _p2->w )
            return 1;
        return 0;
    }

    void getParticlePose(int id, cv::Vec3d& pose);

    void getOutputPose(cv::Vec3d& pose);

    void getOutputAvgPose(cv::Vec3d& pose);

    void getOutputVelocity(cv::Vec3d& vel);

    double getLambda()
    {
        return mParams.LAMBDA;
    }

private:

    void normalize();

    void resample();

private:

    ParticleFilterParams mParams;

    Particle* mParticles;
    cv::Vec3d mOutputPose;

    Eigen::MatrixXd A_Matrix;
    Eigen::MatrixXd G_Matrix;
    Eigen::MatrixXd W_Matrix;

    Eigen::MatrixXd State_Matrix;
    Eigen::MatrixXd State_Prev_Matrix;

    gsl_rng* rng;

};

}
#endif /* PARTICLEFILTER_H_ */


/**
 * Created              :   2020.09.02;
 * Last Update          :   2021.06.11;
 * Author               :   Gabriel Marino de Oliveira <ra115114@uem.br>;
 * Supervisor/Advisor   :   Breno Ferraz de Oliveira <>;
 * Notes                :
 * Compilation          :   gcc -O3 src/main.c -static -lgls -lglscblas -lm -o simulationRPS.out
 * Execution            :   time ./simulationRPS.out
 */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>           //  add mathematical operations;
#include <time.h>           //  add functions to work with time and date, in this case used only to get the seconds counter from 01/01/1970;
#include <complex.h>        //  add complex number operations;
#include <gsl/gsl_rng.h>    //  add pseudo-random number generators;

#define Ni  500     //  Size of the grid in "i", or abscissa axis;
#define Nj  500     //  Size of the grid in "j", or ordinate axis;
#define dt  0.1     //  Time interval;
#define tf  10000   //  Total Time, or number of generations;
#define Pp  0.3     //  Predation probability;
#define Pm  0.4     //  Mobility probability for sthocastic method;
                    /** Is not necessary to sthocastic method define the Reproduction probability, here we assume which "Pp + Pm + Pr = 1" or "Pr = 1 - (Pp + Pm)",
                     * and this is why Reproduction probability is only needed to define for RK method**/
#define Cm  1.0     //  Mobility coeficient for RK method;
#define Pr  0.4     //  Reproduction probability;

#define np  100                                 //  Number of points which will be printed when used a logarithmic scale;
// const double r = pow((tf/dt), (1.0/np));        //  r stands for ratio, will used to generate a arithmetic progression, useful to print equally spaced point


/*
 * Assignment 4, Question 5 matvec implementation in C using MPI.
 *
 * Copyright (C) 2015, Jonathan Gillett (100437638)
 * All rights reserved.
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <cblas.h>
#include <mpi.h>

// Minimum of two values
#define min(a, b) (((a) < (b)) ? (a) : (b))
// Define process 0 as MASTER
#define MASTER 0
// Message tags
#define REQUEST 1
#define REPLY   2

// Master process, divides up the matrix into rows for computation by slaves
void master(int n_proc, int n_rows, int n_cols) {
    // 0. Allocate space for arrays as needed to compute y = A*x
    double *x = (double *) malloc(n_cols*sizeof(double));
    double *y = (double *) malloc(n_rows*sizeof(double));
    double *z = (double *) malloc(n_rows*sizeof(double));
    double *A = (double *) malloc(n_rows*n_cols*sizeof(double));
    double *sub = (double *) malloc(n_cols*sizeof(double));
    double ans = 0.0;
    int n_sent = 0, row = 0, sender = 0;
    MPI_Status status;

    // 1. Define data A (n_rows x n_cols matrix) and x (n_cols x 1 vector)
    for (int i = 0; i < n_cols; ++i) {
        x[i] = i+1;
    }
    for (int i = 0; i < n_rows; ++i) {
        for (int j = 0; j < n_cols; ++j) {
            A[(i*n_cols)+j] = i+1;
        }
    }

    // 2. Broadcast vector x to all processes
    MPI_Bcast(x, n_cols, MPI_DOUBLE, MASTER, MPI_COMM_WORLD);

    // 3. Send first rows to other processes
    for (int i = 1; i < min(n_rows+1, n_proc); ++i) {
    	memcpy((void *) sub, (void *) &A[n_sent*n_cols], n_cols * sizeof(double));
        MPI_Send(sub, n_cols, MPI_DOUBLE, i, n_sent, MPI_COMM_WORLD);
        n_sent++;
    }

    // Now loop over remaining rows, receiving dot products computed
    for (int i = 0; i < n_rows; ++i) {
        // Receive a computed inner product from one of the processes
        MPI_Recv(&ans, 1, MPI_DOUBLE, MPI_ANY_SOURCE, MPI_ANY_TAG,
                 MPI_COMM_WORLD, &status);
        sender = status.MPI_SOURCE;
        row = status.MPI_TAG;
        y[row] = ans;

        // Either send another row back to sender or send final y=Ax
        // along with a tag to signal completion.
        if (n_sent < n_rows) {
        	memcpy((void *) sub, (void *) &A[n_sent*n_cols], n_cols * sizeof(double));
            MPI_Send(sub, n_cols, MPI_DOUBLE, sender, n_sent, MPI_COMM_WORLD);
            n_sent++;
        }
        // Inform sender that there is no more work
        else {
            MPI_Send(y, n_rows, MPI_DOUBLE, sender, n_rows+1, MPI_COMM_WORLD);
        }
    }

    printf("Finished!\n");
    // Calculate the norm to verify ||y-Ax|| is correct
    cblas_dgemv(CblasRowMajor, CblasNoTrans, n_rows, n_cols, 1.0, A, n_cols,
	      		x, 1, 0.0, z, 1);
    for (int i = 0; i < n_rows; ++i) {
    	y[i] -= z[i];
    }
    printf("Check: ||y-Ax|| = %f\n", cblas_dnrm2(n_rows, y, 1));

    free(x);
    free(y);
    free(z);
    free(A);
    free(sub);
}

// Slave process computes inner product and returns result to master process
void slave(int proc_id, int n_rows, int n_cols) {
    // Allocate arrays for receiving and sending rows & dot products
    double *x = (double *) malloc(n_cols*sizeof(double));
    double *my_row = (double *) malloc(n_cols*sizeof(double));
    double ans = 0.0;
    int row = 0;
    MPI_Status status;

    // 1. Receive vector x from master process
    MPI_Bcast(x, n_cols, MPI_DOUBLE, MASTER, MPI_COMM_WORLD);

    // Receive row to calculate from master process
    MPI_Recv(my_row, n_cols, MPI_DOUBLE, MASTER, MPI_ANY_TAG,
             MPI_COMM_WORLD, &status);
    row = status.MPI_TAG;

    while (row < n_rows) {
        // Compute inner product and return result to master process
        ans = cblas_ddot(n_cols, my_row, 1, x, 1);
        MPI_Send(&ans, 1, MPI_DOUBLE, MASTER, row, MPI_COMM_WORLD);
        // Receive new row from master
        MPI_Recv(my_row, n_cols, MPI_DOUBLE, MASTER, MPI_ANY_TAG,
                 MPI_COMM_WORLD, &status);
        row = status.MPI_TAG;
    }
}

int main(int argc, char *argv[]) {
    int    proc_id;       // Process ID number
    int    n_proc;        // Number of processes
    int    n_rows = 1000; // Number of rows
    int    n_cols = 1000; // Number of columns

    // Initialize MPI
    MPI_Init(&argc, &argv);
    MPI_Comm_size(MPI_COMM_WORLD, &n_proc);
    MPI_Comm_rank(MPI_COMM_WORLD, &proc_id);

    if (n_proc == 1) {
        puts("Error: requires two or more processes (no work will be done).");
        MPI_Finalize();
        exit(1);
    }

    if (proc_id == MASTER) {
        master(n_proc, n_rows, n_cols);
    }
    else {
        slave(proc_id, n_rows, n_cols);
    }

    MPI_Finalize();
    return 0;
}


/*
  File Name: grad.c
  Last Modification Date:	3/9/94	10:03:45
  Current Version: grad.c	1.7
  %File Creation Date: 26/10/91
  Author: Ramesh Gopinath  <ramesh@dsp.rice.edu>

  Copyright: All software, documentation, and related files in this distribution
  are Copyright (c) 1993  Rice University

  Permission is granted for use and non-profit distribution providing that this
  notice be clearly maintained. The right to distribute any portion for profit
  or as part of any commercial product is specifically reserved for the author.
*/

/*
#################################################################################
grad.mex4:
function [g,f,h] = grad(k,M,N,fs); Computes the gradient of stopband energy of the
                   linear-phase prototype filter in a cosine-modulated filter bank
		   with respect to lattice parameters of the J lattices.

      Input:
            k : Vector of denormalized lattice parameters for the J lattices of
	        CMFB
            M : M-channel, M-band etc
            N : 2Mm, length of the prototype filter
           fs : Stopband edge (as a fraction of pi)
      Output:
            f : Stopband Energy
            h : Prototype filter (second half)
#################################################################################
*/

#ifndef PROTOTYPE_DESIGN_H
#define PROTOTYPE_DESIGN_H

#include <stdio.h>
#include <assert.h>

#include <gsl/gsl_block.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>

#include "common/refcount.h"
#include "common/jexception.h"

// ----- definition for class `CosineModulatedPrototypeDesign' -----
//
class CosineModulatedPrototypeDesign {
 public:
  CosineModulatedPrototypeDesign(int M = 256, int N = 3072, double fs = 1.0);
  ~CosineModulatedPrototypeDesign();

  void fcn(const double* x, double* f);
  void grad(const double* x, double* g);

  int M() const { return _M; }
  int N() const { return _N; }
  int m() const { return _m; }
  int J() const { return _J; }

  // return (one-half) of the prototype filter
  const gsl_vector* proto();

 private:
  const int					_M;	// No. of channels
  const int					_N;	// filter length 2 * _M * _m
  const int					_M2;	// 2 * _M
  const int					_m;
  const int					_Mm;
  const int					_J;
  const int					_Jm;
  const int					_oddM;
  const int					_oddm;

  double*					_sinews;
  double*					_h;
  double*					_hh;
  double*					_Ph;
  double*					_jac;

  int*						_index;

  gsl_vector*					_proto;
};

double design_f(const gsl_vector* v, void* params);
void   design_df(const gsl_vector* v, void* params, gsl_vector* df);
void   design_fdf(const gsl_vector* v, void* params, double* f, gsl_vector* df);

// ----- definition for class `PrototypeDesignBase' -----
//
class PrototypeDesignBase {
public:
  PrototypeDesignBase(int M, int m, unsigned r, double wp, int tau);
  ~PrototypeDesignBase();

protected:
  virtual void _calculateAb();
  void _calculateC();
  void _svd(gsl_matrix* U, gsl_matrix* V, gsl_vector* S, gsl_vector* workSpace);
  gsl_matrix* _nullSpace(const gsl_matrix* A, double tolerance);
  gsl_vector* _pseudoInverse(const gsl_matrix* A, const gsl_vector* b, double tolerance);
  void _solveNonSingular(const gsl_matrix* H, const gsl_vector* c0, const gsl_matrix* P, double tolerance);
  void _solveSingular(gsl_matrix* Ac, gsl_vector* bc, gsl_matrix* K, gsl_vector* dp, double tolerance);
  double _condition(gsl_matrix* H, gsl_matrix* P);
  void _printMatrix(const gsl_matrix* A, FILE* fp = stdout) const;
  void _printVector(const gsl_vector* b, FILE* fp = stdout) const;

  void save(const String& fileName);

  const int					_M;	// the number of subbands
  const int					_m;
  const	double					_wp;
  const int					_R;
  const int					_D;
  const int					_L;
  	int					_tau;

  gsl_matrix*					_A;
  gsl_vector*					_b;
  gsl_matrix*					_cpA;
  gsl_vector*					_cpb;
  gsl_matrix*					_C;
  gsl_matrix*					_cpC;

  gsl_vector*					_singularVals;
  gsl_vector*					_scratch;
  gsl_vector*					_workSpace;

  gsl_vector*					_prototype;
};

// ----- definition for class `AnalysisOversampledDFTDesign' -----
//
typedef Countable AnalysisOversampledDFTDesignCountable;
class AnalysisOversampledDFTDesign : public AnalysisOversampledDFTDesignCountable, protected PrototypeDesignBase {
public:
  AnalysisOversampledDFTDesign(int M = 512, int m = 2, int r = 1, double wp = 1.0, int tau_h = -1 );
  ~AnalysisOversampledDFTDesign();

  // design analysis prototype
  // tolerance for double precision: 2.2204e-16
  // tolerance for single precision: 1.1921e-07
  const gsl_vector* design(double tolerance = 2.2204e-16);

  // calculate distortion measures
  const gsl_vector* calcError(bool doPrint = true);

  void save(const String& fileName) { PrototypeDesignBase::save(fileName); }

protected:
  virtual void _solve(double tolerance);
  double _passbandResponseError();
  double _inbandAliasingDistortion();

  gsl_vector*					_error;	// [0] the total response error
                                                        // [1] the residual aliasing distortion
                                                        // [2] objective fuction
};

typedef refcountable_ptr<AnalysisOversampledDFTDesign> AnalysisOversampledDFTDesignPtr;

// ----- definition for class `SynthesisOversampledDFTDesign' -----
//
typedef Countable SynthesisOversampledDFTDesignCountable;
class SynthesisOversampledDFTDesign : public SynthesisOversampledDFTDesignCountable, protected PrototypeDesignBase {
public:
  SynthesisOversampledDFTDesign(const gsl_vector* h, int M = 512, int m = 2, int r = 1,
				double v = 0.01, double wp = 1.0, int tau_T = -1 );
  ~SynthesisOversampledDFTDesign();

  // design synthesis prototype
  // tolerance for double precision: 2.2204e-16
  // tolerance for single precision: 1.1921e-07
  const gsl_vector* design(double tolerance = 2.2204e-16);

  // calculate distortion measures
  virtual const gsl_vector* calcError(bool doPrint = true);

  void save(const String& fileName) { PrototypeDesignBase::save(fileName); }

protected:
  void _calculateEfP();
  virtual void _solve(double tolerance);
  double _totalResponseError();
  double _residualAliasingDistortion();

        gsl_vector*				_h;
  const double					_v;

  gsl_matrix*					_E;
  gsl_matrix*					_P;
  gsl_vector*					_f;

  gsl_matrix*					_cpE;
  gsl_matrix*					_cpP;
  gsl_vector*					_cpf;
  gsl_vector*					_error;	// [0] the total response error
                                                        // [1] the residual aliasing distortion
                                                        // [2] objective fuction
};

typedef refcountable_ptr<SynthesisOversampledDFTDesign> SynthesisOversampledDFTDesignPtr;

// ----- definition for class `AnalysisNyquistMDesign' -----
//
class AnalysisNyquistMDesign : public AnalysisOversampledDFTDesign {
public:
  AnalysisNyquistMDesign(int M = 512, int m = 2, int r = 1, double wp = 1.0, int tau_h = -1 );
  ~AnalysisNyquistMDesign();

private:
  virtual void _solve(double tolerance);
  void _calculateFd();
  void _calculateKdp();

  gsl_matrix*					_F;
  gsl_vector*					_d;
  gsl_matrix*					_K;
  gsl_vector*					_dp;
};

typedef Inherit<AnalysisNyquistMDesign, AnalysisOversampledDFTDesignPtr>	AnalysisNyquistMDesignPtr;

// ----- definition for class `SynthesisNyquistMDesign' -----
//
class SynthesisNyquistMDesign : public SynthesisOversampledDFTDesign {
public:
  SynthesisNyquistMDesign(const gsl_vector* h, int M = 512, int m = 2, int r = 1,
			  double wp = 1.0, int tau_g = -1);
  ~SynthesisNyquistMDesign();

  virtual const gsl_vector* calcError(bool doPrint = true);

private:
  virtual void _solve(double tolerance);

  double _passbandResponseError();
  void _calculateHc0();
  void _calculateJcp();

  gsl_matrix*					_H;
  gsl_vector*					_c0;
  gsl_matrix*					_J;
  gsl_vector*					_cp;
};

typedef Inherit<SynthesisNyquistMDesign, SynthesisOversampledDFTDesignPtr> SynthesisNyquistMDesignPtr;

// ----- definition for class `SynthesisNyquistMDesignCompositeResponse' -----
//
class SynthesisNyquistMDesignCompositeResponse : public SynthesisNyquistMDesign {
public:
  SynthesisNyquistMDesignCompositeResponse(const gsl_vector* h, int M = 512, int m = 2, int r = 1,
					   double wp = 1.0, int tau = -1);
  ~SynthesisNyquistMDesignCompositeResponse();

private:
  virtual void _calculateAb();
  inline double _sinc(int m);

  double* 					_sincValues;
};

typedef Inherit<SynthesisNyquistMDesignCompositeResponse, SynthesisNyquistMDesignPtr> SynthesisNyquistMDesignCompositeResponsePtr;

double SynthesisNyquistMDesignCompositeResponse::_sinc(int m)
{
  int am     = abs(m);

  if (am >= 2 * _L)
    throw jindex_error("Problem: index %d > 2 * L = %d", am, 2 * _L);

  double val = _sincValues[am];
  if (val != -HUGE_VAL) return val;

  if (m == 0)
    val = _sincValues[am] = 1.0;
  else
    val = _sincValues[am] = sin(_wp * am) / (_wp * am);

  return val;
}

#endif