#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_roots.h>

#include "cosmocalc.h"

static double gaussiannorm_linear_powspec_exact_lnk_integ_funct(double lnk, void *p);
static double onederiv_gaussiannorm_linear_powspec_exact_lnk_integ_funct(double lnk, void *p);
static double twoderiv_gaussiannorm_linear_powspec_exact_lnk_integ_funct(double lnk, void *p);
static double nonlinear_gaussnorm_scale_funct(double gaussR, void *p);

static double gaussiannorm_linear_powspec_exact_lnk_integ_funct(double lnk, void *p)
{
  double gaussRad = (*(double*)p);
  double k = exp(lnk);
  return linear_powspec(k,1.0)*k*k*k/2.0/M_PI/M_PI*exp(-1.0*k*k*gaussRad*gaussRad);
}

static double onederiv_gaussiannorm_linear_powspec_exact_lnk_integ_funct(double lnk, void *p)
{
  double gaussRad = (*(double*)p);
  double k = exp(lnk);
  return linear_powspec(k,1.0)*k*k*k/2.0/M_PI/M_PI*exp(-1.0*k*k*gaussRad*gaussRad)*(-1.0*k*k*2.0*gaussRad);
}

static double twoderiv_gaussiannorm_linear_powspec_exact_lnk_integ_funct(double lnk, void *p)
{
  double gaussRad = (*(double*)p);
  double k = exp(lnk);
  return  linear_powspec(k,1.0)*k*k*k/2.0/M_PI/M_PI*exp(-1.0*k*k*gaussRad*gaussRad)*(-2.0*k*k + 4.0*k*k*k*k*gaussRad*gaussRad);
}

//uses Takahashi et al. (2012) arXiv:1208.2701 unless macro below is set to use true Smith+03
//#define SMITH03

double nonlinear_powspec(double k, double a)
{
  static int initFlag = 1;
  static int currCosmoNum;
  static gsl_spline *spline[4];
  static gsl_interp_accel *accel[4];
  int i;
  double xtab[COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH],ytab[COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH];
  double Rsigma,C,neff,ksigma,sigma2;
  double an,bn,cn,alphan,gamman,betan,mun,nun;
  double f1,f2,f3;
  double DeltakNL,dsigma2dR,d2sigma2d2R,PkNL,PkL;
  double y,DeltakL,fy,DeltakQ,DeltakHprime,DeltakH;
  //double t;
  double I0,I1;
  double abserr;
  gsl_integration_workspace *workspace;
  gsl_function F;
  double gaussRad;

#define WORKSPACE_NUM 10000000
#define ABSERR 1e-6
#define RELERR 0.0

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      currCosmoNum = cosmoData.cosmoNum;

      if(initFlag)
	{
	  for(i=0;i<4;++i)
	    spline[i] = gsl_spline_alloc(gsl_interp_akima,(size_t) (COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH));
	  for(i=0;i<4;++i)
	    accel[i] = gsl_interp_accel_alloc();

	  initFlag = 0;
	}
      else
	{
	  for(i=0;i<4;++i)
	    gsl_spline_free(spline[i]);
	  for(i=0;i<4;++i)
	    spline[i] = gsl_spline_alloc(gsl_interp_akima,(size_t) (COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH));
	  for(i=0;i<4;++i)
	    gsl_interp_accel_reset(accel[i]);
	}

      //t = -wtime();

      for(i=0;i<COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH;++i)
	{
	  xtab[i] = i*(1.0-AEXPN_MIN_NONLINPK)/(COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH-1.0) + AEXPN_MIN_NONLINPK;
	  ytab[i] = get_nonlinear_gaussnorm_scale(xtab[i]);
	}
      gsl_spline_init(spline[0],xtab,ytab,(size_t) (COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH));

      gsl_sort(ytab,(size_t) 1,(size_t) (COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH));
      for(i=0;i<COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH;++i)
	{
	  xtab[i] = ytab[i];
	  ytab[i] = gaussiannorm_linear_powspec(xtab[i]);
	}
      gsl_spline_init(spline[1],xtab,ytab,(size_t) (COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH));

      workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
      F.params = &gaussRad;

      F.function = &onederiv_gaussiannorm_linear_powspec_exact_lnk_integ_funct;
      for(i=0;i<COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH;++i)
	{
	  gaussRad = xtab[i];
	  gsl_integration_qags(&F,log(1e-4),log(2.0*M_PI/gaussRad),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I0,&abserr);
	  gsl_integration_qags(&F,log(2.0*M_PI/gaussRad),log(1e3),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I1,&abserr);
	  ytab[i] = I0+I1;
	}
      gsl_spline_init(spline[2],xtab,ytab,(size_t) (COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH));

      F.function = &twoderiv_gaussiannorm_linear_powspec_exact_lnk_integ_funct;
      for(i=0;i<COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH;++i)
	{
	  gaussRad = xtab[i];
	  gsl_integration_qags(&F,log(1e-4),log(2.0*M_PI/gaussRad),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I0,&abserr);
	  gsl_integration_qags(&F,log(2.0*M_PI/gaussRad),log(1e3),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I1,&abserr);
	  ytab[i] = I0+I1;
	}
      gsl_spline_init(spline[3],xtab,ytab,(size_t) (COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH));

      gsl_integration_workspace_free(workspace);

#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM

      //t += wtime();
      //fprintf(stderr,"comp of non-linear Pk took %f seconds.\n",t);
    }

  Rsigma = gsl_spline_eval(spline[0],a,accel[0]);
  sigma2 = gsl_spline_eval(spline[1],Rsigma,accel[1]);
  dsigma2dR = gsl_spline_eval(spline[2],Rsigma,accel[2]);
  d2sigma2d2R = gsl_spline_eval(spline[3],Rsigma,accel[3]);

  ksigma = 1.0/Rsigma;
  neff = -1.0*Rsigma/sigma2*dsigma2dR - 3.0;
  C = -1.0*(d2sigma2d2R*Rsigma*Rsigma/sigma2 + dsigma2dR*Rsigma/sigma2 - dsigma2dR*dsigma2dR*Rsigma*Rsigma/sigma2/sigma2);

  double ha,weffa,omegaMz,omegaDEwz;

  ha = hubble_noscale(a);
  weffa = weff(a);
  omegaMz = cosmoData.OmegaM/a/a/a/ha/ha;
  omegaDEwz = (1.0-cosmoData.OmegaM)/ha/ha/pow(a,3.0*(1.0 + weffa));

#ifdef SMITH03
  an = pow(10.0,1.4861 + 1.8369*neff + 1.6762*neff*neff + 0.7940*neff*neff*neff + 0.1670*neff*neff*neff*neff - 0.6206*C);
  bn = pow(10.0,0.9463 + 0.9466*neff + 0.3084*neff*neff - 0.9400*C);
  cn = pow(10.0,-0.2807 + 0.6669*neff + 0.3214*neff*neff - 0.0793*C);
  gamman = 0.8649 + 0.2989*neff + 0.1631*C;
  alphan = 1.3884 + 0.3700*neff - 0.1452*neff*neff;
  betan = 0.8291 + 0.9854*neff + 0.3401*neff*neff;
  mun = pow(10.0,-3.5442 + 0.1908*neff);
  nun = pow(10.0,0.9589 + 1.2857*neff);

  f1 = pow(OmegaMz,-0.0307);
  f2 = pow(OmegaMz,-0.0585);
  f3 = pow(OmegaMz,0.0743);
#else
  an = pow(10.0,1.5222 + 2.8553*neff + 2.3706*neff*neff + 0.9903*neff*neff*neff + 0.2250*neff*neff*neff*neff - 0.6038*C + 0.1749*omegaDEwz*(1.0 + weffa));
  bn = pow(10.0,-0.5642 + 0.5864*neff + 0.5716*neff*neff - 1.5474*C + 0.2279*omegaDEwz*(1.0 + weffa));
  cn = pow(10.0,0.3698 + 2.0404*neff + 0.8161*neff*neff + 0.5869*C);
  gamman = 0.1971 - 0.0843*neff + 0.8460*C;
  alphan = fabs(6.0835 + 1.3373*neff - 0.1959*neff*neff - 5.5274*C);
  betan = 2.0379 - 0.7354*neff + 0.3157*neff*neff + 1.2490*neff*neff*neff + 0.3980*neff*neff*neff*neff - 0.1682*C;
  mun = 0.0;
  nun = pow(10.0,5.2105 + 3.6902*neff);

  f1 = pow(omegaMz,-0.0307);
  f2 = pow(omegaMz,-0.0585);
  f3 = pow(omegaMz,0.0743);
#endif

  PkL = linear_powspec(k,a);
  y = k/ksigma;
  fy = y/4.0 + y*y/8.0;
  DeltakL = PkL*k*k*k/2.0/M_PI/M_PI;

  DeltakQ = DeltakL*pow(1.0 + DeltakL,betan)/(1.0 + alphan*DeltakL)*exp(-1.0*fy);

  DeltakHprime = an*pow(y,3.0*f1)/(1.0 + bn*pow(y,f2) + pow(cn*f3*y,3.0 - gamman));
  DeltakH = DeltakHprime/(1.0 + mun/y + nun/y/y);

  DeltakNL = DeltakQ + DeltakH;
  PkNL = DeltakNL/(k*k*k/2.0/M_PI/M_PI);

  return PkNL;
}

