/** @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 <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


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;
}


#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 */


/* ============================================================ *
 * cosebi_demo.c						*
 * Martin Kilbinger, Liping Fu 2012.				*
 *  2013. Tomography.						*
 * Reference:							*
 * Schneider, Eifler, Krause 2010                               *
 * ============================================================ */

#include <stdio.h>
#include <stdlib.h>
#include "errorlist.h"
#include "io.h"
#include "maths.h"
#include "cosmo.h"
#include "lensing.h"
#include "nofz.h"
#include "decomp_eb.h"

#include <getopt.h>
#include <gsl/gsl_sf_gamma.h>

void write_tpmlog(int n, double thmin, double thmax, const double *c, error **err)
{
   FILE *F;
   double z, theta, tp, tm, dth;
   char name[128];

   sprintf(name, "T_%d_%.3f_%.3f.dat", n, thmin/arcmin, thmax/arcmin);

   F = fopen_err(name, "w", err); forwardError(*err, __LINE__,);

   fprintf(F, "# z=log(thmax/thmin) theta[arcmin] T_+^log T_-^log, (n, thmin, thmax = %d, %g, %g)\n",
	   n, thmin/arcmin, thmax/arcmin);

   dth = 1.01;
   for (theta=thmin; theta<=thmax; theta*=dth) {

      z = log(theta / thmin);

      tp = Tplog_c(z, c, n, err);
      forwardError(*err, __LINE__,);

      tm = Tmlog(z, c, n, err);
      forwardError(*err, __LINE__,);

      fprintf(F, "%f %g % g % g\n", z, theta/arcmin, tp, tm);
   }

   fclose(F);
}

void write_xi_pm_EB(int N, double thmin, double thmax, const double *c_cosebi,
		    const double *E_cos, const double *B_cos, error **err)
{
   double xi_pm_EB[4], theta, fth;
   const char name[128] = "xi_pm_EB.dat";
   int n;
   FILE *F;

   F = fopen_err(name, "w", err);       forwardError(*err, __LINE__,);
   fprintf(F, "# theta[arcmin] E+ E- B+ B-\n");

   fth = 1.1;
   for (theta=thmin; theta<=thmax; theta *= fth) {
      xipmEB(theta, thmin, thmax, c_cosebi, E_cos, B_cos, N, xi_pm_EB, err);
      forwardError(*err, __LINE__,);

      fprintf(F, "%12.3g  ", theta/arcmin);
      for (n=0; n<4; n++) fprintf(F, " % g", xi_pm_EB[n]);
      fprintf(F, "\n");
   }

   fclose(F);
}

double E_cosebi_data(const double *xip, const double *xim, const double *theta, int Nxi,
		     int n, double Psimin, double Psimax, const double *c_cosebi,
		     double *B_cosebi, error **err)
{
   double z, delta_theta, rp, rm, summand;
   int i;

   testErrorRetVA(Psimin < theta[0], mr_range, "COSEBI theta_min = %g' is smaller than xi theta_min = %g'",
		  *err, __LINE__, -1.0, Psimin / arcmin, theta[0] / arcmin);
   testErrorRetVA(Psimax > theta[Nxi-1], mr_range, "COSEBI theta_max = %g' is larger than xi theta_max = %g'",
		  *err, __LINE__, -1.0, Psimax / arcmin, theta[Nxi-1] / arcmin);

   rp = rm = 0.0;
   for (i=0; i<Nxi; i++) {

      if (theta[i] < Psimin) continue;
      if (theta[i] > Psimax) break;

      if (i > 0) {
	 delta_theta = theta[i] - theta[i-1];
      } else {
	 delta_theta = theta[1] - theta[0];
      }

      z = log(theta[i] / Psimin);

      summand  = xip[i];
      summand *= theta[i] * delta_theta;
      summand *= Tplog_c(z, c_cosebi, n, err);
      forwardError(*err, __LINE__, -1.0);
      rp      += summand;

      summand  = xim[i];
      summand *= theta[i] * delta_theta;
      summand *= Tmlog(z, c_cosebi, n, err);
      forwardError(*err, __LINE__, -1.0);
      rm      += summand;

      //printf("%g %g   %g %g\n", theta[i]/arcmin, z, xip[i], xim[i]);

   }

   if (B_cosebi != NULL) {
      *B_cosebi = 0.5 * (rp - rm);
   }

   return 0.5 * (rp + rm);
}

void usage(int ex)
{
   fprintf(stderr, "Calculates the COSEBIs from Schneider, Eifler, Krause 2010\n");
   fprintf(stderr, "Usage: coseb_demo [OPTIONS]\n");
   fprintf(stderr, "OPTIONS\n");
   fprintf(stderr, "  -N N       Maximum COSEBIs mode N\n");
   fprintf(stderr, "  -m THMIN   Minimum COSEBIs scale THMIN (in arcmin; default 1)\n");
   fprintf(stderr, "  -M THMAX   Maximum COSEBIs scale THMAX (in arcmin; default 400)\n");
   fprintf(stderr, "  -d XI      Data file XI (cosmo_pmc input ['xipm'] format; default none)\n");
   fprintf(stderr, "  -T         Write filter functions to files 'T_n_thmin_thmax.dat'\n");
   fprintf(stderr, "  -X         Write EB correlation functions to files 'xEB_n_thmin_thmax.dat'\n");
   fprintf(stderr, "  -P PATH    Path for coefficient files ('cosebi_tplog_rN_N_thmin_thmax';\n");
   fprintf(stderr, "              default $COSMOPMC/par_files/COSEBIs)\n");
   fprintf(stderr, "  -h         This message\n");

   if (ex >= 0) exit(ex);
}

