/* cdf/gauss.c
 *
 * Copyright (C) 2002, 2004 Jason H. Stover.
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307, USA.
 */

/*
 * Computes the cumulative distribution function for the Gaussian
 * distribution using a rational function approximation.  The
 * computation is for the standard Normal distribution, i.e., mean 0
 * and standard deviation 1. If you want to compute Pr(X < t) for a
 * Gaussian random variable X with non-zero mean m and standard
 * deviation sd not equal to 1, find gsl_cdf_ugaussian ((t-m)/sd).
 * This approximation is accurate to at least double precision. The
 * accuracy was verified with a pari-gp script.  The largest error
 * found was about 1.4E-20. The coefficients were derived by Cody.
 *
 * References:
 *
 * W.J. Cody. "Rational Chebyshev Approximations for the Error
 * Function," Mathematics of Computation, v23 n107 1969, 631-637.
 *
 * W. Fraser, J.F Hart. "On the Computation of Rational Approximations
 * to Continuous Functions," Communications of the ACM, v5 1962.
 *
 * W.J. Kennedy Jr., J.E. Gentle. "Statistical Computing." Marcel Dekker. 1980.
 *
 *
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>

#ifndef M_1_SQRT2PI
#define M_1_SQRT2PI (M_2_SQRTPI * M_SQRT1_2 / 2.0)
#endif

#define SQRT32 (4.0 * M_SQRT2)

/*
 * IEEE double precision dependent constants.
 *
 * GAUSS_EPSILON: Smallest positive value such that
 *                gsl_cdf_gaussian(x) > 0.5.
 * GAUSS_XUPPER: Largest value x such that gsl_cdf_gaussian(x) < 1.0.
 * GAUSS_XLOWER: Smallest value x such that gsl_cdf_gaussian(x) > 0.0.
 */

#define GAUSS_EPSILON  (GSL_DBL_EPSILON / 2)
#define GAUSS_XUPPER (8.572)
#define GAUSS_XLOWER (-37.519)

#define GAUSS_SCALE (16.0)

static double
get_del (double x, double rational)
{
  double xsq = 0.0;
  double del = 0.0;
  double result = 0.0;

  xsq = floor (x * GAUSS_SCALE) / GAUSS_SCALE;
  del = (x - xsq) * (x + xsq);
  del *= 0.5;

  result = exp (-0.5 * xsq * xsq) * exp (-1.0 * del) * rational;

  return result;
}

/*
 * Normal cdf for fabs(x) < 0.66291
 */
static double
gauss_small (const double x)
{
  unsigned int i;
  double result = 0.0;
  double xsq;
  double xnum;
  double xden;

  const double a[5] = {
    2.2352520354606839287,
    161.02823106855587881,
    1067.6894854603709582,
    18154.981253343561249,
    0.065682337918207449113
  };
  const double b[4] = {
    47.20258190468824187,
    976.09855173777669322,
    10260.932208618978205,
    45507.789335026729956
  };

  xsq = x * x;
  xnum = a[4] * xsq;
  xden = xsq;

  for (i = 0; i < 3; i++)
    {
      xnum = (xnum + a[i]) * xsq;
      xden = (xden + b[i]) * xsq;
    }

  result = x * (xnum + a[3]) / (xden + b[3]);

  return result;
}

/*
 * Normal cdf for 0.66291 < fabs(x) < sqrt(32).
 */
static double
gauss_medium (const double x)
{
  unsigned int i;
  double temp = 0.0;
  double result = 0.0;
  double xnum;
  double xden;
  double absx;

  const double c[9] = {
    0.39894151208813466764,
    8.8831497943883759412,
    93.506656132177855979,
    597.27027639480026226,
    2494.5375852903726711,
    6848.1904505362823326,
    11602.651437647350124,
    9842.7148383839780218,
    1.0765576773720192317e-8
  };
  const double d[8] = {
    22.266688044328115691,
    235.38790178262499861,
    1519.377599407554805,
    6485.558298266760755,
    18615.571640885098091,
    34900.952721145977266,
    38912.003286093271411,
    19685.429676859990727
  };

  absx = fabs (x);

  xnum = c[8] * absx;
  xden = absx;

  for (i = 0; i < 7; i++)
    {
      xnum = (xnum + c[i]) * absx;
      xden = (xden + d[i]) * absx;
    }

  temp = (xnum + c[7]) / (xden + d[7]);

  result = get_del (x, temp);

  return result;
}

/*
 * Normal cdf for
 * {sqrt(32) < x < GAUSS_XUPPER} union { GAUSS_XLOWER < x < -sqrt(32) }.
 */
static double
gauss_large (const double x)
{
  int i;
  double result;
  double xsq;
  double temp;
  double xnum;
  double xden;
  double absx;

  const double p[6] = {
    0.21589853405795699,
    0.1274011611602473639,
    0.022235277870649807,
    0.001421619193227893466,
    2.9112874951168792e-5,
    0.02307344176494017303
  };
  const double q[5] = {
    1.28426009614491121,
    0.468238212480865118,
    0.0659881378689285515,
    0.00378239633202758244,
    7.29751555083966205e-5
  };

  absx = fabs (x);
  xsq = 1.0 / (x * x);
  xnum = p[5] * xsq;
  xden = xsq;

  for (i = 0; i < 4; i++)
    {
      xnum = (xnum + p[i]) * xsq;
      xden = (xden + q[i]) * xsq;
    }

  temp = xsq * (xnum + p[4]) / (xden + q[4]);
  temp = (M_1_SQRT2PI - temp) / absx;

  result = get_del (x, temp);

  return result;
}

double
gsl_cdf_ugaussian_P (const double x)
{
  double result;
  double absx = fabs (x);

  if (absx < GAUSS_EPSILON)
    {
      result = 0.5;
      return result;
    }
  else if (absx < 0.66291)
    {
      result = 0.5 + gauss_small (x);
      return result;
    }
  else if (absx < SQRT32)
    {
      result = gauss_medium (x);

      if (x > 0.0)
        {
          result = 1.0 - result;
        }

      return result;
    }
  else if (x > GAUSS_XUPPER)
    {
      result = 1.0;
      return result;
    }
  else if (x < GAUSS_XLOWER)
    {
      result = 0.0;
      return result;
    }
  else
    {
      result = gauss_large (x);

      if (x > 0.0)
        {
          result = 1.0 - result;
        }
    }

  return result;
}

double
gsl_cdf_ugaussian_Q (const double x)
{
  double result;
  double absx = fabs (x);

  if (absx < GAUSS_EPSILON)
    {
      result = 0.5;
      return result;
    }
  else if (absx < 0.66291)
    {
      result = gauss_small (x);

      if (x < 0.0)
        {
          result = fabs (result) + 0.5;
        }
      else
        {
          result = 0.5 - result;
        }

      return result;
    }
  else if (absx < SQRT32)
    {
      result = gauss_medium (x);

      if (x < 0.0)
        {
          result = 1.0 - result;
        }

      return result;
    }
  else if (x > -(GAUSS_XLOWER))
    {
      result = 0.0;
      return result;
    }
  else if (x < -(GAUSS_XUPPER))
    {
      result = 1.0;
      return result;
    }
  else
    {
      result = gauss_large (x);

      if (x < 0.0)
        {
          result = 1.0 - result;
        }

    }

  return result;
}

double
gsl_cdf_gaussian_P (const double x, const double sigma)
{
  return gsl_cdf_ugaussian_P (x / sigma);
}

double
gsl_cdf_gaussian_Q (const double x, const double sigma)
{
  return gsl_cdf_ugaussian_Q (x / sigma);
}


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

#include <nlopt.h>

typedef struct {
     int N;
     double *x, *y; /* length N; */
} lorentzdata;

static double sqr(double x) { return x * x; }

static int count = 0;

static double lorentzerr(int n, const double *p, double *grad, void *data)
{
     lorentzdata *d = (lorentzdata *) data;
     int N = d->N;
     const double *xs = d->x;
     const double *ys = d->y;
     double val = 0;
     int i, j;

     for (i = 0; i < N; ++i) {
	  double x = xs[i], y = ys[i];
	  double lorsum = 0;

	  for (j = 0; j < n; j += 3) {
	       double A = p[j + 0];
	       double w = p[j + 1];
	       double G = p[j + 2];
	       double lor = A / (sqr(x - w) + G*G);

	       lorsum += lor;
	  }

	  val += sqr(y - lorsum);

	  if (grad)
	       for (j = 0; j < n; j += 3) {
		    double A = p[j + 0];
		    double w = p[j + 1];
		    double G = p[j + 2];
		    double deninv =  1.0 / (sqr(x - w) + G*G);

		    grad[j + 0] += -2 * (y - lorsum) * deninv;
		    grad[j + 1] += 4*A * (w - x) * (y - lorsum) * sqr(deninv);
		    grad[j + 2] += 4*A * G * (y - lorsum) * sqr(deninv);
	       }
     }
     ++count;
     // printf("%d: f(%g,%g,%g) = %g\n", count, p[0],p[1],p[2], val);
     return val;
}

extern double nlopt_urand(double a, double b);

int main(void)
{
     lorentzdata d;
     int i;
     double A = 1, w = 0, G = 1, noise=0.01;
     double lb[3] = {-HUGE_VAL,-HUGE_VAL,0};
     double ub[3] = {HUGE_VAL,HUGE_VAL,HUGE_VAL};
     double p[3] = {0,1,2}, minf;

     nlopt_srand_time();

     d.N = 200;
     d.x = (double *) malloc(sizeof(double) * d.N * 2);
     d.y = d.x + d.N;
     for (i = 0; i < d.N; ++i) {
	  d.x[i] = nlopt_urand(-0.5, 0.5) * 8*G + w;
	  d.y[i] = 2*noise*nlopt_urand(-0.5,0.5) + A / (sqr(d.x[i]-w) + G*G);
     }

     nlopt_minimize(NLOPT_LN_NEWUOA_BOUND, 3, lorentzerr, &d,
		    lb, ub, p, &minf,
		    -HUGE_VAL, 0,0, 1e-6,NULL, 0,0);

     printf("%d minf=%g at A=%g, w=%g, G=%g\n", count, minf, p[0],p[1],p[2]);

     count = 0;
     nlopt_minimize(NLOPT_LN_COBYLA, 3, lorentzerr, &d,
		    lb, ub, p, &minf,
		    -HUGE_VAL, 0,0, 1e-6,NULL, 0,0);

     printf("%d minf=%g at A=%g, w=%g, G=%g\n", count, minf, p[0],p[1],p[2]);

     count = 0;
     nlopt_minimize(NLOPT_LN_NELDERMEAD, 3, lorentzerr, &d,
		    lb, ub, p, &minf,
		    -HUGE_VAL, 0,0, 1e-6,NULL, 0,0);

     printf("%d minf=%g at A=%g, w=%g, G=%g\n", count, minf, p[0],p[1],p[2]);

     count = 0;
     nlopt_minimize(NLOPT_LN_SBPLX, 3, lorentzerr, &d,
		    lb, ub, p, &minf,
		    -HUGE_VAL, 0,0, 1e-6,NULL, 0,0);

     printf("%d minf=%g at A=%g, w=%g, G=%g\n", count, minf, p[0],p[1],p[2]);

     return 0;
}


#ifndef __GSLSTREAM_H__
#define __GSLSTREAM_H__

#include "RngStream.h"
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

namespace rng {
  class GSLStream : public RngStream {
    public:
      GSLStream() : type(gsl_rng_taus2) {}
      virtual ~GSLStream() { free(); }

      inline void alloc(unsigned long seed = time(NULL)) {
        if (! is_alloc) {
          rng = gsl_rng_alloc(type);
          is_alloc = 1;
          gsl_rng_set(rng,seed);
        }
      }

      inline void free() {
        if (is_alloc) {
          gsl_rng_free(rng);
          is_alloc = 0;
        }
      }

      inline void uniform(size_t n, double* r, double a = 0.0, double b = 1.0) {
        for (size_t i = 0; i < n; ++i) {
          r[i] = a + (b-a)*gsl_rng_uniform(rng);
        }
      }

      inline void uniform_int(size_t n, int* r, int a = 0, int b = 10) {
        for (size_t i = 0; i < n; ++i) {
          r[i] = a + gsl_rng_uniform_int(rng,b-a);
        }
      }

      inline void shuffle(int* x, size_t n) { gsl_ran_shuffle(rng,x,n,sizeof(int)); }
      inline void shuffle(double* x, size_t n) { gsl_ran_shuffle(rng,x,n,sizeof(double)); }

      inline void multinomial(size_t k, size_t n, const double* p, unsigned* a) {
        gsl_ran_multinomial(rng,k,n,p,a);
      }

      inline void gaussian(size_t n, double* r, double mu = 0.0, double sigma = 1.0) {
        *r = gsl_ran_gaussian(rng,sigma) + mu;
      };

      inline void poisson(size_t n, int* k, double lambda) {
        *k = gsl_ran_poisson(rng,lambda);
      }

    protected:
      const gsl_rng_type* type;
      gsl_rng* rng;
  };
}

#endif


/** @file */
#ifndef __CCL_UTILS_H_INCLUDED__
#define __CCL_UTILS_H_INCLUDED__

#include <gsl/gsl_spline.h>

#define CCL_MIN(a, b)  (((a) < (b)) ? (a) : (b))
#define CCL_MAX(a, b)  (((a) > (b)) ? (a) : (b))

CCL_BEGIN_DECLS

typedef enum ccl_integration_t {
  ccl_integration_qag_quad = 500,  // GSL's quad
  ccl_integration_spline = 501,  // Spline integral
} ccl_integration_t;

/**
 * Compute bin edges of N-1 linearly spaced bins on the interval [xmin,xmax]
 * @param xmin minimum value of spacing
 * @param xmax maximum value of spacing
 * @param N number of bins plus one (number of bin edges)
 * @return x, bin edges in range [xmin, xmax]
 */
double * ccl_linear_spacing(double xmin, double xmax, int N);