double gaussiannorm_linear_powspec(double gaussRad)
{
#define PNL_RGAUSS_MIN 0.0001
#define PNL_RGAUSS_MAX 100.0
#define WORKSPACE_NUM 10000000
#define ABSERR 1e-6
#define RELERR 0.0

  static int initFlag = 1;
  static int currCosmoNum;
  static gsl_spline *spline = NULL;
  static gsl_interp_accel *accel = NULL;

  double I0,I1;
  double abserr;
  gsl_integration_workspace *workspace;
  gsl_function F;
  int i;
  double xtab[COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH];
  double ytab[COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH];
  double dlnr;
  double lnrmin;
  double lnr;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
      F.function = &gaussiannorm_linear_powspec_exact_lnk_integ_funct;

      dlnr = log(PNL_RGAUSS_MAX/PNL_RGAUSS_MIN)/(COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH-1.0);
      lnrmin = log(PNL_RGAUSS_MIN);

      for(i=0;i<COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH;++i)
	{
	  lnr = dlnr*i + lnrmin;
	  xtab[i] = exp(lnr);
	  F.params = &(xtab[i]);

	  gsl_integration_qags(&F,log(1e-4),log(2.0*M_PI/gaussRad),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I0,&abserr);
	  gsl_integration_qags(&F,log(2.0*M_PI/gaussRad),log(1e3),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I1,&abserr);

	  //gsl_integration_qagil(&F,log(2.0*M_PI/gaussRad),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I0,&abserr);
	  //gsl_integration_qagiu(&F,log(2.0*M_PI/gaussRad),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I1,&abserr);

	  xtab[i] = lnr;
	  ytab[i] = log(I0+I1);
	}

      gsl_integration_workspace_free(workspace);

      if(spline != NULL)
	gsl_spline_free(spline);
      spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH));
      gsl_spline_init(spline,xtab,ytab,(size_t) (COSMOCALC_NONLINEAR_POWSPEC_TABLE_LENGTH));
      if(accel != NULL)
	gsl_interp_accel_reset(accel);
      else
	accel = gsl_interp_accel_alloc();

#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM
#undef PNL_RGAUSS_MIN
#undef PNL_RGAUSS_MAX
    }

  return exp(gsl_spline_eval(spline,log(gaussRad),accel));
}

static double nonlinear_gaussnorm_scale_funct(double gaussR, void *p)
{
  double gf = ((double*)p)[0];

  return gaussiannorm_linear_powspec(gaussR)*gf*gf-1.0;
}

double get_nonlinear_gaussnorm_scale(double a)
{
  double gf = growth_function(a);
  double Rsigma,Rlow=0.001,Rhigh=10.0;
  int itr,maxItr=1000,status;

#define ABSERR 1e-6
#define RELERR 1e-6
  const gsl_root_fsolver_type *T;
  gsl_root_fsolver *s;
  gsl_function F;

  F.function = &nonlinear_gaussnorm_scale_funct;
  F.params = &gf;

  T = gsl_root_fsolver_brent;
  s = gsl_root_fsolver_alloc(T);
  gsl_root_fsolver_set(s,&F,Rlow,Rhigh);
  itr = 0;

  do
    {
      itr++;
      status = gsl_root_fsolver_iterate(s);
      Rsigma = gsl_root_fsolver_root(s);
      Rlow = gsl_root_fsolver_x_lower(s);
      Rhigh = gsl_root_fsolver_x_upper(s);
      status = gsl_root_test_interval(Rlow,Rhigh,ABSERR,RELERR);
    }
  while(status == GSL_CONTINUE && itr < maxItr);

#undef ABSERR
#undef RELERR

  gsl_root_fsolver_free(s);

  return Rsigma;
}


#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multilarge_nlinear.h>
#include <gsl/gsl_spblas.h>
#include <gsl/gsl_spmatrix.h>

/* parameters for functions */
struct model_params
{
  double alpha;
  gsl_spmatrix *J;
};

/* penalty function */
int
penalty_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  struct model_params *par = (struct model_params *) params;
  const double sqrt_alpha = sqrt(par->alpha);
  const size_t p = x->size;
  size_t i;
  double sum = 0.0;

  for (i = 0; i < p; ++i)
    {
      double xi = gsl_vector_get(x, i);

      gsl_vector_set(f, i, sqrt_alpha*(xi - 1.0));

      sum += xi * xi;
    }

  gsl_vector_set(f, p, sum - 0.25);

  return GSL_SUCCESS;
}

int
penalty_df (CBLAS_TRANSPOSE_t TransJ, const gsl_vector * x,
            const gsl_vector * u, void * params, gsl_vector * v,
            gsl_matrix * JTJ)
{
  struct model_params *par = (struct model_params *) params;
  const size_t p = x->size;
  size_t j;

  /* store 2*x in last row of J */
  for (j = 0; j < p; ++j)
    {
      double xj = gsl_vector_get(x, j);
      gsl_spmatrix_set(par->J, p, j, 2.0 * xj);
    }

  /* compute v = op(J) u */
  if (v)
    gsl_spblas_dgemv(TransJ, 1.0, par->J, u, 0.0, v);

  if (JTJ)
    {
      gsl_vector_view diag = gsl_matrix_diagonal(JTJ);

      /* compute J^T J = [ alpha*I_p + 4 x x^T ] */
      gsl_matrix_set_zero(JTJ);

      /* store 4 x x^T in lower half of JTJ */
      gsl_blas_dsyr(CblasLower, 4.0, x, JTJ);

      /* add alpha to diag(JTJ) */
      gsl_vector_add_constant(&diag.vector, par->alpha);
    }

  return GSL_SUCCESS;
}

int
penalty_fvv (const gsl_vector * x, const gsl_vector * v,
             void *params, gsl_vector * fvv)
{
  const size_t p = x->size;
  double normv = gsl_blas_dnrm2(v);

  gsl_vector_set_zero(fvv);
  gsl_vector_set(fvv, p, 2.0 * normv * normv);

  (void)params; /* avoid unused parameter warning */

  return GSL_SUCCESS;
}

void
solve_system(const gsl_vector *x0, gsl_multilarge_nlinear_fdf *fdf,
             gsl_multilarge_nlinear_parameters *params)
{
  const gsl_multilarge_nlinear_type *T = gsl_multilarge_nlinear_trust;
  const size_t max_iter = 200;
  const double xtol = 1.0e-8;
  const double gtol = 1.0e-8;
  const double ftol = 1.0e-8;
  const size_t n = fdf->n;
  const size_t p = fdf->p;
  gsl_multilarge_nlinear_workspace *work =
    gsl_multilarge_nlinear_alloc(T, params, n, p);
  gsl_vector * f = gsl_multilarge_nlinear_residual(work);
  gsl_vector * x = gsl_multilarge_nlinear_position(work);
  int info;
  double chisq0, chisq, rcond, xsq;
  struct timeval tv0, tv1;

  gettimeofday(&tv0, NULL);

  /* initialize solver */
  gsl_multilarge_nlinear_init(x0, fdf, work);

  /* store initial cost */
  gsl_blas_ddot(f, f, &chisq0);

  /* iterate until convergence */
  gsl_multilarge_nlinear_driver(max_iter, xtol, gtol, ftol,
                                NULL, NULL, &info, work);

  gettimeofday(&tv1, NULL);

  /* store final cost */
  gsl_blas_ddot(f, f, &chisq);

  /* compute final ||x||^2 */
  gsl_blas_ddot(x, x, &xsq);

  /* store cond(J(x)) */
  gsl_multilarge_nlinear_rcond(&rcond, work);

  /* print summary */
  fprintf(stderr, "%-25s %-5zu %-4zu %-5zu %-6zu %-4zu %-10.4e %-10.4e %-7.2f %-11.4e %.2f\n",
          gsl_multilarge_nlinear_trs_name(work),
          gsl_multilarge_nlinear_niter(work),
          fdf->nevalf,
          fdf->nevaldfu,
          fdf->nevaldf2,
          fdf->nevalfvv,
          chisq0,
          chisq,
          1.0 / rcond,
          xsq,
          (tv1.tv_sec - tv0.tv_sec) + 1.0e-6 * (tv1.tv_usec - tv0.tv_usec));

  gsl_multilarge_nlinear_free(work);
}

int
main (void)
{
  const size_t p = 2000;
  const size_t n = p + 1;
  gsl_vector *f = gsl_vector_alloc(n);
  gsl_vector *x = gsl_vector_alloc(p);

  /* allocate sparse Jacobian matrix with 2*p non-zero elements in triplet format */
  gsl_spmatrix *J = gsl_spmatrix_alloc_nzmax(n, p, 2 * p, GSL_SPMATRIX_TRIPLET);

  gsl_multilarge_nlinear_fdf fdf;
  gsl_multilarge_nlinear_parameters fdf_params =
    gsl_multilarge_nlinear_default_parameters();
  struct model_params params;
  size_t i;

  params.alpha = 1.0e-5;
  params.J = J;

  /* define function to be minimized */
  fdf.f = penalty_f;
  fdf.df = penalty_df;
  fdf.fvv = penalty_fvv;
  fdf.n = n;
  fdf.p = p;
  fdf.params = &params;

  for (i = 0; i < p; ++i)
    {
      /* starting point */
      gsl_vector_set(x, i, i + 1.0);

      /* store sqrt(alpha)*I_p in upper p-by-p block of J */
      gsl_spmatrix_set(J, i, i, sqrt(params.alpha));
    }

  fprintf(stderr, "%-25s %-4s %-4s %-5s %-6s %-4s %-10s %-10s %-7s %-11s %-10s\n",
          "Method", "NITER", "NFEV", "NJUEV", "NJTJEV", "NAEV", "Init Cost",
          "Final cost", "cond(J)", "Final |x|^2", "Time (s)");

  fdf_params.scale = gsl_multilarge_nlinear_scale_levenberg;

  fdf_params.trs = gsl_multilarge_nlinear_trs_lm;
  solve_system(x, &fdf, &fdf_params);

  fdf_params.trs = gsl_multilarge_nlinear_trs_lmaccel;
  solve_system(x, &fdf, &fdf_params);

  fdf_params.trs = gsl_multilarge_nlinear_trs_dogleg;
  solve_system(x, &fdf, &fdf_params);

  fdf_params.trs = gsl_multilarge_nlinear_trs_ddogleg;
  solve_system(x, &fdf, &fdf_params);

  fdf_params.trs = gsl_multilarge_nlinear_trs_subspace2D;
  solve_system(x, &fdf, &fdf_params);

  fdf_params.trs = gsl_multilarge_nlinear_trs_cgst;
  solve_system(x, &fdf, &fdf_params);

  gsl_vector_free(f);
  gsl_vector_free(x);
  gsl_spmatrix_free(J);

  return 0;
}