int main(int argc, char* argv[])
{
   error *myerr = NULL, **err;
   double *E_cos, *B_cos;
   double Psimin, Psimax;
   int c, N, n, Nxi, write_T, write_X, i_bin, j_bin, Nzbin;
   cosmo_lens *model;
   char *xi_name, *path, *strp;
   FILE *F;
   datcov *dc;
   double *xip, *xim, *theta, *theta2, *c_cosebi;

   fprintf(stderr, "# cosebi_demo (M. Kilbinger, L. Fu, 2012)\n");

   err = &myerr;

   /* Command line arguments */
   Psimin  = 0.174;
   Psimax  = 200.0;
   N       = 10;
   xi_name = NULL;
   write_T = write_X = 0;
   path    = NULL;
   while (1) {

      static struct option long_options[] = {
	  {"", required_argument, 0, 'N'},
	  {"", required_argument, 0, 'd'},
	  {"", required_argument, 0, 'P'},
	  {0, 0, 0, 0}
	};

      int option_index = 0;

      c = getopt_long(argc, argv, "N:d:m:M:TXP:h", long_options, &option_index);
      switch (c) {
	 case 'N' :
	    N = atoi(optarg);
	    break;
	 case 'd' :
	    xi_name = optarg;
	    break;
	 case 'P' :
	    path = optarg;
	    break;
	 case 'm' :
       Psimin = atof(optarg);
	    break;
	 case 'M' :
	    Psimax = atof(optarg);
	    break;
	 case 'T' :
	    write_T = 1;
	    break;
	 case 'X' :
	    write_X = 1;
	    break;
	 case 'h' :
	    usage(0);
	 case -1 :
	    goto end_arg;
	 default :
	    usage(1);
      }

   }
 end_arg:

   if (N<0 || N>NMAX_COSEBI) {
      fprintf(stderr, "N=%d out of range [0; %d]", N, NMAX_COSEBI);
      exit(4);
   }

   if (path == NULL) {
      strp = getenv("COSMOPMC");
      if (strp == NULL) {
         strp = ".";
      }
      path = malloc_err(sizeof(char) * 1024, err);
      quitOnError(*err, __LINE__, stderr);
      sprintf(path, "%s/par_files/COSEBIs", strp);
   }

   E_cos = malloc_err(sizeof(double) * (N+1), err);      quitOnError(*err, __LINE__, stderr);
   B_cos = malloc_err(sizeof(double) * (N+1), err);      quitOnError(*err, __LINE__, stderr);

   if (xi_name == NULL) {

      /* COSEBIs from theoretical model */

      F = fopen_err("cosmo_lens.par", "r", err);
      quitOnError(*err, __LINE__, stderr);
      read_cosmological_parameters_lens(&model, F, err);
      quitOnError(*err, __LINE__, stderr);
      fclose(F);
      Nzbin = model->redshift->Nzbin;

      dump_param(model->cosmo, stdout);
      fprintf(stdout, "# n E_n^{ij} B_n^{ij}\n# n");
      for (i_bin=0; i_bin<Nzbin; i_bin++) {
         for (j_bin=i_bin; j_bin<Nzbin; j_bin++) {
            if (model->tomo==tomo_auto_only && i_bin!=j_bin) continue;
            if (model->tomo==tomo_cross_only && i_bin==j_bin) continue;
            fprintf(stdout, "        E_n^{%d%d}        B_n^{%d%d}", i_bin, j_bin, i_bin, j_bin);
         }
      }
      fprintf(stdout, "\n");

      for (n=1; n<=N; n++) {
         fprintf(stdout, "%3d", n);

         for (i_bin=0; i_bin<Nzbin; i_bin++) {
            for (j_bin=i_bin; j_bin<Nzbin; j_bin++) {
               if (model->tomo==tomo_auto_only && i_bin!=j_bin) continue;
               if (model->tomo==tomo_cross_only && i_bin==j_bin) continue;
               E_cos[n] = E_cosebi(model, n, Psimin*arcmin, Psimax*arcmin, i_bin, j_bin, path, B_cos+n, err);
               quitOnError(*err, __LINE__, stderr);
               fprintf(stdout, " % 15.6g % 15.6g", E_cos[n], B_cos[n]);
            }
         }

         fprintf(stdout, "\n");
      }

      c_cosebi = model->c_cosebi;

   } else {

      /* COSEBIs from data */
      c_cosebi = read_zeros_norm_cosebi_auto_check(Psimin*arcmin, Psimax*arcmin, path, err);
      quitOnError(*err, __LINE__, stderr);

      dc = malloc_err(sizeof(datcov), err);
      quitOnError(*err, __LINE__, stderr);
      dc->format = angle_center;
      /* Reading xi file */
      read_data_tomo(dc, xi_name, 0, second_order, err);
      quitOnError(*err, __LINE__, stderr);

      Nzbin    = dc->Nzbin;
      dc->type = xipm;

      /* Header */
      fprintf(stdout, "# n E_n^{ij} B_n^{ij}\n# n   ");
      for (i_bin=0; i_bin<Nzbin; i_bin++) {
         for (j_bin=i_bin; j_bin<Nzbin; j_bin++) {
            fprintf(stdout, "E_n^{%d%d}        B_n^{%d%d}          ", i_bin, j_bin, i_bin, j_bin);
         }
      }
      fprintf(stdout, "\n");

      for (n=1; n<=N; n++) {
	 fprintf(stdout, "%3d", n);

	 for (i_bin=0; i_bin<Nzbin; i_bin++) {
	    for (j_bin=i_bin; j_bin<Nzbin; j_bin++) {
	       datcov2xipm(dc, i_bin, j_bin, &xip, &xim, &theta, &theta2, &Nxi, err);
	       quitOnError(*err, __LINE__, stderr);

	       E_cos[n] = E_cosebi_data(xip, xim, theta, Nxi, n, Psimin*arcmin, Psimax*arcmin, c_cosebi, B_cos+n, err);
	       quitOnError(*err, __LINE__, stderr);
	       fprintf(stdout, "   % .9g % .9g", E_cos[n], B_cos[n]);
	    }
	 }
	 fprintf(stdout, "\n");

	 free(xip);
	 free(xim);
	 free(theta);
      }

      del_data_cov(&dc);

   }

   if (write_T) {

      /* Write filter functions to file */

      for (n=1; n<=N; n++) {
         write_tpmlog(n, Psimin*arcmin, Psimax*arcmin, c_cosebi, err);
         quitOnError(*err, __LINE__, stderr);
      }

   }

   if (write_X) {

      /* Calculate xipmEB according to SEK 10 eq (40) */

      write_xi_pm_EB(N, Psimin*arcmin, Psimax*arcmin, c_cosebi, E_cos, B_cos, err);

   }

   free(E_cos);
   free(B_cos);

   return 0;
}