/**
 * Compute bin edges of N-1 logarithmically and then linearly spaced bins on the interval [xmin,xmax]
 * @param xminlog minimum value of spacing
 * @param xmin value when logarithmical ends and linear spacing begins
 * @param xmax maximum value of spacing
 * @param Nlin number of linear bins plus one (number of bin edges)
 * @param Nlog number of logarithmic bins plus one (number of bin edges)
 * @return x, bin edges in range [xminlog, xmax]
 */
double * ccl_linlog_spacing(double xminlog, double xmin, double xmax, int Nlin, int Nlog);

/**
 * Compute bin edges of N-1 logarithmically spaced bins on the interval [xmin,xmax]
 * @param xmin minimum value of spacing
 * @param xmax maximum value of spacing
 * @param N number of bins plus one (number of bin edges)
 * @return x, bin edges in range [xmin, xmax]
 */
double * ccl_log_spacing(double xmin, double xmax, int N);
//Returns array of N logarithmically-spaced values between xmin and xmax

double ccl_j_bessel(int l,double x);
//Spherical Bessel function of order l (adapted from CAMB)

/**
 * Compute spline integral.
 * @param nx number of elements in input array.
 * @param ny number of y arrays.
 * @param x input x-values.
 * @param y input y-values (ny arrays with nx elements).
 * @param a lower end of integration range.
 * @param b upper end of integration range (use b<a if you just want to integrate over all of y).
 * @param result array of output spline integral values.
 * @param T spline type.
 * @param status status flag.
 */
void ccl_integ_spline(int ny, int nx,double *x,double **y,
                      double a, double b, double *result,
                      const gsl_interp_type *T, int *status);

CCL_END_DECLS

#endif


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

#define MATRIX_SIZE 3

static const double matrix_1[MATRIX_SIZE][MATRIX_SIZE] = {
  {1., 4., 2.},
  {-1., -2., 1.},
  {3., 20., 19.},
};
static const double vector_1[] = {8., 3., 71.};

static const double matrix_2[MATRIX_SIZE][MATRIX_SIZE] = {
  {1.,    0.1,    0.225},
  {-0.15, 1.,     0.2},
  {-0.4,  -0.025, 1.}
};
static double const vector_2[] = {0.1, 0.9, 0.8};

void ulDecomposition(const gsl_matrix *A, const gsl_vector *b) {
  gsl_matrix *L = gsl_matrix_calloc(MATRIX_SIZE, MATRIX_SIZE);
  gsl_matrix *U = gsl_matrix_calloc(MATRIX_SIZE, MATRIX_SIZE);

  for (size_t i = 0; i < MATRIX_SIZE; i++) {
    gsl_matrix_set(L, i, i, 1.);
  }

}

int main(void) {
  printf("Gauss.\n\n");

  // Alloc matrix`s
  gsl_matrix_const_view matrix_1v = gsl_matrix_const_view_array((const double *) matrix_1, MATRIX_SIZE, MATRIX_SIZE);
  gsl_vector_const_view vector_1v = gsl_vector_const_view_array(vector_1, MATRIX_SIZE);

  const gsl_matrix *matrix1 = &matrix_1v.matrix;
  const gsl_vector *vector1 = &vector_1v.vector;

  ulDecomposition(matrix1, vector1);
  // gsl_matrix *orig_matrix = gsl_matrix_alloc(MATRIX_SIZE, MATRIX_SIZE);

  // gsl_vector *vector1 = gsl_vector_alloc(MATRIX_SIZE);
  // gsl_vector *orig_vector = gsl_vector_alloc(MATRIX_SIZE);

  // gsl_vector *result1 = gsl_vector_alloc(MATRIX_SIZE);
  // gsl_vector *result2 = gsl_vector_alloc(MATRIX_SIZE);

  // Init matrix

  // Init vector
}

void gaussSeidelSolve() {

}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_multifit.h>

//DEFINES
#define SUCCESS 1
#define FAILURE -1
#define PROGRESS "===================="
#define PRG_WIDTH 20

//FUNCTIONS DECLARATION
int rows_number(char *);
double mean(double *, int);
void cumsum(double *, double *, int);
void slice_vec(double *, double *, int, int);
void polynomialFit(int, int, double *, double *, double *);

//MAIN
int main(int argc, char **argv)
{
    //inputs
    int input_args = 6;
    if(argc < input_args){
        printf("Not enough input arguments!\n");
        return FAILURE;
    }else if(argc > input_args){
        printf("Too many input arguments!\n");
        return FAILURE;
    }
    char file_name[255];
    memset(file_name, 0x00, sizeof(file_name));
    sprintf(file_name, "%s", argv[1]);
    int min_win = atoi(argv[2]);
    int ord = atoi(argv[3]);
    int rev_seg = atoi(argv[4]);
    char path_tot[255];
    memset(path_tot, 0x00, sizeof(path_tot));
    sprintf(path_tot, "%s/dfa_mw=%d_pol=%d_bck=%d.txt", argv[5], min_win, ord, rev_seg);
    //file length
    int N = rows_number(file_name);
    if(N == FAILURE){
        printf("Cannot open input file.\n");
        return FAILURE;
    }
    //time series
    double *pn;
    pn = calloc(N, sizeof(double));
    if(!pn){
        printf("MALLOC ERROR (pn)\n");
        return FAILURE;
    }
    FILE *f;
    f = fopen(file_name, "r");
    if(!f){
        printf("Cannot open input file.\n");
        return FAILURE;
    }
    for(int i = 0; i < N; i++)
        fscanf(f, "%lf", pn+i);
    fclose(f);
    printf("Data length: <%d>\n", N);
    //time vector
    double *t;
    t = calloc(N, sizeof(double));
    if(!t){
        printf("MALLOC ERROR (t)\n");
        return FAILURE;
    }
    for(int i = 0; i < N; i++)
        t[i] = (double)(i+1);
    //time series minus its mean
    double a_ave = mean(pn, N);
    double *pn_nomean;
    pn_nomean = calloc(N, sizeof(double));
    if(!pn_nomean){
        printf("MALLOC ERROR (pn_nomean)\n");
        return FAILURE;
    }
    for(int i = 0; i < N; i++)
        pn_nomean[i] = pn[i] - a_ave;
    //cumulative sum
    double *y;
    y = calloc(N, sizeof(double));
    if(!y){
        printf("MALLOC ERROR (y)\n");
        return FAILURE;
    }
    printf("Data integration...");
    cumsum(pn_nomean, y, N);
    printf("done!\n");
    //defining parameters
    int max_win = 5;
    int end_dfa = N / max_win;
    int *s;
    int range_dfa = end_dfa - min_win + 1;
    s = calloc(range_dfa, sizeof(int));
    if(!s){
        printf("MALLOC ERROR (s)\n");
        return FAILURE;
    }
    for(int i = 0; i < range_dfa; i++)
        s[i] = i + min_win;
    //fluctuations vector and other arrays
    double *F, *F_nu1, *F_nu2, *t_fit, *y_fit, *diff_vec;
    int F_len = N / min_win;
    F = calloc(range_dfa, sizeof(double));
    if(!F){
        printf("MALLOC ERROR (F)\n");
        return FAILURE;
    }
    F_nu1 = calloc(F_len, sizeof(double));
    if(!F_nu1){
        printf("MALLOC ERROR (F_nu1)\n");
        return FAILURE;
    }
    F_nu2 = calloc(F_len, sizeof(double));
    if(!F_nu2){
        printf("MALLOC ERROR (F_nu2)\n");
        return FAILURE;
    }
    t_fit = calloc(end_dfa, sizeof(double));
    if(!t_fit){
        printf("MALLOC ERROR (t_fit)\n");
        return FAILURE;
    }
    y_fit = calloc(end_dfa, sizeof(double));
    if(!y_fit){
        printf("MALLOC ERROR (y_fit)\n");
        return FAILURE;
    }
    diff_vec = calloc(end_dfa, sizeof(double));
    if(!diff_vec){
        printf("MALLOC ERROR (diff_vec)\n");
        return FAILURE;
    }
    //computation
    int start_lim, end_lim;
    double *fit_coeffs;
    fit_coeffs = calloc(ord+1, sizeof(double));
    if(!fit_coeffs){
        printf("MALLOC ERROR (fit_coeffs)\n");
        return FAILURE;
    }
    for(int i = 0; i < range_dfa; i++){
        int N_s = N / s[i];
        double perc = i * 100 / (double)range_dfa;
        int prg = (i * PRG_WIDTH) / range_dfa;
        printf("Computing fluctuations => [%.*s%*s] %.2lf%%\r", prg, PROGRESS, PRG_WIDTH-prg, "", perc);
        fflush(stdout);
        for(int v = 0; v < N_s; v++){
            start_lim = v * s[i];
            end_lim = (v + 1) * s[i] - 1;
            slice_vec(t, t_fit, start_lim, end_lim);
            slice_vec(y, y_fit, start_lim, end_lim);
            polynomialFit(s[i], ord+1, t_fit, y_fit, fit_coeffs);
            for(int j = 0; j < s[i]; j++){
                for(int k = 0; k < ord+1; k++)
                    y_fit[j] -= fit_coeffs[k] * pow(t_fit[j], k);
                diff_vec[j] = pow(y_fit[j], 2.0);
            }
            F_nu1[v] = mean(diff_vec, s[i]);
        }
        if(rev_seg == 1){
            for(int v = 0; v < N_s; v++){
                start_lim = v * s[i] + (N - N_s * s[i]);
                end_lim = (v + 1) * s[i] + (N - N_s * s[i]);
                slice_vec(t, t_fit, start_lim, end_lim);
                slice_vec(y, y_fit, start_lim, end_lim);
                polynomialFit(s[i], ord+1, t_fit, y_fit, fit_coeffs);
                for(int j = 0; j < s[i]; j++){
                    for(int k = 0; k < ord+1; k++)
                        y_fit[j] -= fit_coeffs[k] * pow(t_fit[j], k);
                    diff_vec[j] = pow(y_fit[j], 2.0);
                }
                F_nu2[v] = mean(diff_vec, s[i]);
            }
            F[i] = sqrt((mean(F_nu1, N_s) + mean(F_nu2, N_s)) / (double)2);
        }else{
            F[i] = sqrt(mean(F_nu1, N_s));
        }
    }
    printf("Computing fluctuations => [%s] 100.00%%\r", PROGRESS);
    fflush(stdout);
    printf("\n");
    free(F_nu1); free(F_nu2);
    free(t_fit); free(y_fit);
    free(diff_vec); free(fit_coeffs);
    //HURST EXPONENT
    printf("Computing hurst exponent...");
    double *log_s, *log_F, *H_fit;
    log_s = calloc(range_dfa, sizeof(double));
	if(!log_s){
        printf("MALLOC ERROR (log_s)\n");
        return FAILURE;
    }
    log_F = calloc(range_dfa, sizeof(double));
    if(!log_F){
        printf("MALLOC ERROR (log_F)\n");
        return FAILURE;
    }
    H_fit = calloc(2, sizeof(double));
    if(!H_fit){
        printf("MALLOC ERROR (H_fit)\n");
        return FAILURE;
    }
    for(int i = 0; i < range_dfa; i++){
        log_s[i] = log(s[i]);
        log_F[i] = log(F[i]);
    }
    polynomialFit(range_dfa, 2, log_s, log_F, H_fit);
    printf("done\n");
    printf("Output file...");
    f = fopen(path_tot, "w");
    if(!f){
        printf("Cannot open output file.\n");
        return FAILURE;
    }
    for(int i = 0; i < range_dfa; i++)
        fprintf(f, "%d %lf %lf %lf\n", s[i], F[i], H_fit[0]+log_s[i]*H_fit[1], H_fit[1]);
    fclose(f);
    printf("done\n");
    free(F); free(log_s); free(log_F); free(H_fit);
    return 0;
}

//FUNCTIONS
int rows_number(char *file_name)
{
    FILE *f;
    int stop;
    int lines = 0;
    f = fopen(file_name,"r");
    if(!f){
        printf("Cannot open file %s\n", file_name);
        return FAILURE;
    }
    while(!feof(f)){
        stop = fgetc(f);
        if(stop == '\n')
            lines++;
    }
    fclose(f);
    return lines;
}

double mean(double *vec, int L)
{
    double avg = 0.0;
    for(int i = 0; i < L; i++)
        avg += vec[i];
    avg /= (double)L;
    return avg;
}

void cumsum(double *vec, double *sum_vec, int L)
{
    sum_vec[0] = vec[0];
    for(int i = 1; i < L; i++)
        sum_vec[i] = sum_vec[i-1] + vec[i];
}

void slice_vec(double *all_vec, double *sliced_vec, int start, int end)
{
    for(int i = 0; i <= (end-start); i++)
        sliced_vec[i] = all_vec[start+i];
}

void polynomialFit(int obs, int degree, double *dx, double *dy, double *store)
{
    gsl_multifit_linear_workspace *ws;
    gsl_matrix *cov, *X;
    gsl_vector *y, *c;
    double chisq;
    int i, j;
    //alloc
    X = gsl_matrix_alloc(obs, degree);
    y = gsl_vector_alloc(obs);
    c = gsl_vector_alloc(degree);
    cov = gsl_matrix_alloc(degree, degree);
    //computation
    for(i = 0; i < obs; i++){
        for(j = 0; j < degree; j++)
            gsl_matrix_set(X, i, j, pow(dx[i], j));
        gsl_vector_set(y, i, dy[i]);
    }
    ws = gsl_multifit_linear_alloc(obs, degree);
    gsl_multifit_linear(X, y, c, cov, &chisq, ws);
    for(i = 0; i < degree; i++)
        store[i] = gsl_vector_get(c, i);
    //free memory
    gsl_multifit_linear_free(ws);
    gsl_matrix_free(X);
    gsl_matrix_free(cov);
    gsl_vector_free(y);
    gsl_vector_free(c);
}


/*
 * Copyright 2009-2019 The VOTCA-MPIP Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * author: Denis Andrienko
 */

#ifndef __VOTCA_CTP_GSL_H
#define __VOTCA_CTP_GSL_H

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>

#include <boost/numeric/conversion/cast.hpp>

#define DEBUG_LINALG
#ifdef DEBUG_LINALG
#include <chrono>
#endif