//
//  Extrap1d.h

#ifndef __NeutronDetectorSim__Extrap1d__
#define __NeutronDetectorSim__Extrap1d__

#include <iostream>
#include <vector>
#include <eigen3/Eigen/Dense>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>
#include "ConsolePrint.hpp"

class Extrap1d {
public:
	Extrap1d(Eigen::ArrayXd x, Eigen::ArrayXd y);
	Extrap1d(std::vector<double> x, std::vector<double> y);
	~Extrap1d();
	double operator()(const double &x);
	Eigen::ArrayXd operator()(const Eigen::ArrayXd &x);
private:
	double min, max;
	Eigen::ArrayXd x;
	Eigen::ArrayXd y;
	gsl_spline *interpolator;
	gsl_interp_accel *accelerator;
};

#endif /* defined(__NeutronDetectorSim__Extrap1d__) */


#ifndef INCLUDED_TELEPATH_BLAS_3_GEMM_H
#define INCLUDED_TELEPATH_BLAS_3_GEMM_H

#include <cblas.h>

namespace telepath{

    template< typename T >
    void gemm( CBLAS_ORDER order, CBLAS_TRANSPOSE transA,
        CBLAS_TRANSPOSE transB, std::size_t m std::size_t n, std::size_t k,
        T alpha, const T* A, std::size_t lda, const T* B, std::size_t ldb,
        T beta, T* C, std::size_t ldc );
    template<>
    void gemm<float>( CBLAS_ORDER order, CBLAS_TRANSPOSE transA,
        CBLAS_TRANSPOSE transB, std::size_t m, std::size_t n, std::size_t k,
        float alpha, const float* A, std::size_t lda, const float* B,
        std::size_t ldb, float beta, float* C, std::size_t ldc ){
        return cblas_sgemm( order, transA, transB, m, n, k, alpha, A, lda, B,
            ldb, beta, C, ldc );
    }
    template<>
    void gemm<double>( CBLAS_ORDER order, CBLAS_TRANSPOSE transA,
        CBLAS_TRANSPOSE transB, std::size_t m, std::size_t n, std::size_t k,
        double alpha, const double* A, std::size_t lda, const double* B,
        std::size_t ldb, double beta, double* C, std::size_t ldc ){
        return cblas_dgemm( order, transA, transB, m, n, k, alpha, A, lda, B,
            ldb, beta, C, ldc );
    }
    template<>
    void gemm<std::complex<float>>( CBLAS_ORDER order, CBLAS_TRANSPOSE transA,
        CBLAS_TRANSPOSE transB, std::size_t m, std::size_t n, std::size_t k,
        const std::complex<float>* alpha, const std::complex<float>* A,
        std::size_t lda, std::const complex<float>* B, std::size_t ldb,
        const std::complex<float>* beta, std::complex<float>* C,
        std::size_t ldc ){
        return cblas_cgemm( order, transA, transB, m, n, k, (float*) alpha,
            (float*) A, lda, (float*) B, ldb, (float*) beta, (float*) C, ldc );
    }
    template<>
    void gemm<std::complex<double>>( CBLAS_ORDER order, CBLAS_TRANSPOSE transA,
        CBLAS_TRANSPOSE transB, std::size_t m, std::size_t n, std::size_t k,
        const std::complex<double>* alpha, const std::complex<double>* A,
        std::size_t lda, const std::complex<double>* B, std::size_t ldb,
        const std::complex<double>* beta, std::complex<double>* C,
        std::size_t ldc ){
        return cblas_zgemm( order, transA, transB, m, n, k, (double*) alpha,
            (double*) A, lda, (double*) B, ldb, (double*) beta, (double*) C,
            ldc );
    }

} //namespace telepath

#endif


/*System includes*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <sys/stat.h>
#include <float.h>

/*GSL includes*/
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_statistics_double.h>
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>
#include <pthread.h>
#include <gsl/gsl_sf_exp.h>

/*User includes*/
#include "c_vbgmm_fit.h"

void readInputData(const char *szFile, t_Data *ptData);

void c_vbgmm_fit (double* adX, int nN, int nD, int nK, int* anAssign, int debug, int bAssign);

void readAssigns(const char *szFile, int *anAssign, int nN);

int main()
{
    t_Data tData;
    int *anAssign = NULL;
    int i = 0, j = 0, nN = -1, nD = -1, nK = 0;
    double* adX = NULL;
    const gsl_rng_type * T;
    gsl_rng * r;

    gsl_rng_env_setup();

    T = gsl_rng_default;
    r = gsl_rng_alloc (T);

    readInputData("PCA_transformed_data_gt2000.csv", &tData);

    nN = tData.nN;
    nD = tData.nD;

    adX = (double *) malloc(nN*nD*sizeof(double));
    anAssign = (int *) malloc(nN*sizeof(int));

    readAssigns("clustering_gt2000.csv", anAssign, nN);

    for(i = 0; i < nN; i++){
        if(anAssign[i] > nK){
            nK = anAssign[i];
        }
        for(j = 0; j < nD; j++){
            adX[i*nD + j] = tData.aadX[i][j];
        }
    }

    nK = nK + 1;
    fprintf(stderr,"Run c_vbgmm_fit with %d clusters\n",nK);
    fflush(stderr);

    c_vbgmm_fit (adX, nN, nD, nK, anAssign, FALSE, TRUE);
    for(i = 0; i < nN; i++){
        printf("%d,%d\n",i,anAssign[i]);
    }
    free(adX);
    free(anAssign);
    return 0;
}

void readInputData(const char *szFile, t_Data *ptData)
{
  double  **aadX = NULL;
  int  i = 0, j = 0, nD = 0, nN = 0;
  char *szLine = (char *) malloc(sizeof(char)*MAX_LINE_LENGTH);
  FILE* ifp = NULL;

  if(!szLine)
    goto memoryError;

  ifp = fopen(szFile, "r");

  if(ifp){
    char* szTok   = NULL;
    char* pcError = NULL;

    if(fgets(szLine, MAX_LINE_LENGTH, ifp) == NULL)
      goto formatError;

    szTok = strtok(szLine, DELIM);
    /*count dimensions*/
    while(strtok(NULL, DELIM) != NULL){

      nD++;
    }
    /*count data points*/
    while(fgets(szLine, MAX_LINE_LENGTH, ifp) != NULL){
    	nN++;
    }
    fclose(ifp);

    /*reopen input file*/
    ifp = fopen(szFile, "r");

    if(fgets(szLine, MAX_LINE_LENGTH, ifp) == NULL)
      goto formatError;

    /*allocate memory for dimension names*/
    //ptData->aszDimNames = (char **) malloc(nD*sizeof(char*));
    //if(!ptData->aszDimNames)
      //goto memoryError;

    szTok = strtok(szLine, DELIM);
    /*read in dim names*/
    for(i = 0; i < nD; i++){
      szTok = strtok(NULL, DELIM);
     // ptData->aszDimNames[i] = strdup(szTok);
    }

    /*allocate memory for data matrix*/
    aadX = (double **) malloc(nN*sizeof(double*));
    if(!aadX)
      goto memoryError;
    for(i = 0; i < nN; i++){
      aadX[i] = (double *) malloc(nD*sizeof(double));
      if(!aadX[i])
	goto memoryError;
    }

    /*read in input data*/
    //ptData->aszSampleNames = (char **) malloc(nN*sizeof(char*));
    //if(!ptData->aszSampleNames)
      //goto memoryError;

    for(i = 0; i < nN; i++){

      if(fgets(szLine, MAX_LINE_LENGTH, ifp) == NULL)
	goto formatError;

      szTok = strtok(szLine, DELIM);
     // ptData->aszSampleNames[i] = strdup(szTok);
      for(j = 0; j < nD; j++){
	szTok = strtok(NULL, DELIM);

	aadX[i][j] = strtod(szTok,&pcError);

	if(*pcError != '\0'){
	  goto formatError;
	}
      }
    }
  }
  else{
    fprintf(stderr, "Failed to open abundance data file %s aborting\n", szFile);
    fflush(stderr);
    exit(EXIT_FAILURE);
  }

  free(szLine);
  ptData->nD = nD;
  ptData->nN = nN;
  ptData->aadX = aadX;
  return;

 memoryError:
  fprintf(stderr, "Failed allocating memory in readInputData\n");
  fflush(stderr);
  exit(EXIT_FAILURE);

 formatError:
  fprintf(stderr, "Incorrectly formatted abundance data file\n");
  fflush(stderr);
  exit(EXIT_FAILURE);
}

void readAssigns(const char *szFile, int *anAssign, int nN)
{
  int  i = 0;
  char *szLine = (char *) malloc(sizeof(char)*MAX_LINE_LENGTH);
  FILE* ifp = NULL;

  if(!szLine)
    goto memoryError;

  ifp = fopen(szFile, "r");

  if(ifp){
    char* szTok   = NULL;
    char* pcError = NULL;

    if(fgets(szLine, MAX_LINE_LENGTH, ifp) == NULL)
        goto formatError;

    for(i = 0; i < nN; i++){
        if(fgets(szLine, MAX_LINE_LENGTH, ifp) == NULL)
            goto formatError;

        szTok = strtok(szLine, DELIM);
        szTok = strtok(NULL, DELIM);
    	anAssign[i] = strtod(szTok,&pcError);

	    if(*pcError != '\0'){
	        goto formatError;
	    }
    }
  }
  else{
    fprintf(stderr, "Failed to open abundance data file %s aborting\n", szFile);
    fflush(stderr);
    exit(EXIT_FAILURE);
  }

  free(szLine);

  return;

 memoryError:
  fprintf(stderr, "Failed allocating memory in readInputData\n");
  fflush(stderr);
  exit(EXIT_FAILURE);

 formatError:
  fprintf(stderr, "Incorrectly formatted abundance data file\n");
  fflush(stderr);
  exit(EXIT_FAILURE);
}


#include <config.h>
#include <math.h>
#include <gsl/gsl_ntuple.h>
#include <gsl/gsl_histogram.h>

struct data
{
  double x;
  double y;
  double z;
};