/*! \file Fitting.h
 *  \brief header file for fitting functions to data.
 */

#ifndef FITTING_H
#define FITTING_H

#include <cmath>
#include <iostream>
#include <limits>
#include <gsl/gsl_math.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_errno.h>
#ifdef HAVE_GSL22
#include <gsl/gsl_multifit_nlinear.h>
#endif
#include <Precision.h>
#include <Function.h>
#include <Matrix.h>
#include <GMatrix.h>
#include <Interpolate.h>
#include <ExtremaRootFinding.h>

using namespace std;
namespace Math
{
    /*! \brief Nonlinear fitting using Levenberg–Marquardt algorithm.

        Least Squares fitting of function f(x) from x[lower] to x[upper] to data y[lower]-y[upper]. Returns chi2 and altered params (which must be passed with initial guess)
        one can also pass a weight matrix (which is inverse of covariance matrix of data points), error on the relative change in chi2, and also a pointer that indicates whether
        the parameters are held fixed. finally, if data binned dof reduced by 1 and can specify maximum number of iterations
        Note the function does not explicitly take into account errors on the x and y points save in the use of the weight matrix which is the inverse of the covariance matrix
        Furthermore, the covariance matrix for the parameters is determined using the confidence level assuming normaly distributed errors. If that is not the case,
        the returned covariance matrix will not be correct and the degree to which it is correct cannot be correct for. Instead a more rigours determination of the confidence
        region used to determine the covariance matrix, such as MC methods using the data as a sample of the underlying distribution and fitting resulting data numerous times
        must be done. The resulting distribution of parameters can then be fit itself using a function if necessary.
    */
    Double_t FitNonLinLS(const math_function fitfunc, const math_function *difffuncs,
        const int nparams, Double_t *params, GMatrix &covar,
        const int npoints, const Double_t x[], const Double_t y[], GMatrix *Weight=NULL,
        Double_t error=1e-3, Double_t cl=0.95,
        int *fixparam=NULL, int binned=1, int maxiterations=1000, int iestimateerror=0,
        bool icallgsl=true);

    Double_t FitNonLinLSNoGSL(const math_function fitfunc, const math_function *difffuncs,
        const int nparams, Double_t *params, GMatrix &covar,
        const int npoints, const Double_t x[], const Double_t y[], GMatrix *Weight=NULL,
        Double_t error=1e-3, Double_t cl=0.95,
        int *fixparam=NULL, int binned=1, int maxiterations=1000, int iestimateerror=0);

#ifdef HAVE_GSL22
    Double_t FitNonLinLSWithGSL(const math_function fitfunc, const math_function *difffuncs,
        const int nparams, Double_t *params, GMatrix &covar,
        const int npoints, const Double_t x[], const Double_t y[],
        GMatrix *W, Double_t error, Double_t cl,
        int *fixparam, int binned, int maxiit, int iestimateerror);
#endif

    /*! \brief Given n points of data, determine optimal bin size and return binned data and bin centers.

        This is done by dividing the data range into N bins of width Delta. Count the number of events k_i that enter the i'th bin.
        Calculate the mean and variance of the number of events as kmean=1/N *Sum k_i and kvar=1/N*Sum (k_i -kmean)^2
        Then calculate C(Delta)= (2*kmean-kvar)/Delta^2, a cost function
        Repeat while changing Delta and find Delta' that minimizes C(Delta). Return Delta'
        Based on Shimazaki H. and Shinomoto S., A method for selecting the bin size of a time histogram Neural Computation (2007) Vol. 19(6), 1503-1527 \n
        Can also provide weights for the data points.
    */
    Double_t OptimalBins(const int npoints, Double_t *points, Double_t xmin, Double_t xmax, Double_t *weights=NULL);
}

#endif


#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_histogram2d.h>