using namespace std;
namespace ub = boost::numeric::ublas;

namespace votca { namespace ctp { namespace linalg {

    /**
     * \brief inverts A
     * @param A symmetric positive definite matrix
     * @param V inverse matrix
     */
inline void invert_symm( const ub::matrix<double> &A, ub::matrix<double> &V){
        // matrix inversion using gsl

    #ifdef DEBUG_LINALG
        auto start = std::chrono::system_clock::now();
    #endif

        gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();
	// signum s (for LU decomposition)
	int s;
        //make copy of A as A is destroyed by GSL
        ub::matrix<double> work=A;
        V.resize(N, N, false);

	// Define all the used matrices
        gsl_matrix_view A_view = gsl_matrix_view_array(&work(0,0), N, N);
        gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_permutation * perm = gsl_permutation_alloc (N);

	// Make LU decomposition of matrix A_view
	gsl_linalg_LU_decomp (&A_view.matrix, perm, &s);

	// Invert the matrix A_view
	(void)gsl_linalg_LU_invert (&A_view.matrix, perm, &V_view.matrix);

        gsl_set_error_handler(handler);

    #ifdef DEBUG_LINALG
        auto end = std::chrono::system_clock::now();

        std::chrono::duration<double> elapsed_seconds = end - start;

        std::cout << "\n... ... ... \x1b[0;34mGSL Invert"
                  << "[" << A.size1() << "x" << A.size2() << "]"
                  << " time: " << elapsed_seconds.count()
                  << "s\x1b[0;39m";
    #endif

}

/**
 * \brief eigenvalues of a symmetric matrix A*x=E*x
 * @param E vector of eigenvalues
 * @param V input: matrix to diagonalize
 * @param V output: eigenvectors
 *
 * This function wraps gsl_eigen_symmv / DSYEV
 *
 */
inline void eigenvalues_symm(const ub::matrix<double> &A,
                                     ub::vector<double> &E,
                                     ub::matrix<double> &V)
{

   #ifdef DEBUG_LINALG
        auto start = std::chrono::system_clock::now();
    #endif

	gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        // gsl does not handle conversion of a symmetric_matrix
        ub::matrix<double> _A( N,N );
        _A = A;

	E.resize(N, false);
	V.resize(N, N, false);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(N);

#ifdef NDEBUG
	(void)gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
#else
	int status = gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
        assert(status == 0);
#endif
	gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_VAL_ASC);
	gsl_eigen_symmv_free(w);
	gsl_set_error_handler(handler);
	//return (status == 0);

    #ifdef DEBUG_LINALG
        auto end = std::chrono::system_clock::now();
        std::chrono::duration<double> elapsed_seconds = end - start;
        std::cout << "\n... ... ... \x1b[0;34mGSL Eigenvalues"
                  << "[" << A.size1() << "x" << A.size2() << "]"
                  << " time: " << elapsed_seconds.count()
                  << "s\x1b[0;39m";
    #endif

};

}}}

/*
 * Since ublas has very slow prod() function, we overwrite it here
 * by the gsl (blas) product function. GSL has separate implementations
 * for floats and doubles hence we have two specializations for these types
 * Separate templates are provided for diagonal and symmetric matreces
 * and their combinations. To check  whether a specific routine is called,
 * define the GSLDEBUG flag
 */
namespace boost { namespace numeric { namespace ublas {

// specialization for double precision (dgemm)
template<class F, class A>
inline matrix<double,F,A>    // prod( m1, m2 )
prod(const matrix<double,F,A> &m1, const matrix<double,F,A> &m2)
{

    #ifdef DEBUG_LINALG
        auto start = std::chrono::system_clock::now();
    #endif

       gsl_matrix_const_view mA = gsl_matrix_const_view_array (&m1(0,0), m1.size1(), m1.size2());
       gsl_matrix_const_view mB = gsl_matrix_const_view_array (&m2(0,0), m2.size1(), m2.size2());

       boost::numeric::ublas::matrix<double,F,A> AxB( m1.size1(), m2.size2() );
       gsl_matrix_view mC = gsl_matrix_view_array (&AxB(0,0), AxB.size1(), AxB.size2());

#ifdef NDEBUG
       (void)gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
                  1.0, &mA.matrix, &mB.matrix,
                  0.0, &mC.matrix);
#else
       int status = gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
                  1.0, &mA.matrix, &mB.matrix,
                  0.0, &mC.matrix);
       assert(status == 0);
#endif

    #ifdef DEBUG_LINALG
        auto end = std::chrono::system_clock::now();

        std::chrono::duration<double> elapsed_seconds = end - start;

        std::cout << "\n... ... ... \x1b[0;34mGSL "
                  << "[" << m1.size1() << "x" << m1.size2() << "]"
                  << "*[" << m2.size1() << "x" << m2.size2() << "]"
                  << " time: " << elapsed_seconds.count()
                  << "s\x1b[0;39m";
    #endif

    return AxB;

}

    // specialization for single precision (sgemm)
    template<class F, class A>
    inline matrix<float,F,A>    // prod( m1, m2 )
    prod(const matrix<float,F,A> &m1, const matrix<float,F,A> &m2)
    {
    #ifdef DEBUG_LINALG
        auto start = std::chrono::system_clock::now();
    #endif

       gsl_matrix_float_const_view mA = gsl_matrix_float_const_view_array (&m1(0,0), m1.size1(), m1.size2());
       gsl_matrix_float_const_view mB = gsl_matrix_float_const_view_array (&m2(0,0), m2.size1(), m2.size2());

       boost::numeric::ublas::matrix<float,F,A> AxB( m1.size1(), m2.size2() );
       gsl_matrix_float_view mC = gsl_matrix_float_view_array (&AxB(0,0), AxB.size1(), AxB.size2());

       gsl_blas_sgemm (CblasNoTrans, CblasNoTrans,
                  1.0, &mA.matrix, &mB.matrix,
                  0.0, &mC.matrix);

    #ifdef DEBUG_LINALG
        auto end = std::chrono::system_clock::now();
        std::chrono::duration<double> elapsed_seconds = end - start;
        std::cout << "\n... ... ... \x1b[0;34mGSL "
                  << "[" << m1.size1() << "x" << m1.size2() << "]"
                  << "*[" << m2.size1() << "x" << m2.size2() << "]"
                  << " time: " << elapsed_seconds.count()
                  << "s\x1b[0;39m";
    #endif

       return AxB;
    }

}}}

#endif  // __VOTCA_CTP_GSL_H


// randomizer.h
// (C) 2013-2020 Nicholas G Davies

#ifndef RANDOMIZER_H
#define RANDOMIZER_H

#include <vector>
#include <limits>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <cmath>
#include <stdexcept>
#include <string>
#include <iostream>

class Randomizer
{
public:
    Randomizer(unsigned long int seed = 0);
    ~Randomizer();

    void Reset();

    double Uniform(double min = 0.0, double max = 1.0);
    double RoundedUniform(double min = 0.0, double max = 1.0, double shoulder = 0.01);
    double Normal(double mean = 0.0, double sd = 1.0);
    double Normal(double mean, double sd, double clamp);
    double Cauchy(double x0 = 0.0, double gamma = 1.0);
    double LogNormal(double zeta = 0.0, double sd = 1.0);
    double Exponential(double rate = 1.0);
    double Gamma(double shape, double scale);
    double Beta(double alpha, double beta);
    unsigned int Discrete(unsigned int size);
    int Discrete(int min, int max);
    // int Discrete(std::vector<unsigned int>& cumulative_weights);
    // int Discrete(std::vector<double>& cumulative_weights);
    void Multinomial(unsigned int N, std::vector<double>& p, std::vector<unsigned int>& n);
    bool Bernoulli(double p);
    unsigned int Binomial(unsigned int n, double p);
    unsigned int BetaBinomial(unsigned int n, double p, double a_plus_b);
    int Poisson(double mean);
    int Geometric(double p);
    int Round(double x);

    template <typename T>
    void Shuffle(std::vector<T>& vec);

    inline gsl_rng* GSL_RNG() { return r; }

private:
    unsigned long int seed;
    gsl_rng* r;
};

Randomizer::Randomizer(unsigned long int s)
 : seed(s), r(gsl_rng_alloc(gsl_rng_mt19937))
{
    Reset();
}

Randomizer::~Randomizer()
{
    gsl_rng_free(r);
}

void Randomizer::Reset()
{
    gsl_rng_set(r, seed);
}

double Randomizer::Uniform(double min, double max)
{
    return min + gsl_rng_uniform(r) * (max - min);
}

double Randomizer::RoundedUniform(double min, double max, double shoulder)
{
    if (min >= max)
        return min;
    double z = Uniform();
    double sd = shoulder * (max - min) / ((1 - shoulder) * 2.50662827463);
    if (z < shoulder / 2)
        return min - abs(Normal(0, sd));
    else if (z < shoulder)
        return max + abs(Normal(0, sd));
    else
        return Uniform(min, max);
}

double Randomizer::Normal(double mean, double sd)
{
    return mean + gsl_ran_gaussian_ziggurat(r, sd);
}

double Randomizer::Normal(double mean, double sd, double clamp)
{
    double n;
    do n = Normal(mean, sd); while (std::fabs(n - mean) > clamp);
    return n;
}

double Randomizer::LogNormal(double zeta, double sd)
{
    return gsl_ran_lognormal(r, zeta, sd);
}

double Randomizer::Cauchy(double x0, double gamma)
{
    return x0 + gsl_ran_cauchy(r, gamma);
}

double Randomizer::Exponential(double rate)
{
    return gsl_ran_exponential(r, 1. / rate);
}

double Randomizer::Gamma(double shape, double scale)
{
    return gsl_ran_gamma(r, shape, scale);
}

double Randomizer::Beta(double alpha, double beta)
{
    return gsl_ran_beta(r, alpha, beta);
}

unsigned int Randomizer::Discrete(unsigned int size)
{
    // TODO improve...
    if (size > gsl_rng_max(r))
        throw std::runtime_error("Generator cannot produce integers larger than " + std::to_string(gsl_rng_max(r)));
    return gsl_rng_get(r) % size;
}

int Randomizer::Discrete(int min, int max)
{
    return min + gsl_rng_uniform_int(r, max - min + 1);
}
// ///
// int Randomizer::Discrete(std::vector<unsigned int>& cumulative_weights)
// {

//     if (cumulative_weights.back() == 0)
//         return Discrete(cumulative_weights.size());
//     return std::distance(cumulative_weights.begin(),
//                 std::lower_bound(cumulative_weights.begin(),
//                     cumulative_weights.end(),
//                     Discrete(cumulative_weights.back())));
// }
// ///
// int Randomizer::Discrete(std::vector<double>& cumulative_weights)
// {
//     if (cumulative_weights.back() == 0)
//         return Discrete(cumulative_weights.size());
//     return std::distance(cumulative_weights.begin(),
//                 std::lower_bound(cumulative_weights.begin(),
//                     cumulative_weights.end(),
//                     Uniform(0.0, cumulative_weights.back())));
// }

void Randomizer::Multinomial(unsigned int N, std::vector<double>& p, std::vector<unsigned int>& n)
{
    gsl_ran_multinomial(r, p.size(), N, &p[0], &n[0]);
}

bool Randomizer::Bernoulli(double p)
{
    if (p <= 0) return false;
    if (p >= 1) return true;
    return gsl_rng_uniform(r) < p;
}

unsigned int Randomizer::Binomial(unsigned int n, double p)
{
    if (p <= 0) return 0;
    if (p >= 1) return n;
    return gsl_ran_binomial(r, p, n);
}

unsigned int Randomizer::BetaBinomial(unsigned int n, double p, double a_plus_b)
{
    if (a_plus_b > 0)
        p = gsl_ran_beta(r, a_plus_b * p, a_plus_b * (1 - p));
    return gsl_ran_binomial(r, p, n);
}

int Randomizer::Poisson(double mean)
{
    if (mean <= 0) return 0;
    return gsl_ran_poisson(r, mean);
}

int Randomizer::Geometric(double p)
{
    if (p <= 0) return 0;
    return gsl_ran_geometric(r, p);
}

int Randomizer::Round(double x)
{
    int sign = x < 0 ? -1 : 1;
    double intpart, fracpart;
    fracpart = std::modf(std::fabs(x), &intpart);
    return sign * (intpart + Bernoulli(fracpart));
}

template <typename T>
void Randomizer::Shuffle(std::vector<T>& vec)
{
    gsl_ran_shuffle(r, &vec[0], vec.size(), sizeof(vec[0]));
}

#endif


#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_spmatrix.h>
#include <assert.h>

#include "graph_struct.h"

// pour améliorer la complexité:
// -> Les matrices de graph sont symétriques, on peut donc n'effectuer les opérations lors de la création du graph que sur une moitié de celui-ci
// -> Faire l'attribution des couleurs en même temps que la matrice d'adjacence

struct graph_t* graph__empty (int vertices){

  struct graph_t* g = malloc(sizeof(*g));
  g -> num_vertices = vertices;
  g -> t = gsl_spmatrix_alloc(vertices, vertices);
  g -> o = gsl_spmatrix_alloc(2, vertices);

  return g;
}

struct graph_t* graph_copy(struct graph_t * g){
  struct graph_t *copy = malloc(sizeof(struct graph_t));
  copy -> num_vertices = g -> num_vertices;
  copy -> o = gsl_spmatrix_alloc(2, g->num_vertices);
  gsl_spmatrix_memcpy(copy -> o, g -> o);
  copy -> t = gsl_spmatrix_alloc(g -> num_vertices, g -> num_vertices);
  gsl_spmatrix_memcpy(copy -> t, g -> t);
  return copy;
}

struct graph_t* make_graph(int m, char c){
  struct graph_t* g;

  switch (c){
  case 'c' :
    g = square_graph__empty(m);
    return g;
  case 't':
    g = triangular_graph__empty(m);
    return g;
  case 'h':
    g = hexa_graph__empty(m);
    return g;
  default:
    return NULL;
  }
}

struct graph_t* square_graph__empty(int m){
  int vertices = (m+1) * (m+1);
  struct graph_t* g = graph__empty (vertices);