int sel_func (void *ntuple_data, void *params);
double val_func (void *ntuple_data, void *params);

int
main (void)
{
  struct data ntuple_row;
  int i;

  gsl_ntuple *ntuple = gsl_ntuple_open ("test.dat", &ntuple_row,
                                        sizeof (ntuple_row));

  gsl_histogram *h = gsl_histogram_calloc_uniform (100, 0., 10.);

  gsl_ntuple_select_fn S;
  gsl_ntuple_value_fn V;

  double scale = 1.5;

  S.function = &sel_func;
  S.params = &scale;

  V.function = &val_func;
  V.params = 0;

  gsl_ntuple_project (h, ntuple, &V, &S);

  gsl_histogram_fprintf (stdout, h, "%f", "%f");

  gsl_histogram_free (h);

  gsl_ntuple_close (ntuple);
}

int
sel_func (void *ntuple_data, void *params)
{
  double x, y, z, E, scale;
  scale = *(double *) params;

  x = ((struct data *) ntuple_data)->x;
  y = ((struct data *) ntuple_data)->y;
  z = ((struct data *) ntuple_data)->z;

  E = x * x + y * y + z * z;

  return E / scale > 1;
}

double
val_func (void *ntuple_data, void *params)
{
  double x, y, z;

  x = ((struct data *) ntuple_data)->x;
  y = ((struct data *) ntuple_data)->y;
  z = ((struct data *) ntuple_data)->z;

  return x * x + y * y + z * z;
}


/* rng/fishman2x.c
 *
 * 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.
 */

/*
 * This generator is taken from
 *
 * Donald E. Knuth
 * The Art of Computer Programming
 * Volume 2
 * Third Edition
 * Addison-Wesley
 * Page 108
 *
 * It is called "Fishman - L'Ecuyer"
 *
 * This implementation copyright (C) 2001 Carlo Perassi
 * and (C) 2003 Heiko Bauke.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

/* Fishman */
#define AAA_F 48271UL
#define MMM_F 0x7fffffffUL      /* 2 ^ 31 - 1 */
#define QQQ_F 44488UL
#define RRR_F 3399UL

/* L'Ecuyer */
#define AAA_L 40692UL
#define MMM_L 0x7fffff07UL      /* 2 ^ 31 - 249 */
#define QQQ_L 52774UL
#define RRR_L 3791UL

static inline unsigned long int ran_get (void *vstate);
static double ran_get_double (void *vstate);
static void ran_set (void *state, unsigned long int s);

typedef struct
{
  unsigned long int x;
  unsigned long int y;
  unsigned long int z;
}
ran_state_t;

static inline unsigned long int
ran_get (void *vstate)
{
  ran_state_t *state = (ran_state_t *) vstate;

  long int y, r;

  r = RRR_F * (state->x / QQQ_F);
  y = AAA_F * (state->x % QQQ_F) - r;
  if (y < 0)
    y += MMM_F;
  state->x = y;

  r = RRR_L * (state->y / QQQ_L);
  y = AAA_L * (state->y % QQQ_L) - r;
  if (y < 0)
    y += MMM_L;
  state->y = y;

  state->z = (state->x > state->y) ? (state->x - state->y) :
    MMM_F + state->x - state->y;

  return state->z;
}

static double
ran_get_double (void *vstate)
{
  ran_state_t *state = (ran_state_t *) vstate;

  return ran_get (state) / 2147483647.0;
}

static void
ran_set (void *vstate, unsigned long int s)
{
  ran_state_t *state = (ran_state_t *) vstate;

  if ((s % MMM_F) == 0 || (s % MMM_L) == 0)
    s = 1;                      /* default seed is 1 */

  state->x = s % MMM_F;
  state->y = s % MMM_L;
  state->z = (state->x > state->y) ? (state->x - state->y) :
    MMM_F + state->x - state->y;

  return;
}

static const gsl_rng_type ran_type = {
  "fishman2x",                  /* name */
  MMM_F - 1,                    /* RAND_MAX */
  0,                            /* RAND_MIN */
  sizeof (ran_state_t),
  &ran_set,
  &ran_get,
  &ran_get_double
};

const gsl_rng_type *gsl_rng_fishman2x = &ran_type;


#include <stdio.h>
#include "sqlite3.h"
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

// returned row
static int callback(void *NotUsed, int argc, char **argv, char **azColName){
	int i;
	for(i=0; i<argc; i++){
		printf("%s = %s\t", azColName[i], argv[i] ? argv[i] : "NULL");
	}
	printf("\n");
	return 0;
}

// create test_int table in database
void create_int_tables(sqlite3 *db) {
	int r;
	char *err = 0;

	r = sqlite3_exec(db, "CREATE TABLE IF NOT EXISTS test_int (id INTEGER PRIMARY KEY, uniform INTEGER, normal5 INTEGER, normal20 INTEGER, t20 INTEGER)",
		callback, 0, &err);

	if(r != SQLITE_OK) {
		fprintf(stderr, "SQL error: %s\n", err);
		sqlite3_free(err);
	}
}

// create test table in database
void create_tables(sqlite3 *db) {
	int r;
	char *err = 0;

	r = sqlite3_exec(db, "CREATE TABLE IF NOT EXISTS test (id INTEGER PRIMARY KEY, uniformi INTEGER, normali5 INTEGER, normali20 INTEGER, uniformf FLOAT, normalf5 FLOAT, normalf20 FLOAT)",
		callback, 0, &err);

	if(r != SQLITE_OK) {
		fprintf(stderr, "SQL error: %s\n", err);
		sqlite3_free(err);
	}
}

// insert rows into test with varying random distributions
void insert_data(sqlite3 *db, int rows, gsl_rng *ran) {
	char* err = 0;
	int i;
	int r;

	r = sqlite3_exec(db, "PRAGMA synchronous = 0", NULL, 0, &err);

	if(r != SQLITE_OK) {
		fprintf(stderr, "SQL error: %s\n", err);
		sqlite3_free(err);
	}

	fprintf(stderr, "executed pragma\n");

	r = sqlite3_exec(db, "BEGIN TRANSACTION", NULL, 0, &err);

	if(r != SQLITE_OK) {
		fprintf(stderr, "SQL error: %s\n", err);
		sqlite3_free(err);
	}

	fprintf(stderr, "started transaction\n");

	sqlite3_stmt *stmt;
	sqlite3_prepare_v2(db,
		"INSERT INTO test (id, uniformi, normali5, normali20, uniformf, normalf5, normalf20) VALUES (?,?,?,?,?,?,?)",
		512,
		&stmt,
		0);

	for(i = 0; i < rows; i++) {
		sqlite3_bind_int(stmt, 1, i);
		sqlite3_bind_int(stmt, 2, (int)gsl_ran_flat(ran, -100, 100));
		sqlite3_bind_int(stmt, 3, (int)gsl_ran_gaussian(ran, 5));
		sqlite3_bind_int(stmt, 4, (int)gsl_ran_gaussian(ran, 20));
		sqlite3_bind_int(stmt, 5, (float)gsl_ran_flat(ran, -100, 100));
		sqlite3_bind_int(stmt, 6, (float)gsl_ran_gaussian(ran, 5));
		sqlite3_bind_int(stmt, 7, (float)gsl_ran_gaussian(ran, 20));

		int r = sqlite3_step(stmt);

		if(r != SQLITE_DONE && r != SQLITE_OK) {
			fprintf(stderr, "SQL error: %s\n", sqlite3_errmsg(db));
			sqlite3_free(err);
			return;
		}

		sqlite3_reset(stmt);

		if(i % 10000 == 0)
			printf("%i\n", i);
	}

	sqlite3_finalize(stmt);

	r=sqlite3_exec(db, "CREATE TEMPORARY TABLE tmp AS SELECT * FROM test", 0, 0, &err);

	if(r != SQLITE_OK) {
		fprintf(stderr, "SQL error: %s\n", err);
		sqlite3_free(err);
	}
        r=sqlite3_exec(db, "UPDATE tmp SET id = NULL;", 0, 0, &err);

	if(r != SQLITE_OK) {
		fprintf(stderr, "SQL error: %s\n", err);
		sqlite3_free(err);
	}
        for(int i=0;i<24;i++){
        r=sqlite3_exec(db, "INSERT INTO test SELECT * FROM tmp;", 0, 0, &err);

	if(r != SQLITE_OK) {
		fprintf(stderr, "SQL error: %s\n", err);
		sqlite3_free(err);
	}
}
        r=sqlite3_exec(db, "DROP TABLE tmp;", 0, 0, &err);

	if(r != SQLITE_OK) {
		fprintf(stderr, "SQL error: %s\n", err);
		sqlite3_free(err);
	}
	r=sqlite3_exec(db, "COMMIT", 0, 0, &err);

	if(r != SQLITE_OK) {
		fprintf(stderr, "SQL error: %s\n", err);
		sqlite3_free(err);
	}
}