int
main (void)
{
  const gsl_rng_type * T;
  gsl_rng * r;

  gsl_histogram2d * h = gsl_histogram2d_alloc (10, 10);

  gsl_histogram2d_set_ranges_uniform (h,
                                      0.0, 1.0,
                                      0.0, 1.0);

  gsl_histogram2d_accumulate (h, 0.3, 0.3, 1);
  gsl_histogram2d_accumulate (h, 0.8, 0.1, 5);
  gsl_histogram2d_accumulate (h, 0.7, 0.9, 0.5);

  gsl_rng_env_setup ();

  T = gsl_rng_default;
  r = gsl_rng_alloc (T);

  {
    int i;
    gsl_histogram2d_pdf * p
      = gsl_histogram2d_pdf_alloc (h->nx, h->ny);

    gsl_histogram2d_pdf_init (p, h);

    for (i = 0; i < 1000; i++) {
      double x, y;
      double u = gsl_rng_uniform (r);
      double v = gsl_rng_uniform (r);

      gsl_histogram2d_pdf_sample (p, u, v, &x, &y);

      printf ("%g %g\n", x, y);
    }

    gsl_histogram2d_pdf_free (p);
  }

  gsl_histogram2d_free (h);
  gsl_rng_free (r);

  return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_errno.h>

#include "ccl.h"

//Spline creator
//n     -> number of points
//x     -> x-axis
//y     -> f(x)-axis
//y0,yf -> values of f(x) to use beyond the interpolation range
ccl_f1d_t *ccl_f1d_t_new(int n,double *x,double *y,double y0,double yf)
{
  ccl_f1d_t *spl=malloc(sizeof(ccl_f1d_t));
  if(spl==NULL)
    return NULL;

  spl->spline=gsl_spline_alloc(gsl_interp_cspline,n);
  if (spl->spline == NULL) {
    free(spl);
    return NULL;
  }
  int parstatus=gsl_spline_init(spl->spline,x,y,n);
  if(parstatus) {
    gsl_spline_free(spl->spline);
    free(spl);
    return NULL;
  }

  spl->x0=x[0];
  spl->xf=x[n-1];
  spl->y0=y0;
  spl->yf=yf;

  return spl;
}

//Evaluates spline at x checking for bound errors
double ccl_f1d_t_eval(ccl_f1d_t *spl,double x)
{
  if(x<=spl->x0)
    return spl->y0;
  else if(x>=spl->xf)
    return spl->yf;
  else {
    double y;
    int stat=gsl_spline_eval_e(spl->spline,x,NULL,&y);
    if (stat!=GSL_SUCCESS) {
      ccl_raise_gsl_warning(stat, "ccl_utils.c: ccl_splin_eval():");
      return NAN;
    }
    return y;
  }
}

//Spline destructor
void ccl_f1d_t_free(ccl_f1d_t *spl)
{
  if (spl != NULL) {
    gsl_spline_free(spl->spline);
  }
  free(spl);
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>

#include <fftw3.h> /* Note: when included AFTER complex.h, fftw_complex type defaults to native double complex */

#include "constants.h"
#include "struct.h"

/* Window functions */
double WindowFunction(double x, double xi, double xf, double deltaxi, double deltaxf)
{
  double di =  deltaxi/(20*log(10));
  double df =  deltaxf/(20*log(10));
  if (x <= xi + di) return 0;
  else if (xi +di < x && x < xi + deltaxi - di) {
    return 1./(1 + exp(deltaxi/(x - xi) + deltaxi/(x - (xi + deltaxi))));
  }
  else if (xi + deltaxi - di <= x && x <= xf - deltaxf + df) return 1.;
  else if (xf - deltaxf + df < x && x < xf - df) {
    return 1./(1 + exp(-(deltaxf/(x - (xf - deltaxf))) - deltaxf/(x - xf)));
  }
  else return 0;
}
double WindowFunctionLeft(double x, double xf, double deltaxf)
{
  double df =  deltaxf/(20*log(10));
  if (x <= xf - deltaxf + df) return 1;
  else if (xf - deltaxf + df < x && x < xf - df) {
    return 1./(1 + exp(-deltaxf/(x - (xf - deltaxf)) - deltaxf/(x - xf)));
  }
  else return 0;
}
double WindowFunctionRight(double x, double xi, double deltaxi)
{
  double di =  deltaxi/(20*log(10));
  if (x <= xi + di) return 0;
  else if (xi + di < x && x < xi + deltaxi - di) {
    return 1./(1 + exp(deltaxi/(x - xi) + deltaxi/(x - (xi + deltaxi))));
  }
  else return 1;
}

/* FFT of real time series */
/* Note: FFT uses flipped convention (i.e. h(f) = int e^(+2ipift)h(t)) */
int FFTRealTimeSeries(ReImFrequencySeries** freqseries, RealTimeSeries* timeseries, double twindowbeg, double twindowend, int nzeropad)
{

  /* deltat of time series */
  /* Warning: assumes linear sampling in time */
  double* times = timeseries->times->data;
  double deltat = times[1] - times[0];
  double tshift = times[0]; /* time shift to be re-applied later */

  /* Initialize vector for windowed, 0-padded FFT input */
  int n = (int) timeseries->times->size;
  int nzeros = (int) pow(2, ((int) ceil(log(n)/log(2))) + nzeropad) - n; /* Here defined with ceil, but with floor in IFFT */
  gsl_vector* hvalues = gsl_vector_alloc(n + nzeros);
  gsl_vector_set_zero(hvalues);

  /* Compute input TD values, with windowing */
  int nbptswindowbeg = (int) ceil(twindowbeg/deltat) + 1;
  int nbptswindowend = (int) ceil(twindowend/deltat) + 1;
  double t1windowbeg = times[0];
  double t2windowbeg = times[nbptswindowbeg-1];
  double t1windowend = times[n-nbptswindowend];
  double t2windowend = times[n-1];
  double deltatwindowbeg = t2windowbeg - t1windowbeg;
  double deltatwindowend = t2windowend - t1windowend;
  double* h = timeseries->h->data;
  double* hval = hvalues->data;

  for (int i=0; i<nbptswindowbeg; i++) {
    hval[i] = WindowFunctionRight(times[i], t1windowbeg, deltatwindowbeg) * h[i];
  }
  for (int i=nbptswindowbeg; i<n-nbptswindowend; i++) {
    hval[i] =  h[i];
  }
  for (int i=n-nbptswindowend; i<n; i++) {
    hval[i] = WindowFunctionLeft(times[i], t2windowend, deltatwindowend) * h[i];
  }

  /* FFT - uses flipped convention (i.e. h(f) = int e^(+2ipift)h(t)) */
  int N = (int) hvalues->size;
  fftw_plan p;
  double* in = hvalues->data;
  fftw_complex* out;
  out = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * (N/2+1)); /* Note: N/2+1 elements */
  p = fftw_plan_dft_r2c_1d(N, in, out, FFTW_ESTIMATE);
  fftw_execute(p);

  /* Initialize output structure */
  ReImFrequencySeries_Init(freqseries, N/2); /* Note: N/2+1 elements as output of fftw, we drop the last one (at Nyquist frequency) */

  /* Extracting and converting data from FFTW output */
  double deltaf = 1./(N*deltat);
  double* freq = (*freqseries)->freq->data;
  double* hreal = (*freqseries)->h_real->data;
  double* himag = (*freqseries)->h_imag->data;
  double f;
  double complex hcomplex;
  double factorshift = 2*PI*tshift; /* Reminder: Flipped sign convention */
  for(int i=0; i<N/2; i++) {
    f = i*deltaf;
    freq[i] = f;
    hcomplex = deltat * conj(out[i]) * cexp(I*factorshift*f); /* Note that we convert here the FT sign convention */
    hreal[i] = creal(hcomplex);
    himag[i] = cimag(hcomplex);
  }

  /* Clean up */
  gsl_vector_free(hvalues);
  fftw_destroy_plan(p);
  fftw_free(out);

  return SUCCESS;
}

/* FFT of Re/Im time series */
/* Note: FFT uses flipped convention (i.e. h(f) = int e^(+2ipift)h(t)) */
/* NOTE: only keeps positive frequencies in the output */
int FFTTimeSeries(ReImFrequencySeries** freqseries, ReImTimeSeries* timeseries, double twindowbeg, double twindowend, int nzeropad)
{
  /* deltat of time series */
  /* Warning: assumes linear sampling in time */
  double* times = timeseries->times->data;
  double deltat = times[1] - times[0];
  double tshift = times[0]; /* time shift to be re-applied later */

  /* Initialize vector for windowed, 0-padded FFT input */
  int n = (int) timeseries->times->size;
  int nzeros = (int) pow(2, ((int) ceil(log(n)/log(2))) + nzeropad) - n; /* Here defined with ceil, but with floor in IFFT */
  int N = n + nzeros;

  /* Compute input TD values, with windowing */
  int nbptswindowbeg = (int) ceil(twindowbeg/deltat) + 1;
  int nbptswindowend = (int) ceil(twindowend/deltat) + 1;
  double t1windowbeg = times[0];
  double t2windowbeg = times[nbptswindowbeg-1];
  double t1windowend = times[n-nbptswindowend];
  double t2windowend = times[n-1];
  double deltatwindowbeg = t2windowbeg - t1windowbeg;
  double deltatwindowend = t2windowend - t1windowend;
  double* htdreal = timeseries->h_real->data;
  double* htdimag = timeseries->h_imag->data;

  fftw_complex* in = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N);
  for (int i=0; i<nbptswindowbeg; i++) {
    in[i] = WindowFunctionRight(times[i], t1windowbeg, deltatwindowbeg) * (htdreal[i] + I*htdimag[i]);
  }
  for (int i=nbptswindowbeg; i<n-nbptswindowend; i++) {
    in[i] =  (htdreal[i] + I*htdimag[i]);
  }
  for (int i=n-nbptswindowend; i<n; i++) {
    in[i] = WindowFunctionLeft(times[i], t2windowend, deltatwindowend) * (htdreal[i] + I*htdimag[i]);
  }

  /* FFT - uses flipped convention (i.e. h(f) = int e^(+2ipift)h(t)) */
  /* Represented here by the use of FFTW_BACKWARD (plus sign in the exp) */
  fftw_plan p;
  fftw_complex* out;
  out = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N); /* Note: N/2+1 elements */
  p = fftw_plan_dft_1d(N, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);
  fftw_execute(p);

  /* Initialize output structure */
  ReImFrequencySeries_Init(freqseries, N/2); /* NOTE: N/2 first elements of output of fftw are positive freqs, we eliminate negative frequency (the second half of the series) */

  /* Extracting and converting data from FFTW output */
  double deltaf = 1./(N*deltat);
  double* freq = (*freqseries)->freq->data;
  double* hreal = (*freqseries)->h_real->data;
  double* himag = (*freqseries)->h_imag->data;
  double f;
  double complex hcomplex;
  double factorshift = 2*PI*tshift; /* Reminder: Flipped sign convention */
  for(int i=0; i<N/2; i++) {
    f = i*deltaf;
    freq[i] = f;
    hcomplex = deltat * out[i] * cexp(I*factorshift*f); /* Note the FT sign convention  */
    hreal[i] = creal(hcomplex);
    himag[i] = cimag(hcomplex);
  }

  /* Clean up */
  fftw_destroy_plan(p);
  fftw_free(in);
  fftw_free(out);

  return SUCCESS;
}