  int line_length = m+1;

  for (int i = 0; i < vertices; i++){
    int index = i%line_length;

    // Adds right links
    if (index+1 < line_length)
      gsl_spmatrix_set(g -> t, i, i+1, 1);
    // Adds left links
    if (index-1 >= 0)
      gsl_spmatrix_set(g -> t, i, i-1, 1);
    // Adds down links
    if (i+line_length < vertices)
      gsl_spmatrix_set(g -> t, i, i+line_length, 1);
    // Adds up links
    if (i-line_length >= 0)
      gsl_spmatrix_set(g -> t, i, i-line_length, 1);
  }

  // Ownership of initial boxes
  for (int i = 0; i < line_length-1; i++){
    gsl_spmatrix_set(g -> o, 0, i, 1);
    gsl_spmatrix_set(g -> o, 0, vertices-1-i, 1);
    gsl_spmatrix_set(g -> o, 1, (i+1)*line_length, 1);
    gsl_spmatrix_set(g -> o, 1, vertices-1-(i+1)*line_length, 1);
  }

  return g;
}

struct graph_t* triangular_graph__empty(int m){
  int num_vertices = 6 * m * m;
  struct graph_t* g = graph__empty(num_vertices);
  int first_vertice_row=0;
  int row = 0;
  while(row < m){
    //Edges in a row
    for(int vertice = first_vertice_row; vertice < first_vertice_row + m + row; vertice++){
      gsl_spmatrix_set(g -> t, vertice, vertice + m + row, 1);
      gsl_spmatrix_set(g -> t, vertice + m + row, vertice, 1);
      gsl_spmatrix_set(g -> t, vertice, vertice + m + row + 1, 1);
      gsl_spmatrix_set(g -> t, vertice + m + row + 1, vertice, 1);

      gsl_spmatrix_set(g -> t, num_vertices - 1 - (vertice + m + row), num_vertices - 1 - vertice, 1);
      gsl_spmatrix_set(g -> t, num_vertices - 1 - vertice, num_vertices - 1 - (vertice + m + row ), 1);
      gsl_spmatrix_set(g -> t, num_vertices - 1 - (vertice + m + row + 1), num_vertices - 1 - vertice, 1);
      gsl_spmatrix_set(g -> t, num_vertices - 1 - vertice, num_vertices-1 - (vertice + m + row + 1), 1);
    }
    //Edges between 2 rows
    for(int vertice = first_vertice_row + m + row; vertice <= first_vertice_row + 2*(m + row); vertice++){
      gsl_spmatrix_set(g -> t, vertice, vertice + m + row + 1, 1);
      gsl_spmatrix_set(g -> t, vertice + m + row + 1, vertice, 1);

      gsl_spmatrix_set(g -> t, num_vertices - 1 - vertice, num_vertices - 1 - (vertice + m + row + 1), 1);
      gsl_spmatrix_set(g -> t, num_vertices - 1 - (vertice + m + row + 1), num_vertices - 1 - vertice, 1);
    }
    row++;
    first_vertice_row += 2*m + row*(row+1)/2;
  }
  //black starting tiles
  for(int vertice = 0; vertice < m; vertice ++){
    gsl_spmatrix_set(g -> o, BLACK, vertice, 1);
    gsl_spmatrix_set(g -> o, BLACK, vertice + m + 1, 1);

    gsl_spmatrix_set(g -> o, BLACK, num_vertices - 1 - vertice, 1);
    gsl_spmatrix_set(g -> o, BLACK, num_vertices - 1 - (vertice + m + 1), 1);
  }
  for(int row = 1; row < m/2 ; row++){
    gsl_spmatrix_set(g -> o, BLACK, m-1 + 2*row*m + row*(row+1), 1);
    gsl_spmatrix_set(g -> o, BLACK, m-1 + 2*row*m + row*(row+1) + m + row + 1, 1);

    gsl_spmatrix_set(g -> o, BLACK, num_vertices - 1 - (m-1 + 2*row*m + row*(row+1)), 1);
    gsl_spmatrix_set(g -> o, BLACK, num_vertices - 1 - (m-1 + 2*row*m + row*(row+1) + m + row + 1), 1);
  }
  //white starting tiles
  for(int row = 0; row < m; row++){
    gsl_spmatrix_set(g -> o, WHITE, m + 2*row*m + row*(row+1), 1);
    gsl_spmatrix_set(g -> o, WHITE, m + 2*row*m + row*(row+1) + m + row + 1, 1);

    gsl_spmatrix_set(g -> o, WHITE, num_vertices - 1 - (m + 2*row*m + row*(row+1)), 1);
    gsl_spmatrix_set(g -> o, WHITE, num_vertices - 1 - (m + 2*row*m + row*(row+1) + m + row + 1), 1);
  }
  for(int row = m/2+1; row < m; row++){
    gsl_spmatrix_set(g -> o, WHITE, m-1 + 2*row*m + row*(row+1), 1);
    gsl_spmatrix_set(g -> o, WHITE, m-1 + 2*row*m + row*(row+1) - (m + row), 1);

    gsl_spmatrix_set(g -> o, WHITE, num_vertices - 1 - (m-1 + 2*row*m + row*(row+1)), 1);
    gsl_spmatrix_set(g -> o, WHITE, num_vertices - 1 - (m-1 + 2*row*m + row*(row+1) - (m + row)),1);
  }
  return g;
}

struct graph_t* hexa_graph__empty(int m){
  int vertices = (m+1) * (m+1);
  struct graph_t* g = graph__empty (vertices);

  int line_length = m+1;

  for (int i = 0; i < vertices; i++){
    int index = i%line_length;

    // Adds right links
    if (index+1 < line_length)
      gsl_spmatrix_set(g -> t, i, i+1, 1);
    // Adds left links
    if (index-1 >= 0)
      gsl_spmatrix_set(g -> t, i, i-1, 1);
    // Adds down links
    if (i/line_length < line_length-1 && index > 0)
      gsl_spmatrix_set(g -> t, i, i+line_length - 1, 1);
    if (i+line_length < vertices && i/line_length < line_length-1)
      gsl_spmatrix_set(g -> t, i, i+line_length, 1);
    // Adds up links
    if (i/m > 0 && index < line_length-1)
      gsl_spmatrix_set(g -> t, i, i-line_length + 1, 1);
    if (i-line_length >= 0 && i/line_length > 0)
      gsl_spmatrix_set(g -> t, i, i-line_length, 1);

    // Ownership of initial boxes
    for (int i = 0; i < line_length-1; i++){
      gsl_spmatrix_set(g -> o, 0, i, 1);
      gsl_spmatrix_set(g -> o, 0, vertices-1-i, 1);
      gsl_spmatrix_set(g -> o, 1, (i+1)*line_length, 1); // Pour m = 3, l_l = 4 : 4, 8, 12
      gsl_spmatrix_set(g -> o, 1, vertices-1-(i+1)*line_length, 1); //
    }
  }
  return g;
}

void add__move_to_graph (struct graph_t* g, int vertice, int player){
  assert (g != NULL);

  gsl_spmatrix_set(g -> o, player, vertice, 1);
}

int get__box_owner (struct graph_t* g, int vertice){
  for (int p = 0; p < 2; p++){
    if (gsl_spmatrix_get(g -> o, p, vertice) == 1)
      return p;
  }
  return EMPTY_BOX;
}

void graph__free (struct graph_t* g){
  assert (g != NULL);

  gsl_spmatrix_free(g -> o);
  gsl_spmatrix_free(g -> t);

  free(g);
}

void print_graph (struct graph_t* g, char c){
  switch (c){
  case 'c' :
    square_graph__print (g);
    break;
  case 't':
    triangular_graph__print (g);
    break;
  case 'h':
    hexa_graph__print (g);
    break;
  default:
    printf("Don't find this type of graph\n");
    break;
  }
}

void square_graph__print (struct graph_t* g){
  assert (g != NULL);

  int n = sqrt(g -> num_vertices);

  for (int i = 0; i < n; i++){
    printf("\t");
    for (int j = 0; j < n; j++){
      int box_owner = get__box_owner (g, i*n + j);
      if (box_owner == 0)
	printf("\033[34;01m■ \033[00m");
      else if (box_owner == 1)
	printf("\033[31;01m■ \033[00m");
      else
	printf("■ ");
    }
    printf("\n");
  }
}

void triangular_graph__print (struct graph_t* g){(void) g;}

void hexa_graph__print (struct graph_t* g){
    assert (g != NULL);

  int n = sqrt(g -> num_vertices);
  char space[1000] = "";

  for (int i = 0; i < n; i++){
    printf("\t%s", space);

    for (int space = 0; space < i; space++){
      printf(" ");
    }
    for (int j = 0; j < n; j++){
      int box_owner = get__box_owner (g, i*n + j);
      if (box_owner == 0)
	printf("\033[34;01m⬢ \033[00m");
      else if (box_owner == 1)
	printf("\033[31;01m⬢ \033[00m");
      else
	printf("⬢ ");
    }
    printf("\n");
  }
}


static char help[] =
"Solves 2D advection-diffusion problems using FD discretization,\n"
"structured-grid (DMDA), and -snes_fd_color.  Option prefix -bth_.\n"
"Equation:\n"
"    - eps Laplacian u + Div (a(x,y) u) = g(x,y),\n"
"where the (vector) wind a(x,y) and (scalar) source g(x,y) are given smooth\n"
"functions.  The domain is S = (-1,1)^2 with Dirichlet boundary conditions:\n"
"    u = b(x,y) on boundary S\n"
"where b(x,y) is a given smooth function.  Problems include: NOWIND, LAYER,\n"
"and GLAZE.  The first of these has a=0 while LAYER and GLAZE are\n"
"Examples 6.1.1 and 6.1.4 in Elman et al (2014), respectively.\n"
"Advection can be discretized by first-order upwinding (none), centered, or a\n"
"van Leer limiter scheme.  Option allows switching to none limiter on all grids\n"
"for which the mesh Peclet P^h exceeds a threshold (default: 1).\n\n";

#include <petsc.h>

typedef enum {NONE, CENTERED, VANLEER} LimiterType;
static const char *LimiterTypes[] = {"none","centered","vanleer",
                                     "LimiterType", "", NULL};

static PetscReal centered(PetscReal theta) {
    return 0.5;
}

static PetscReal vanleer(PetscReal theta) {
    const PetscReal abstheta = PetscAbsReal(theta);
    return 0.5 * (theta + abstheta) / (1.0 + abstheta);   // 4 flops
}

typedef PetscReal (*LimiterFcn)(PetscReal);

static LimiterFcn limiterptr[] = {NULL, &centered, &vanleer};

typedef enum {NOWIND, LAYER, GLAZE} ProblemType;
static const char *ProblemTypes[] = {"nowind", "layer", "glaze",
                                     "ProblemType", "", NULL};

typedef struct {
    ProblemType  problem;
    PetscReal    eps,                              // diffusion eps > 0
                 a_scale,                          // scale for wind
                 peclet_threshold,
                 (*limiter_fcn)(PetscReal),
                 (*g_fcn)(PetscReal, PetscReal, void*),  // right-hand-side source
                 (*b_fcn)(PetscReal, PetscReal, void*);  // boundary condition
    PetscBool    none_on_peclet,                   // if true use none limiter when P^h > threshold
                 small_peclet_achieved;            // true if on finest grid P^h <= threshold
} AdCtx;

// used for source functions
static PetscReal zero(PetscReal x, PetscReal y, void *user) {
    return 0.0;
}

// problem NOWIND: same problem as ./fish -fsh_problem manuexp
static PetscReal nowind_u(PetscReal x, PetscReal y, void *user) {  // exact solution
    return - x * PetscExpReal(y);
}

static PetscReal nowind_g(PetscReal x, PetscReal y, void *user) {
    AdCtx* usr = (AdCtx*)user;
    return usr->eps * x * PetscExpReal(y);
}

static PetscReal nowind_b(PetscReal x, PetscReal y, void *user) {
    return nowind_u(x,y,user);
}

// problem LAYER:  Elman page 237, Example 6.1.1
static PetscReal layer_u(PetscReal x, PetscReal y, void *user) {  // exact solution
    AdCtx* usr = (AdCtx*)user;
    return x * (1.0 - PetscExpReal((y-1) / usr->eps))
             / (1.0 - PetscExpReal(- 2.0 / usr->eps));
}

static PetscReal layer_b(PetscReal x, PetscReal y, void *user) {
    return layer_u(x,y,user);
}

// problem GLAZE:  Elman page 240, Example 6.1.4
static PetscReal glaze_b(PetscReal x, PetscReal y, void *user) {
    if (x > 0.0 && y < x && y > -x)
       return 1.0;   // along x=1 boundary
    else
       return 0.0;
}

typedef PetscReal (*PointwiseFcn)(PetscReal,PetscReal,void*);

static PointwiseFcn uexptr[] = {&nowind_u, &layer_u, NULL};
static PointwiseFcn gptr[]   = {&nowind_g, &zero,    &zero};
static PointwiseFcn bptr[]   = {&nowind_b, &layer_b, &glaze_b};

/* This vector function returns q=0,1 component.  It is used in
   FormFunctionLocal() to get a(x,y). */
static PetscReal wind_a(PetscReal x, PetscReal y, PetscInt q, AdCtx *user) {
    switch (user->problem) {
        case NOWIND:
            return 0.0;
        case LAYER:
            return (q == 0) ? 0.0 : 1.0;
        case GLAZE:
            return (q == 0) ? 2.0*y*(1.0-x*x) : -2.0*x*(1.0-y*y);
        default:
            return NAN;
    }
}

extern PetscErrorCode FormUExact(DMDALocalInfo*,AdCtx*,
                                 PetscReal (*)(PetscReal, PetscReal, void*),Vec);