int main(int argc, char **argv){
	sqlite3 *db;
	char *zErrMsg = 0;
	int rc;

	if(argc != 2) {
		fprintf(stderr, "%s <database name>\n", argv[0]);
		exit(1);
	}

	const gsl_rng_type *type;
	gsl_rng *ran;

	type = gsl_rng_ranlxd2;
	ran = gsl_rng_alloc(type);
	gsl_rng_set(ran, time(0));

	rc = sqlite3_open(argv[1], &db);
	if( rc ){
		fprintf(stderr, "Can't open database: %s\n", sqlite3_errmsg(db));
		sqlite3_close(db);
		exit(1);
	}

	create_tables(db);

	fprintf(stderr, "tables created\n");

	insert_data(db, 100000, ran);

	if(rc != SQLITE_OK) {
		fprintf(stderr, "SQL error: %s\n", zErrMsg);
		sqlite3_free(zErrMsg);
	}

	sqlite3_close(db);
	gsl_rng_free(ran);

	return 0;
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_roots.h>

#include "cosmocalc.h"

double fourierTransformTopHat(double y);
double tophatradnorm_linear_powspec_exact_nonorm_lnk_integ_funct_I0(double lnk, void *p);
double tophatradnorm_linear_powspec_exact_nonorm_k_integ_funct_I0(double k, void *p);

double convert_cmbnorm2sigma8_k_integ_funct_I0(double k, void *p)
{
  //k is in units of 1/Mpc
  //to get to units of h/Mpc
  // k -> k/h
  double ft = fourierTransformTopHat(k*8.0/cosmoData.h);
  double tf = transfer_function(k/cosmoData.h);

  /*
  fprintf(stderr,"Tf^2 = %g, W(k)^2 = %g (2/5/OmegaM)^2 = %g (k/H0)^4/k = %g, nspow = %g\n",
	  tf*tf,ft*ft,4.0/25.0/cosmoData.OmegaM/cosmoData.OmegaM,
	  pow(k/100.0,4.0)/k,pow(k*cosmoData.h/cosmoData.As_pivot,cosmoData.SpectralIndex-1.0)
	  );
  */
  return cosmoData.As*4.0/25.0/cosmoData.OmegaM/cosmoData.OmegaM
    *pow(k/cosmoData.h*DH,4.0)/k*pow(k/cosmoData.As_pivot,cosmoData.SpectralIndex-1.0)
    *ft*ft*tf*tf;
}

double convert_cmbnorm2sigma8(void)
{
  double I0,I1;
  double abserr;
  double epsrel,epsabs;
  gsl_integration_workspace *workspace;
  gsl_function F;

#define WORKSPACE_NUM 10000000
#define ABSERR 1e-8
#define RELERR 0.0
  workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);

  epsabs = 1e-20;
  epsrel = 1e-6;
  F.function = &convert_cmbnorm2sigma8_k_integ_funct_I0;
  gsl_integration_qags(&F,0.0,2.0*M_PI/8.0,epsabs,epsrel,(size_t) WORKSPACE_NUM,workspace,&I0,&abserr);
  gsl_integration_qagiu(&F,2.0*M_PI/8.0,epsabs,epsrel,(size_t) WORKSPACE_NUM,workspace,&I1,&abserr);

  gsl_integration_workspace_free(workspace);
#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM

  //fprintf(stderr,"I0 = %g, I1 = %g, g = %g\n",I0,I1,growth_function_exact_nonorm(1.0));

  return sqrt(I0 + I1)*growth_function_exact_nonorm(1.0);
}

double fourierTransformTopHat(double y)
{
  if(y < 1e-3)
    return 1.0;
  else
    return 3.0/y/y/y*(sin(y) - y*cos(y));
}

double tophatradnorm_linear_powspec_exact_nonorm_lnk_integ_funct_I0(double lnk, void *p)
{
  // topHatRad = (*((double*)p));
  double k = exp(lnk);
  double ft = fourierTransformTopHat(k*(*((double*)p)));
  double tf = transfer_function(k);
  return ft*ft*tf*tf*pow(k,cosmoData.SpectralIndex)*k*k/2.0/M_PI/M_PI*k;
}

double tophatradnorm_linear_powspec_exact_nonorm_k_integ_funct_I0(double k, void *p)
{
  // topHatRad = (*((double*)p));
  double ft = fourierTransformTopHat(k*(*((double*)p)));
  double tf = transfer_function(k);
  return ft*ft*tf*tf*pow(k,cosmoData.SpectralIndex)*k*k/2.0/M_PI/M_PI;
}

double tophatradnorm_linear_powspec_exact_nonorm(double topHatRad)
{
  double I0,I1;
  double abserr;
  double epsrel,epsabs;
  gsl_integration_workspace *workspace;
  gsl_function F;

#define WORKSPACE_NUM 10000000
#define ABSERR 1e-8
#define RELERR 0.0
  workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);

  F.params = &(topHatRad);
  if(topHatRad > 1e-4)
    {
      epsabs = 1e-20;
      epsrel = 1e-6;
      F.function = &tophatradnorm_linear_powspec_exact_nonorm_k_integ_funct_I0;
      gsl_integration_qags(&F,0.0,2.0*M_PI/topHatRad,epsabs,epsrel,(size_t) WORKSPACE_NUM,workspace,&I0,&abserr);
      gsl_integration_qagiu(&F,2.0*M_PI/topHatRad,epsabs,epsrel,(size_t) WORKSPACE_NUM,workspace,&I1,&abserr);
    }
  else
    {
      F.function = &tophatradnorm_linear_powspec_exact_nonorm_lnk_integ_funct_I0;
      gsl_integration_qagil(&F,log(2.0*M_PI/topHatRad),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I0,&abserr);
      gsl_integration_qagiu(&F,log(2.0*M_PI/topHatRad),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I1,&abserr);
    }

  gsl_integration_workspace_free(workspace);
#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM

  return I0 + I1;
}

double linear_powspec_exact(double k, double a)
{
  static int initFlag = 1;
  static int currCosmoNum;
  static double linear_powspec_norm = 1.0;
  double gf = growth_function(a);
  double tf = transfer_function(k);

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      linear_powspec_norm = cosmoData.Sigma8*cosmoData.Sigma8/tophatradnorm_linear_powspec_exact_nonorm(8.0);
    }

  return tf*tf*pow(k,cosmoData.SpectralIndex)*gf*gf*linear_powspec_norm;
}

double linear_powspec(double k, double a)
{
  static int initFlag = 1;
  static int currCosmoNum;
  static double linear_powspec_norm = 1.0;
  static gsl_spline *cosmocalc_linear_powspec_spline = NULL;
  static gsl_interp_accel *cosmocalc_linear_powspec_acc = NULL;
  static double c0,c1;

  double linear_powspec_table[COSMOCALC_LINEAR_POWSPEC_TABLE_LENGTH];
  double k_table[COSMOCALC_LINEAR_POWSPEC_TABLE_LENGTH];
  long i;
  double gf,cov00,cov01,cov11,sumsq,tf;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      for(i=0;i<COSMOCALC_LINEAR_POWSPEC_TABLE_LENGTH;++i)
	{
	  k_table[i] = log(K_MAX/K_MIN)/(COSMOCALC_LINEAR_POWSPEC_TABLE_LENGTH-1.0)*((double) i) + log(K_MIN);
	  tf = transfer_function(exp(k_table[i]));
	  linear_powspec_table[i] = log(tf*tf*pow(exp(k_table[i]),cosmoData.SpectralIndex));
	}

      //init the spline and accelerators
      if(cosmocalc_linear_powspec_spline != NULL)
	gsl_spline_free(cosmocalc_linear_powspec_spline);
      cosmocalc_linear_powspec_spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_LINEAR_POWSPEC_TABLE_LENGTH));
      gsl_spline_init(cosmocalc_linear_powspec_spline,k_table,linear_powspec_table,(size_t) (COSMOCALC_LINEAR_POWSPEC_TABLE_LENGTH));
      if(cosmocalc_linear_powspec_acc != NULL)
	gsl_interp_accel_reset(cosmocalc_linear_powspec_acc);
      else
	cosmocalc_linear_powspec_acc = gsl_interp_accel_alloc();

      gsl_fit_linear(k_table+COSMOCALC_LINEAR_POWSPEC_TABLE_LENGTH-COSMOCALC_LINEAR_POWSPEC_FIT_LENGTH,(size_t) 1,
		     linear_powspec_table+COSMOCALC_LINEAR_POWSPEC_TABLE_LENGTH-COSMOCALC_LINEAR_POWSPEC_FIT_LENGTH,(size_t) 1,
		     (size_t) COSMOCALC_LINEAR_POWSPEC_FIT_LENGTH,&c0,&c1,&cov00,&cov01,&cov11,&sumsq);

      linear_powspec_norm = cosmoData.Sigma8*cosmoData.Sigma8/tophatradnorm_linear_powspec_exact_nonorm(8.0);
    }

  gf = growth_function(a);

  if(k < K_MIN)
    {
      tf = transfer_function(k);
      return tf*tf*pow(k,cosmoData.SpectralIndex)*linear_powspec_norm*gf*gf;
    }
  else if(k < K_MAX)
    return exp(gsl_spline_eval(cosmocalc_linear_powspec_spline,log(k),cosmocalc_linear_powspec_acc))*linear_powspec_norm*gf*gf;
  else
    return exp(c0+c1*log(k))*linear_powspec_norm*gf*gf;
}

double tophatnorm_linear_powspec(double topHatRad)
{
#define NL_RTOPHAT_MIN 0.0001
#define NL_RTOPHAT_MAX 100.0
#define WORKSPACE_NUM 10000000
#define ABSERR 1e-8
#define RELERR 0.0

  static int initFlag = 1;
  static int currCosmoNum;
  static gsl_spline *spline = NULL;
  static gsl_interp_accel *accel = NULL;
  static double linear_powspec_norm;

  double I0,I1;
  double abserr,epsabs,epsrel;
  gsl_integration_workspace *workspace;
  gsl_function F;
  int i;
  double xtab[COSMOCALC_LINEAR_POWSPEC_NORM_TABLE_LENGTH];
  double ytab[COSMOCALC_LINEAR_POWSPEC_NORM_TABLE_LENGTH];
  double dlnr;
  double lnrmin;
  double lnr;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);

      dlnr = log(NL_RTOPHAT_MAX/NL_RTOPHAT_MIN)/(COSMOCALC_LINEAR_POWSPEC_NORM_TABLE_LENGTH-1.0);
      lnrmin = log(NL_RTOPHAT_MIN);

      for(i=0;i<COSMOCALC_LINEAR_POWSPEC_NORM_TABLE_LENGTH;++i)
        {
          lnr = dlnr*i + lnrmin;
          xtab[i] = exp(lnr);

	  F.params = &(xtab[i]);
	  if(topHatRad > 1e-4)
	    {
	      epsabs = 1e-20;
	      epsrel = 1e-6;
	      F.function = &tophatradnorm_linear_powspec_exact_nonorm_k_integ_funct_I0;
	      gsl_integration_qags(&F,0.0,2.0*M_PI/topHatRad,epsabs,epsrel,(size_t) WORKSPACE_NUM,workspace,&I0,&abserr);
	      gsl_integration_qagiu(&F,2.0*M_PI/topHatRad,epsabs,epsrel,(size_t) WORKSPACE_NUM,workspace,&I1,&abserr);
	    }
	  else
	    {
	      F.function = &tophatradnorm_linear_powspec_exact_nonorm_lnk_integ_funct_I0;
	      gsl_integration_qagil(&F,log(2.0*M_PI/topHatRad),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I0,&abserr);
	      gsl_integration_qagiu(&F,log(2.0*M_PI/topHatRad),ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,&I1,&abserr);
	    }

          xtab[i] = lnr;
          ytab[i] = log(I0+I1);
        }

      gsl_integration_workspace_free(workspace);

      if(spline != NULL)
        gsl_spline_free(spline);
      spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_LINEAR_POWSPEC_NORM_TABLE_LENGTH));
      gsl_spline_init(spline,xtab,ytab,(size_t) (COSMOCALC_LINEAR_POWSPEC_NORM_TABLE_LENGTH));
      if(accel != NULL)
        gsl_interp_accel_reset(accel);
      else
        accel = gsl_interp_accel_alloc();

      linear_powspec_norm = cosmoData.Sigma8*cosmoData.Sigma8/exp(gsl_spline_eval(spline,log(8.0),accel));