/* IFFT of frequency series */
/* Note: assumes frequency series is FT of real data - produces real time series */
/* Note: FFT uses flipped convention (i.e. h(f) = int e^(+2ipift)h(t)) */
int IFFTFrequencySeriesReal(RealTimeSeries** timeseries, ReImFrequencySeries* freqseries, double f1windowbeg, double f2windowbeg, double f1windowend, double f2windowend, int nzeropad)
{
  /* Checking sanity of windowing frequencies */
  if(!((f1windowbeg<f2windowbeg)&&(f1windowend<f2windowend)&&(f2windowbeg<f1windowend))) {
    printf("Error in IFFTFrequencySeriesReal: inconsistent windowing frequencies.\n");
    printf("(f1windowbeg, f2windowbeg, f1windowend, f2windowend) = (%g, %g, %g, %g)\n", f1windowbeg, f2windowbeg, f1windowend, f2windowend);
  }

  /* deltaf of frequency series */
  /* Warning: assumes linear sampling in frequency */
  double* freq = freqseries->freq->data;
  double deltaf = freq[1] - freq[0];

  /* Initialize vector for windowed, 0-padded FFT input */
  int n = (int) freqseries->freq->size;
  while(freq[n-1] > f2windowend) n--;
  int nzeros = (int) pow(2, ((int) ceil(log(n)/log(2))) + nzeropad) - n; /* Here defined with floor, but with ceil in FFT */
  gsl_vector* hrealvalues = gsl_vector_alloc(n + nzeros);
  gsl_vector* himagvalues = gsl_vector_alloc(n + nzeros);
  gsl_vector_set_zero(hrealvalues);
  gsl_vector_set_zero(himagvalues);

  /* Compute input FD values, with windowing */
  double deltafwindowbeg = f2windowbeg - f1windowbeg;
  double deltafwindowend = f2windowend - f1windowend;
  double* hreal = freqseries->h_real->data;
  double* himag = freqseries->h_imag->data;
  double* hrealval = hrealvalues->data;
  double* himagval = himagvalues->data;
  for (int i=0; i<n; i++) {
    double window = WindowFunction(freq[i], f1windowbeg, f2windowend, deltafwindowbeg, deltafwindowend);
    hrealval[i] = window * hreal[i];
    himagval[i] = window * himag[i];
  }

  /* Input as array of fftw_complex */
  int N = (int) hrealvalues->size;
  fftw_complex* in = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N);
  /* NOTE: Restoring the standard sign convention for the FT, used by FFTW - change in convention amounts to f->-f, equivalent to a conjugation only for FFT of a real series  */
  for(int i=0; i<n; i++) {
    in[i] = hrealval[i] - I*himagval[i] ;
  }
  for(int i=n; i<N; i++) {
    in[i] = 0;
  }

  /* FFT - uses flipped convention (i.e. h(f) = int e^(+2ipift)h(t)) */
  fftw_plan p;
  double* out = fftw_malloc(sizeof(double) * N);
  p = fftw_plan_dft_c2r_1d(N, in, out, FFTW_ESTIMATE);
  fftw_execute(p);

  /* Initialize output structure */
  RealTimeSeries_Init(timeseries, N);

  /* Extracting and converting data from FFTW output - moving negative times to the left */
  double deltat = 1./(N*deltaf);
  double fac = 1./(N*deltat); /* Additional 1/N to put the FFTW convention in agreement with numpy IFFT */
  double* times = (*timeseries)->times->data;
  double* h = (*timeseries)->h->data;
  for(int i=0; i<N/2; i++) {
    times[i] = (i-N/2)*deltat;
    times[N/2+i] = i*deltat;
    h[i] = fac * out[N/2+i];
    h[N/2+i] = fac * out[i];
  }

  /* Clean up */
  gsl_vector_free(hrealvalues);
  gsl_vector_free(himagvalues);
  fftw_destroy_plan(p);
  fftw_free(in);
  fftw_free(out);

  return SUCCESS;
}

