/*
** BCM - bipartite connectivity mapping
**
** G.Lohmann, MPI-KYB, Aug 2017
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include "viaio/Vlib.h"
#include "viaio/VImage.h"
#include "viaio/mu.h"
#include "viaio/option.h"

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_math.h>

extern void gsl_sort_vector_index(gsl_permutation *,gsl_vector *);

#define SQR(x) ((x) * (x))
#define ABS(x) ((x) > 0 ? (x) : -(x))

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

extern gsl_matrix_float *DataMatrix(VImage *src,int nslices,VImage roi,int);
extern size_t NumVoxels(VImage src);
extern VImage VoxelMap(VImage roi);

typedef struct SpointStruct{
  VShort x;
  VShort y;
  VShort z;
} SPoint;

VDictEntry MetricDict[] = {
  { "pearson", 0 },
  { "spearman", 1 },
  { "MI", 2 },
  { NULL }
};

VImage VCallocImage(int nslices,int nrows,int ncols,VRepnKind repn,VImage ref)
{
  VImage image = VCreateImage(nslices,nrows,ncols,repn);
  if (image == NULL) VError(" error allocating image");
  VFillImage(image,VAllBands,0);
  if (ref != NULL) VCopyImageAttrs (ref,image);
  return image;
}

void ShowImage(gsl_matrix_float *A,char *filename)
{
  int i,j;
  int n=A->size1;
  int m=A->size2;
  VImage tstimage = VCallocImage(1,n,m,VFloatRepn,NULL);
  for (i=0; i <n; i++) {
    for (j=0; j<m; j++) {
      VPixel(tstimage,0,i,j,VFloat) = gsl_matrix_float_get(A,i,j);
    }
  }
  VAttrList out_list = VCreateAttrList();
  VAppendAttr(out_list,"image",NULL,VImageRepn,tstimage);
  FILE *out_file = fopen(filename,"w");
  VWriteFile (out_file, out_list);
}

VImage GetRoi(SPoint addr,int nslices,int nrows,int ncols,int radius)
{
  int i,b,r,c;
  int rad2 = radius*radius+1;

  int b0 = addr.z;
  int r0 = addr.y;
  int c0 = addr.x;

  i=0;
  for (b=b0-radius; b<=b0+radius; b++) {
    for (r=r0-radius; r<=r0+radius; r++) {
      for (c=c0-radius; c<=c0+radius; c++) {
	if (SQR(b-b0) + SQR(r-r0) + SQR(c-c0) > rad2) continue;
	i++;
      }
    }
  }
  VImage roi = VCreateImage(nslices,nrows,ncols,VBitRepn);
  VFillImage(roi,VAllBands,0);
  for (b=b0-radius; b<=b0+radius; b++) {
    if (b < 0 || b >= nslices) VError(" b %d",b);
    for (r=r0-radius; r<=r0+radius; r++) {
      if (r < 0 || r >= nrows) VError(" r %d",r);
      for (c=c0-radius; c<=c0+radius; c++) {
	if (c < 0 || c >= ncols) VError(" c %d",c);
	if (SQR(b-b0) + SQR(r-r0) + SQR(c-c0) > rad2) continue;
	VPixel(roi,b,r,c,VBit) = 1;
      }
    }
  }
  return roi;
}

double Pearson(const float *data1,const float *data2,int n)
{
  int i;
  double corr=0;
  for (i=0; i<n; i++) {
    const double u = (double)data1[i];
    const double v = (double)data2[i];
    corr += u*v;
  }
  corr /= (double)n;
  return corr;
}

double Spearman(const float *data1,const float *data2,int n)
{
  int i;
  double nx = (double)n;
  double kx = nx*(nx*nx-1.0);
  double sxy=0.0;
  for (i=0; i<n; i++) {
    const double u = (double)data1[i];
    const double v = (double)data2[i];
    const double d = (u-v);
    sxy += d*d;
  }
  double rho = 1.0 - 6.0*sxy/kx;
  return rho;
}

/* if X,Y are normalized Gaussians, then I=-0.5*log(1-r^2), with r covariance */
double MutualInformation(const float *data1,const float *data2,int nt)
{
  int i;
  double covar,mi,u;
  double tiny=1.0e-10;

  covar = 0;
  for (i=0; i<nt; i++) {
    const double u = data1[i];
    const double v = data2[i];
    covar += u*v;
  }
  const double nx= nt;
  covar /= nx;

  mi = 0;
  u  = 1.0 - covar*covar;
  if (u > tiny) mi = -0.5 * log(u);
  if (mi < 0) {
    VWarning("MI: %f",mi);
    mi = 0;
  }
  return mi;
}

double Correlation(const float *data1,const float *data2,int n,int metric)
{
  double corr=0.0;
  if (metric == 0) corr = Pearson(data1,data2,n);
  if (metric == 1) corr = Spearman(data1,data2,n);
  if (metric == 2) corr = MutualInformation(data1,data2,n);
  if (gsl_isnan(corr) || gsl_isinf(corr)) corr = 0;
  return corr;
}

void VectorNormalize(gsl_vector_float *x)
{
  int i;
  double nx = (float)x->size;
  double sum1=0,sum2=0;
  for (i=0; i<x->size; i++) {
    sum1 += x->data[i];
    sum2 += x->data[i]*x->data[i];
  }
  double mean = sum1/nx;
  double sigma = sqrt((double)((sum2 - nx * mean * mean) / (nx - 1.0)));

  for (i=0; i<x->size; i++) {
    x->data[i] = (x->data[i] - mean)/sigma;
  }
}

/* bipartite eigenvector centrality mapping */
void VBiadjacencyECM(gsl_matrix_float *C,gsl_vector_float *xa,gsl_vector_float *xb)
{
  int na = C->size1;
  int nb = C->size2;
  int i,iter,maxiter=100;
  double d=0,sum=0,sum_old=0;

  /* ini */
  gsl_vector_float *ya = gsl_vector_float_calloc(na);
  gsl_vector_float *yb = gsl_vector_float_calloc(nb);
  double nx = (double)(na+nb);
  for (i=0; i<na; i++) xa->data[i] = 1.0/nx;
  for (i=0; i<nb; i++) xb->data[i] = 1.0/nx;

  /* iterations */
  for (iter=0; iter<maxiter; iter++) {
    gsl_blas_sgemv (CblasNoTrans,1.0,C,xb,0.0,ya);
    gsl_blas_sgemv (CblasTrans,1.0,C,xa,0.0,yb);

    sum = 0;
    for (i=0; i<na; i++) sum += ya->data[i]*ya->data[i];
    for (i=0; i<nb; i++) sum += yb->data[i]*yb->data[i];
    sum = sqrt(sum);

    for (i=0; i<na; i++) xa->data[i] = ya->data[i]/sum;
    for (i=0; i<nb; i++) xb->data[i] = yb->data[i]/sum;

    d = fabs(sum-sum_old);
    if (iter > 0) fprintf(stderr," %5d   %f\n",(int)iter,d);
    if (d < 1.0e-8 && iter > 3) break;
    if (iter > 5 && sum > sum_old) break;
    sum_old = sum;
  }

  gsl_vector_float_free(ya);
  gsl_vector_float_free(yb);
}

/* bipartite degree centrality mapping */
void VBiadjacencyDegreeMap(gsl_matrix_float *C,gsl_vector_float *xa,gsl_vector_float *xb)
{
  size_t na = C->size1;
  size_t nb = C->size2;
  size_t i;

  gsl_vector_float *ya = gsl_vector_float_calloc(na);
  gsl_vector_float *yb = gsl_vector_float_calloc(nb);
  for (i=0; i<na; i++) xa->data[i] = 1.0/(double)na;
  for (i=0; i<nb; i++) xb->data[i] = 1.0/(double)nb;

  gsl_blas_sgemv (CblasNoTrans,1.0,C,xb,0.0,ya);
  gsl_blas_sgemv (CblasTrans,1.0,C,xa,0.0,yb);

  gsl_vector_float_memcpy(xa,ya);
  gsl_vector_float_memcpy(xb,yb);

  gsl_vector_float_free(ya);
  gsl_vector_float_free(yb);
}

/* project onto ROI */
void VNetworkProjection(gsl_matrix_float *C,gsl_vector_float *x)
{
  size_t na = C->size1;
  size_t nb = C->size2;
  size_t i,n1=0,n2=0;

  if (x->size == na) {
    n1 = na;
    n2 = nb;
  }
  else {
    n1 = nb;
    n2 = na;
  }
  float nx = (float)n2;

  fprintf(stderr," network projection\n");

  /* project bipartite graph onto ROI */
  gsl_matrix_float *A = gsl_matrix_float_calloc(n1,n1);
  if (!A) VError(" err allocating projection network");
  int progress=0;

#pragma omp parallel for schedule(guided) firstprivate(C,A)
  for (i=0; i<n1; i++) {
    size_t j=0,k=0;
    if (i%10 == 0) fprintf(stderr," %d0  of  %lu\r",(int)(++progress),n1);
    for (j=0; j<n1; j++) {

      float w=0;
      for (k=0; k<n2; k++) {
	float u = gsl_matrix_float_get(C,i,k);
	float v = gsl_matrix_float_get(C,j,k);
	w += u*v;
      }
#pragma omp critical
      {
	gsl_matrix_float_set(A,i,j,w/nx);
      }
    }
  }
  fprintf(stderr,"\n");

  /* Hubs of projected network */
  float w=0;
  size_t j=0;
  for (i=0; i<n1; i++) {
    w=0;
    for (j=0; j<n1; j++) {
      w += gsl_matrix_float_get(A,i,j);
    }
    x->data[i] = w/nx;
  }
  gsl_matrix_float_free(A);
}

/* make matrix positive */
void VThresholdMatrix(gsl_matrix_float *C,float threshold)
{
  int i,j;
  size_t nneg=0,npos=0;
  float u=0,tiny=1.0e-6;
  double sum1=0,sum2=0;
  double nx = (double)(C->size1 * C->size2);
  for (i=0; i<C->size1; i++) {
    for (j=0; j<C->size2; j++) {
      u = gsl_matrix_float_get(C,i,j);
      sum1 += u;
      sum2 += u*u;
      if (u < threshold+tiny) nneg++;
      else npos++;
      if (u < threshold+tiny) gsl_matrix_float_set(C,i,j,0.0);
    }
  }
  if (npos < 1) VWarning(" Subthreshold correlations only");
  double mean = sum1/nx;
  double sigma = sqrt((double)((sum2 - nx * mean * mean) / (nx - 1.0)));
  fprintf(stderr," matrix mean,std: %f %f,  npos: %lu, nneg: %lu\n",mean,sigma,npos,nneg);
}