#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM
#undef NL_RTOPHAT_MIN
#undef NL_RTOPHAT_MAX
    }

  return exp(gsl_spline_eval(spline,log(topHatRad),accel))*linear_powspec_norm;
}

double linear_tophatnorm_scale_funct(double rad, void *p)
{
  double gf = ((double*)p)[0];

  return tophatnorm_linear_powspec(rad)*gf*gf-1.0;
}

double get_linear_tophatnorm_scale(double a)
{
  double gf = growth_function(a);
  double Rsigma,Rlow=0.001,Rhigh=10.0;
  int itr,maxItr=1000,status;

  //fprintf(stderr,"sigma2(Rlow) = %f, sigma2(Rhigh) = %f\n",tophatnorm_linear_powspec(Rlow)*gf*gf,tophatnorm_linear_powspec(Rhigh)*gf*gf);

#define ABSERR 1e-6
#define RELERR 1e-6
  const gsl_root_fsolver_type *T;
  gsl_root_fsolver *s;
  gsl_function F;

  F.function = &linear_tophatnorm_scale_funct;
  F.params = &gf;

  T = gsl_root_fsolver_brent;
  s = gsl_root_fsolver_alloc(T);
  gsl_root_fsolver_set(s,&F,Rlow,Rhigh);
  itr = 0;

  do
    {
      itr++;
      status = gsl_root_fsolver_iterate(s);
      Rsigma = gsl_root_fsolver_root(s);
      Rlow = gsl_root_fsolver_x_lower(s);
      Rhigh = gsl_root_fsolver_x_upper(s);
      status = gsl_root_test_interval(Rlow,Rhigh,ABSERR,RELERR);
    }
  while(status == GSL_CONTINUE && itr < maxItr);

#undef ABSERR
#undef RELERR

  gsl_root_fsolver_free(s);

  return Rsigma;
}


 // MIT License

 // Copyright (c) [2017] [Vinay Yuvashankar]

 // Permission is hereby granted, free of charge, to any person obtaining a copy
 // of this software and associated documentation files (the "Software"), to deal
 // in the Software without restriction, including without limitation the rights
 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 // copies of the Software, and to permit persons to whom the Software is
 // furnished to do so, subject to the following conditions:

 // The above copyright notice and this permission notice shall be included in all
 // copies or substantial portions of the Software.

 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 // SOFTWARE.
/**
	\file "Wavelet.cc"
	\brief This file contains all of the functions that support the ERSP and CWT functions
*/
#include "wavelet.h"
#include <omp.h>
#include <assert.h>
#include <gsl/gsl_statistics.h>

#define TEST 0.00001
#define SETTLING_PERCENTAGE 0.02
#define NORMALIZATION_FACTOR 0.375402913609157562

int Wavelet(double* raw_data, double* scales,
	double sampling_frequency, int n, int J,
	double* result)
{
	//Variable Declarations
	int i, j;
	fftw_complex *data_in, *fft_data;
	fftw_plan plan_forward;

	//Calculate Padding Required
    // const int PADDED_SIZE = CalculatePaddingSize(n, 1);
    const int PADDED_SIZE = n;

    const double dw = (2 * M_PI * sampling_frequency)/(PADDED_SIZE); //NOT IN RAD/SAMPLE in RAD/SEC

    data_in  = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * PADDED_SIZE );
	fft_data = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * PADDED_SIZE );

	//populate the FFTW data vector
	for (i = 0; i < n; ++i)
    {
    	data_in[i][0] = raw_data[i];
    	// data_in[i + n][0] = raw_data[i];

    	// data_in[i + n][1] = 0.0;
    	data_in[i    ][1] = 0.0;
    }

    // //Force the rest of the data vector to zero just in case
    // for (int i = n; i < PADDED_SIZE; ++i)
    // {
    // 	data_in[i][0] = 0.0;
    // 	data_in[i][1] = 0.0;
    // }

    double *temp = (double*) malloc(n * sizeof(double));
    FILE* debug_file = fopen("debug.log", "w");

	//Calculate the FFT of the data and store it in fft_data
	plan_forward = fftw_plan_dft_1d(PADDED_SIZE, data_in, fft_data,
									FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_execute(plan_forward);

	// #pragma omp parallel num_threads(1) private(i, j) shared (result, sampling_frequency, J, n, scales,  fft_data) default(none)
	// {
		double value;

		fftw_plan plan_backward;
		fftw_complex *filter_convolution, *fftw_result;

		filter_convolution = (fftw_complex *) fftw_malloc( sizeof( fftw_complex )* PADDED_SIZE );
		fftw_result  = 		 (fftw_complex *) fftw_malloc( sizeof( fftw_complex )* PADDED_SIZE );

		// #pragma omp critical (make_plan)
		// {
			//Preapre for the plan backwards
			plan_backward = fftw_plan_dft_1d(PADDED_SIZE, filter_convolution, fftw_result,
				FFTW_BACKWARD, FFTW_ESTIMATE);
		// }

	    // #pragma omp for
		for (i = 0; i < J; ++i)
		{
			//Force the arrays to zero
			memset(filter_convolution, 0.0, sizeof( fftw_complex ) * PADDED_SIZE);
			memset(fftw_result,        0.0, sizeof( fftw_complex ) * PADDED_SIZE);

			//Compute the Fourier Morlet at 0 and N/2
			double norm = sqrt(scales[i]);

			value = CompleteFourierMorlet(0.0, scales[i], norm);

			filter_convolution[0][0] = ( fft_data[0][0] / PADDED_SIZE ) * value;
			filter_convolution[0][1] = ( fft_data[0][1] / PADDED_SIZE ) * value;

			filter_convolution[PADDED_SIZE/2][0] = 0.0;
			filter_convolution[PADDED_SIZE/2][1] = 0.0;

			//Compute the Fourier Morlet Convolution in between
			for (j = 1; j < PADDED_SIZE/2 - 1; ++j)
			{
				value = CompleteFourierMorlet( j * dw , scales[i], norm);

				filter_convolution[j][0] = ( fft_data[j][0] / PADDED_SIZE ) * value;
				filter_convolution[j][1] = ( fft_data[j][1] / PADDED_SIZE ) * value;

				filter_convolution[PADDED_SIZE- j][0] = 0.0;
				filter_convolution[PADDED_SIZE- j][1] = 0.0;
			}

			//Take the inverse FFT.
			fftw_execute(plan_backward);

			//Calculate the power and store it in result
			for (j = 0; j < n; ++j)
			{
				result[i * n + j] = MAGNITUDE(fftw_result[j][0], fftw_result[j][1]) / ( NORMALIZATION_FACTOR * sqrt(scales[i]) );
				temp[j] = result[i * n + j];
			}

			double total = 0;
			for (int j = 0; j < n; ++j)
			{
				total += temp[j];
			}

			double variance = gsl_stats_variance(temp, 1, n);
			fprintf(debug_file, "%.16f\t%.16f\t%.16f\n", SCALE_TO_FREQ(scales[i]), variance, total);

		}

		free(temp);
		fclose(debug_file);

		//FFTW sanitation engineering.
		fftw_destroy_plan(plan_backward);
	    fftw_free(fftw_result);
	    fftw_free(filter_convolution);
	// }
	//Sanitation Engineering
	fftw_destroy_plan(plan_forward);
	fftw_free(fft_data); fftw_free(data_in);
    return(0);
} /*Wavelet */

double* ShortTimeFourierTransform(double * raw_data, double sampling_frequency, int n, int window_size)
{
	//Variable Declarations
	int i, j;
	fftw_complex *data_in, *fft_data;
	fftw_plan plan_forward;
	double* result;

	// FILE * STFT_FILE = fopen("STFT_Result.log", "w");

	int num_windows = ceil((double) n / window_size);
	assert(num_windows > 0);

	// const int PADDED_SIZE = CalculatePaddingSize(window_size, 1);
	const int PADDED_SIZE = window_size;

	result =   (double*)        malloc(num_windows * PADDED_SIZE * sizeof(double));
	data_in  = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * PADDED_SIZE );
	fft_data = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * PADDED_SIZE );

	//Calculate the FFT of the data and store it in fft_data
	plan_forward = fftw_plan_dft_1d(PADDED_SIZE, data_in, fft_data,
									FFTW_FORWARD, FFTW_ESTIMATE);

	for (i = 0; i < num_windows; ++i)
	{
		memset(data_in,  0.0, sizeof( fftw_complex ) * PADDED_SIZE);
		memset(fft_data, 0.0, sizeof( fftw_complex ) * PADDED_SIZE);

		//Fill Data into FFT Array
		for (j = 0; j < window_size; ++j)
		{
			if (i * window_size + j < n)
			{
				data_in[j][0] = raw_data[i * window_size + j];
				data_in[j][1] = 0.0;
			}
			else
			{
				data_in[j][0] = 0.0;
				data_in[j][1] = 0.0;
			}
		}

		fftw_execute(plan_forward);

		for (j = 0; j < window_size / 2; ++j)
		{
			result[j * num_windows + i] = MAGNITUDE(fft_data[j][0], fft_data[j][1]);
			// fprintf(STFT_FILE, "%d\t%f\t%d\n", j, result[j * num_windows + i], j * num_windows + i);
		}

	}

	// fclose(STFT_FILE);
	fftw_destroy_plan(plan_forward);
	fftw_free(data_in); fftw_free(fft_data);

	return(result);
}