/* IFFT of frequency series */
/* Note: does not assume frequency series is FT of real data - produces complex time series */
/* Note: FFT uses flipped convention (i.e. h(f) = int e^(+2ipift)h(t)) */
int IFFTFrequencySeries(ReImTimeSeries** timeseries, ReImFrequencySeries* freqseries, double f1windowbeg, double f2windowbeg, double f1windowend, double f2windowend, int nzeropad)
{
  /* Checking sanity of windowing frequencies */
  if(!((f1windowbeg<f2windowbeg)&&(f1windowend<f2windowend)&&(f2windowbeg<f1windowend))) {
    printf("Error in IFFTFrequencySeries: inconsistent windowing frequencies.\n");
    printf("(f1windowbeg, f2windowbeg, f1windowend, f2windowend) = (%g, %g, %g, %g)\n", f1windowbeg, f2windowbeg, f1windowend, f2windowend);
  }

  /* deltaf of frequency series */
  /* Warning: assumes linear sampling in frequency */
  double* freq = freqseries->freq->data;
  double deltaf = freq[1] - freq[0];

  /* Initialize vector for windowed, 0-padded FFT input */
  int n = (int) freqseries->freq->size;
  while(freq[n-1] > f2windowend) n--;
  int nzeros = (int) pow(2, ((int) ceil(log(n)/log(2))) + nzeropad) - n; /* Here defined with floor, but with ceil in FFT */
  gsl_vector* hrealvalues = gsl_vector_alloc(n + nzeros);
  gsl_vector* himagvalues = gsl_vector_alloc(n + nzeros);
  gsl_vector_set_zero(hrealvalues);
  gsl_vector_set_zero(himagvalues);

  /* Compute input FD values, with windowing */
  double deltafwindowbeg = f2windowbeg - f1windowbeg;
  double deltafwindowend = f2windowend - f1windowend;
  double* hreal = freqseries->h_real->data;
  double* himag = freqseries->h_imag->data;
  double* hrealval = hrealvalues->data;
  double* himagval = himagvalues->data;
  for (int i=0; i<n; i++) {
    double window = WindowFunction(freq[i], f1windowbeg, f2windowend, deltafwindowbeg, deltafwindowend);
    hrealval[i] = window * hreal[i];
    himagval[i] = window * himag[i];
  }

  /* Input as array of fftw_complex */
  int N = (int) hrealvalues->size;
  fftw_complex* in = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N);
  /* NOTE: the sign convention for the FT used by FFTW is different - change in convention amounts to f->-f, equivalent to a conjugation only for FFT of a real series - for a FFT of a complex series, we do not conjugate and we keep our convention */
  for(int i=0; i<n; i++) {
    in[i] = hrealval[i] + I*himagval[i] ;
  }
  for(int i=n; i<N; i++) {
    in[i] = 0;
  }

  /* FFT - FFTW uses flipped convention (i.e. h(f) = int e^(+2ipift)h(t)) - our convention is h(f) = int e^(-2ipift)h(t) */
  /* NOTE: due to the difference in convention for the FT, we use the FFTW_FORWARD sign (sign - in the exponential) */
  fftw_plan p;
  double complex* out = fftw_malloc(sizeof(fftw_complex) * N);
  p = fftw_plan_dft_1d(N, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_execute(p);

  /* Initialize output structure */
  ReImTimeSeries_Init(timeseries, N);

  /* Extracting and converting data from FFTW output - moving negative times to the left */
  double deltat = 1./(N*deltaf);
  double fac = 1./(N*deltat); /* Additional 1/N to put the FFTW convention in agreement with numpy IFFT */
  double* times = (*timeseries)->times->data;
  double* htdreal = (*timeseries)->h_real->data;
  double* htdimag = (*timeseries)->h_imag->data;
  for(int i=0; i<N/2; i++) {
    times[i] = (i-N/2)*deltat;
    times[N/2+i] = i*deltat;
    htdreal[i] = fac * creal(out[N/2+i]);
    htdimag[i] = fac * cimag(out[N/2+i]);
    htdreal[N/2+i] = fac * creal(out[i]);
    htdimag[N/2+i] = fac * cimag(out[i]);
  }

  /* Clean up */
  gsl_vector_free(hrealvalues);
  gsl_vector_free(himagvalues);
  fftw_destroy_plan(p);
  fftw_free(in);
  fftw_free(out);

  return SUCCESS;
}