extern PetscErrorCode FormFunctionLocal(DMDALocalInfo*,PetscReal**,PetscReal**,AdCtx*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    DM             da, da_after;
    SNES           snes;
    Vec            u_initial, u;
    DMDALocalInfo  info;
    PointwiseFcn   uexact_fcn;
    LimiterType    limiter = NONE;
    PetscBool      init_exact = PETSC_FALSE;
    AdCtx          user;

    ierr = PetscInitialize(&argc,&argv,NULL,help); if (ierr) return ierr;

    user.eps = 0.005;
    user.none_on_peclet = PETSC_FALSE;
    user.small_peclet_achieved = PETSC_FALSE;
    user.problem = LAYER;
    user.a_scale = 1.0;   // this could be made dependent on problem
    user.peclet_threshold = 1.0;
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"bth_",
               "both (2D advection-diffusion solver) options",""); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-eps","positive diffusion coefficient",
               "both.c",user.eps,&(user.eps),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-init_exact","use exact solution for initialization",
               "both.c",init_exact,&init_exact,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnum("-limiter","flux-limiter type",
               "both.c",LimiterTypes,
               (PetscEnum)limiter,(PetscEnum*)&limiter,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-none_on_peclet",
               "on coarse grids such that mesh Peclet P^h exceeds threshold, switch to none limiter",
               "both.c",user.none_on_peclet,&(user.none_on_peclet),NULL);
               CHKERRQ(ierr);
    ierr = PetscOptionsReal("-peclet_threshold",
               "if mesh Peclet P^h is above this value, switch to none (used with -bth_none_on_peclet)",
               "both.c",user.peclet_threshold,&(user.peclet_threshold),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnum("-problem","problem type",
               "both.c",ProblemTypes,
               (PetscEnum)(user.problem),(PetscEnum*)&(user.problem),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    if (user.eps <= 0.0) {
        SETERRQ1(PETSC_COMM_SELF,1,"eps=%.3f invalid ... eps > 0 required",user.eps);
    }
    user.limiter_fcn = limiterptr[limiter];
    uexact_fcn = uexptr[user.problem];
    user.g_fcn = gptr[user.problem];
    user.b_fcn = bptr[user.problem];

    ierr = DMDACreate2d(PETSC_COMM_WORLD,
        DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DMDA_STENCIL_STAR,
        3,3,                          // default to hx=hy=1 grid
        PETSC_DECIDE,PETSC_DECIDE,
        1,2,                          // d.o.f, stencil width
        NULL,NULL,&da); CHKERRQ(ierr);
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);
    if (user.problem == NOWIND) {
        ierr = DMDASetUniformCoordinates(da,0.0,1.0,0.0,1.0,-1.0,1.0); CHKERRQ(ierr);
    } else {
        ierr = DMDASetUniformCoordinates(da,-1.0,1.0,-1.0,1.0,-1.0,1.0); CHKERRQ(ierr);
    }
    ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);

    ierr = SNESCreate(PETSC_COMM_WORLD,&snes);CHKERRQ(ierr);
    ierr = SNESSetDM(snes,da);CHKERRQ(ierr);
    ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
            (DMDASNESFunction)FormFunctionLocal,&user);CHKERRQ(ierr);
    ierr = SNESSetApplicationContext(snes,&user); CHKERRQ(ierr);
    ierr = SNESSetFromOptions(snes);CHKERRQ(ierr);

    ierr = DMGetGlobalVector(da,&u_initial); CHKERRQ(ierr);
    if (init_exact) {
        ierr = FormUExact(&info,&user,uexact_fcn,u_initial); CHKERRQ(ierr);
    } else {
        ierr = VecSet(u_initial,0.0); CHKERRQ(ierr);
    }
    ierr = SNESSolve(snes,NULL,u_initial); CHKERRQ(ierr);
    ierr = DMRestoreGlobalVector(da,&u_initial); CHKERRQ(ierr);
    ierr = DMDestroy(&da); CHKERRQ(ierr);

    ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr);
    ierr = SNESGetDM(snes,&da_after); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da_after,&info); CHKERRQ(ierr);
    if (user.none_on_peclet && !user.small_peclet_achieved) {
         ierr = PetscPrintf(PETSC_COMM_WORLD,
             "WARNING: -bth_none_on_peclet set but finest grid used NONE limiter\n");
             CHKERRQ(ierr);
    }
    ierr = PetscPrintf(PETSC_COMM_WORLD,
         "done on %d x %d grid (problem = %s, eps = %g, limiter = %s)\n",
         info.mx,info.my,ProblemTypes[user.problem],user.eps,LimiterTypes[limiter]);
         CHKERRQ(ierr);

    if (uexact_fcn != NULL) {
        Vec     u_exact;
        PetscReal  xymin[2], xymax[2], hx, hy, err2;
        ierr = DMCreateGlobalVector(da_after,&u_exact); CHKERRQ(ierr);
        ierr = FormUExact(&info,&user,uexact_fcn,u_exact); CHKERRQ(ierr);
        ierr = VecAXPY(u,-1.0,u_exact); CHKERRQ(ierr);    // u <- u + (-1.0) u_exact
        ierr = VecNorm(u,NORM_2,&err2); CHKERRQ(ierr);
        ierr = DMGetBoundingBox(da_after,xymin,xymax); CHKERRQ(ierr);
        hx = (xymax[0] - xymin[0]) / (info.mx - 1);
        hy = (xymax[1] - xymin[1]) / (info.my - 1);
        err2 *= PetscSqrtReal(hx * hy);
        ierr = PetscPrintf(PETSC_COMM_WORLD,
             "numerical error:  |u-uexact|_2 = %.4e\n",err2); CHKERRQ(ierr);
        ierr = VecDestroy(&u_exact); CHKERRQ(ierr);
    }

    ierr = SNESDestroy(&snes); CHKERRQ(ierr);
    return PetscFinalize();
}

PetscErrorCode FormUExact(DMDALocalInfo *info, AdCtx *usr,
                          PetscReal (*uexact)(PetscReal, PetscReal, void*), Vec uex) {
    PetscErrorCode  ierr;
    PetscInt        i, j;
    PetscReal       xymin[2], xymax[2], hx, hy, x, y, **auex;

    if (uexact == NULL) {
        SETERRQ(PETSC_COMM_SELF,1,"exact solution not available");
    }
    ierr = DMGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    ierr = DMDAVecGetArray(info->da, uex, &auex);CHKERRQ(ierr);
    for (j=info->ys; j<info->ys+info->ym; j++) {
        y = xymin[1] + j * hy;
        for (i=info->xs; i<info->xs+info->xm; i++) {
            x = xymin[0] + i * hx;
            auex[j][i] = (*uexact)(x,y,usr);
        }
    }
    ierr = DMDAVecRestoreArray(info->da, uex, &auex);CHKERRQ(ierr);
    return 0;
}

/* compute residuals:
     F_ij = hx * hy * (- eps Laplacian u + Div (a(x,y) u) - g(x,y))
at boundary points:
     F_ij = c (u - b(x,y))
note compass notation for faces of cells; advection scheme evaluates flux at
each E,N face once and then includes it in two residuals:
     N
  -------
  |     |
W |  *  | E
  |     |
  -------
     S
*/
PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, PetscReal **au,
                                 PetscReal **aF, AdCtx *usr) {
    PetscErrorCode ierr;
    PetscInt        i, j, p;
    PetscReal       xymin[2], xymax[2], hx, hy, Ph, hx2, hy2, scF, scBC,
                    x, y, uE, uW, uN, uS, uxx, uyy,
                    ap, flux, u_up, u_dn, u_far, theta;
    PetscReal       (*limiter)(PetscReal);
    PetscBool       iowned, jowned, ip1owned, jp1owned;
    PetscLogDouble  ff;

    ierr = DMGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    limiter = usr->limiter_fcn;
    if (usr->none_on_peclet) {
        Ph = usr->a_scale * PetscMax(hx,hy) / usr->eps;  // mesh Peclet number
        if (Ph > usr->peclet_threshold)
            limiter = NULL;
        else
            usr->small_peclet_achieved = PETSC_TRUE;
    }
    hx2 = hx * hx;
    hy2 = hy * hy;
    scF = hx * hy;  // scale residuals
    scBC = scF * usr->eps * 2.0 * (1.0 / hx2 + 1.0 / hy2); // scale b.c. residuals

    // for owned cells, compute non-advective parts of residual at cell center
    for (j=info->ys; j<info->ys+info->ym; j++) {
        y = xymin[1] + j * hy;
        for (i=info->xs; i<info->xs+info->xm; i++) {
            x = xymin[0] + i * hx;
            if (i == 0 || i == info->mx-1 || j == 0 || j == info->my-1) {
                aF[j][i] = scBC * (au[j][i] - (*usr->b_fcn)(x,y,usr));
            } else {
                uE = (i+1 == info->mx-1) ? (*usr->b_fcn)(xymax[0],y,usr) : au[j][i+1];
                uW = (i-1 == 0)          ? (*usr->b_fcn)(xymin[0],y,usr) : au[j][i-1];
                uxx = (uE - 2.0 * au[j][i] + uW) / hx2;
                uN = (j+1 == info->my-1) ? (*usr->b_fcn)(x,xymax[1],usr) : au[j+1][i];
                uS = (j-1 == 0)          ? (*usr->b_fcn)(x,xymin[1],usr) : au[j-1][i];
                uyy = (uN - 2.0 * au[j][i] + uS) / hy2;
                aF[j][i] = scF * (- usr->eps * (uxx + uyy) - (*usr->g_fcn)(x,y,usr));
            }
        }
    }
    ierr = PetscLogFlops(14.0*info->xm*info->ym); CHKERRQ(ierr);

    // for each E,N face of an *owned* cell at (x,y) and (i,j), compute flux at
    //     the face center and then add that to the correct residual
    // note start offset of -1; gets W,S faces of owned cells living on ownership
    //     boundaries for i,j resp.
    // there are (xm+1)*(ym+1)*2 fluxes to evaluate
    for (j=info->ys-1; j<info->ys+info->ym; j++) {
        y = xymin[1] + j * hy;
        // if y<0 or y=1 at cell center then no need to compute *any* E,N face-center fluxes
        if (j < 0 || j == info->my-1)
            continue;
        for (i=info->xs-1; i<info->xs+info->xm; i++) {
            x = xymin[0] + i * hx;
            // if x<0 or x=1 at cell center then no need to compute *any* E,N face-center fluxes
            if (i < 0 || i == info->mx-1)
                continue;
            // get E (p=0) and N (p=1) cell face-center flux contributions
            for (p = 0; p < 2; p++) {
                // get pth component of wind and locations determined by wind direction
                ap = (p == 0) ? wind_a(x+hx/2.0,y,p,usr) : wind_a(x,y+hy/2.0,p,usr);
                if (p == 0)
                    if (ap >= 0.0) {
                        u_up  = (i == 0)            ? (*usr->b_fcn)(xymin[0],y,usr) : au[j][i];
                        u_dn  = (i+1 == info->mx-1) ? (*usr->b_fcn)(xymax[0],y,usr) : au[j][i+1];
                        u_far = (i-1 <= 0)          ? (*usr->b_fcn)(xymin[0],y,usr) : au[j][i-1];
                    } else {
                        u_up  = (i+1 == info->mx-1) ? (*usr->b_fcn)(xymax[0],y,usr) : au[j][i+1];
                        u_dn  = (i == 0)            ? (*usr->b_fcn)(xymin[0],y,usr) : au[j][i];
                        u_far = (i+2 >= info->mx-1) ? (*usr->b_fcn)(xymax[0],y,usr) : au[j][i+2];
                    }
                else  // p == 1
                    if (ap >= 0.0) {
                        u_up  = (j == 0)            ? (*usr->b_fcn)(x,xymin[1],usr) : au[j][i];
                        u_dn  = (j+1 == info->my-1) ? (*usr->b_fcn)(x,xymax[1],usr) : au[j+1][i];
                        u_far = (j-1 <= 0)          ? (*usr->b_fcn)(x,xymin[1],usr) : au[j-1][i];
                    } else {
                        u_up  = (j+1 == info->my-1) ? (*usr->b_fcn)(x,xymax[1],usr) : au[j+1][i];
                        u_dn  = (j == 0)            ? (*usr->b_fcn)(x,xymin[1],usr) : au[j][i];
                        u_far = (j+2 >= info->my-1) ? (*usr->b_fcn)(x,xymax[1],usr) : au[j+2][i];
                    }
                // first-order upwind flux plus correction if have limiter
                flux = ap * u_up;
                if (limiter != NULL && u_dn != u_up) {
                    theta = (u_up - u_far) / (u_dn - u_up);
                    flux += ap * (*limiter)(theta) * (u_dn - u_up);
                }
                // update non-boundary and owned residual F_ij on both sides of computed flux
                // note: 1) aF[] does not have stencil width, 2) F_ij is scaled by scF = hx * hy
                iowned = (i >= info->xs);
                jowned = (j >= info->ys);
                ip1owned = (i+1 < info->xs + info->xm);
                jp1owned = (j+1 < info->ys + info->ym);
                switch (p) {
                    case 0:  // flux at E
                        if (i > 0 && j > 0 && iowned && jowned)
                            aF[j][i] += hy * flux;  // flux out of i,j at E
                        if (i+1 < info->mx-1 && j > 0 && ip1owned && jowned)
                            aF[j][i+1] -= hy * flux;  // flux into i+1,j at W
                        break;
                    case 1:  // flux at N
                        if (j > 0 && i > 0 && iowned && jowned)
                            aF[j][i] += hx * flux;  // flux out of i,j at N
                        if (j+1 < info->my-1 && i > 0 && iowned && jp1owned)
                            aF[j+1][i] -= hx * flux;  // flux into i,j+1 at S
                        break;
                }
            }
        }
    }
    // ff = flops per flux evaluation
    ff = (limiter == NULL) ? 6.0 : 13.0;
    if (limiter == &vanleer)
        ff += 4.0;
    ierr = PetscLogFlops(ff*2.0*(1.0+info->xm)*(1.0+info->ym)); CHKERRQ(ierr);
    return 0;
}


/**
 * @file aec.h
 * @brief Cancelation of a voice prompt based on either NLMS or Kalman filtering algorithms
 * @author John McDonough, Wei Chu and Kenichi Kumatani
 */

#ifndef AEC_H
#define AEC_H

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