int Find_Peaks(double* array, double* frequency, int n, int J)
{
	FILE*       maximum_file  = fopen("maximum.log", "w");
	ARRAY_DATA *maximum_array = (ARRAY_DATA*) malloc (J * sizeof(ARRAY_DATA));
	int local_maximum_location[J];
	double* temp = (double*) malloc(n * sizeof(double));

	//Find the local maximum at every frequency
	for (int i = 0; i < J; ++i)
	{
		ARRAY_DATA max;
		max.value = array[i * n];
		max.index = i * n;

		for (int j = 0; j < n; ++j)
		{
			if (array[i * n + j] > max.value)
			{
				max.value = array[i * n + j];
				max.index = i * n + j;
			}
		}

		maximum_array[i] = max;
		fprintf(maximum_file, "%f\t%f\n", frequency[i], maximum_array[i].value);
	}

	//Calculate the deravitive of the signal and isolate the peaks
	double sign = (maximum_array[1].value - maximum_array[0].value) / (frequency[1] - frequency[0]);
	int    max_count = 0;
	for (int i = 0; i < J - 1; ++i)
	{
		double slope = (maximum_array[i + 1].value - maximum_array[i].value) / (frequency[i + 1] - frequency[i]);
		if (signbit(slope) != signbit(sign) && sign < 0)
		{
			local_maximum_location[max_count] = i;
			max_count++;
		}

		sign = slope;
	}

	for (int i = 0; i < max_count; ++i)
	{
		int arr_index = local_maximum_location[i];

		//Copy data into memory block
		for (int j = 0; j < n; ++j)
		{
			temp[j] = array[arr_index * n + j];
			// if (i == 1)
			// 	fprintf(maximum_file, "%f\t%.16f\n", (double) j/FS, array[arr_index * n + j]);

		}

		ARRAY_DATA impact_site = Max(temp, n);

		int system_setteled = 0;
		int setteled_index = 0;
		for (int j = impact_site.index; j < n; ++j)
		{
			if (temp[j] < SETTLING_PERCENTAGE * impact_site.value && system_setteled == 0)
			{
				setteled_index = j;
				double setteled_time = (double) (setteled_index - impact_site.index)/FS;
				printf("Frequency[%d]: %f, Settled Time = %f\n", i, frequency[arr_index], setteled_time);
				system_setteled = 1;
			}
		}
	}

	fclose(maximum_file);
	free(maximum_array);
	free(temp);
	return(0);
}

int CalculatePaddingSize(const int array_size, const int pad_flag)
{
	const int pad = ceil(log2(array_size));
	int out = array_size;
	switch(pad_flag)
	{
		case 0: //No Padding what so ever.
			out = array_size;
			break;
		case 1: //Zero - Padding and preforming a Radix-2 Operation
			out = (int) pow(2, pad + 1);
    		break;
    	case 2: //Duplicate array and ramp up and ramp down output
    		out = 2 * array_size;
    		break;

    	case 3: //Repeat the array once.
    		out = 2 * array_size;
    		break;

    	default: //Else return the array size
    		out = array_size;
    		break;
	}
	return(out);
}

void GenerateScalesAndFrequency(const int min_i, const int max_i, const double s_0,
	double* scales, double* frequency)
{
	int count = ( max_i - min_i ) + 1;

	//Populate the scales array
	for (int i = 0; i < count; ++i)
	{
		int counterVariable = min_i + i;
		scales[i] = s_0 * pow(2, counterVariable * D_J);
	}

	IdentifyFrequencies(scales, count, frequency);

}

void IdentifyFrequencies(double* scales, int count, double* frequency)
{
	// double * frequency = (double*) malloc( count * sizeof(double) );

	for (int i = 0; i < count; ++i)
	{
		frequency[i] = (W_0)/(scales[i] * 2 * M_PI);
	}

}

double CompleteFourierMorlet(double w, const double scale, double norm)
{
	// double norm = 1.0/sqrt(scale);
	// double norm = sqrt(scale);

	w = w * scale;
	double out = exp( -0.5 * ( W_0 - w ) * (W_0 - w) )
                                       - K_SIGMA * (exp ( -0.5 * w * w));

	out = norm * C_SIGMA * QUAD_ROOT_PI * out;
	return(out);
}

void TestCases(double *data, const int flag, double freq, double sampling_frequency, int data_size)
{
	// Fit a freq signal at two points
	// double DT = 1./sampling_frequency;
	double fsig = freq/sampling_frequency;
	double dw = 2 * M_PI * fsig;
	double w0 =  0.001; // A SMALL PHASE SHIFT SO ITS NOT ALL INTERGER ALIGNED
	int one_peri = (int)1./fsig;

	double frequency_increment = (MAX_FREQUENCY - MIN_FREQUENCY)/ 3.0; //3.0 seconds.

	int t = 2 * sampling_frequency; //At 2 seconds.

	switch(flag)
	{
		//Impulse at T = 2 seconds
		case 1:
			for (int i = 0; i < data_size; ++i)
			{
				data[i] = 0.0;
			}

			data[t] = 1.0;
			break;

		//Multiple Sines at t = 1500
		case 2:
			for (int i = data_size/2; i < data_size/2 + 2*one_peri; ++i)
			{
				data[i] = sin((i - data_size/2)* dw + w0) + sin((i - data_size/2)* 2* dw + w0);
			}
			break;

		//Multiple Sines at all times
		case 3:
			for (int i = 0; i < data_size; ++i)
			{
				data[i] = sin(i*dw + w0) + sin(i*2*dw + w0);
			}
			break;

		//Single sine at t = 1.0s;
		case 4:
			for (int i = data_size/3; i < data_size/3 + 2 * one_peri; ++i)
			{
				data[i] = sin( (i - data_size/2) * dw + w0 );
			}
			break;

		case 5:
			for (int i = 0; i < data_size; ++i)
			{
				data[i] = cos( i * dw + w0 );

				if (i >= data_size/2 && i <= 2 * (data_size)/3)
				{
					data[i] = 2 * cos(i * dw + w0);
				}
			}
			break;

		case 6:
			for (int i = 0; i < data_size; ++i)
			{
				data[i] = cos(i * dw + w0 );
				if (i >= data_size/3 && i <= data_size/2)
				{
					data[i] = cos(i * (dw - 0.005) + w0);
				}
			}
			break;

		//Frequency Sweep
		case 7:
			for (int i = 0; i < data_size; ++i)
			{
				// fsig = frequency/sampling_frequency;
				// dw = 2*M_PI*fsig;

				data[i] = sin(w0 + 2 * M_PI * (MIN_FREQUENCY + (frequency_increment/2) * pow(i/sampling_frequency, 2)) );

				// frequency += frequency_increment;
			}
			break;

		//Single sine all the way through.
		case 8:
			for (int i = 0; i < data_size; ++i)
			{
				data[i] = cos(i*dw + w0);
			}
			break;
	}
}

int WriteDebug(const double *data, const int length, const int sampling_frequency,
	const char* filename)
{
	FILE* out_file=fopen(filename,"w");
    if (out_file == NULL) return -1;

    double t = 0.0;
    double dt = 1.0/sampling_frequency;

	for (int i = 0; i < length; ++i)
    {
    	// double value = (double) i/length;
    	fprintf(out_file, "%f\t%.16e\n", t, data[i]);
    	t += dt;
    }

    fclose(out_file);
    return 0;
}

void FillData(double * data)
{
	// Fit a FREQ signal at two points
	// double DT = 1./FS;
	double fsig = FREQ/FS;
	double dw = 2*M_PI*fsig;
	double w0 =  0.01; // A SMALL PHASE SHIFT SO ITS NOT ALL INTERGER ALIGNED
	int one_peri = (int)1./fsig;
	// printf("FS  %.2d   Pitch %.f   Discrete Period = %d \n",FS,FREQ,one_peri);

	for (int i = 0; i < DATA_SIZE; ++i)
	{
		data[i] = 0.0;
	}

	// //Impulse Sample
	// data[2000] = 1.0;
	int i;
	// double t=0;
	for(i=0;i<DATA_SIZE;i++){
		// data[i] = sin(i*dw) + sin(i*dw*4);
		data[i]=0.;
		// if((i>200)&(i<400))data[i]=sin( (i-200)*dw+w0);
		if((i>0.25*DATA_SIZE)&(i<0.25*DATA_SIZE+one_peri)) data[i]=sin( (i-200)*dw+w0);
		if((i>0.5*DATA_SIZE)&(i<0.5*DATA_SIZE+2*one_peri))data[i]=sin( (i-1000)*dw+w0);
		if((i>0.75*DATA_SIZE)&(i<0.75*DATA_SIZE+3*one_peri))data[i]=sin( (i-2000)*dw+w0);
	}
}

int GetFileSize(char filename[])
{
	FILE* signalFile = fopen(filename, "r");
	assert(signalFile != NULL);
	// obtain file size:
	fseek (signalFile , 0 , SEEK_END);
	long lSize = ftell (signalFile);
	rewind (signalFile);

	char * buffer = (char*) malloc(sizeof(char)*lSize);
	assert(buffer != NULL);

	int result = fread (buffer, 1, lSize, signalFile);
	assert(result == lSize);

	char * token = strtok(buffer, "\n");

    //Get input from text.
	int counterVariable = 0;
	while (token !=NULL)
    {
    	// data[counterVariable] = atof(token);
    	counterVariable++;
        token = strtok (NULL, "\n");

    }
    fclose(signalFile);

    return (counterVariable);
}