int main (int argc,char *argv[])
{
  static SPoint addr1;
  static SPoint addr2;
  static VString roi1_filename = "";
  static VString roi2_filename = "";
  static VShort type=1;
  static VFloat threshold = 0.0;
  static VShort radius=5;
  static VShort   metric = 0;
  static VBoolean plotboth=FALSE;
  static VBoolean normalize=TRUE;
  static VOptionDescRec  options[] = {
    {"roi1",VStringRepn,1,(VPointer) &roi1_filename,VOptionalOpt,NULL,"ROI 1"},
    {"roi2",VStringRepn,1,(VPointer) &roi2_filename,VOptionalOpt,NULL,"ROI 2"},
    {"seed1",VShortRepn,3,(VPointer) &addr1,VOptionalOpt,NULL,"Voxel address of seed point (x,y,z)"},
    {"seed2",VShortRepn,3,(VPointer) &addr2,VOptionalOpt,NULL,"Voxel address of seed point (x,y,z)"},
    {"type", VShortRepn,1,(VPointer) &type,VOptionalOpt,NULL,"Type, 0:ECM, 1: DCM, 2: project onto ROI1, 3: project onto ROI2"},
    {"threshold", VFloatRepn,1,(VPointer) &threshold,VOptionalOpt,NULL,"Matrix threshold"},
    {"metric",VShortRepn,1,(VPointer) &metric,VOptionalOpt,MetricDict,"Correlation metric"},
    {"radius", VShortRepn,1,(VPointer) &radius,VOptionalOpt,NULL,"Radius around seed voxel"},
    {"both", VBooleanRepn,1,(VPointer) &plotboth,VOptionalOpt,NULL,"Whether to plot results of both ROIs"},
    {"normalize", VBooleanRepn,1,(VPointer) &normalize,VOptionalOpt,NULL,"Whether to normalize results"},
  };
  FILE *out_file=NULL;
  VString in_file=NULL;
  VImage roi1=NULL,roi2=NULL;
  int i;
  char *prg = GetLipsiaName("vbcm");

  /* Parse command line arguments and identify files: */
  VParseFilterCmdX (VNumber (options), options, argc, argv,&in_file,&out_file);
  fprintf(stderr," type= %d\n",type);

  /* read functional data */
  VAttrList list = VReadAttrList(in_file,0L,TRUE,FALSE);
  if (list == NULL) VError(" error reading input file %s",in_file);

  /* get pointers to image data */
  int nrows=0,ncols=0,nt=0;
  int nslices = VAttrListNumImages(list);
  VImage *src = VAttrListGetImages(list,nslices);
  VImageDimensions(src,nslices,&nt,&nrows,&ncols);
  fprintf(stderr," image dims:  %d  %d  %d,  nt: %d\n",nslices,nrows,ncols,nt);

  /* omp-stuff */
#ifdef _OPENMP
  int num_procs=omp_get_num_procs();
  printf("using %d cores\n",(int)num_procs);
  omp_set_num_threads(num_procs);
#endif /* _OPENMP */

  /* ROI 1 */
  if (strlen(roi1_filename) > 0) {
    VAttrList list1 = VReadAttrList(roi1_filename,0L,TRUE,FALSE);
    if (list1 == NULL) VError(" error reading roi file %s",roi1_filename);
    roi1 = VReadImage(list1);
    if (roi1 == NULL) VError(" err reading %s",roi1_filename);
  }

  /* ROI 2 */
  if (strlen(roi2_filename) > 0) {
    VAttrList list2 = VReadAttrList(roi2_filename,0L,TRUE,FALSE);
    if (list2 == NULL) VError(" error reading roi file %s",roi2_filename);
    roi2 = VReadImage(list2);
    if (roi2 == NULL) VError(" err reading %s",roi2_filename);
  }

  /* ROIs from seed voxels */
  if (roi1 == NULL) roi1 = GetRoi(addr1,nslices,nrows,ncols,radius);
  if (roi2 == NULL) roi2 = GetRoi(addr2,nslices,nrows,ncols,radius);

  /* voxel maps */
  VImage map1  = VoxelMap(roi1);
  VImage map2  = VoxelMap(roi2);
  size_t nvox1 = NumVoxels(roi1);
  size_t nvox2 = NumVoxels(roi2);
  fprintf(stderr," nvox1: %lu,  nvox2: %lu\n",nvox1,nvox2);

  /* read data matrices */
  gsl_matrix_float *X1 = DataMatrix(src,nslices,roi1,(int)metric);
  gsl_matrix_float *X2 = DataMatrix(src,nslices,roi2,(int)metric);

  /* correlation matrix */
  fprintf(stderr," CorrMatrix...\n");
  int rad2 = 3*3;     /* exclusion radius */
  gsl_matrix_float *A = gsl_matrix_float_calloc(nvox1,nvox2);
  if (A == NULL) VError(" err allocating corr matrix, %d x %d",nvox1,nvox2);
  int progress=0;

#pragma omp parallel for schedule(guided) firstprivate(A)
  for (i=0; i<nvox1; i++) {
    if (i%100 == 0) fprintf(stderr," %d00\r",(int)(++progress));
    int bi = VPixel(map1,0,0,i,VShort);
    int ri = VPixel(map1,0,1,i,VShort);
    int ci = VPixel(map1,0,2,i,VShort);

    int j=0;
    float u=0;
    float *tmp = (float *) VCalloc(nvox2,sizeof(float));
    for (j=0; j<nvox2; j++) tmp[j] = -1.0;
    const float *data1 = gsl_matrix_float_const_ptr(X1,i,0);
    for (j=0; j<nvox2; j++) {
      int bj = VPixel(map2,0,0,j,VShort);
      int rj = VPixel(map2,0,1,j,VShort);
      int cj = VPixel(map2,0,2,j,VShort);
      int di = SQR(bi-bj) + SQR(ri-rj) + SQR(ci-cj);
      if (di < rad2) continue;

      const float *data2 = gsl_matrix_float_const_ptr(X2,j,0);
      tmp[j] = Correlation(data1,data2,nt,(int)metric);
    }

#pragma omp critical
    {
      for (j=0; j<nvox2; j++) {
	u = tmp[j];
	gsl_matrix_float_set(A,i,j,u);
      }
    }
    VFree(tmp);
  }
  fprintf(stderr,"\n");

  /*
  ShowImage(A,"test.v");
  exit(0);
  */

  /* Biadjacency ECM */
  fprintf(stderr," Biadjacency Connectivity...\n");
  gsl_vector_float *xa = gsl_vector_float_calloc(nvox1);
  gsl_vector_float *xb = gsl_vector_float_calloc(nvox2);

  fprintf(stderr," type= %d\n",type);

  VThresholdMatrix(A,threshold);

  switch(type) {
  case 0:
    VBiadjacencyECM(A,xa,xb);
    break;
  case 1:
    VBiadjacencyDegreeMap(A,xa,xb);
    break;
  case 2:
    VNetworkProjection(A,xa);
    break;
  case 3:
    VNetworkProjection(A,xb);
    break;
  default:
    VError(" unknown type");
  }

  /* normalize */
  if (normalize) {
    VectorNormalize(xa);
    VectorNormalize(xb);
  }

  /* output */
  VImage dest = VCallocImage(nslices,nrows,ncols,VFloatRepn,src[0]);
  int b,r,c;
  for (i=0; i<nvox1; i++) {
    b = VPixel(map1,0,0,i,VShort);
    r = VPixel(map1,0,1,i,VShort);
    c = VPixel(map1,0,2,i,VShort);
    VPixel(dest,b,r,c,VFloat) = 100.0*xa->data[i];
  }
  if (plotboth) {
    for (i=0; i<nvox2; i++) {
      b = VPixel(map2,0,0,i,VShort);
      r = VPixel(map2,0,1,i,VShort);
      c = VPixel(map2,0,2,i,VShort);
      VPixel(dest,b,r,c,VFloat) = 100.0*xb->data[i];
    }
  }

  /* write output to disk */
  VAttrList out_list = VCreateAttrList();
  VAttrList geolist = VGetGeoInfo(list);
  if (geolist != NULL) {
    double *D = VGetGeoDim(geolist,NULL);
    D[0] = 3;  /* 3D */
    D[4] = 1;
    VSetGeoDim(geolist,D);
    VSetGeoInfo(geolist,out_list);
  }
  VHistory(VNumber(options),options,prg,&list,&out_list);
  VAppendAttr(out_list,"image",NULL,VImageRepn,dest);
  VWriteFile (out_file, out_list);
  fclose(out_file);
  fprintf (stderr, "%s: done.               \n", argv[0]);
  exit(0);
}


/* ode-initval/rk4.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 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.
 */

/* Runge-Kutta 4, Classical */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

typedef struct
{
  double *k;
  double *y0;
  double *ytmp;
}
rk4_state_t;

static void *
rk4_alloc (size_t dim)
{
  rk4_state_t *state = (rk4_state_t *) malloc (sizeof (rk4_state_t));

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

  state->k = (double *) malloc (dim * sizeof (double));

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

  state->y0 = (double *) malloc (dim * sizeof (double));

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

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->y0);
      free (state->k);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  return state;
}

static int
rk4_apply (void *vstate,
           size_t dim,
           double t,
           double h,
           double y[],
           double yerr[],
           const double dydt_in[],
           double dydt_out[],
           const gsl_odeiv_system * sys)
{
  rk4_state_t *state = (rk4_state_t *) vstate;

  size_t i;
  int status = 0;

  double *const k = state->k;
  double *const y0 = state->y0;
  double *const ytmp = state->ytmp;

  /* Copy the starting value. We will write over
   * the y[] vector, using it for scratch and
   * then filling it with the final result.
   */

  DBL_MEMCPY (y0, y, dim);

  if (dydt_in != NULL)
    {
      DBL_MEMCPY (k, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y0, k);
      GSL_STATUS_UPDATE (&status, s);
    }

  for (i = 0; i < dim; i++)
    {
      y[i] = h / 6.0 * k[i];    /* use y[] to store delta_y */
      ytmp[i] = y0[i] + 0.5 * h * k[i];
    }

  /* k2 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);
    GSL_STATUS_UPDATE (&status, s);
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 3.0 * k[i];
      ytmp[i] = y0[i] + 0.5 * h * k[i];
    }

  /* k3 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);
    GSL_STATUS_UPDATE (&status, s);
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 3.0 * k[i];
      ytmp[i] = y0[i] + h * k[i];
    }

  /* k4 step, error estimate, and final sum */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k);
    GSL_STATUS_UPDATE (&status, s);
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 6.0 * k[i];
      yerr[i] = h * y[i];
      y[i] += y0[i];
      if (dydt_out != NULL)
        dydt_out[i] = k[i];
    }

  return status;
}