#include <gsl/gsl_block.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_blas.h>
#include "common/jexception.h"
#include <gsl/gsl_eigen.h>

#include "stream/stream.h"
#include "btk.h"
#include "beamformer/tracker.h"

/**
* \defgroup NLMSAcousticEchoCancellationFeature NLMS Echo Cancellation Feature
*/
/*@{*/

// ----- definition for class `NLMSAcousticEchoCancellationFeature' -----
//
class NLMSAcousticEchoCancellationFeature : public VectorComplexFeatureStream {
 public:
   NLMSAcousticEchoCancellationFeature(const VectorComplexFeatureStreamPtr& played, const VectorComplexFeatureStreamPtr& recorded, double delta = 100.0, double epsilon = 1.0E-04, double threshold = 100.0, const String& nm = "AEC");
  virtual ~NLMSAcousticEchoCancellationFeature();

  virtual const gsl_vector_complex* next(int frame_no = -5);

  virtual void reset() { played_->reset(); recorded_->reset(); gsl_vector_complex_set_zero(filterCoefficient_); }

private:
  bool update_(const gsl_complex Vk);

  VectorComplexFeatureStreamPtr         played_;                    // v(n)
  VectorComplexFeatureStreamPtr         recorded_;                  // a(n)

  unsigned                              fftLen_;
  unsigned                              fftLen2_;
  gsl_vector_complex*                   filterCoefficient_;

  const double                          delta_;
  const double                          epsilon_;
  const double                          threshold_;
};

typedef Inherit<NLMSAcousticEchoCancellationFeature, VectorComplexFeatureStreamPtr> NLMSAcousticEchoCancellationFeaturePtr;
/*@}*/

/**
* \defgroup KalmanFilterEchoCancellationFeature Kalman Filer Echo Cancellation Feature
*/
/*@{*/

// ----- definition for class `KalmanFilterEchoCancellationFeature' -----
//
class KalmanFilterEchoCancellationFeature : public VectorComplexFeatureStream {
 public:
  KalmanFilterEchoCancellationFeature(const VectorComplexFeatureStreamPtr& played, const VectorComplexFeatureStreamPtr& recorded, double beta = 0.95, double sigma2 = 100.0, double threshold = 100.0, const String& nm = "KFEchoCanceller");
  virtual ~KalmanFilterEchoCancellationFeature();

  virtual const gsl_vector_complex* next(int frame_no = -5);

  virtual void reset() { played_->reset(); recorded_->reset(); gsl_vector_complex_set_zero(filterCoefficient_); }

private:
  bool update_(const gsl_complex Vk);

  VectorComplexFeatureStreamPtr         played_;                    // v(n)
  VectorComplexFeatureStreamPtr         recorded_;                  // a(n)

  unsigned                              fftLen_;
  unsigned                              fftLen2_;

  gsl_vector_complex*                   filterCoefficient_;
  gsl_vector*                           sigma2_v_;
  gsl_vector*                           K_k_;

  const double                          beta_;
  const double                          threshold_;
  const double                          sigma2_u_;
};

typedef Inherit<KalmanFilterEchoCancellationFeature, VectorComplexFeatureStreamPtr> KalmanFilterEchoCancellationFeaturePtr;
/*@}*/

// ----- definition for class `BlockKalmanFilterEchoCancellationFeature' -----
//
class BlockKalmanFilterEchoCancellationFeature : public VectorComplexFeatureStream {
  public:
    BlockKalmanFilterEchoCancellationFeature(const VectorComplexFeatureStreamPtr& played, const VectorComplexFeatureStreamPtr& recorded, unsigned sampleN = 1, double beta = 0.95, double sigmau2 = 10e-4, double sigmauk2 = 5.0, double threshold = 100.0, double amp4play = 1.0, const String& nm = "KFEchoCanceller");
    virtual ~BlockKalmanFilterEchoCancellationFeature();

    virtual const gsl_vector_complex* next(int frame_no = -5);

    virtual void reset()
    {
      played_->reset(); recorded_->reset();
    }

  protected:
    class ComplexBuffer_ {
      public:
    /*
        @brief Construct a circular buffer to hold past and current subband samples
        It keeps nsamp arrays which is completely updated with the period 'nsamp'.
        Each array holds actual values of the samples.
        @param unsigned len[in] is the size of each vector of samples
        @param unsigned nsamp[in] is the period of the circular buffer
    */
      ComplexBuffer_(unsigned len, unsigned sampleN)
        : len_(len), sampleN_(sampleN), zero_(sampleN_ - 1),
          samples_(new gsl_vector_complex*[sampleN_]), subbandSamples_(gsl_vector_complex_calloc(sampleN_))
      {
        for (unsigned i = 0; i < sampleN_; i++)
          samples_[i] = gsl_vector_complex_calloc(len_);
      }

      ~ComplexBuffer_()
      {
        for (unsigned i = 0; i < sampleN_; i++)
          gsl_vector_complex_free(samples_[i]);
        delete[] samples_;
        gsl_vector_complex_free(subbandSamples_);
      }

      gsl_complex sample(unsigned timeX, unsigned binX) const {
        unsigned idx = index_(timeX);
        const gsl_vector_complex* vec = samples_[idx];
        return gsl_vector_complex_get(vec, binX);
      }

      const gsl_vector_complex* get_samples(unsigned m)
      {
        for (unsigned timeX = 0; timeX < sampleN_; timeX++)
          gsl_vector_complex_set(subbandSamples_, timeX, sample(timeX, m));

        return subbandSamples_;
      }

      void next_sample(const gsl_vector_complex* s = NULL, double amp4play = 1.0 ) {
        zero_ = (zero_ + 1) % sampleN_;

        gsl_vector_complex* nextBlock = samples_[zero_];

        if (s == NULL) {
          gsl_vector_complex_set_zero(nextBlock);
        } else {
          if (s->size != len_)
            throw jdimension_error("'ComplexBuffer_': Sizes do not match (%d vs. %d)", s->size, len_);
          assert( s->size == len_ );
          gsl_vector_complex_memcpy(nextBlock, s);
          if( amp4play != 1.0 )
            gsl_blas_zdscal( amp4play, nextBlock );
        }
      }

      void zero() {
        for (unsigned i = 0; i < sampleN_; i++)
          gsl_vector_complex_set_zero(samples_[i]);
        zero_ = sampleN_ - 1;
      }

    private:
      unsigned index_(unsigned idx) const {
        assert(idx < sampleN_);
        unsigned ret = (zero_ + sampleN_ - idx) % sampleN_;
        return ret;
      }

      const unsigned                             len_;
      const unsigned                             sampleN_;
      unsigned                                   zero_; // index of most recent sample
      gsl_vector_complex**                       samples_;
      gsl_vector_complex*                        subbandSamples_;
    };

  static gsl_complex                             ComplexOne_;
  static gsl_complex                             ComplexZero_;

  bool update_(const gsl_vector_complex* Vk);
  void conjugate_(gsl_vector_complex* dest, const gsl_vector_complex* src) const;

  VectorComplexFeatureStreamPtr                 played_;   // v(n)
  VectorComplexFeatureStreamPtr                 recorded_; // a(n)

  unsigned                                      fftLen_;
  unsigned                                      fftLen2_;
  unsigned                                      sampleN_;

  ComplexBuffer_                                buffer_;

  gsl_vector_complex**                          filterCoefficient_;
  gsl_vector*                                   sigma2_v_;
  gsl_matrix_complex**                          K_k_;
  gsl_matrix_complex*                           K_k_k1_;

  const double                                  beta_;
  const double                                  threshold_;
  gsl_matrix_complex**                          Sigma2_u_;
  gsl_vector_complex*                           Gk_;
  gsl_vector_complex*                           scratch_;
  gsl_vector_complex*                           scratch2_;
  gsl_matrix_complex*                           scratchMatrix_;
  gsl_matrix_complex*                           scratchMatrix2_;
  double                                        amp4play_;
  double                                        floorVal_;
  int                                           skippedN_;
  int                                           maxSkippedN_;
};

typedef Inherit<BlockKalmanFilterEchoCancellationFeature, VectorComplexFeatureStreamPtr> BlockKalmanFilterEchoCancellationFeaturePtr;
/*@}*/

/**
* \defgroup DTDBlockKalmanFilterEchoCancellationFeature Block Kalman Filer Echo Cancellation Feature
*/
/*@{*/

// ----- definition for class `InformationFilterEchoCancellationFeature' -----
//
class InformationFilterEchoCancellationFeature : public BlockKalmanFilterEchoCancellationFeature {
 public:
  InformationFilterEchoCancellationFeature(const VectorComplexFeatureStreamPtr& played, const VectorComplexFeatureStreamPtr& recorded, unsigned sampleN = 1, double beta = 0.95, double sigmau2 = 10e-4, double sigmauk2 = 5.0, double snrTh = 2.0, double engTh = 100.0, double smooth = 0.9, double loading = 1.0e-02,  double amp4play = 1.0, const String& nm = "Information Echo Canceller");

  virtual ~InformationFilterEchoCancellationFeature();

  virtual const gsl_vector_complex* next(int frame_no = -5);

protected:
  static double _EigenValueThreshold;

  double update_band_(const gsl_complex Ak, const gsl_complex Ek, int frame_no, unsigned m);
  void invert_(gsl_matrix_complex* matrix);
  static void print_matrix_(const gsl_matrix_complex* mat);
  static void print_vector_(const gsl_vector_complex* vec);

  const double                smoothEk_; // for smoothing the error signal, ->1, less smooth
  const double                smoothSk_; // for smoothing the estimated signal
  const double                engTh_;    // threshold of energy

  double*                     snr_;
  double*                     EkEnergy_;
  double*                     SkEnergy_;
  const double                loading_;

  gsl_matrix_complex*         inverse_;
  gsl_eigen_hermv_workspace*  eigenWorkSpace_;
  gsl_vector*                 evalues_;

  gsl_vector_complex*         scratchInverse_;
  gsl_matrix_complex*         scratchMatrixInverse_;
  gsl_matrix_complex*         scratchMatrixInverse2_;
  gsl_matrix_complex*         matrixCopy_;
};

typedef Inherit<InformationFilterEchoCancellationFeature, BlockKalmanFilterEchoCancellationFeaturePtr> InformationFilterEchoCancellationFeaturePtr;

// ----- definition for class `SquareRootInformationFilterEchoCancellationFeature' -----
//
class SquareRootInformationFilterEchoCancellationFeature : public InformationFilterEchoCancellationFeature {
 public:
  SquareRootInformationFilterEchoCancellationFeature(const VectorComplexFeatureStreamPtr& played, const VectorComplexFeatureStreamPtr& recorded, unsigned sampleN = 1, double beta = 0.95, double sigmau2 = 10e-4, double sigmauk2 = 5.0, double snrTh = 2.0, double engTh = 100.0, double smooth = 0.9, double loading = 1.0e-02, double amp4play = 1.0, const String& nm = "Square Root Information Echo Canceller");

  virtual ~SquareRootInformationFilterEchoCancellationFeature();

  virtual const gsl_vector_complex* next(int frame_no = -5);

private:
  const gsl_complex load_;
  static gsl_complex calc_givens_rotation_(const gsl_complex& v1, const gsl_complex& v2, gsl_complex& c, gsl_complex& s);
  static void apply_givens_rotation_(const gsl_complex& v1, const gsl_complex& v2, const gsl_complex& c, const gsl_complex& s,
                                   gsl_complex& v1p, gsl_complex& v2p);
  void negative_(gsl_matrix_complex* dest, const gsl_matrix_complex* src);
  void extract_covariance_state_(const gsl_matrix_complex* K_k, const gsl_vector_complex* sk, gsl_vector_complex* xk);

  void temporal_update_(unsigned m);
  void observational_update_(unsigned m, const gsl_complex& Ak, double sigma2_v);
  void diagonal_loading_(unsigned m);

  gsl_vector_complex**                                informationState_;
};

typedef Inherit<SquareRootInformationFilterEchoCancellationFeature, InformationFilterEchoCancellationFeaturePtr> SquareRootInformationFilterEchoCancellationFeaturePtr;

// ----- definition for class `DTDBlockKalmanFilterEchoCancellationFeature' -----
//
class DTDBlockKalmanFilterEchoCancellationFeature : public BlockKalmanFilterEchoCancellationFeature {
 public:
   DTDBlockKalmanFilterEchoCancellationFeature(const VectorComplexFeatureStreamPtr& played, const VectorComplexFeatureStreamPtr& recorded, unsigned sampleN = 1, double beta = 0.95, double sigmau2 = 10e-4, double sigmauk2 = 5.0, double snrTh = 2.0, double engTh = 100.0, double smooth = 0.9, double amp4play = 1.0, const String& nm = "DTDKFEchoCanceller");
  virtual ~DTDBlockKalmanFilterEchoCancellationFeature();

  virtual const gsl_vector_complex* next(int frame_no = -5);

private:
  double update_band_(const gsl_complex Ak, const gsl_complex Ek, int frame_no);
  void conjugate_(gsl_vector_complex* dest, const gsl_vector_complex* src) const;

  const double        smoothSk_; // for smoothing the estimated signal
  const double        smoothEk_; // for smoothing the error signal, ->1, less smooth
  const double        engTh_;    // threshold of energy
  double              snr_;

  double              EkEnergy_;
  double              SkEnergy_;
  FILE*               fdb_;
};

typedef Inherit<DTDBlockKalmanFilterEchoCancellationFeature, BlockKalmanFilterEchoCancellationFeaturePtr> DTDBlockKalmanFilterEchoCancellationFeaturePtr;
/*@}*/

#endif // AEC_H


#include <stdio.h>
#include <stdlib.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_filter.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>