int ReadFile(double data[], char filename[])
{
	FILE* signalFile = fopen(filename, "r");
	assert(signalFile != NULL);
	// obtain file size:
	fseek (signalFile , 0 , SEEK_END);
	long lSize = ftell (signalFile);
	rewind (signalFile);

	char * buffer = (char*) malloc(sizeof(char)*lSize);
	assert(buffer != NULL);

	int result = fread (buffer, 1, lSize, signalFile);
	assert(result == lSize);
	// puts(buffer);

	char * token = strtok(buffer, "\n");

    //Get input from text.
	int counterVariable = 0;
	while (token !=NULL)
    {
    	data[counterVariable] = atof(token);
    	counterVariable++;
        token = strtok (NULL, "\n");

    }
    fclose(signalFile);

    return (counterVariable);
}

double CWT_Cosine_Real(double time, double scale)
{
    double norm = sqrt(scale);
    double w_o = FREQ * 2 * M_PI;
    w_o *= scale;
    double mor = exp( - 0.5 * (W_0 - w_o) * (W_0 - w_o) ) + K_SIGMA * exp( -0.5 * w_o * w_o );
    double cosine = cos(w_o * time);

    double out = 0.5 * C_SIGMA * QUAD_ROOT_PI * norm * mor * cosine;
    return(out);
}

double CWT_Cosine_Complex(double time, double scale)
{
    double norm = sqrt(scale);
    double w_o = FREQ * 2 * M_PI;
    w_o *= scale;
    double mor = exp( - 0.5 * (W_0 - w_o) * (W_0 - w_o) ) + K_SIGMA * exp( -0.5 * w_o * w_o );
    double sine = sin(w_o * time);

    double out = 0.5 * C_SIGMA * QUAD_ROOT_PI * norm * mor * sine;
    return(out);
}

double CWT_Dirac_Real(double time, double scale)
{
    double impulse = 2.0;
    time = (impulse - time )/scale;
    double norm = 1.0/sqrt(scale);
    double out = exp( - 0.5 * time * time ) * ( cos( W_0 * time ) - K_SIGMA );

    out = norm * C_SIGMA * QUAD_ROOT_PI * out;
    return(out);
}

double CWT_Dirac_Complex(double time, double scale)
{
    double impulse = 2.0;
    time = (impulse - time )/scale;
    double norm = 1.0/sqrt(scale);
    double out = exp( - 0.5 * time * time ) * ( sin( W_0 * time ) - K_SIGMA );
    out = norm * C_SIGMA * QUAD_ROOT_PI * out;
    return(out);
}

ARRAY_DATA Max(double * array, int size)
{
	double max = array[0];
	int array_index = 0;

	ARRAY_DATA out;

	for (int i = 0; i < size; ++i)
	{
		if (array[i] > max)
		{
			max = array[i];
			// if (max != max)
			// {
			// 	printf("Naan Alert!\n");
			// }
			array_index = i;
		}
	}

	out.value = max;
	out.index = array_index;

	// printf("Max: Array[%d] = %.17f\n", array_index, array[array_index]);
	return(out);
}

/**
    \fn int OpenFile(const char* fileName, struct edf_hdr_struct *header)

    \brief Openes a .BDF file and allocates it to an edf_hdr_struct.

    \param fileName The name and location of the file to be opened
    \param header The pointer to the edf header structure

    \return 0 if file is opened successfully
    \return -1 if there is an error

*/
int OpenFile(const char* fileName, struct edf_hdr_struct *header)
{
    if(edfopen_file_readonly(fileName, header, EDFLIB_READ_ALL_ANNOTATIONS))
    {
        switch(header->filetype)
        {
            case EDFLIB_MALLOC_ERROR                : printf("\nmalloc error\n\n");
                                                    break;
            case EDFLIB_NO_SUCH_FILE_OR_DIRECTORY   : printf("\ncan not open file, no such file or directory\n\n");
                                                    break;
            case EDFLIB_FILE_CONTAINS_FORMAT_ERRORS : printf("\nthe file is not EDF(+) or BDF(+) compliant\n"
                                                           "(it contains format errors)\n\n");
                                                    break;
            case EDFLIB_MAXFILES_REACHED            : printf("\nto many files opened\n\n");
                                                    break;
            case EDFLIB_FILE_READ_ERROR             : printf("\na read error occurred\n\n");
                                                    break;
            case EDFLIB_FILE_ALREADY_OPENED         : printf("\nfile has already been opened\n\n");
                                                    break;
            default                                 : printf("\nunknown error\n\n");
                                                    break;
        }

    return(-1);
    }
    return(0);
}

/**
    \fn int64_t FindTriggers(const int * statusInput, const int64_t numberOfRecords,
                        int64_t * outputBuffer)

    \brief This function should take an array input and return the rising and falling edges of the triggers.

    \param statusInput: The Status Channel Input from the BDF or EDF flie. use the edfread_digital_samples
    \param numberOfRecords: The size of statusInput
    \param outputBuffer: a 1 x 2 * MAXIMUM_TRIGGERS int64_t array with the odd entries being the
                rising edge and the even entries being the falling edges.

    \return counterVariable The number of triggers that were found.
*/

int FindTriggers(const int * statusInput, const int64_t numberOfRecords,
                        int64_t * outputBuffer)
{
    int64_t counterVariable = 0;
    int counterVariable_int = 0;
    //int i needs to be int64_t because we are recording it into a int64_t arrray.
    int edge = 0;
    for (int64_t i = 0; i < numberOfRecords; ++i)
    {
        //Bit and the lower 16 bits to see if any of the triggers have been triggered to on.
        if ( ((statusInput[i] & 0x0000FFFF) > 0) && (edge == 0) && (statusInput[i-1] != statusInput[i]) ) //Rising Edge Detected.
        {
            outputBuffer[counterVariable] = i;
            // printf("outputBuffer[%" PRId64 "] = %" PRId64"\n", counterVariable, outputBuffer[counterVariable]);

            counterVariable++;
            counterVariable_int++;
            edge = 1;
        }

        if (statusInput[i-1] != statusInput[i] && edge == 1) //Falling Edge Detected.
        {
            edge = 0;
        }

        if (counterVariable > MAXIMUM_TRIGGERS)
            return -1;
    }

    return(counterVariable_int);
}

/**
    \fn int FilterTriggers(const int code, const int button, const int numberOfRecords,
            const int64_t * triggerList,
            const int * readBuffer,
            int * outputBuffer)

    \brief Filteres the triggers coming in, and finds the specified events

    \param code The code of the trigger list that is needed
                Possible Inputs: 1, or 2
    \param button The code of the button that is needed
                Possible Inputs 1, or 2
    \param triggerList The list of all of the possible triggers
    \param readBuffer The Status Channel input from the file

    \param outputBuffer The buffer that FilterTriggers will populate with the location of the location of the triggers that we're looking for

    \return counterVariable The number of triggers found.
*/

int FilterTriggers(const int code,
    const int button,
    const int numberOfRecords,
    const int64_t * triggerList,
    const int * readBuffer,
    int * outputBuffer)
{
    int readCode;
    int buttonCode;
    int counterVariable = 0;
    for (int i = 0; i < numberOfRecords; ++i)
    {
        readCode = readBuffer[i] & 255;
        buttonCode = (readBuffer[i] >> 8) & 3;

        if ( (readCode == code) && (buttonCode == button) )
        {
            outputBuffer[counterVariable] = triggerList[i];
            counterVariable++;
        }
    }
    return counterVariable;
}

void CleanData(double * data, double n)
{
    double mean = gsl_stats_mean(data, 1, n);
    double sDeviation = gsl_stats_sd_m(data, 1, n, mean);
    // printf("Mean: %f, SD: %f\n", mean, sDeviation);

    //Compute the Z-Score or Standard Score
    for (int i = 0; i < n; ++i)
    {
        data[i] = (data[i] - mean)/sDeviation;
    }
}

int  WriteFile(const double *data, const double *frequency, const int x, const int y, int sampling_frequency,
    const char filename[])
{

    FILE* out_file=fopen(filename,"w");
    if (out_file == NULL) return -1;

    //Xticks
    fprintf(out_file, "%d\t", x);
    for (int i = 0; i < y; ++i)
    {
        fprintf(out_file, "%f\t", (double) i/sampling_frequency);
    }
    fprintf(out_file, "\n");

    // double small_eps = 0.00001; //Add a small eps so that logs of zero don't happen.
    for (int i = 0; i < x; ++i)
    {
        //Feed Frequency
        fprintf(out_file, "%f\t", frequency[i]);

        //Feed Data
        for (int j = 0; j < y; ++j)
        {
            // value = Magnitude(result[i*n + j], result[i*n + j]);
            fprintf(out_file, "%.16e\t", data[i*y + j]);
        }
        //Ready for the next line.
        fprintf(out_file, "\n");

    }

    fclose(out_file);
    return(0);
}

int WriteGnuplotScript(const char graph_title[], const char filename[])
{
    FILE* gnuplot_file = fopen("script.gplot", "w");
    if (gnuplot_file == NULL) return -1;

    // fprintf(gnuplot_file, "set term x11\n");

    // fprintf(gnuplot_file, "set logscale z 10\n");

    fprintf(gnuplot_file, "set term pngcairo enhanced font 'arial,12'\n");
    fprintf(gnuplot_file, "%s%s%s","set output '", graph_title,  ".png' \n");
    fprintf(gnuplot_file, "set pm3d map\n");
    fprintf(gnuplot_file, "set logscale y 2\n");
    fprintf(gnuplot_file, "set ticslevel 0\n");
    fprintf(gnuplot_file, "set xlabel \"time (s)\"\n");
    fprintf(gnuplot_file, "set ylabel \"Frequency (Hz)\"\n");

    //Input Graph Title
    fprintf(gnuplot_file, "%s%s%s\n", "set title \"", graph_title, "\"");
    //Plot filename
    // plot "DATA.log" matrix nonuniform with pm3d t ''
    fprintf(gnuplot_file, "%s%s%s\n", "splot \"", filename, "\" matrix nonuniform with pm3d t ''");
    // fprintf(gnuplot_file, "pause -1 \"Hit Return to continue\"");

    return(0);
}