static int
rk4_reset (void *vstate, size_t dim)
{
  rk4_state_t *state = (rk4_state_t *) vstate;

  DBL_ZERO_MEMSET (state->k, dim);
  DBL_ZERO_MEMSET (state->y0, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk4_order (void *vstate)
{
  rk4_state_t *state = (rk4_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 4;
}

static void
rk4_free (void *vstate)
{
  rk4_state_t *state = (rk4_state_t *) vstate;
  free (state->k);
  free (state->y0);
  free (state->ytmp);
  free (state);
}

static const gsl_odeiv_step_type rk4_type = { "rk4",    /* name */
  1,                            /* can use dydt_in */
  0,                            /* gives exact dydt_out */
  &rk4_alloc,
  &rk4_apply,
  &rk4_reset,
  &rk4_order,
  &rk4_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk4 = &rk4_type;


/* ode-initval/rk4imp.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 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.
 */

/* Runge-Kutta 4, Gaussian implicit */

/* Author:  G. Jungman */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

typedef struct
{
  double *k1nu;
  double *k2nu;
  double *ytmp1;
  double *ytmp2;
}
rk4imp_state_t;

static void *
rk4imp_alloc (size_t dim)
{
  rk4imp_state_t *state = (rk4imp_state_t *) malloc (sizeof (rk4imp_state_t));

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

  state->k1nu = (double *) malloc (dim * sizeof (double));

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

  state->k2nu = (double *) malloc (dim * sizeof (double));

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

  state->ytmp1 = (double *) malloc (dim * sizeof (double));

  if (state->ytmp1 == 0)
    {
      free (state->k2nu);
      free (state->k1nu);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp1", GSL_ENOMEM);
    }

  state->ytmp2 = (double *) malloc (dim * sizeof (double));

  if (state->ytmp2 == 0)
    {
      free (state->ytmp1);
      free (state->k2nu);
      free (state->k1nu);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp2", GSL_ENOMEM);
    }

  return state;
}

static int
rk4imp_apply (void *vstate,
              size_t dim,
              double t,
              double h,
              double y[],
              double yerr[],
              const double dydt_in[],
              double dydt_out[],
              const gsl_odeiv_system * sys)
{
  rk4imp_state_t *state = (rk4imp_state_t *) vstate;

  const double ir3 = 1.0 / M_SQRT3;
  const int iter_steps = 3;
  int status = 0;
  int nu;
  size_t i;

  double *const k1nu = state->k1nu;
  double *const k2nu = state->k2nu;
  double *const ytmp1 = state->ytmp1;
  double *const ytmp2 = state->ytmp2;

  /* initialization step */
  if (dydt_in != 0)
    {
      DBL_MEMCPY (k1nu, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1nu);
      GSL_STATUS_UPDATE (&status, s);
    }

  DBL_MEMCPY (k2nu, k1nu, dim);

  /* iterative solution */
  for (nu = 0; nu < iter_steps; nu++)
    {
      for (i = 0; i < dim; i++)
        {
          ytmp1[i] =
            y[i] + h * (0.25 * k1nu[i] + 0.5 * (0.5 - ir3) * k2nu[i]);
          ytmp2[i] =
            y[i] + h * (0.25 * k2nu[i] + 0.5 * (0.5 + ir3) * k1nu[i]);
        }
      {
        int s =
          GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h * (1.0 - ir3), ytmp1, k1nu);
        GSL_STATUS_UPDATE (&status, s);
      }
      {
        int s =
          GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h * (1.0 + ir3), ytmp2, k2nu);
        GSL_STATUS_UPDATE (&status, s);
      }
    }

  /* assignment */
  for (i = 0; i < dim; i++)
    {
      const double d_i = 0.5 * (k1nu[i] + k2nu[i]);
      if (dydt_out != NULL)
        dydt_out[i] = d_i;
      y[i] += h * d_i;
      yerr[i] = h * h * d_i;    /* FIXME: is this an overestimate ? */
    }

  return status;
}

static int
rk4imp_reset (void *vstate, size_t dim)
{
  rk4imp_state_t *state = (rk4imp_state_t *) vstate;

  DBL_ZERO_MEMSET (state->k1nu, dim);
  DBL_ZERO_MEMSET (state->k2nu, dim);
  DBL_ZERO_MEMSET (state->ytmp1, dim);
  DBL_ZERO_MEMSET (state->ytmp2, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk4imp_order (void *vstate)
{
  rk4imp_state_t *state = (rk4imp_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 4;
}

static void
rk4imp_free (void *vstate)
{
  rk4imp_state_t *state = (rk4imp_state_t *) vstate;
  free (state->k1nu);
  free (state->k2nu);
  free (state->ytmp1);
  free (state->ytmp2);
  free (state);
}

static const gsl_odeiv_step_type rk4imp_type = { "rk4imp",      /* name */
  1,                            /* can use dydt_in */
  0,                            /* gives exact dydt_out */
  &rk4imp_alloc,
  &rk4imp_apply,
  &rk4imp_reset,
  &rk4imp_order,
  &rk4imp_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk4imp = &rk4imp_type;


#include <math.h>
#include <string.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_sf_erf.h>
#include "prob_functions.h"
#include "data.h"

static const double ALPHA_PRIOR_MEAN = 1, BETA_PRIOR_MEAN = 1;

double my_f (const gsl_vector *x, void *params)
{
	Dataset *data = (Dataset *) params;

	unpackX(x, data);

	return - computeQ(data);
}

void my_df (const gsl_vector *x, void *params, gsl_vector *g)
{
	int i, j;

	Dataset *data = (Dataset *) params;
	double *dQdAlpha = (double *) malloc(sizeof(double) * data->numLabelers * data->numWorkflows);
	double *dQdBeta = (double *) malloc(sizeof(double) * data->numImages * data->numWorkflows);

	unpackX(x, data);

	gradientQ(data, dQdAlpha, dQdBeta);

	/* Pack dQdAlpha and dQdBeta into gsl_vector */
	for (i = 0; i < data->numLabelers * data->numWorkflows; i++) {
		gsl_vector_set(g, i, - dQdAlpha[i]);  /* Flip the sign since we want to minimize */
	}
	for (j = 0; j < data->numImages * data->numWorkflows; j++) {
	  gsl_vector_set(g, (data->numLabelers * data->numWorkflows) + j, - dQdBeta[j]);  /* Flip the sign since we want to minimize */
	}

	free(dQdAlpha);
	free(dQdBeta);
}

void my_fdf (const gsl_vector *x, void *params, double *f, gsl_vector *g)
{
	*f = my_f(x, params);
	my_df(x, params, g);
}

void gradientQ (Dataset *data, double *dQdAlpha, double *dQdBeta)
{
	int i, j;
	int idx;

	/* This comes from the priors */
	/*RandoPriors*/
	/*
	for (i = 0; i < data->numLabelers; i++) {
		dQdAlpha[i] = - 1 * (data->alpha[i] - data->priorAlpha[i]);
	}

	for (j = 0; j < data->numImages; j++) {
		dQdBeta[j] = - 5 * (data->beta[j] - data->priorBeta[j]);
	}
	*/
	/*Here's where the real shit starts*/
	for (idx = 0; idx < data->numLabels; idx++) {
		int i = data->labels[idx].labelerId;
		int j = data->labels[idx].imageIdx;
		int k = data->labels[idx].workflowId;
		int alphaidx = k * data->numLabelers + i;
		int betaidx = k * data->numImages + j;
		int lij = data->labels[idx].label;
		double sigma = prob(data->alpha[alphaidx], data->beta[betaidx]);

		double increa, increb;
		double weight = data->probZ1[j] * (lij - sigma) + data->probZ0[j] * (1 - lij - sigma);
		//double weight = data->probZ1[j] * (2 * lij - 1) + data->probZ0[j] * (-2 * lij + 1);
		increa = .5 * weight * log(1-data->beta[betaidx]) * pow( 1-data->beta[betaidx],
									 data->alpha[alphaidx] );
		increb = -.5 * weight * data->alpha[alphaidx] * pow( 1-data->beta[betaidx],
								     data->alpha[alphaidx]-1 );
		dQdAlpha[alphaidx] += increa;
		dQdBeta[betaidx] += increb;
		//printf("Alpha: %f Beta: %f", data->alpha[alphaidx], data->beta[betaidx]);
		//printf("Blah%f %f %f\n", log(1-data->beta[j]), 1-data->beta[j], data->alpha[i]);
		//printf("%d %d %d %f %f %f %f\n", i, j, lij, sigma, increa, increb, weight);
		//printf("%f %f\n", data->probZ1[j], data->probZ0[j]);
		/*
		if ( isnan(prob(data->alpha[i], data->beta[j])) )
{
printf("%d %d\n", i, j);
printf("%f %f\n", data->probZ1[j], data->probZ0[j]);
printf("alpha %f beta %f\n", data->alpha[i], data->beta[j]);
printf("sigma %f\n", sigma);
printf("label %d\n", lij);
printf("prob: %f\n", prob);
printf("%f\n", pow( 1-data->beta[j], data->alpha[i]-1 ));
printf("%f\n", data->alpha[i]);
printf("%f %f\n", increa, increb);
}
fflush(0);

	for (i = 0; i < data->numLabelers; i++) {
		printf("i %d %f %f\n", i, data->alpha[i], dQdAlpha[i]);
	}

	for (j = 0; j < data->numImages; j++) {
		printf("j %d %f %f\n", j, data->beta[j], dQdBeta[j]);
	}
		*/
	  }

}
/*
double computeLikelihood (Dataset *data)
{
	int i, j;
	int idx;
	double L = 0;

	double *alpha = data->alpha, *beta = data->beta;

	for (j = 0; j < data->numImages; j++) {
		double P1 = data->priorZ1[j];
		double P0 = 1 - data->priorZ1[j];
		for (idx = 0; idx < data->numLabels; idx++) {
			if (data->labels[idx].imageIdx == j) {
				int i = data->labels[idx].labelerId;
				int lij = data->labels[idx].label;
		double sigma = prob(alpha[i], beta[j]);
				P1 *= pow(sigma, lij) * pow(1 - sigma, 1 - lij);
				P0 *= pow(sigma, 1 - lij) * pow(1 - sigma, lij);
			}
		}
		L += log(P1 + P0);
	}

	/// Add Gaussian (standard normal) prior for alpha
	for (i = 0; i < data->numLabelers; i++) {
		L += log(gsl_sf_erf_Z(alpha[i] - data->priorAlpha[i]));
	}

	/// Add Gaussian (standard normal) prior for beta
	for (j = 0; j < data->numImages; j++) {
		L += log(gsl_sf_erf_Z(beta[j] - data->priorBeta[j]));
	}

	return L;
}*/

double computeQ (Dataset *data)
{
	int i, j;
	int idx;
	double Q = 0;

	double *alpha = data->alpha, *beta = data->beta;

	/* Start with the expectation of the sum of priors over all images */
	for (j = 0; j < data->numImages; j++) {
		Q += data->probZ1[j] * log(data->priorZ1[j]);
		Q += data->probZ0[j] * log(1 - data->priorZ1[j]);
	}

	for (idx = 0; idx < data->numLabels; idx++) {
		int i = data->labels[idx].labelerId;
		int j = data->labels[idx].imageIdx;
		int k = data->labels[idx].workflowId;
		int alphaidx = k * data->numLabelers + i;
		int betaidx = k * data->numImages + j;
		int lij = data->labels[idx].label;
		/* Do some analytic manipulation first for numerical stability! */
		double logSigma = log( prob(alpha[alphaidx], beta[betaidx]) );
		double logOneMinusSigma = log( 1-prob(alpha[alphaidx], beta[betaidx]) );
		if (beta[betaidx] > 1 || beta[betaidx] < 0 || alpha[alphaidx] < 0) {
		  Q += -100000000000000000;
		} else {
		  Q += data->probZ1[j] * (lij * logSigma + (1 - lij) * logOneMinusSigma) +
		    data->probZ0[j] * ((1 - lij) * logSigma + lij * logOneMinusSigma);

		}
		if (isnan(Q)) {
		  printf("%d %d %d %f %f %f %f %f %f %f %f\n", i, j, lij, logSigma,
			 logOneMinusSigma,
			 prob(alpha[i], beta[j]),
			 data->probZ1[j],
			 data->probZ0[j],
			 alpha[alphaidx],
			 beta[betaidx],
			 Q);
		  abort();
		}
	}

	/* Add Gaussian (standard normal) prior for alpha */
	for (i = 0; i < data->numLabelers * data->numWorkflows; i++) {
		//Q += log(gsl_sf_erf_Z(alpha[i] - data->priorAlpha[i]));
	}

	/* Add Gaussian (standard normal) prior for beta */
	for (j = 0; j < data->numImages * data->numWorkflows; j++) {
		//Q += log(gsl_sf_erf_Z(beta[j] - data->priorBeta[j]));
	}

	// add penalty of beta not between 0 and 1
	for (j = 0; j < data->numImages * data->numWorkflows; j++) {
	  if ( data->beta[j] > 1 || data->beta[j] < 0 ) {
	    Q -= 100;
	  }
	}
	return Q;
}

double logProbL (int l, int z, double alphaI, double betaJ)
{
	double p;

	if (z == l) {
		p = log(prob(alphaI, betaJ));
	} else {
		p = log(1-prob(alphaI, betaJ));
	}
/*
  printf("p: %f\n", prob(alphaI, betaJ) );
  printf("1-p: %f\n", 1-prob(alphaI, betaJ) );
  printf("log: %f\n", p);
*/
	return p;
}

double prob(double alpha, double beta)
{
  return .5 + .5 * pow(1-beta, alpha);
}

void EStep (Dataset *data)
{
	int j;
	int idx;
/*
  printf("begin Estep\n");
  fflush(0);
*/
	for (j = 0; j < data->numImages; j++) {
		data->probZ1[j] = log(data->priorZ1[j]);
		data->probZ0[j] = log(1 - data->priorZ1[j]);
		//printf("%d %f %f\n", j, data->probZ1[j], data->probZ0[j]);
	}

	for (idx = 0; idx < data->numLabels; idx++) {
		int i = data->labels[idx].labelerId;
		int j = data->labels[idx].imageIdx;
		int k = data->labels[idx].workflowId;
		int alphaidx = k * data->numLabelers + i;
		int betaidx = k * data->numImages + j;
		int lij = data->labels[idx].label;
		//printf("JOE%d %d %d %f %f\n", k, alphaidx, betaidx, data->alpha[alphaidx], data->beta[betaidx]);
		data->probZ1[j] += logProbL(lij, 1, data->alpha[alphaidx],
					    data->beta[betaidx]);
		data->probZ0[j] += logProbL(lij, 0, data->alpha[alphaidx],
					    data->beta[betaidx]);
		//printf("%d %d %f %f\n", j, lij, data->probZ1[j], data->probZ0[j]);
	}

	/* Exponentiate and renormalize */
	for (j = 0; j < data->numImages; j++) {
	  //printf("Before: %f %f\n", data->probZ1[j], data->probZ0[j]);
	  /*Chris adding code here to do this better */
	  double normalizingDenominator = data->probZ1[j] +
	    log(1 + exp(data->probZ0[j] -
			data->probZ1[j]));
	  data->probZ1[j] = data->probZ1[j] - normalizingDenominator;
	  data->probZ0[j] = data->probZ0[j] - normalizingDenominator;
	  //printf("Hmm: %f %f %f\n", normalizingDenominator, data->probZ1[j], data->probZ0[j]);
	  data->probZ1[j] = exp(data->probZ1[j]);
	  data->probZ0[j] = exp(data->probZ0[j]);
	  /*
	    data->probZ1[j] = exp(data->probZ1[j]);
	    data->probZ0[j] = exp(data->probZ0[j]);
	    printf("%f %f\n", data->probZ1[j], data->probZ0[j]);
	    data->probZ1[j] = data->probZ1[j] / (data->probZ1[j] + data->probZ0[j]);
	    data->probZ0[j] = 1 - data->probZ1[j];
	  */
	  //printf("%d %f %f\n", j, data->probZ1[j], data->probZ0[j]);
	  if (isnan(data->probZ1[j])) {
	    printf("ABORTING?");
	    abort();
	  }
	}
/*
  printf("end Estep\n");
  fflush(0);
*/
}

void MStep (Dataset *data)
{
	double lastF;
	int iter, status;
	int i, j;

	int numLabelers = data->numLabelers;
	int numImages = data->numImages;
	int numWorkflows = data->numWorkflows;

	gsl_vector *x;
	gsl_multimin_fdfminimizer *s;
	const gsl_multimin_fdfminimizer_type *T;
	gsl_multimin_function_fdf my_func;

	x = gsl_vector_alloc((numLabelers * numWorkflows) + (numImages * numWorkflows));

	/* Pack alpha and beta into a gsl_vector */
	packX(x, data);

	/* Initialize objective function */
	my_func.n = (numLabelers * numWorkflows) + (numImages * numWorkflows);
	my_func.f = &my_f;
	my_func.df = &my_df;
	my_func.fdf = &my_fdf;
	my_func.params = data;

	/* Initialize minimizer */
	T = gsl_multimin_fdfminimizer_conjugate_pr;
	s = gsl_multimin_fdfminimizer_alloc(T, (numLabelers * numWorkflows) + (numImages * numWorkflows));

	gsl_multimin_fdfminimizer_set(s, &my_func, x, 0.01, 0.01);
	iter = 0;
	do {
		lastF = s->f;
		iter++;
		/*printf("iter=%d f=%f\n", iter, s->f);*/

		status = gsl_multimin_fdfminimizer_iterate(s);
		if (status) {
			break;
		}
		status = gsl_multimin_test_gradient(s->gradient, 1e-3);
//	} while (lastF - s->f > 0.01 && status == GSL_CONTINUE && iter < 25);
	} while (iter < 5);

	/* Unpack alpha and beta from gsl_vector */
	unpackX(s->x, data);

	gsl_multimin_fdfminimizer_free(s);
	gsl_vector_free(x);
}


/*
 * BIG_DATA.c
 *
 *  Created on: 3 Sep 2020
 *      Author: heine
 */

#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>

#include "kalman_filter.h"
#include "particle_filters.h"

#define N_SETTINGS 10
#define N_SCALES 11

void init_with_zeros(double* array, long length);
void scale_sample_sizes(double scaler, long sample_size_collection[N_SETTINGS][2], int n_settings);
void particle_allocation_time_matching(int bpf_sample_size, double std_sig, double obs_std, int Nobs, int data_length, double x0, double *y, double *Rinv1, double *Rinv0, gsl_matrix *R);

int main(void) {

  gsl_rng * rng = gsl_rng_alloc(gsl_rng_taus);
  gsl_rng_set(rng,clock());

  double scaler;
  int N_top = 50;
  int N_bpf = 250;
  int run_time_matching = 0;
  int data_length = 50;
  int Nobs = 500;
  double sig_std = .1;
  double obs_std = 1;
  double x0 = 0;
  double p0 = sig_std * sig_std;
  double sample_size_coeff = (double)68000 / (double) 250;

  /* -----------------------------------------------------------------------------------
   *
   * Create data
   *
   -----------------------------------------------------------------------------------*/
  double *x = (double*) malloc(data_length * sizeof(double));
  double *y = (double*) malloc(data_length * Nobs * sizeof(double));
  double *noise = (double*) malloc(Nobs * sizeof(double));
  gsl_matrix * A;

  double x_prev = 0;

  /* Create the measurement covariance square root */
  A = gsl_matrix_alloc(Nobs, Nobs);
  for (int i = 0; i < Nobs; i++) {
    for (int j = 0; j < Nobs; j++) {
      /* Uncomment for identity matrix */
      /*
       *       if (i == j) {
       *         gsl_matrix_set(A, i, j, 1);
       *       } else {
       *         gsl_matrix_set(A, i, j, 0);
       } */
//      gsl_matrix_set(A, i, j, gsl_ran_gaussian(rng, 1));
      gsl_matrix_set(A, i, j, gsl_rng_uniform(rng));
    }
  }

  /* Calculate the observation covariance */
  gsl_matrix * R = gsl_matrix_alloc(Nobs, Nobs);
  double tmp;
  double decay = 2; // Decay of correlations
  FILE * R_out = fopen("R_matrix.txt", "w");
  for (int i = 0; i < Nobs; i++) {
    for (int j = 0; j < Nobs; j++) {
      tmp = 0;
      for (int k = 0; k < Nobs; k++) {
        tmp += gsl_matrix_get(A, i, k) * gsl_matrix_get(A, j, k);
      }
      tmp = obs_std * obs_std * exp(-decay * (double) abs(i - j)) * tmp;
      gsl_matrix_set(R, i, j, tmp);
      fprintf(R_out, "%e ", tmp);
    }
    fprintf(R_out, "\n");
  }
  fclose(R_out);

  // Copy R to A
  for (int i = 0; i < Nobs; i++) {
    for (int j = 0; j < Nobs; j++) {
      gsl_matrix_set(A, i, j, gsl_matrix_get(R, i, j));
    }
  }
  gsl_linalg_cholesky_decomp1(A);

  printf("Obsevation covariance written in R_matrix.txt\n");

  /* Main loop for generating the signal and the data */
  FILE *data_file = fopen("data.txt", "w");
  for (int n = 0; n < data_length; n++) {

    x[n] = x_prev + gsl_ran_gaussian(rng, sig_std);
    x_prev = x[n];
    fprintf(data_file, "%i %e", n, x[n]);

    /* Uncorrelated observation noise */
    for (int i = 0; i < Nobs; i++) {
      noise[i] = gsl_ran_gaussian(rng, obs_std);
    }
    /* Make the noise correlated by multiplying by A */
    for (int i = 0; i < Nobs; i++) {
      y[n * Nobs + i] = x[n];
      for (int j = 0; j <= i; j++)
        y[n * Nobs + i] += gsl_matrix_get(A, i, j) * noise[j];
      fprintf(data_file, " %e,", y[n * Nobs + i]);
    }
    fprintf(data_file, "\n");

  }
  fclose(data_file);
  printf("Signal and data of length %i generated and written in data.txt\n", data_length);

  /*
   * Calculate the inverse observation covariances
   */
  printf("Computing the observation covariance inverse...\n");
  fflush(stdout);
  clock_t start = clock();

  /* Observation noise covariance */
  gsl_vector * eval = gsl_vector_alloc(Nobs);
  gsl_matrix * evec = gsl_matrix_alloc(Nobs, Nobs);
  double * Rinv1 = (double*) malloc(Nobs * Nobs * sizeof(double));
  gsl_eigen_symmv_workspace * workspace = gsl_eigen_symmv_alloc(Nobs);

  /* Compute the eigen decomposition of R to get the inverse */
  gsl_eigen_symmv(R, eval, evec, workspace);
  FILE* rinv_out = fopen("R_inv.txt", "w");
  for (int i = 0; i < Nobs; i++) {
    for (int k = 0; k < Nobs; k++) {
      tmp = 0;
      for (int j = 0; j < Nobs; j++) {
        tmp += gsl_matrix_get(evec, i, j) * gsl_matrix_get(evec, k, j)
        / gsl_vector_get(eval, j);

      }
      Rinv1[i * Nobs + k] = tmp;
      fprintf(rinv_out, "%e, ", tmp);
    }
    fprintf(rinv_out, "\n");
  }
  fclose(rinv_out);
  printf("Observation covariance inverse computed and written in R_inv.txt\n");

  gsl_matrix_free(evec);
  gsl_vector_free(eval);
  gsl_eigen_symmv_free(workspace);

  /* Limit the observation correlation to a band and ... */
  int band = 0; // Note we only consider diagonal, i.e. band = 0.
  gsl_matrix * R_low = gsl_matrix_alloc(Nobs, Nobs);
  for (int i = 0; i < Nobs; i++) {
    for (int j = 0; j < Nobs; j++) {
      tmp = abs(i - j) <= band ? gsl_matrix_get(R, i, j) : 0;
      gsl_matrix_set(R_low, i, j, tmp);
    }
  }
  /* ...take inverse */
  gsl_vector * eval_low = gsl_vector_alloc(Nobs);
  gsl_matrix * evec_low = gsl_matrix_alloc(Nobs, Nobs);
  gsl_eigen_symmv_workspace * workspace_low = gsl_eigen_symmv_alloc(Nobs);
  double * Rinv0 = (double*) malloc(Nobs * Nobs * sizeof(double));

  gsl_eigen_symmv(R_low, eval_low, evec_low, workspace_low);

  FILE * band_R_out = fopen("R_band_inv.txt", "w");
  for (int i = 0; i < Nobs; i++) {
    for (int k = 0; k < Nobs; k++) {
      tmp = 0;
      for (int j = 0; j < Nobs; j++) {
        tmp += gsl_matrix_get(evec_low, i, j) * gsl_matrix_get(evec_low, k, j)
        / gsl_vector_get(eval_low, j);

      }
      Rinv0[i * Nobs + k] = tmp;
      fprintf(band_R_out, "%e ", tmp);
    }
    fprintf(band_R_out, "\n");
  }
  fclose(band_R_out);
  printf(
         "Inverse of the diagonal covariance approximation computed and written in R_band_inv.txt\n");

  gsl_matrix_free(evec_low);
  gsl_vector_free(eval_low);
  gsl_eigen_symmv_free(workspace_low);

  printf("elapsed %5.2f sec\n", (double) (clock() - start) / CLOCKS_PER_SEC);

  if (run_time_matching == 1) {
    /*
     * Particle allocation time matching
     *
     * This part of the code should be run when trying to find the correct N0 sample sizes to match
     * the BPF running time
     */

    particle_allocation_time_matching(N_bpf, sig_std, obs_std, Nobs, data_length, x0, y, Rinv1, Rinv0, R);

    printf("Particle allocation time matching done! Exiting...\n");
    return 0; // the code exits
  }

  /* ------------------------------------------------------------
   * Kalman filter
   * --------------------------------------------------------- */
  double *x_hat = (double*) malloc(data_length * sizeof(double));
  double *p_hat = (double*) malloc(data_length * sizeof(double));

  kalman_filter(x, y, Rinv1, sig_std, obs_std, x_hat, p_hat, data_length, x0, p0, Nobs);

  /* Write KF results in a file */
  FILE *kf_out = fopen("kf_out.txt", "w");
  for (int i = 0; i < data_length; i++)
    fprintf(kf_out, "%i %f %f %f\n", i, x[i], x_hat[i], p_hat[i]);
  fclose(kf_out);

  /*
   * Top level loop
   * */
  double scalers[N_SCALES] = {0.5,1,2,3,4,5,6,7,8,9,10};
  short N_levels = 2;
  long sample_sizes[2] = {0,0};
  long sample_size[1] = { N_bpf }; // This is for one level BPF

  double *x_hat_bpf = (double*) malloc(data_length * sizeof(double));
  double *errors = (double *) malloc((N_SETTINGS + 1) * sizeof(double));
  double *times = (double *) malloc((N_SETTINGS + 1) * sizeof(double));
  init_with_zeros(errors, N_SETTINGS + 1);
  init_with_zeros(times, N_SETTINGS + 1);

  double error;
  FILE * error_out = fopen("error.txt", "w");

  double filtering_time[4];
  double worst_case_sign_ratio[1] = { 100 }; // This is basically sustitute for "Inf"
  long unilevel_sample_size = N_bpf;
  long error_matched_unilevel_sample_size = 7 * N_bpf;
  int N1, N0;
  double step;

  /*
   * MAIN LOOP FOR TOP LEVEL MONTE CARLO
   */
  for (int i = 0; i < N_top; i++) {

    printf("Top level iteration %i\n", i);

    for (int k = 0; k < N_SCALES; k++) {

      scaler = scalers[k];

      printf("Scaler %f\n", scaler);

      /* Run the basic BPF (time matched) */
      sample_size[0] = (long) round(unilevel_sample_size * scaler);
      bootstrapfilter(y, sig_std, obs_std, data_length, x0, Nobs, sample_size[0],
                      x_hat_bpf, filtering_time, Rinv1, 1);

      /* Sum of squared errors over the whole signal */
      error = 0;
      for (int n = 0; n < data_length; n++) {
        error += (x_hat[n] - x_hat_bpf[n]) * (x_hat[n] - x_hat_bpf[n]);
      }

      fprintf(error_out, "%e %i %lu %f %i %e %e %f %e %f\n",
              error / (double) data_length, 0, sample_size[0], filtering_time[0], 0,
              filtering_time[1], filtering_time[2], worst_case_sign_ratio[0],
              filtering_time[3], scaler);
      fflush(error_out);

      if(scaler < 8) {
        /* Run the basic BPF (error matched) */
        sample_size[0] = (long) round(error_matched_unilevel_sample_size * scaler);
        bootstrapfilter(y, sig_std, obs_std, data_length, x0, Nobs, sample_size[0],
                        x_hat_bpf, filtering_time, Rinv1, 1);

        /* Sum of squared errors over the whole signal */
        error = 0;
        for (int n = 0; n < data_length; n++) {
          error += (x_hat[n] - x_hat_bpf[n]) * (x_hat[n] - x_hat_bpf[n]);
        }

        fprintf(error_out, "%e %i %lu %f %i %e %e %f %e %f\n",
                error / (double) data_length, 0, sample_size[0], filtering_time[0], -1,
                filtering_time[1], filtering_time[2], worst_case_sign_ratio[0],
                filtering_time[3], scaler);
        fflush(error_out);
      }

      /* Iterate over different configurations */
      N1 = 0;
      for (int j = 0; j < N_SETTINGS; j++) {

        step = (double)(N_bpf - 5) / (double)(N_SETTINGS-1);
        N1 = (int) j * step;
        N0 = (int) floor((N_bpf-N1)*sample_size_coeff);
        worst_case_sign_ratio[0] = 100;

        sample_sizes[0] = N0 * scaler;
        sample_sizes[1] = N1 * scaler;

        printf("N0 = %lu, N1 = %lu, N = %lu\n", sample_sizes[0], sample_sizes[1], sample_sizes[0]+sample_sizes[1]);

        MLbootstrapfilter(y, sig_std, obs_std, data_length, x0, Nobs, N_levels,
                          sample_sizes, x_hat_bpf, filtering_time, Rinv0, Rinv1,
                          worst_case_sign_ratio);

        /* Sum of squared errors over the whole signal */
        error = 0;
        for (int n = 0; n < data_length; n++) {
          error += (x_hat[n] - x_hat_bpf[n]) * (x_hat[n] - x_hat_bpf[n]);
        }

        fprintf(error_out, "%e %lu %lu %f %i %e %e %f %e %f\n",
                error / (double) data_length, sample_sizes[0],
                sample_sizes[1], filtering_time[0], j+1, filtering_time[1],
                filtering_time[2], worst_case_sign_ratio[0], filtering_time[3], scaler);

      }
      fflush(error_out);
    }

  }

  fclose(error_out);

  printf("Results of all BPF runs written in error.txt\n");
  printf("Error summary written in error_summary.txt\n");

  return 0;
}

void init_with_zeros(double *array, long length) {
  for (int i = 0; i < length; i++)
    array[i] = 0;
}

void scale_sample_sizes(double scaler, long sample_size_collection[N_SETTINGS][2], int n_settings) {

  for (int i = 0; i < n_settings; i++)
    for (int j = 0; j < 1; j++)
      sample_size_collection[i][j] = (long) round(scaler * sample_size_collection[i][j]);

}

void particle_allocation_time_matching(int bpf_sample_size, double std_sig, double obs_std, int Nobs, int data_length, double x0, double *y, double *Rinv1, double *Rinv0, gsl_matrix *R) {

  int N1_step = 20;
  int N0_step = 10000;
  int N0_init = 10;
  int N_top = 5;
  double time_per_iteration;
  double time_tolerance = 1.00; //
  double filtering_time[1];
  double worst_case_sign_ratio[1] = { 100 };

  double *x_hat_bpf = (double*) malloc(data_length * sizeof(double));

  /* Find the reference time per iteration for BPF */
  double bpf_time_per_iteration = 0;

  for(int nmc = 0; nmc < N_top; nmc++) {
    bootstrapfilter(y, std_sig, obs_std, data_length, x0, Nobs, bpf_sample_size,
                    x_hat_bpf, filtering_time, Rinv1, 1);
    bpf_time_per_iteration += filtering_time[0] / (double) N_top;
  }

  printf("Time per iteration (BPF): %0.5f\n",bpf_time_per_iteration);
  fflush(stdout);

  long sample_sizes[2] = {0,0};
  int N0;
  short direction;

  FILE* out = fopen("time_matched_sample_sizes.txt","w");

  /* For given L0 mesh, iterate over N1 */
  for(int N1 = 0; N1 <= bpf_sample_size ; N1 = N1 + N1_step) {

    N0 = N0_init;
    N0_step = 10000;

    /* For given L0 mesh and N1 iterate over increasing N0 until MLBPF time per iteration exceeds
     BPF time per iteration */
    time_per_iteration = 0;

    direction = 1; // up
    while(1) {

      sample_sizes[0] = N0;
      sample_sizes[1] = N1;

      /* Estimate the time per iteration by averaging N_top runs */
      time_per_iteration = 0;
      for(int nmc = 0; nmc < N_top; nmc++) {

        MLbootstrapfilter(y, std_sig, obs_std, data_length, x0, Nobs, 2,
                          sample_sizes, x_hat_bpf, filtering_time, Rinv0, Rinv1,
                          worst_case_sign_ratio);

        time_per_iteration += filtering_time[0] / (double) N_top;
      }

      if (direction == 1 && time_per_iteration > bpf_time_per_iteration * time_tolerance && N0_step > 10) {
        direction = -1; // start going down
        N0_step /= 2; // halve the step

      } else if (direction == -1 && time_per_iteration < bpf_time_per_iteration * 1.00 && N0_step > 10) {
        direction = 1;
        N0_step /= 2;
      } else if (N0_step < 10) {
        break;
      }
      N0 = N0 + direction * N0_step;

      if(N0 < 1) { // Make sure N0 remains positive and go up if this happens
        N0 = 1;
        direction = 1;
      }

    }
    fprintf(out,"%i %i %i %i\n",N0-N0_step,N1,0,0);
    fflush(out);
  }

  fclose(out);
  free(x_hat_bpf);
}


/* integration/qaws.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 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 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.
 */

#include <config.h>
#include <math.h>
#include <float.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

#include "initialise.c"
#include "append.c"
#include "qpsrt.c"
#include "util.c"
#include "qc25s.c"

int
gsl_integration_qaws (gsl_function * f,
                      const double a, const double b,
                      gsl_integration_qaws_table * t,
                      const double epsabs, const double epsrel,
                      const size_t limit,
                      gsl_integration_workspace * workspace,
                      double *result, double *abserr)
{
  double area, errsum;
  double result0, abserr0;
  double tolerance;
  size_t iteration = 0;
  int roundoff_type1 = 0, roundoff_type2 = 0, error_type = 0;

  /* Initialize results */

  initialise (workspace, a, b);

  *result = 0;
  *abserr = 0;

  if (limit > workspace->limit)
    {
      GSL_ERROR ("iteration limit exceeds available workspace", GSL_EINVAL) ;
    }

  if (b <= a)
    {
      GSL_ERROR ("limits must form an ascending sequence, a < b", GSL_EINVAL) ;
    }

  if (epsabs <= 0 && (epsrel < 50 * GSL_DBL_EPSILON || epsrel < 0.5e-28))
    {
      GSL_ERROR ("tolerance cannot be acheived with given epsabs and epsrel",
                 GSL_EBADTOL);
    }

  /* perform the first integration */

  {
    double area1, area2;
    double error1, error2;
    int err_reliable1, err_reliable2;
    double a1 = a;
    double b1 = 0.5 * (a + b);
    double a2 = b1;
    double b2 = b;

    qc25s (f, a, b, a1, b1, t, &area1, &error1, &err_reliable1);
    qc25s (f, a, b, a2, b2, t, &area2, &error2, &err_reliable2);

    if (error1 > error2)
      {
        append_interval (workspace, a1, b1, area1, error1);
        append_interval (workspace, a2, b2, area2, error2);
      }
    else
      {
        append_interval (workspace, a2, b2, area2, error2);
        append_interval (workspace, a1, b1, area1, error1);
      }

    result0 = area1 + area2;
    abserr0 = error1 + error2;
  }

  /* Test on accuracy */

  tolerance = GSL_MAX_DBL (epsabs, epsrel * fabs (result0));

  /* Test on accuracy, use 0.01 relative error as an extra safety
     margin on the first iteration (ignored for subsequent iterations) */

  if (abserr0 < tolerance && abserr0 < 0.01 * fabs(result0))
    {
      *result = result0;
      *abserr = abserr0;

      return GSL_SUCCESS;
    }
  else if (limit == 1)
    {
      *result = result0;
      *abserr = abserr0;

      GSL_ERROR ("a maximum of one iteration was insufficient", GSL_EMAXITER);
    }

  area = result0;
  errsum = abserr0;

  iteration = 2;

  do
    {
      double a1, b1, a2, b2;
      double a_i, b_i, r_i, e_i;
      double area1 = 0, area2 = 0, area12 = 0;
      double error1 = 0, error2 = 0, error12 = 0;
      int err_reliable1, err_reliable2;

      /* Bisect the subinterval with the largest error estimate */

      retrieve (workspace, &a_i, &b_i, &r_i, &e_i);

      a1 = a_i;
      b1 = 0.5 * (a_i + b_i);
      a2 = b1;
      b2 = b_i;

      qc25s (f, a, b, a1, b1, t, &area1, &error1, &err_reliable1);
      qc25s (f, a, b, a2, b2, t, &area2, &error2, &err_reliable2);

      area12 = area1 + area2;
      error12 = error1 + error2;

      errsum += (error12 - e_i);
      area += area12 - r_i;

      if (err_reliable1 && err_reliable2)
        {
          double delta = r_i - area12;

          if (fabs (delta) <= 1.0e-5 * fabs (area12) && error12 >= 0.99 * e_i)
            {
              roundoff_type1++;
            }
          if (iteration >= 10 && error12 > e_i)
            {
              roundoff_type2++;
            }
        }

      tolerance = GSL_MAX_DBL (epsabs, epsrel * fabs (area));

      if (errsum > tolerance)
        {
          if (roundoff_type1 >= 6 || roundoff_type2 >= 20)
            {
              error_type = 2;   /* round off error */
            }

          /* set error flag in the case of bad integrand behaviour at
             a point of the integration range */

          if (subinterval_too_small (a1, a2, b2))
            {
              error_type = 3;
            }
        }

      update (workspace, a1, b1, area1, error1, a2, b2, area2, error2);

      retrieve (workspace, &a_i, &b_i, &r_i, &e_i);

      iteration++;

    }
  while (iteration < limit && !error_type && errsum > tolerance);

  *result = sum_results (workspace);
  *abserr = errsum;

  if (errsum <= tolerance)
    {
      return GSL_SUCCESS;
    }
  else if (error_type == 2)
    {
      GSL_ERROR ("roundoff error prevents tolerance from being achieved",
                 GSL_EROUND);
    }
  else if (error_type == 3)
    {
      GSL_ERROR ("bad integrand behavior found in the integration interval",
                 GSL_ESING);
    }
  else if (iteration == limit)
    {
      GSL_ERROR ("maximum number of subdivisions reached", GSL_EMAXITER);
    }
  else
    {
      GSL_ERROR ("could not integrate function", GSL_EFAILED);
    }

}


/* eigen/gsl_eigen.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, 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.
 */

#ifndef __GSL_EIGEN_H__
#define __GSL_EIGEN_H__

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct {
  size_t size;
  double * d;
  double * sd;
} gsl_eigen_symm_workspace;

gsl_eigen_symm_workspace * gsl_eigen_symm_alloc (const size_t n);
void gsl_eigen_symm_free (gsl_eigen_symm_workspace * w);
int gsl_eigen_symm (gsl_matrix * A, gsl_vector * eval, gsl_eigen_symm_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * gc;
  double * gs;
} gsl_eigen_symmv_workspace;

gsl_eigen_symmv_workspace * gsl_eigen_symmv_alloc (const size_t n);
void gsl_eigen_symmv_free (gsl_eigen_symmv_workspace * w);
int gsl_eigen_symmv (gsl_matrix * A, gsl_vector * eval, gsl_matrix * evec, gsl_eigen_symmv_workspace * w);

typedef struct {
  size_t size;           /* matrix size */
  size_t max_iterations; /* max iterations since last eigenvalue found */
  size_t n_iter;         /* number of iterations since last eigenvalue found */
  size_t n_evals;        /* number of eigenvalues found so far */

  gsl_vector *hv2;       /* temporary 2-by-1 householder vector */
  gsl_vector *hv3;       /* temporary 3-by-1 householder vector */

  int compute_t;         /* compute Schur form T = Z^t A Z */

  gsl_matrix *H;         /* pointer to Hessenberg matrix */

  gsl_matrix *Z;         /* pointer to Schur vector matrix */
} gsl_eigen_francis_workspace;

gsl_eigen_francis_workspace * gsl_eigen_francis_alloc (void);
void gsl_eigen_francis_free (gsl_eigen_francis_workspace * w);
void gsl_eigen_francis_T (const int compute_t,
                          gsl_eigen_francis_workspace * w);
int gsl_eigen_francis (gsl_matrix * H, gsl_vector_complex * eval,
                       gsl_eigen_francis_workspace * w);
int gsl_eigen_francis_Z (gsl_matrix * H, gsl_vector_complex * eval,
                         gsl_matrix * Z,
                         gsl_eigen_francis_workspace * w);

typedef struct {
  size_t size;                 /* size of matrices */
  gsl_vector *diag;            /* diagonal matrix elements from balancing */
  gsl_vector *tau;             /* Householder coefficients */
  gsl_matrix *Z;               /* pointer to Z matrix */
  int do_balance;              /* perform balancing transformation? */
  size_t n_evals;              /* number of eigenvalues found */

  gsl_eigen_francis_workspace *francis_workspace_p;
} gsl_eigen_nonsymm_workspace;

gsl_eigen_nonsymm_workspace * gsl_eigen_nonsymm_alloc (const size_t n);
void gsl_eigen_nonsymm_free (gsl_eigen_nonsymm_workspace * w);
void gsl_eigen_nonsymm_params (const int compute_t, const int balance,
                               gsl_eigen_nonsymm_workspace *w);
int gsl_eigen_nonsymm (gsl_matrix * A, gsl_vector_complex * eval,
                       gsl_eigen_nonsymm_workspace * w);
int gsl_eigen_nonsymm_Z (gsl_matrix * A, gsl_vector_complex * eval,
                         gsl_matrix * Z, gsl_eigen_nonsymm_workspace * w);

typedef struct {
  size_t size;                 /* size of matrices */
  gsl_vector *work;            /* scratch workspace */
  gsl_vector *work2;           /* scratch workspace */
  gsl_vector *work3;           /* scratch workspace */

  gsl_matrix *Z;               /* pointer to Schur vectors */

  gsl_eigen_nonsymm_workspace *nonsymm_workspace_p;
} gsl_eigen_nonsymmv_workspace;

gsl_eigen_nonsymmv_workspace * gsl_eigen_nonsymmv_alloc (const size_t n);
void gsl_eigen_nonsymmv_free (gsl_eigen_nonsymmv_workspace * w);
int gsl_eigen_nonsymmv (gsl_matrix * A, gsl_vector_complex * eval,
                        gsl_matrix_complex * evec,
                        gsl_eigen_nonsymmv_workspace * w);
int gsl_eigen_nonsymmv_Z (gsl_matrix * A, gsl_vector_complex * eval,
                          gsl_matrix_complex * evec, gsl_matrix * Z,
                          gsl_eigen_nonsymmv_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * tau;
} gsl_eigen_herm_workspace;

gsl_eigen_herm_workspace * gsl_eigen_herm_alloc (const size_t n);
void gsl_eigen_herm_free (gsl_eigen_herm_workspace * w);
int gsl_eigen_herm (gsl_matrix_complex * A, gsl_vector * eval,
                         gsl_eigen_herm_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * tau;
  double * gc;
  double * gs;
} gsl_eigen_hermv_workspace;

gsl_eigen_hermv_workspace * gsl_eigen_hermv_alloc (const size_t n);
void gsl_eigen_hermv_free (gsl_eigen_hermv_workspace * w);
int gsl_eigen_hermv (gsl_matrix_complex * A, gsl_vector * eval,
                           gsl_matrix_complex * evec,
                           gsl_eigen_hermv_workspace * w);

typedef enum {
  GSL_EIGEN_SORT_VAL_ASC,
  GSL_EIGEN_SORT_VAL_DESC,
  GSL_EIGEN_SORT_ABS_ASC,
  GSL_EIGEN_SORT_ABS_DESC
}
gsl_eigen_sort_t;

/* Sort eigensystem results based on eigenvalues.
 * Sorts in order of increasing value or increasing
 * absolute value.
 *
 * exceptions: GSL_EBADLEN
 */

int gsl_eigen_symmv_sort(gsl_vector * eval, gsl_matrix * evec,
                         gsl_eigen_sort_t sort_type);

int gsl_eigen_hermv_sort(gsl_vector * eval, gsl_matrix_complex * evec,
                         gsl_eigen_sort_t sort_type);

int gsl_eigen_nonsymmv_sort(gsl_vector_complex * eval,
                            gsl_matrix_complex * evec,
                            gsl_eigen_sort_t sort_type);

/* The following functions are obsolete: */

/* Eigensolve by Jacobi Method
 *
 * The data in the matrix input is destroyed.
 *
 * exceptions:
 */
int
gsl_eigen_jacobi(gsl_matrix * matrix,
                      gsl_vector * eval,
                      gsl_matrix * evec,
                      unsigned int max_rot,
                      unsigned int * nrot);

/* Invert by Jacobi Method
 *
 * exceptions:
 */
int
gsl_eigen_invert_jacobi(const gsl_matrix * matrix,
                             gsl_matrix * ainv,
                             unsigned int max_rot);

__END_DECLS

#endif /* __GSL_EIGEN_H__ */


#ifdef HAVE_PETSC
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>

#include <mpi.h>
#include <petsc.h>

#include "common_c.h"
#include "FCMangle.h"

#define fillsparsecpetscs FortranCInterface_GLOBAL_(fillsparsecpetscs, FILLSPARSECPETSCS)
#define fillsparsecpetscc FortranCInterface_GLOBAL_(fillsparsecpetscc, FILLSPARSECPETSCC)

#define COLMAJ2D(row,col,numrow) (row-1)+(col-1)*numrow
#define COLMAJ3D(a,b,c,amax,bmax,cmax) (a-1)+amax*((b-1)+bmax*(c-1))
#define ROWMAJ2D_ONE(row,col,numcol) (row-1)*numcol+(col-1)
typedef long long int gcorp_t;

void fillsparsecpetscs(gcorp_t* ieng, double* EGmass, Mat* lhsP)
{
        int npro = propar.npro;
        int nshl = shpdat.nshl;
        double* mb = (double*) malloc(sizeof(double)*nshl*nshl); //block to insert
        int e,i,j,aa; //following along with fillsparse.f
        PetscInt* locat = (PetscInt*) malloc(sizeof(PetscInt)*nshl);
        for(e=0;e<npro;e++)
        {
         for(aa=0;aa<nshl;aa++) locat[aa]=ieng[e+npro*aa]-1;
//         for(aa=0;aa<nshl;aa++) assert(locat[aa]>=0);
         for (i=0; i<nshl; i++)  {   // fill up Ke with respective egmass
           for (j=0; j<nshl; j++)  {
            mb[nshl*i + j] = EGmass[e + npro*(i + nshl*j)];
           }
         }
         //MatSetValuesBlocked(*lhsP, nshl , locat, nshl, locat, mb, ADD_VALUES);
         PetscInt petsc_nshl;
         petsc_nshl = (PetscInt) nshl;
         MatSetValues(*lhsP, petsc_nshl , locat, petsc_nshl, locat, mb, ADD_VALUES);
        }
        free(mb);
	free(locat);
}
void fillsparsecpetscc(gcorp_t* ieng, double* EGmass, Mat* lhsP)
{
        int npro = propar.npro;
        int nshl = shpdat.nshl;
        int nedof = conpar.nedof;
        double* mb = (double*) malloc(sizeof(double)*nedof*nedof); //block to insert
        int e,i,j,aa; //following along with fillsparse.f
        //int* locat = (int*) malloc(sizeof(int)*nshl);
        PetscInt* locat = (PetscInt*) malloc(sizeof(PetscInt)*nshl);
        for(e=0;e<npro;e++)
        {
         for(aa=0;aa<nshl;aa++) locat[aa]=ieng[e+npro*aa]-1;
//         for(aa=0;aa<nshl;aa++) assert(locat[aa]>=0);
         for (i=0; i<nedof; i++)  {   /* fill up Ke with respective egmass */
           for (j=0; j<nedof; j++)  {
            mb[nedof*i + j] = EGmass[e + npro*(i + nedof*j)];
           }
         }
         //MatSetValuesBlocked(*lhsP, nshl , locat, nshl, locat, mb, ADD_VALUES);
         PetscInt petsc_nshl;
         petsc_nshl = (PetscInt) nshl;
         MatSetValuesBlocked(*lhsP, petsc_nshl , locat, petsc_nshl, locat, mb, ADD_VALUES);
        }
        free(mb);
	free(locat);
}
#endif


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "funcs.h"
#include "initial.h"

#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_ieee_utils.h>

int main ( int argc, char* argv[] )
{
	gsl_ieee_env_setup () ;			/* read GSL_IEEE_MODE */

	printf("Temperature: %g\n\n", T*HBAR*Delta/BOLTZ) ;

	double beta = 1.0/T ;           	/* Boltzmann factor: beta */
	double omega_1 = gsl_hypot(OMEGA,D) ;   /* omega' */

	struct f_params params;
	params.omega_c = omega_c ;
	params.beta = beta ;
	params.Omega = OMEGA ;
	params.omega_1 = omega_1 ;
	params.alpha = alpha ;

	int status1 = save_integrals ( &params ) ;
	int status2 = save_matrices ( &params ) ;

	/* read the Redfield matrix from a file */
	gsl_matrix* red_m = gsl_matrix_calloc ( 4, 4 ) ;
	int status3 = mat_read ( red_m, "REDFIELD_MATRIX" ) ;

	/* read the CP matrix from a file */
	gsl_matrix* cp_m = gsl_matrix_calloc ( 4, 4 ) ;
	int status4 = mat_read ( cp_m, "CP_MATRIX" ) ;

	/* Hamiltonian generator */
	const double om[] = { 0 , 0 , 0 , omega_1/2 } ;
	gsl_matrix* H = gsl_matrix_calloc ( 4, 4 ) ;
	int status5 = ham_gen ( H, om ) ;

	/*
	 * Find the stationary state by solving the linear systems
	 * and the associated stationary currents
	 */
	gsl_vector* req_red = gsl_vector_calloc (4) ;
	gsl_vector* req_cp = gsl_vector_calloc (4) ;

	printf("REDFIELD DYNAMICS\n") ;
	int status6 = stationary ( red_m , req_red ) ;
	printf("Stationary state: ( %.1f , %.9f , %.9f , %.9f )\n",
			VECTOR(req_red,0), VECTOR(req_red,1),
			VECTOR(req_red,2), VECTOR(req_red,3) ) ;
	printf("Stationary normalized (I/I0) DC current: %.9f\n\n",
			-VECTOR(req_red,3)*OMEGA/omega_1) ;

	printf("CP DYNAMICS\n") ;
	int status7 = stationary ( cp_m , req_cp ) ;
	printf("Stationary state: ( %.1f , %.9f , %.9f , %.9f )\n",
			VECTOR(req_cp,0), VECTOR(req_cp,1),
			VECTOR(req_cp,2), VECTOR(req_cp,3) ) ;
	printf("Stationary normalized (I/I0) DC current: %.9f\n\n",
			-VECTOR(req_cp,3)*OMEGA/omega_1) ;

	/* Save the stationary states into files */
	FILE* f = fopen ( "RED_STATIONARY.dat", "w" ) ;
	gsl_vector_fprintf ( f, req_red, "%.9f" ) ;
	FILE* g = fopen ( "CP_STATIONARY.dat", "w" ) ;
	gsl_vector_fprintf ( g, req_cp, "%.9f" ) ;

	fclose (f) ; fclose (g) ;

	/* polarization */
	double D30 = gsl_matrix_get(cp_m,3,0) ;
	double D33 = gsl_matrix_get(cp_m,3,3) ;
	double pol = -D30/D33 ;
	printf("n-Polarization of the CP dynamics -D30/D33: %.9f\n", pol ) ;

	/* polarization through the formula (for checking) */
	double P = polarization ( &params, omega_c ) ;
	printf("\n") ;
	printf("n-Polarization through the given formula: %.9f\n", P ) ;
	printf("Stationary normalized (I/I0) DC current: %.9f\n", -P*OMEGA/omega_1 ) ;

	/* free memory for matrices */
	gsl_matrix_free(red_m) ;
 	gsl_matrix_free(cp_m) ;

	return status1 + status2 + status3 + status4 + status5 + status6 + status7 ;
}


#ifndef PRIMAL_DUAL_LOOPLESS_KATYUSHA0_H
#define PRIMAL_DUAL_LOOPLESS_KATYUSHA0_H

#include "Primal_Dual_LOOPLESS.h"

#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <sstream>

//This class implements the method loopless kATYUSHA with arbitrary sampling

/*
The optimization problem to solve is:

\sum_{i=1}^n \lambda_i\phi_i(A_i^{\top} w)+ g(w)
Assumption 1: For each i, \phi_i is 1-smooth
By default, lambda_i=1/n for all i.
*
* The dual problem is
*         g*(-sum_{i=1}^n \lambda_i \alpha_i A_i)+\sum_{i=1}^n \lambda_i \phi*_i(\alpha_i)
*/

template<typename L, typename D>
class Primal_Dual_LOOPLESS_Katyusha0: public Primal_Dual_LOOPLESS<L, D>
{

protected:
   D theta1;
   D theta2;
   D theta3;
   D eta; //the stepsize parameter alpha in the paper

   D x;
   D w;
   D y;
   D z;

   std::vector<D> primal_y;     // the y variable

   std::vector<D> primal_z;     // the z variable

   std::vector<D> primal_w;     // the w variable

public:

  virtual inline D gradient_of_phi_i(D, L){return D(NULL);}
  virtual inline D gradient_of_gstar_j(D, L){return D(NULL);}
  virtual inline D value_of_phi_i(D, L) {return D(NULL);}
  virtual inline D value_of_g_j(D, L){return D(NULL);}
  virtual inline D value_of_phistar_i(D,L) {return D(NULL);}
  virtual inline D value_of_gstar(vector<D> &){return D(NULL);}
  virtual inline D value_of_gstar_minus(vector<D> &, D){return D(NULL);}
  virtual inline D feasible_dual(vector<D> &){return D(NULL);}
  virtual inline D compute_delta_alpha(D,D,L){return D(NULL);}
  virtual inline void set_auxiliary_v(){}
  virtual inline D get_lambda1(){return D(NULL);}
  virtual inline D get_lambda2(){return D(NULL);}
  virtual inline void compute_just_in_time_prox_grad(D, D, D &, L,L, D, D &, L){}

  Primal_Dual_LOOPLESS_Katyusha0()
  : Primal_Dual_LOOPLESS<L,D>()
  {

  }

  Primal_Dual_LOOPLESS_Katyusha0(const char* matrix_file, const char* vector_file)
  : Primal_Dual_LOOPLESS<L,D>(matrix_file, vector_file)
  {

  }

  Primal_Dual_LOOPLESS_Katyusha0(const char* matrix_file)
  : Primal_Dual_LOOPLESS<L,D>(matrix_file)
  {

  }

  void set_theta(){
    theta2=this->L2/2/max(this->Lf,this->L2);
    if(this->Lf<=this->L2/this->p){
      D tmp=this->mu/this->L2/this->p;
      cout<<"this->L2="<<this->L2<<endl;
      cout<<"tmp="<<tmp<<endl;
      if(tmp>=1){
        theta1=theta2;
      }
      else{
        theta1=sqrt(tmp)*theta2;
      }
    }else
      theta1=min(sqrt(this->mu/this->Lf),this->p/2);
    theta3=1-theta1-theta2;
    eta=1./(theta1*(this->Lf+2*max(this->L2,this->Lf)));
    cout<<"this->Lf="<<this->Lf<<"; this->L2="<<this->L2<<"; theta1="<<theta1<<"; theta2="<<theta2<<"; theta3="<<theta3<<endl;
    cout<<"eta="<<eta<<endl;
  }

  inline D compute_current_xj_value(D ui, L j, L t0 , L t1)
  {
    w=primal_w[j];
    y=primal_y[j];
    z=primal_z[j];
    compute_just_in_time_prox_grad(eta, ui , z, t0, t1, w, y, j);
    x=theta1*z+theta2*w+theta3*y;
    return x;
  }

  inline void set_stepsize(){
    primal_y.clear();
    primal_z.clear();
    primal_w.clear();
    primal_y.resize(this->nfeatures,0);
    primal_z.resize(this->nfeatures,0);
    primal_w.resize(this->nfeatures,0);
    for(L j=0; j<this->nfeatures; j++){
      primal_y[j]=this->primal_x[j];
      primal_z[j]=this->primal_x[j];
      primal_w[j]=this->primal_x[j];
    }
    cout<<"w size="<<primal_w.size()<<endl;
    set_theta();
    cout<<"w 2="<<primal_w.size()<<endl;
  }

  inline void update_x(D xj,L j){
    this->primal_x[j]=xj;
    primal_z[j]=z;
    primal_y[j]=y;
  }

  inline void update_baralpha()
   {
     D yi=gsl_rng_uniform(this->rng);
     if(yi<=this->p){
         this->nb_iters+=this->nsamples/this->tau;
         D aitg=0;
         D deltaalphai=0;
         for(L i=0;i<this->nsamples;i++)
         {
           aitg=this->compute_AiTxk(i);
           deltaalphai=gradient_of_phi_i(aitg,i)-this->dual_alpha[i];
           this->dual_alpha[i]+=deltaalphai;
           for (L k = this->ptr[i]; k < this->ptr[i + 1];k++)
           {
             L j=this->row_idx[k];
             this->baralpha[j]+=this->lambda_f[i]*deltaalphai*this->A[k];
           }
         }

       for(L j=0;j<this->nfeatures;j++)
       primal_w[j]=this->primal_x[j];
     }
   }

  void loopless_Katyusha(vector<D> & x0, vector<D> & w0, string filename, vector<D> & L_phi, D val_mu, D val_epsilon, L max_nb, L nb_tau, L nb_c, L u, L p_mod, D scal_p)
  {
    filename="Katyusha"+filename;
    this->loopless( x0,  w0,  filename,  L_phi,  val_mu,  val_epsilon, max_nb, nb_tau, nb_c, u, p_mod,scal_p);
  }

  void loopless_Katyusha2(vector<D> & x0, vector<D> & w0, string filename, vector<D> & L_phi, D val_mu, D val_epsilon, L max_nb, L nb_tau, L nb_c, L u, L p_mod, D scal_p)
  {
    filename="Katyusha"+filename;
    this->loopless2( x0,  w0,  filename,  L_phi,  val_mu,  val_epsilon, max_nb, nb_tau, nb_c, u, p_mod,scal_p);
  }

  };

  #endif /* MIN_SMOOTH_CONVEX_H */


/* ode-initval/gear2.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 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.
 */

/* Gear 2 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "odeiv_util.h"
#include <gsl/gsl_odeiv.h>

/* gear2 state object */
typedef struct
{
  int primed;                   /* flag indicating that yim1 is ready */
  double t_primed;              /* system was primed for this value of t */
  double last_h;                /* last step size */
  gsl_odeiv_step *primer;       /* stepper to use for priming */
  double *yim1;                 /* y_{i-1}    */
  double *k;                    /* work space */
  double *y0;                   /* work space */
  double *y0_orig;
  double *y_onestep;
  int stutter;
}
gear2_state_t;

static void *
gear2_alloc (size_t dim)
{
  gear2_state_t *state = (gear2_state_t *) malloc (sizeof (gear2_state_t));

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

  state->yim1 = (double *) malloc (dim * sizeof (double));

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

  state->k = (double *) malloc (dim * sizeof (double));

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

  state->y0 = (double *) malloc (dim * sizeof (double));

  if (state->y0 == 0)
    {
      free (state->k);
      free (state->yim1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

  state->y0_orig = (double *) malloc (dim * sizeof (double));

  if (state->y0_orig == 0)
    {
      free (state->y0);
      free (state->k);
      free (state->yim1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0_orig", GSL_ENOMEM);
    }

  state->y_onestep = (double *) malloc (dim * sizeof (double));

  if (state->y_onestep == 0)
    {
      free (state->y0_orig);
      free (state->y0);
      free (state->k);
      free (state->yim1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0_orig", GSL_ENOMEM);
    }

  state->primed = 0;
  state->primer = gsl_odeiv_step_alloc (gsl_odeiv_step_rk4imp, dim);

  if (state->primer == 0)
    {
      free (state->y_onestep);
      free (state->y0_orig);
      free (state->y0);
      free (state->k);
      free (state->yim1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for primer", GSL_ENOMEM);
    }

  state->last_h = 0.0;

  return state;
}

static int
gear2_step (double *y, gear2_state_t * state,
            const double h, const double t,
            const size_t dim, const gsl_odeiv_system * sys)
{
  /* Makes a Gear2 advance with step size h.
     y0 is the initial values of variables y.
     The implicit matrix equations to solve are:
     k = y0 + h * f(t + h, k)
     y = y0 + h * f(t + h, k)
   */

  const int iter_steps = 3;
  int nu;
  size_t i;
  double *y0 = state->y0;
  double *yim1 = state->yim1;
  double *k = state->k;

  /* Iterative solution of k = y0 + h * f(t + h, k)
     Note: This method does not check for convergence of the
     iterative solution!
   */

  for (nu = 0; nu < iter_steps; nu++)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, k);

      if (s != GSL_SUCCESS)
        {
          return s;
        }

      for (i = 0; i < dim; i++)
        {
          y[i] = ((4.0 * y0[i] - yim1[i]) + 2.0 * h * k[i]) / 3.0;
        }
    }

  return GSL_SUCCESS;
}

static int
gear2_apply (void *vstate,
             size_t dim,
             double t,
             double h,
             double y[],
             double yerr[],
             const double dydt_in[],
             double dydt_out[], const gsl_odeiv_system * sys)
{
  gear2_state_t *state = (gear2_state_t *) vstate;

  state->stutter = 0;

  if (state->primed == 0 || t == state->t_primed || h != state->last_h)
    {
      /* Execute a single-step method to prime the process.  Note that
       * we do this if the step size changes, so frequent step size
       * changes will cause the method to stutter.
       *
       * Note that we reuse this method if the time has not changed,
       * which can occur when the adaptive driver is attempting to find
       * an appropriate step-size on its first iteration */

      int status;
      DBL_MEMCPY (state->yim1, y, dim);

      status =
        gsl_odeiv_step_apply (state->primer, t, h, y, yerr, dydt_in, dydt_out,
                              sys);

      /* Make note of step size and indicate readiness for a Gear step. */

      state->primed = 1;
      state->t_primed = t;
      state->last_h = h;
      state->stutter = 1;

      return status;
    }
  else
    {
      /* We have a previous y value in the buffer, and the step
       * sizes match, so we go ahead with the Gear step.
       */

      double *const k = state->k;
      double *const y0 = state->y0;
      double *const y0_orig = state->y0_orig;
      double *const yim1 = state->yim1;
      double *y_onestep = state->y_onestep;

      int s;
      size_t i;

      DBL_MEMCPY (y0, y, dim);

      /* iterative solution */

      if (dydt_out != NULL)
        {
          DBL_MEMCPY (k, dydt_out, dim);
        }

      /* First traverse h with one step (save to y_onestep) */

      DBL_MEMCPY (y_onestep, y, dim);

      s = gear2_step (y_onestep, state, h, t, dim, sys);

      if (s != GSL_SUCCESS)
        {
          return s;
        }

      /* Then with two steps with half step length (save to y) */

      s = gear2_step (y, state, h / 2.0, t, dim, sys);

      if (s != GSL_SUCCESS)
        {
          /* Restore original y vector */
          DBL_MEMCPY (y, y0_orig, dim);
          return s;
        }

      DBL_MEMCPY (y0, y, dim);

      s = gear2_step (y, state, h / 2.0, t + h / 2.0, dim, sys);

      if (s != GSL_SUCCESS)
        {
          /* Restore original y vector */
          DBL_MEMCPY (y, y0_orig, dim);
          return s;
        }

      /* Cleanup update */

      if (dydt_out != NULL)
        {
          s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

          if (s != GSL_SUCCESS)
            {
              /* Restore original y vector */
              DBL_MEMCPY (y, y0_orig, dim);
              return s;
            }
        }

      /* Estimate error and update the state buffer. */

      for (i = 0; i < dim; i++)
        {
          yerr[i] = 4.0 * (y[i] - y_onestep[i]);
          yim1[i] = y0[i];
        }

      /* Make note of step size. */
      state->last_h = h;

      return 0;
    }
}

static int
gear2_reset (void *vstate, size_t dim)
{
  gear2_state_t *state = (gear2_state_t *) vstate;

  DBL_ZERO_MEMSET (state->yim1, dim);
  DBL_ZERO_MEMSET (state->k, dim);
  DBL_ZERO_MEMSET (state->y0, dim);

  state->primed = 0;
  state->last_h = 0.0;
  return GSL_SUCCESS;
}

static unsigned int
gear2_order (void *vstate)
{
  gear2_state_t *state = (gear2_state_t *) vstate;
  state = 0;                    /* prevent warnings about unused parameters */
  return 3;
}

static void
gear2_free (void *vstate)
{
  gear2_state_t *state = (gear2_state_t *) vstate;

  free (state->yim1);
  free (state->k);
  free (state->y0);
  free (state->y0_orig);
  free (state->y_onestep);
  gsl_odeiv_step_free (state->primer);

  free (state);
}

static const gsl_odeiv_step_type gear2_type = { "gear2",        /* name */
  1,                            /* can use dydt_in */
  0,                            /* gives exact dydt_out */
  &gear2_alloc,
  &gear2_apply,
  &gear2_reset,
  &gear2_order,
  &gear2_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_gear2 = &gear2_type;


/* specfunc/bessel_K1.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 * Copyright (C) 2016 Pavel Holoborodko, Patrick Alken
 *
 * 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 <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_bessel.h>

#include "error.h"

#include "chebyshev.h"
#include "cheb_eval.c"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/*
 Minimax rational approximation for [0,1), peak relative error = 1.83*GSL_DBL_EPSILON.
 Source: http://www.advanpix.com/?p=3987
*/
static double k1_poly[9] = {
  -3.0796575782920622440538935e-01,
  -8.5370719728650778045782736e-02,
  -4.6421827664715603298154971e-03,
  -1.1253607036630425931072996e-04,
  -1.5592887702110907110292728e-06,
  -1.4030163679125934402498239e-08,
  -8.8718998640336832196558868e-11,
  -4.1614323580221539328960335e-13,
  -1.5261293392975541707230366e-15
};

static double i1_poly[7] = {
  8.3333333333333325191635191e-02,
  6.9444444444467956461838830e-03,
  3.4722222211230452695165215e-04,
  1.1574075952009842696580084e-05,
  2.7555870002088181016676934e-07,
  4.9724386164128529514040614e-09
};

/*
 Chebyshev expansion for [1,8], peak relative error = 1.28*GSL_DBL_EPSILON.
 Source: Pavel Holoborodko.
*/
static double ak1_data[25] = {
  +2.07996868001418246e-01,
  +1.62581565017881476e-01,
  -5.87070423518863640e-03,
  +4.95021520115789501e-04,
  -5.78958347598556986e-05,
  +8.18614610209334726e-06,
  -1.31604832009487277e-06,
  +2.32546031520101213e-07,
  -4.42206518311557987e-08,
  +8.92163994883100361e-09,
  -1.89046270526983427e-09,
  +4.17568808108504702e-10,
  -9.55912361791375794e-11,
  +2.25769353153867758e-11,
  -5.48128000211158482e-12,
  +1.36386122546441926e-12,
  -3.46936690565986409e-13,
  +9.00354564415705942e-14,
  -2.37950577776254432e-14,
  +6.39447503964025336e-15,
  -1.74498363492322044e-15,
  +4.82994547989290473e-16,
  -1.35460927805445606e-16,
  +3.84604274446777234e-17,
  -1.10456856122581316e-17
};

static cheb_series ak1_cs = {
  ak1_data,
  24,
  -1, 1,
  9
};

/*
 Chebyshev expansion for [8,inf), peak relative error = 1.25*GSL_DBL_EPSILON.
 Source: SLATEC/dbsk1e.f
*/
static double ak12_data[14] = {
  +.637930834373900104E-1,
  +.283288781304972094E-1,
  -.247537067390525035E-3,
  +.577197245160724882E-5,
  -.206893921953654830E-6,
  +.973998344138180418E-8,
  -.558533614038062498E-9,
  +.373299663404618524E-10,
  -.282505196102322545E-11,
  +.237201900248414417E-12,
  -.217667738799175398E-13,
  +.215791416161603245E-14,
  -.229019693071826928E-15,
  +.258288572982327496E-16
};

static cheb_series ak12_cs = {
  ak12_data,
  13,
  -1, 1,
  7
};

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_sf_bessel_K1_scaled_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0*GSL_DBL_MIN) {
    OVERFLOW_ERROR(result);
  }
  else if(x < 1.0) {
    const double lx = log(x);
    const double ex = exp(x);
    const double x2 = x*x;
    const double t  = 0.25*x2;
    const double i1 = 0.5 * x * (1.0 + t * (0.5 + t * gsl_poly_eval(i1_poly,6,t)));
    result->val  = ex * (x2 * gsl_poly_eval(k1_poly,9,x2) + x * lx * i1 + 1) / x;
    result->err  = ex * (1.6+fabs(lx)*0.6) * GSL_DBL_EPSILON;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x <= 8.0) {
    const double sx = sqrt(x);
    gsl_sf_result c;
    cheb_eval_e(&ak1_cs, (16.0/x-9.0)/7.0, &c);
    result->val  = (1.375 + c.val) / sx; /* 1.375 = 11/8 */
    result->err  = c.err / sx;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double sx = sqrt(x);
    gsl_sf_result c;
    cheb_eval_e(&ak12_cs, 16.0/x-1.0, &c);
    result->val  = (1.25 + c.val) / sx;
    result->err  = c.err / sx;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int gsl_sf_bessel_K1_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0*GSL_DBL_MIN) {
    OVERFLOW_ERROR(result);
  }
  else if(x < 1.0) {
    const double lx = log(x);
    const double x2 = x*x;
    const double t  = 0.25*x2;
    const double i1 = 0.5 * x * (1.0 + t * (0.5 + t * gsl_poly_eval(i1_poly,6,t)));
    result->val  = (x2 * gsl_poly_eval(k1_poly,9,x2) + x * lx * i1 + 1) / x;
    result->err  = (1.6+fabs(lx)*0.6) * GSL_DBL_EPSILON;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result K1_scaled;
    int stat_K1 = gsl_sf_bessel_K1_scaled_e(x, &K1_scaled);
    int stat_e  = gsl_sf_exp_mult_err_e(-x, 0.0,
                                           K1_scaled.val, K1_scaled.err,
                                           result);
    result->err = fabs(result->val) * (GSL_DBL_EPSILON*fabs(x) + K1_scaled.err/K1_scaled.val);
    return GSL_ERROR_SELECT_2(stat_e, stat_K1);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_K1_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_K1_scaled_e(x, &result));
}

double gsl_sf_bessel_K1(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_K1_e(x, &result));
}


/* integration/rational.c
 *
 * Copyright (C) 2017 Konrad Griessinger, Patrick Alken
 *
 * 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.
 */

/*
 * The code in this module is based on IQPACK, specifically the LGPL
 * implementation found in HERMITE_RULE:
 * https://people.sc.fsu.edu/~jburkardt/c_src/hermite_rule/hermite_rule.html
 */

#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_sf_gamma.h>

static int
rational_check(const size_t n, const gsl_integration_fixed_params * params)
{
  if (fabs(params->b - params->a) <= GSL_DBL_EPSILON)
    {
      GSL_ERROR("|b - a| too small", GSL_EDOM);
    }
  else if (params->alpha <= -1.0)
    {
      GSL_ERROR("alpha must be > -1", GSL_EDOM);
    }
  else if (params->beta >= 0.0 || params->alpha+params->beta+2*n >= 0.0 || 0.0 >= params->alpha+2*n)
    {
      GSL_ERROR("beta < alpha + beta + 2n < 0 is required", GSL_EDOM);
    }
  else if (params->a + params->b <= 0.0)
    {
      GSL_ERROR("a + b <= 0 is not allowed", GSL_EDOM);
    }
  else
    {
      return GSL_SUCCESS;
    }
}

static int
rational_init(const size_t n, double * diag, double * subdiag, gsl_integration_fixed_params * params)
{
  const double absum = params->beta + params->alpha;
  const double a1 = params->alpha + 1.0;
  const double aba1 = absum*a1;
  double ab2i = absum + 2.0;
  size_t i;

  /* construct the diagonal and subdiagonal elements of Jacobi matrix */

  diag[0] = -a1/(absum + 2.0);
  subdiag[0] = sqrt( -diag[0] * ( params->beta + 1.0 ) / ( (absum + 2.0)*(absum + 3.0) ) );

  for (i = 1; i < n-1; i++)
    {
      ab2i += 2.0;
      diag[i] = ( -aba1 - 2.0 * i * ( absum + i + 1.0 ) ) / ( ab2i * ( ab2i - 2.0 ) );
      subdiag[i] = sqrt( (i+1.0) * ( params->alpha + i + 1.0 ) / ( ab2i - 1.0 ) * ( params->beta + i + 1.0 ) / ( ab2i * ab2i ) * ( absum + i + 1.0 ) / ( ab2i + 1.0 ) );
    }

  diag[n-1] = ( -aba1 - 2.0 * (n-1.0) * ( absum + n ) ) / ( (absum + 2.0*n) * ( absum + 2.0*n - 2.0 ) );
  subdiag[n-1] = 0.0;

  params->zemu = gsl_sf_gamma(params->alpha + 1.0) * gsl_sf_gamma(-absum - 1.0) / gsl_sf_gamma(-params->beta);
  params->shft = params->a;
  params->slp = params->b + params->a;
  params->al = params->alpha;
  params->be = params->beta;

  return GSL_SUCCESS;
}

static const gsl_integration_fixed_type rational_type =
{
  rational_check,
  rational_init
};

const gsl_integration_fixed_type *gsl_integration_fixed_rational = &rational_type;


/* stable/stable_integration.h
 *
 * Functions to perform numerical integration used when calculating
 * the PDF and CDF of alpha-stable distributions. Based on GSL
 * numerical quadrature methods.
 *
 * Copyright (C) 2013. Javier Royuela del Val
 *                     Federico Simmross Wattenberg
 *
 * 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; version 3 of the License.
 *
 * 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, see <http://www.gnu.org/licenses/>.
 *
 *
 *  Javier Royuela del Val.
 *  E.T.S.I. Telecomunicación
 *  Universidad de Valladolid
 *  Paseo de Belén 15, 47002 Valladolid, Spain.
 *  jroyval@lpi.tel.uva.es
 *
 */
#ifndef _STABLE_INTEGRATION_H_
#define _STABLE_INTEGRATION_H_

#include "stable.h"

#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_erf.h>
#include <gsl/gsl_math.h>

int stable_integration_METHODNAME(unsigned short method, char *s);

void
stable_integration(StableDist *dist,double(function)(double, void*),
                   double a, double b,
                   double epsabs, double epsrel, unsigned short limit,
                   double *result, double *abserr, unsigned short method);

#endif