int
main(void)
{
  const size_t N = 1000;                               /* length of time series */
  const size_t K = 7;                                  /* window size */
  const double f = 5.0;                                /* frequency of square wave in Hz */
  gsl_filter_median_workspace *median_p = gsl_filter_median_alloc(K);
  gsl_filter_rmedian_workspace *rmedian_p = gsl_filter_rmedian_alloc(K);
  gsl_vector *t = gsl_vector_alloc(N);                 /* time */
  gsl_vector *x = gsl_vector_alloc(N);                 /* input vector */
  gsl_vector *y_median = gsl_vector_alloc(N);          /* median filtered output */
  gsl_vector *y_rmedian = gsl_vector_alloc(N);         /* recursive median filtered output */
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  size_t i;

  /* generate input signal */
  for (i = 0; i < N; ++i)
    {
      double ti = (double) i / (N - 1.0);
      double tmp = sin(2.0 * M_PI * f * ti);
      double xi = (tmp >= 0.0) ? 1.0 : -1.0;
      double ei = gsl_ran_gaussian(r, 0.1);

      gsl_vector_set(t, i, ti);
      gsl_vector_set(x, i, xi + ei);
    }

  gsl_filter_median(GSL_FILTER_END_PADVALUE, x, y_median, median_p);
  gsl_filter_rmedian(GSL_FILTER_END_PADVALUE, x, y_rmedian, rmedian_p);

  /* print results */
  for (i = 0; i < N; ++i)
    {
      double ti = gsl_vector_get(t, i);
      double xi = gsl_vector_get(x, i);
      double medi = gsl_vector_get(y_median, i);
      double rmedi = gsl_vector_get(y_rmedian, i);

      printf("%f %f %f %f\n",
             ti,
             xi,
             medi,
             rmedi);
    }

  gsl_vector_free(t);
  gsl_vector_free(x);
  gsl_vector_free(y_median);
  gsl_vector_free(y_rmedian);
  gsl_rng_free(r);
  gsl_filter_median_free(median_p);

  return 0;
}


/* numerical stuff */

#include <math.h>
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include "numerical.h"
#include <gsl/gsl_sf.h>
//#include "nrutil.h"

/* Numerical Recipes Stuff */

#define GOLD 1.618034
#define GLIMIT 100.0
#define TINY  1.0e-20
#define MAX(a,b) ((a) > (b)  ? (a) : (b))
#define MIN(a,b) ((a) < (b)  ? (a) : (b))
#define SIGN(a,b)  ((b)  > 0.0 ? fabs(a) : -fabs(a))
#define SHFT(a,b,c,d)  (a)=(b);(b)=(c);(c)=(d);
#define FUNC(x) ((*func)(x))
#define FUNC2(x,p) ((*func)(x,p))
#define MIN_P_VALUE 1.0e-200
#define EPS 1.0e-5
#define JMAX 50

/* this is from numerical recipes; brackets a minimum with ax, bx, cx */
void mnbrak(double *ax,double *bx,double *cx,double *fa,double *fb,double *fc,double (*func)()){
	double ulim,u,r,q,fu,dum;

	*fa=(*func)(*ax);
	*fb=(*func)(*bx);
	if(*fb > *fa){
		SHFT(dum,*ax,*bx,dum)
		SHFT(dum,*fb,*fa,dum)
	}
	*cx = *bx + GOLD * (*bx-*ax);
	*fc=(*func)(*cx);
	while(*fb > *fc){
		r=(*bx-*ax)*(*fb-*fc);
		q=(*bx-*cx)*(*fb-*fa);
		u = *bx-((*bx-*cx)*q-(*bx-*ax)*r) /
		    (2.*SIGN(MAX(fabs(q-r),TINY),q-r));
		ulim = *bx + GLIMIT * (*cx-*bx);
		if((*bx-u) * (u-*cx) > 0.0){
			fu=(*func)(u);
			if(fu < *fc){
				*ax = *bx;
				*bx = u;
				*fa = *fb;
				*fb = fu;
				return;
   	 			}
			else if(fu > *fb){
				*cx=u;
				*fc=fu;
				return;
				}
			u = *cx + GOLD * (*cx-*bx);
			fu = (*func)(u);
			}
		else if((*cx-u)*(u-ulim) > 0.0){
			fu=(*func)(u);
			if(fu < *fc){
				SHFT(*bx,*cx,u,*cx + GOLD * (*cx - *bx));
				SHFT(*fb,*fc,fu,(*func)(u));
			}
		}
		else if((u-ulim)*(ulim-*cx) >= 0.0){
			u=ulim;
			fu=(*func)(u);
		}
		else{
			u = *cx+GOLD*(*cx-*bx);
			fu=(*func)(u);
			}
		SHFT(*ax,*bx,*cx,u)
		SHFT(*fa,*fb,*fc,fu)
		}
	return;
}

/* this is from numerical recipes; brackets a minimum with ax, bx, cx */
void mnbrak2(double *ax,double *bx,double *cx,double *fa,double *fb,double *fc,double (*func)(double, void * ), double beta, void * p){
	double ulim,u,r,q,fu,dum;

	*fa=(*func)(*ax, p);
	*fb=(*func)(*bx, p);
	if(*fb > *fa){
		SHFT(dum,*ax,*bx,dum)
		SHFT(dum,*fb,*fa,dum)
	}
	*cx = *bx + GOLD * (*bx-*ax);
	*fc=(*func)(*cx, p);
	while(*fb > *fc){
		r=(*bx-*ax)*(*fb-*fc);
		q=(*bx-*cx)*(*fb-*fa);
		u = *bx-((*bx-*cx)*q-(*bx-*ax)*r) /
		    (2.*SIGN(MAX(fabs(q-r),TINY),q-r));
		ulim = *bx + GLIMIT * (*cx-*bx);
		if((*bx-u) * (u-*cx) > 0.0){
			fu=(*func)(u, p);
			if(fu < *fc){
				*ax = *bx;
				*bx = u;
				*fa = *fb;
				*fb = fu;
				return;
   	 			}
			else if(fu > *fb){
				*cx=u;
				*fc=fu;
				return;
				}
			u = *cx + GOLD * (*cx-*bx);
			fu = (*func)(u, p);
			}
		else if((*cx-u)*(u-ulim) > 0.0){
			fu=(*func)(u, p);
			if(fu < *fc){
				SHFT(*bx,*cx,u,*cx + GOLD * (*cx - *bx));
				SHFT(*fb,*fc,fu,(*func)(u, p));
			}
		}
		else if((u-ulim)*(ulim-*cx) >= 0.0){
			u=ulim;
			fu=(*func)(u, p);
		}
		else{
			u = *cx+GOLD*(*cx-*bx);
			fu=(*func)(u, p);
			}
		SHFT(*ax,*bx,*cx,u)
		SHFT(*fa,*fb,*fc,fu)
		}
	return;
}

/* Numerical Integration Routines */

#define FUNC(x) ((*func)(x))
double d_trapzd(double (*func)(double), double a, double b, int n)
{
	double x,tnm,sum,del;
	static double s;
	int it,j;

	if (n == 1) {
		return (s=0.5*(b-a)*(FUNC(a)+FUNC(b)));
	} else {
		for (it=1,j=1;j<n-1;j++) it <<= 1;
		tnm=it;
		del=(b-a)/tnm;
		x=a+0.5*del;
		for (sum=0.0,j=1;j<=it;j++,x+=del) sum += FUNC(x);
		s=0.5*(s+(b-a)*sum/tnm);
		return s;
	}
}

/* specific version for GSL function */
double d_trapzd2(double (*func)(double, void *), double a, double b, int n, void * p)
{
	double x,tnm,sum,del;
	static double s;
	int it,j;

	if (n == 1) {
	  return (s=0.5*(b-a)*(FUNC2(a,p)+FUNC2(b,p)));
	} else {
		for (it=1,j=1;j<n-1;j++) it <<= 1;
		tnm=it;
		del=(b-a)/tnm;
		x=a+0.5*del;
		for (sum=0.0,j=1;j<=it;j++,x+=del) sum += FUNC2(x,p);
		s=0.5*(s+(b-a)*sum/tnm);
		return s;
	}
}

double d_qtrap(double (*func)(double), double a, double b)
{
	double trapzd(double (*func)(double), double a, double b, int n);
	void nrerror(char error_text[]);
	double s,olds;
	int j;

	olds = -1.0e30;
	for (j=1;j<=JMAX;j++) {
		s=d_trapzd(func,a,b,j);
		if (fabs(s-olds) < EPS*fabs(olds)) return s;
		olds=s;
	}
	nrerror("Too many steps in routine qtrap");
	return 0.0;
}

/* specific version for GSL function */
double d_qtrap2(double (*func)(double, void *), double a, double b, void * p){
  double s,olds;
  int j;

  olds = -1.0e30;
  for (j=1;j<=JMAX;j++) {
    s=d_trapzd2(func,a,b,j,p);
    if (fabs(s-olds) < EPS*fabs(olds)) return s;
    olds=s;
  }
  nrerror("Too many steps in routine qtrap");
  return 0.0;
}

double d_qsimp(double (*func)(double), double a, double b)
{
	double trapzd(double (*func)(double), double a, double b, int n);
	void nrerror(char error_text[]);
	int j;
	double s,st,ost,os;

	ost = os = -1.0e30;
	for (j=1;j<=JMAX;j++) {
		st=d_trapzd(func,a,b,j);
		s=(4.0*st-ost)/3.0;
		if (fabs(s-os) < EPS*fabs(os)) return s;
		os=s;
		ost=st;
	}
	nrerror("Too many steps in routine qsimp");
	return 0.0;
}

#define JMAXP (JMAX+1)
#define K 5
double d_qromb(double (*func)(double), double a, double b)
{
	void polint(double xa[], double ya[], int n, double x, double *y, double *dy);
	double trapzd(double (*func)(double), double a, double b, int n);
	void nrerror(char error_text[]);
	double ss,dss;
	double s[JMAXP+1],h[JMAXP+1];
	int j;

	h[1]=1.0;
	for (j=1;j<=JMAX;j++) {
		s[j]=d_trapzd(func,a,b,j);
		if (j >= K) {
			d_polint(&h[j-K],&s[j-K],K,0.0,&ss,&dss);
			if (fabs(dss) < EPS*fabs(ss)) return ss;
		}
		s[j+1]=s[j];
		h[j+1]=0.25*h[j];
	}
	nrerror("Too many steps in routine qromb");
	return 0.0;
}

double d_qromb2(double (*func)(double, void *), double a, double b, void * p){

  double ss,dss;
  double s[JMAXP+1],h[JMAXP+1];
  int j;

  h[1]=1.0;
  for (j=1;j<=JMAX;j++) {
    s[j]=midpnt2(func,a,b,j,p);
    if (j >= K) {
      d_polint(&h[j-K],&s[j-K],K,0.0,&ss,&dss);
      if (fabs(dss) < EPS*fabs(ss)) return ss;
    }
    s[j+1]=s[j];
    h[j+1]=0.25*h[j];
  }
  nrerror("Too many steps in routine qromb");
  return 0.0;
}
#undef JMAXP
#undef K

#define NRANSI
#include "nrutil.h"
void d_polint(double xa[], double ya[], int n, double x, double *y, double *dy)
{
	int i,m,ns=1;
	double den,dif,dift,ho,hp,w;
	double *c,*d;

	dif=fabs(x-xa[1]);
	c=dvector(1,n);
	d=dvector(1,n);
	for (i=1;i<=n;i++) {
		if ( (dift=fabs(x-xa[i])) < dif) {
			ns=i;
			dif=dift;
		}
		c[i]=ya[i];
		d[i]=ya[i];
	}
	*y=ya[ns--];
	for (m=1;m<n;m++) {
		for (i=1;i<=n-m;i++) {
			ho=xa[i]-x;
			hp=xa[i+m]-x;
			w=c[i+1]-d[i];
			if ( (den=ho-hp) == 0.0) nrerror("Error in routine polint");
			den=w/den;
			d[i]=hp*den;
			c[i]=ho*den;
		}
		*y += (*dy=(2*ns < (n-m) ? c[ns+1] : d[ns--]));
	}
	free_dvector(d,1,n);
	free_dvector(c,1,n);
}
#undef NRANSI

/* from chp. 4 of numerical recipes */
double midpnt(double (*func)(double), double a, double b, int n){
  double x,tnm,sum,del,ddel;
  static double s;
  int it,j;

  if (n == 1) {
    return (s=(b-a)*FUNC(0.5*(a+b)));
  } else {
    for(it=1,j=1;j<n-1;j++) it *= 3;
    tnm=it;
    del=(b-a)/(3.0*tnm);
    ddel=del+del;
    x=a+0.5*del;
    sum=0.0;
    for (j=1;j<=it;j++) {
      sum += FUNC(x);
      x += ddel;
      sum += FUNC(x);
      x += del;
    }
    s=(s+(b-a)*sum/tnm)/3.0;
    return s;
  }
}

/* from chp. 4 of numerical recipes, specific for GSL functions */
double midpnt2(double (*func)(double, void *), double a, double b, int n, void * p){
  double x,tnm,sum,del,ddel;
  static double s;
  int it,j;

  if (n == 1) {
    return (s=(b-a)*FUNC2(0.5*(a+b),p));
  } else {
    for(it=1,j=1;j<n-1;j++) it *= 3;
    tnm=it;
    del=(b-a)/(3.0*tnm);
    ddel=del+del;
    x=a+0.5*del;
    sum=0.0;
    for (j=1;j<=it;j++) {
      sum += FUNC2(x,p);
      x += ddel;
      sum += FUNC2(x,p);
      x += del;
    }
    s=(s+(b-a)*sum/tnm)/3.0;
    return s;
  }
}

/* factorials */

double xchoosey(int n, int k){
  if(n<k) return(0);
  if(n == k || k == 0) return(1);
  return(gsl_sf_choose(n,k));
}

double multinom(int x, int n, ...) {
  int i, y;
  va_list ap;
  double ret_val=0.0;

  va_start(ap, n);
  for (i=0; i<n; i++) {
    y = va_arg(ap, int);
    if (x < y) return 0.0;
    ret_val *= xchoosey(x, y);
    x -= y;
    if (x < 0) return 0.0;
  }
  va_end(ap);
  return exp(ret_val);
}

//from Chuck Langley
/* +++++++++++  chl   19JAN2010  ++++++++++++++++++++++++++++++++++++++++++  */