#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_plain.h>
#include <gsl/gsl_monte_miser.h>
#include <gsl/gsl_monte_vegas.h>

double calculateIntegralValue(int sampleCount)
{
    int correctSamples = 0;

    srand(time(NULL));
    FILE* file = fopen("sqrt.txt", "w");

    for (int i = 0; i < sampleCount; ++i){
        double x = (double)rand() / RAND_MAX; // double [0, 1]
        double y = (double)rand() / RAND_MAX; // double [0, 1000]

        if (y <= 1.0/sqrt(x)){
            fprintf (file,"%g %g\n", x, y); //Save correct samples to file
            correctSamples++;
        }
    }
    return (double)correctSamples / (double)sampleCount; // (b-a) is (1.0-0.0) so we can ignore this multiply
}

void calculateIntegralError(int maxSamples)
{
    srand(time(NULL));
    FILE* file = fopen("sqrt_err.txt", "w");

    for (int sampleCount = 1; sampleCount < maxSamples; ++sampleCount){
        int correctSamples = 0;
        for (int i = 0; i < sampleCount; ++i){
            double x = (double)rand() / RAND_MAX; // double [0, 1]
            double y = (double)rand() / RAND_MAX; // double [0, 1]

            if (y <= x * x){
                correctSamples++;
            }
        }

        double sum = (double)correctSamples / (double)sampleCount; // (b-a) is (1.0-0.0) so we can ignore this multiply
        double sumError = sum - 1.0/3.0;
        fprintf(file, "%d %g\n", sampleCount, sqrt(sumError * sumError));
    }
}

void monteCarloPLAIN()
{
    double res, err;
    double xl[3] = {0,0,0};
    double xu[3] = {M_PI, M_PI, M_PI};

    const gsl_rng_type *T;
    gsl_rng *r;

    gsl_monte_function G;

}

int main(void)
{
    // printf("Sum: %g\n", sum);

    return 0;
}


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

/* L D L^T decomposition of a symmetric positive semi-definite matrix */

#include <config.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

static double ldlt_norm1(const gsl_matrix * A);
static int ldlt_Ainv(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params);