double FsXctTst(int m11, int m12, int m21, int m22) {
	double      onetail, twotail;
	double		ep[(m11+m12+m21+m22 + 1)];
	double		cov[(m11+m12+m21+m22 + 1)];
	int			nt11,nt12,nt21,nt22 ;
	int			i ;
	if( ((m11+m21)<1) || ((m12+m22)<1) )   return(-99.0);
	nt11 = 0 ;
	do
	{
		nt12 = m11 + m12 - nt11 ;
		nt21 = m11 + m21 - nt11 ;
		nt22 = m11 + m12 + m21 + m22 - nt11 - nt12 - nt21 ;

		ep[nt11] = 0.0 ;
		cov[nt11] = 0.0 ;

		if ((nt12 < 0) || (nt21 < 0) || (nt22 < 0))
			nt11++ ;
		else
		{
			ep[nt11] = ExactProb(nt11,nt12,nt21,nt22) ;
			cov[nt11] = nt11*nt22 - nt12*nt21 ;

			/*	fprintf(stderr,"\nep[%2d] = %e\tcov[%2d] = %e", nt11,ep[nt11],nt11,cov[nt11]) ;	*/
			nt11++ ;
		}
	}	while (nt11 <= (MIN( (m11 + m12),(m11 + m21))) ) ;

	if(cov[m11] <= 0.0){
		onetail = 0.0 ;

		for(i=0;i<=m11;i++)	onetail += ep[i] ;

		twotail = onetail ;
		i = nt11 - 1 ;
		if(ep[i] <= (ep[m11] + 0.0001))
		{
			do
			{
				twotail += ep[i] ;
				i-- ;
			}	while ((ep[i] <= (ep[m11] + 0.0001)) && (i > m11)) ;
		}
	}
	else
	{
		onetail = 0.0 ;

		for(i=m11;i<nt11;i++)	onetail += ep[i] ;

		twotail = onetail ;
		i = 0 ;
		if(ep[0] <= (ep[m11] + 0.0001))
		{
			do
			{
				twotail += ep[i] ;
				i++ ;
			}	while ((ep[i] <= (ep[m11] + 0.0001)) && (i < m11)) ;
		}
	}
	if(cov[m11] == 0.0)	onetail = 99 ;

	/*	fprintf(stderr,"\n onetail = %e \t twotail = %e ", *onetail, *twotail) ;		*/
	if(twotail < MIN_P_VALUE)   printf("\nFsXctTst(%d,%d,%d,%d) = %lf", m11, m12, m21, m22, twotail);
	return(twotail) ;
}

double	ExactProb(int nn11, int nn12, int nn21, int nn22){
	double	xp = 0.0;

	xp  = logfact(nn11+nn12) ;
	xp += logfact(nn11+nn21) ;
	xp += logfact(nn12+nn22) ;
	xp += logfact(nn21+nn22) ;
	xp -= logfact(nn11+nn12+nn21+nn22) ;
	xp -= logfact(nn11) ;
	xp -= logfact(nn12) ;
	xp -= logfact(nn21) ;
	xp -= logfact(nn22) ;

	return(exp(xp)) ;
}

double	logfact(int num){
	double	sumlog = 0.0 ;

	if((num == 0) || (num == 1))	return(0.0) ;
	else
	{
		do
		{
			sumlog += log(((double)num)) ;
			num-- ;
		} while (num > 1) ;

		return(sumlog) ;
	}
    return(-99.99) ;
}

/* #define ITMAX 200 */
/* #define NRANSI */

/* void powellMin(double p[], double **xi, int n, double ftol, int *iter, double *fret, */
/* 	double (*func)(double [])) */
/* { */
/* 	void linmin(double p[], double xi[], int n, double *fret, */
/* 		float (*func)(double [])); */
/* 	int i,ibig,j; */
/* 	float del,fp,fptt,t,*pt,*ptt,*xit; */

/* 	pt=vector(1,n); */
/* 	ptt=vector(1,n); */
/* 	xit=vector(1,n); */
/* 	*fret=(*func)(p); */
/* 	for (j=1;j<=n;j++) pt[j]=p[j]; */
/* 	for (*iter=1;;++(*iter)) { */
/* 		fp=(*fret); */
/* 		ibig=0; */
/* 		del=0.0; */
/* 		for (i=1;i<=n;i++) { */
/* 			for (j=1;j<=n;j++) xit[j]=xi[j][i]; */
/* 			fptt=(*fret); */
/* 			linmin(p,xit,n,fret,func); */
/* 			if (fabs(fptt-(*fret)) > del) { */
/* 				del=fabs(fptt-(*fret)); */
/* 				ibig=i; */
/* 			} */
/* 		} */
/* 		if (2.0*fabs(fp-(*fret)) <= ftol*(fabs(fp)+fabs(*fret))) { */
/* 			free_vector(xit,1,n); */
/* 			free_vector(ptt,1,n); */
/* 			free_vector(pt,1,n); */
/* 			return; */
/* 		} */
/* 		if (*iter == ITMAX) nrerror("powell exceeding maximum iterations."); */
/* 		for (j=1;j<=n;j++) { */
/* 			ptt[j]=2.0*p[j]-pt[j]; */
/* 			xit[j]=p[j]-pt[j]; */
/* 			pt[j]=p[j]; */
/* 		} */
/* 		fptt=(*func)(ptt); */
/* 		if (fptt < fp) { */
/* 			t=2.0*(fp-2.0*(*fret)+fptt)*SQR(fp-(*fret)-del)-del*SQR(fp-fptt); */
/* 			if (t < 0.0) { */
/* 				linmin(p,xit,n,fret,func); */
/* 				for (j=1;j<=n;j++) { */
/* 					xi[j][ibig]=xi[j][n]; */
/* 					xi[j][n]=xit[j]; */
/* 				} */
/* 			} */
/* 		} */
/* 	} */
/* } */
/* #undef ITMAX */
/* #undef NRANSI */

/* #define NRANSI */
/* #define TOL 2.0e-4 */

/* int ncom; */
/* double *pcom,*xicom,(*nrfunc)(double []); */

/* void linmin(double p[], double xi[], int n, double *fret, double (*func)(double [])) */
/* { */
/* 	float brent(double ax, double bx, double cx, */
/* 		double (*f)(double), double tol, double *xmin); */
/* 	double f1dim(double x); */
/* 	void mnbrak(double *ax, double *bx, double *cx, double *fa, double *fb, */
/* 		double *fc, double (*func)(double)); */
/* 	int j; */
/* 	double xx,xmin,fx,fb,fa,bx,ax; */

/* 	ncom=n; */
/* 	pcom=vector(1,n); */
/* 	xicom=vector(1,n); */
/* 	nrfunc=func; */
/* 	for (j=1;j<=n;j++) { */
/* 		pcom[j]=p[j]; */
/* 		xicom[j]=xi[j]; */
/* 	} */
/* 	ax=0.0; */
/* 	xx=1.0; */
/* 	mnbrak(&ax,&xx,&bx,&fa,&fx,&fb,f1dim); */
/* 	*fret=brent(ax,xx,bx,f1dim,TOL,&xmin); */
/* 	for (j=1;j<=n;j++) { */
/* 		xi[j] *= xmin; */
/* 		p[j] += xi[j]; */
/* 	} */
/* 	free_vector(xicom,1,n); */
/* 	free_vector(pcom,1,n); */
/* } */
/* #undef TOL */
/* #undef NRANSI */

/* #define NRANSI */
/* #define ITMAX 100 */
/* #define CGOLD 0.3819660 */
/* #define ZEPS 1.0e-10 */
/* #define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d); */

/* double brent(double ax, double bx, double cx, double (*f)(double), double tol, */
/* 	double *xmin) */
/* { */
/* 	int iter; */
/* 	double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm; */
/* 	double e=0.0; */

/* 	a=(ax < cx ? ax : cx); */
/* 	b=(ax > cx ? ax : cx); */
/* 	x=w=v=bx; */
/* 	fw=fv=fx=(*f)(x); */
/* 	for (iter=1;iter<=ITMAX;iter++) { */
/* 		xm=0.5*(a+b); */
/* 		tol2=2.0*(tol1=tol*fabs(x)+ZEPS); */
/* 		if (fabs(x-xm) <= (tol2-0.5*(b-a))) { */
/* 			*xmin=x; */
/* 			return fx; */
/* 		} */
/* 		if (fabs(e) > tol1) { */
/* 			r=(x-w)*(fx-fv); */
/* 			q=(x-v)*(fx-fw); */
/* 			p=(x-v)*q-(x-w)*r; */
/* 			q=2.0*(q-r); */
/* 			if (q > 0.0) p = -p; */
/* 			q=fabs(q); */
/* 			etemp=e; */
/* 			e=d; */
/* 			if (fabs(p) >= fabs(0.5*q*etemp) || p <= q*(a-x) || p >= q*(b-x)) */
/* 				d=CGOLD*(e=(x >= xm ? a-x : b-x)); */
/* 			else { */
/* 				d=p/q; */
/* 				u=x+d; */
/* 				if (u-a < tol2 || b-u < tol2) */
/* 					d=SIGN(tol1,xm-x); */
/* 			} */
/* 		} else { */
/* 			d=CGOLD*(e=(x >= xm ? a-x : b-x)); */
/* 		} */
/* 		u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d)); */
/* 		fu=(*f)(u); */
/* 		if (fu <= fx) { */
/* 			if (u >= x) a=x; else b=x; */
/* 			SHFT(v,w,x,u) */
/* 			SHFT(fv,fw,fx,fu) */
/* 		} else { */
/* 			if (u < x) a=u; else b=u; */
/* 			if (fu <= fw || w == x) { */
/* 				v=w; */
/* 				w=u; */
/* 				fv=fw; */
/* 				fw=fu; */
/* 			} else if (fu <= fv || v == x || v == w) { */
/* 				v=u; */
/* 				fv=fu; */
/* 			} */
/* 		} */
/* 	} */
/* 	nrerror("Too many iterations in brent"); */
/* 	*xmin=x; */
/* 	return fx; */
/* } */

/* #undef ITMAX */
/* #undef CGOLD */
/* #undef ZEPS */
/* #undef SHFT */
/* #undef NRANSI */

/* #define NRANSI */
/* extern int ncom; */
/* extern double *pcom,*xicom,(*nrfunc)(double []); */

/* double f1dim(double x) */
/* { */
/* 	int j; */
/* 	double f,*xt; */

/* 	xt=vector(1,ncom); */
/* 	for (j=1;j<=ncom;j++) xt[j]=pcom[j]+x*xicom[j]; */
/* 	f=(*nrfunc)(xt); */
/* 	free_vector(xt,1,ncom); */
/* 	return f; */
/* } */
/* #undef NRANSI */


/**
@file

Copyright John Reid 2006

*/

#ifndef BIOPSY_LOG_PROBS_H_
#define BIOPSY_LOG_PROBS_H_

#ifdef _MSC_VER
# pragma once
#endif //_MSC_VER

#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_machine.h>

#include <boost/test/floating_point_comparison.hpp>

namespace biopsy {

inline double safe_log( double x )
{
	return x <= 0.0
		? std::numeric_limits< double >::quiet_NaN()
		: gsl_sf_log( x );
}

template<
	typename ParameterRange
>
void
normalise_discrete_probabilities(
	ParameterRange & parameters )
{
	using namespace boost;

	const double sum = std::accumulate( begin( parameters ), end( parameters ), 0.0 );
	BOOST_FOREACH( typename range_value< ParameterRange >::type & p, parameters )
	{
		p /= sum;
	}
}

template<
	typename log_vector,
	typename out_vector
>
void
probabilities_from_logs_of_choose(
	const log_vector & logs,
	out_vector & out )
{
	out.resize( logs.size() );

	// find the largest - as we can get underflow problems
	const double largest = *( std::max_element( logs.begin(), logs.end() ) );
	for( unsigned i = 0; logs.size() != i; ++i )
	{
		//scale by the largest
		const double v = logs[i] - largest;
		out[i] = (v <= GSL_LOG_DBL_MIN) ? 0.0 : gsl_sf_exp( v );
		BOOST_ASSERT( ! _isnan( out[i] ) );
	}

	normalise_discrete_probabilities( out );
}

template<
	typename ValueRange,
	typename ParameterRange
>
double
dirichlet_log_pdf( const ValueRange & values, const ParameterRange & parameters )
{
	using namespace boost;

	const double param_sum = std::accumulate( begin( parameters ), end( parameters ), 0.0 );

	double result = gsl_sf_lngamma( param_sum );

	typedef boost::tuple< double, double > param_value_tuple;
	BOOST_FOREACH(
		const param_value_tuple & t,
		make_iterator_range(
			make_zip_iterator( make_tuple( begin( parameters ), begin( values ) ) ),
			make_zip_iterator( make_tuple( end( parameters ), end( values ) ) ) ) )
	{
		const double p = t.get<0>();
		const double v = t.get<1>();

		result -= gsl_sf_lngamma( p );
		result += ( p - 1.0 ) * gsl_sf_log( v );
	}

	return result;
}

template<
	typename ParameterRange,
	typename OutputRange
>
void
draw_from_dirichlet( const ParameterRange & alpha, OutputRange & output )
{
	using namespace boost;

	BOOST_ASSERT( size( alpha ) == size( output ) );

	gsl_ran_dirichlet(
		get_gsl_rng(),
		size( alpha ),
		&*begin( alpha ),
		&*begin( output ) );
}

template<
	typename ParameterRange
>
unsigned
draw_from_multi( const ParameterRange & alpha )
{
	using namespace boost;
	using namespace boost::test_tools;

	BOOST_ASSERT(
		close_at_tolerance< double >( percent_tolerance_t< double >( 1.0 ) )(
			1.0,
			std::accumulate( begin( alpha ), end( alpha ), 0.0 ) ) );

	double p = gsl_rng_uniform( get_gsl_rng() );

	unsigned result = 0;
	BOOST_FOREACH( const double a, alpha )
	{
		if( p < a )
		{
			break;
		}
		p -= a;
		++result;
	}

	BOOST_ASSERT( result < size( alpha ) );

	return result;
}

} //namespace biopsy

#endif //BIOPSY_LOG_PROBS_H_