/*
gsl_linalg_ldlt_decomp()
  Perform L D L^T decomposition of a symmetric positive
semi-definite matrix using lower triangle

Inputs: A - (input) symmetric, positive semi-definite matrix
            (output) lower triangle contains L factor;
                     diagonal contains D

Return: success/error

Notes:
1) Based on algorithm 4.1.1 of Golub and Van Loan, Matrix Computations (4th ed).
2) The first subrow A(1, 2:end) is used as temporary workspace
3) The 1-norm ||A||_1 of the original matrix is stored in the upper right corner on output
*/

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

  if (M != N)
    {
      GSL_ERROR ("LDLT decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      size_t i, j;
      double a00, anorm;
      gsl_vector_view work, v;

      /* check for quick return */
      if (N == 1)
        return GSL_SUCCESS;

      /* compute ||A||_1 */
      anorm = ldlt_norm1(A);

      /* special case first column */
      a00 = gsl_matrix_get(A, 0, 0);
      if (a00 == 0.0)
        {
          GSL_ERROR ("matrix is singular", GSL_EDOM);
        }

      v = gsl_matrix_subcolumn(A, 0, 1, N - 1);
      gsl_vector_scale(&v.vector, 1.0 / a00);

      /* use first subrow A(1, 2:end) as temporary workspace */
      work = gsl_matrix_subrow(A, 0, 1, N - 1);

      for (j = 1; j < N; ++j)
        {
          gsl_vector_view w = gsl_vector_subvector(&work.vector, 0, j);
          double ajj = gsl_matrix_get(A, j, j);
          double dval;

          for (i = 0; i < j; ++i)
            {
              double aii = gsl_matrix_get(A, i, i);
              double aji = gsl_matrix_get(A, j, i);
              gsl_vector_set(&w.vector, i, aji * aii);
            }

          v = gsl_matrix_subrow(A, j, 0, j); /* A(j,1:j-1) */
          gsl_blas_ddot(&v.vector, &w.vector, &dval);
          ajj -= dval;

          if (ajj == 0.0)
            {
              GSL_ERROR ("matrix is singular", GSL_EDOM);
            }

          gsl_matrix_set(A, j, j, ajj);

          if (j < N - 1)
            {
              double ajjinv = 1.0 / ajj;
              gsl_matrix_view m = gsl_matrix_submatrix(A, j + 1, 0, N - j - 1, j); /* A(j+1:n, 1:j-1) */
              v = gsl_matrix_subcolumn(A, j, j + 1, N - j - 1);                    /* A(j+1:n, j) */
              gsl_blas_dgemv(CblasNoTrans, -ajjinv, &m.matrix, &w.vector, ajjinv, &v.vector);
            }
        }

      /* save ||A||_1 in upper right corner */
      gsl_matrix_set(A, 0, N - 1, anorm);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_ldlt_solve (const gsl_matrix * LDLT,
                       const gsl_vector * b,
                       gsl_vector * x)
{
  if (LDLT->size1 != LDLT->size2)
    {
      GSL_ERROR ("LDLT matrix must be square", GSL_ENOTSQR);
    }
  else if (LDLT->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LDLT->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      int status;

      /* copy x <- b */
      gsl_vector_memcpy (x, b);

      status = gsl_linalg_ldlt_svx(LDLT, x);

      return status;
    }
}

int
gsl_linalg_ldlt_svx (const gsl_matrix * LDLT,
                     gsl_vector * x)
{
  if (LDLT->size1 != LDLT->size2)
    {
      GSL_ERROR ("LDLT matrix must be square", GSL_ENOTSQR);
    }
  else if (LDLT->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      gsl_vector_const_view diag = gsl_matrix_const_diagonal(LDLT);

      /* solve for z using forward-substitution, L z = b */
      gsl_blas_dtrsv (CblasLower, CblasNoTrans, CblasUnit, LDLT, x);

      /* solve for y, D y = z */
      gsl_vector_div(x, &diag.vector);

      /* perform back-substitution, L^T x = y */
      gsl_blas_dtrsv (CblasLower, CblasTrans, CblasUnit, LDLT, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_ldlt_rcond (const gsl_matrix * LDLT, double * rcond,
                       gsl_vector * work)
{
  const size_t M = LDLT->size1;
  const size_t N = LDLT->size2;

  if (M != N)
    {
      GSL_ERROR ("LDLT matrix must be square", GSL_ENOTSQR);
    }
  else if (work->size != 3 * N)
    {
      GSL_ERROR ("work vector must have length 3*N", GSL_EBADLEN);
    }
  else
    {
      int status;
      double Anorm;    /* ||A||_1 */
      double Ainvnorm; /* ||A^{-1}||_1 */

      if (N == 1)
        Anorm = fabs(gsl_matrix_get(LDLT, 0, 0));
      else
        Anorm = gsl_matrix_get(LDLT, 0, N - 1);

      *rcond = 0.0;

      /* don't continue if matrix is singular */
      if (Anorm == 0.0)
        return GSL_SUCCESS;

      /* estimate ||A^{-1}||_1 */
      status = gsl_linalg_invnorm1(N, ldlt_Ainv, (void *) LDLT, &Ainvnorm, work);

      if (status)
        return status;

      if (Ainvnorm != 0.0)
        *rcond = (1.0 / Anorm) / Ainvnorm;

      return GSL_SUCCESS;
    }
}

/* compute 1-norm of symmetric matrix stored in lower triangle */
static double
ldlt_norm1(const gsl_matrix * A)
{
  const size_t N = A->size1;
  double max = 0.0;
  size_t i, j;

  for (j = 0; j < N; ++j)
    {
      gsl_vector_const_view v = gsl_matrix_const_subcolumn(A, j, j, N - j);
      double sum = gsl_blas_dasum(&v.vector);

      /* now add symmetric elements from above diagonal */
      for (i = 0; i < j; ++i)
        {
          double Aij = gsl_matrix_get(A, i, j);
          sum += fabs(Aij);
        }

      if (sum > max)
        max = sum;
    }

  return max;
}

/* x := A^{-1} x = A^{-t} x, A = L D L^T */
static int
ldlt_Ainv(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params)
{
  int status;
  gsl_matrix * A = (gsl_matrix * ) params;
  gsl_vector_const_view diag = gsl_matrix_const_diagonal(A);

  (void) TransA; /* unused parameter warning */

  /* compute L^{-1} x */
  status = gsl_blas_dtrsv(CblasLower, CblasNoTrans, CblasUnit, A, x);
  if (status)
    return status;

  /* compute D^{-1} x */
  gsl_vector_div(x, &diag.vector);

  /* compute L^{-t} x */
  status = gsl_blas_dtrsv(CblasLower, CblasTrans, CblasUnit, A, x);
  if (status)
    return status;

  return GSL_SUCCESS;
}