/* ============================================================ *
 * 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;
}


/**
 *
 * @file example_dpotrf.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @brief Example of Cholesky factorization
 *
 * @version 2.6.0
 * @author Bilel Hadri
 * @date 2010-11-15
 * @generated d Tue Jan  7 11:45:20 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <time.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>
#include "testing_dmain.h"

static void GENMAT_SYM_FULL(int m, double *A)
{
	srand48(time(NULL));

	int j;
	for (j = 0; j < m; ++j ) {
		int i;
		for( i = j; i < m; ++i ) {
			double dran = drand48();
			A[j*m+i] = A[i*m+j] = dran;
		}
  	}
	for(j = 0; j < m; ++j)
		A[j*m+j] += 10 * m;
}

int testing_dspdsolv(int argc, char **argv)
{

    int N     = 4000;
	int LDA   = 4000;
    int info_factorization;

    double *A   = (double *)malloc(LDA*N*sizeof(double));
	double *X	 = (double *)malloc(LDA*N*sizeof(double));
	double *B	 = (double *)malloc(LDA*N*sizeof(double));

    /* Check if unable to allocate memory */
    if ((!A)||(!X)||(!B)){
        fprintf(stderr, "Out of Memory \n ");
        return 0;
    }

	PLASMA_Set( PLASMA_RUNTIME_MODE, PLASMA_OMPSS); //Toggle runtime mdoe to OmpSs
	PLASMA_Set( PLASMA_TILE_SIZE, 384); //Set up tile size

	GENMAT_SYM_FULL(N, A); //Initialize A with a SPD matrix

	LAPACKE_dlarnv(IONE, ISEED, LDA*N, X);

	cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, N, N, N,
			1.0, A, LDA, X, LDA, 0.0, B, LDA);

    PLASMA_dpotrf(PlasmaLower, N, A, LDA);
	PLASMA_dtrsm(PlasmaLeft, PlasmaLower, PlasmaNoTrans, PlasmaNonUnit, N, N, 1.0, A, LDA, B, LDA);
	PLASMA_dtrsm(PlasmaLeft, PlasmaLower, PlasmaTrans, PlasmaNonUnit, N, N, 1.0, A, LDA, B, LDA);

	char infnorm = 'F';
	double Normb = LAPACKE_dlange(LAPACK_COL_MAJOR, infnorm, N, N, B, LDA);
	double Normx = LAPACKE_dlange(LAPACK_COL_MAJOR, infnorm, N, N, X, LDA);
	printf("Residual= %e, norm(b): %e, norm(x): %e\n", fabs(Normb-Normx)/Normx, Normb, Normx);

    free(A); free(X); free(B);

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


/* filter/test_median.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.
 */

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_filter.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_statistics.h>

/* compute filtered data by explicitely constructing window, sorting it and finding median */
int
slow_movmedian(const gsl_filter_end_t etype, const gsl_vector * x, gsl_vector * y, const int K)
{
  const size_t H = K / 2;
  const size_t n = x->size;
  double *window = malloc(K * sizeof(double));
  size_t i;

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(etype, x, i, H, H, window);
      double yi = gsl_stats_median(window, 1, wsize);

      gsl_vector_set(y, i, yi);
    }

  free(window);

  return GSL_SUCCESS;
}

static void
test_median_proc(const double tol, const size_t n, const size_t K,
                 const gsl_filter_end_t etype, gsl_rng *rng_p)
{
  gsl_filter_median_workspace *w = gsl_filter_median_alloc(K);
  gsl_vector *x = gsl_vector_alloc(n);
  gsl_vector *y = gsl_vector_alloc(n);
  gsl_vector *z = gsl_vector_alloc(n);
  char buf[2048];

  /* test moving median with random input */
  random_vector(x, rng_p);

  /* y = median(x) with slow brute force algorithm */
  slow_movmedian(etype, x, y, K);

  /* z = median(x) */
  gsl_filter_median(etype, x, z, w);

  /* test y = z */
  sprintf(buf, "n=%zu K=%zu endtype=%u median random", n, K, etype);
  compare_vectors(tol, z, y, buf);

  /* z = median(x) in-place */
  gsl_vector_memcpy(z, x);
  gsl_filter_median(etype, z, z, w);

  sprintf(buf, "n=%zu K=%zu endtype=%u median random in-place", n, K, etype);
  compare_vectors(tol, z, y, buf);

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(z);
  gsl_filter_median_free(w);
}

static void
test_median(gsl_rng * rng_p)
{
  test_median_proc(GSL_DBL_EPSILON, 1000, 1, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 1000, 3, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 1000, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 100, 301, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 17, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 9, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 901, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 201, GSL_MOVSTAT_END_PADZERO, rng_p);

  test_median_proc(GSL_DBL_EPSILON, 1000, 1, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 1000, 3, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 1000, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 100, 301, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 17, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 9, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 901, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 201, GSL_MOVSTAT_END_PADVALUE, rng_p);

  test_median_proc(GSL_DBL_EPSILON, 1000, 1, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 1000, 3, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 1000, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 100, 301, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 17, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 9, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 901, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 201, GSL_MOVSTAT_END_TRUNCATE, rng_p);
}


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


/** \file filters.h
 * A collection of filters and FFT related routines.
 */
#ifndef __FILTERS_H__
#define __FILTERS_H__
#include <fftw3.h>
#include <gsl/gsl_wavelet.h>
#include "common.h"

#define FFTW_NTHREADS_DEFAULT 4
#define FFTW_FLAGS_DEFAULT FFTW_ESTIMATE

typedef struct filters_handle
{
    /* public for i/o */
    ANALYSIS_WAVEFORM_BASE_TYPE *inWav;
    ANALYSIS_WAVEFORM_BASE_TYPE *outWav;
    ANALYSIS_WAVEFORM_BASE_TYPE *respWav;
    WAVELET_BASE_TYPE *waveletWav;

    /* private for internal work */
    size_t wavLen, respLen;
    int malloced;

    /* fft */
    int fftUsed;
    size_t fftLen;
    size_t fftwNThreads;
    unsigned fftwFlags;
    FFTW(plan) fftwPlan, fftwPlan1, fftwPlan2;
    FFT_BASE_TYPE *fftwWork, *fftwWork1;
    /* window function */
    FFT_BASE_TYPE *fftwWin;
    double fftwS1, fftwS2, dt;

    /* wavelet */
    gsl_wavelet *gslDWT;
    gsl_wavelet_workspace *gslDWTWork;
} filters_t;

/** Initialize the filter.
 * @param[in] inWav waveform input.
 * If inWav == NULL, a space is malloced and freed upon closure.
 * If inWav is given, it is not freed.
 * @param[in] n waveform length.
 * The output is also of length n.  n can be odd in principle,
 * but an even number is preferred.
 */
filters_t *filters_init(ANALYSIS_WAVEFORM_BASE_TYPE *inWav, size_t n);
/** Initialize for convolution and fft
 * @param[in] np length of response function.  Must be an odd number.
 * np=0 is reserved for fft spectrum calculations.
 */
filters_t *filters_init_for_convolution(ANALYSIS_WAVEFORM_BASE_TYPE *inWav, size_t n, size_t np);
/** Destroy the filter */
int filters_close(filters_t *fHdl);
/** Savitzky-Golay filter.
 * @param[in] m order of polynomial.
 * @param[in] np number of points (response length)
 * @param[in] ld degree of derivative
 */
int filters_SavitzkyGolay(filters_t *fHdl, int m, int ld);
/** Raised cosine as convolution kernel.
 * Outermost bin, i=(fHdl->respLen-1)/2, is set to 0.0
 **/
int filters_raisedCosine(filters_t *fHdl);
int filters_convolute(filters_t *fHdl);
int filters_hanning_window(filters_t *fHdl);
/** Compute the spectrum in Fourier space.  Requires init_for_convolution with np = 0.
 * Computed power spectrum is stored in fftwWork(s), normalized.
 * This function handles both odd and even number of input points.
 * The resulting power spectrum has the length (int)(n/2)+1 for both even and odd n.
 * Spectra density (linearized) is stored in fftwWork,
 * Spectrum (linearized) is stored in fftwWork1.
 */
int filters_fft_spectrum(filters_t *fHdl);
/** Discrete wavelet transform. */
int filters_DWT(filters_t *fHdl);
/** Median filter with moving window size n. */
int filters_median(filters_t *fHdl, size_t n);
/** Trapezoidal filter as in Knoll NIMA 345(1994) 337-345.
 * @param[in] k rise time.
 * @param[in] l delay of peak.
 * l-k is the flat-top duration.
 * @param[in] M decay time constant (in number of samples) of the input
 * pulse.  Set M=-1.0 to deal with a step-like input function.
 */
int filters_trapezoidal(filters_t *fHdl, size_t k, size_t l, double M);

#endif // __FILTERS_H__


// @Copyright 2007-2017 Kristjan Haule
#include <iostream>
#include <fstream>
#include <vector>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include "logging.h"
#include "smesh.h"
#include "sfunction.h"
#include "tanmesh.h"
#include "romberg.h"
#include <time.h>

#ifdef _MPI
#include <mpi.h>
#endif

using namespace std;

inline double fermi_kernel(double t, double w, double beta)
{
  double x = beta*w/2;
  double y = 2.*t/beta-1.;
  if (x>100.) return exp(-x*(y+1.));
  if (x<-100) return exp(x*(1.-y));
  return exp(-x*y)/(2*cosh(x));
}
inline double bose_kernel(double t, double w, double beta)
{
  double x = beta*w/2;
  double y = 2.*t/beta-1.;
  if (x>200.) return w*exp(-x*(y+1.));
  if (x<-200.) return -w*exp(x*(1.-y));
  return w*exp(-x*y)/(2*sinh(x));
}
inline void create_log_mesh(function1D<int>& ind_om, int nom_all, int nom, int ntail_)
{
  //Creates logarithmic mesh on Matsubara axis
  //     Takes first istart points from mesh om and the rest of om mesh is replaced by ntail poinst redistribued logarithmically.
  //     Input:
  //         om      -- original long mesh
  //         nom     -- number of points not in the tail
  //         ntail   -- tail replaced by ntail points only
  //     Output:
  //         ind_om  -- index array which conatins index to kept Matsubara points
  int istart = min(nom, nom_all);
  int ntail = min(ntail_, nom_all-istart);
  ind_om.resize(istart+ntail);
  double alpha = log((nom_all-1.)/istart)/(ntail-1.);
  for (int i=0; i< istart; i++) ind_om[i]=i;
  int ii=istart;
  for (int i=0; i < ntail; i++){
    int t = int(istart*exp(alpha*i)+0.5);
    if (t != ind_om[ii-1])
      ind_om[ii++] = t;
  }
  ind_om.resize(ii);
}

class SVDFunc{
public:
  vector<spline1D<double> > fU, fU_bose; // SVD functions in imaginary time
  mesh1D tau;                             // imaginary time optimized mesh
  int lmax, lmax_bose;                   // cutoff for the singular values
  int k_ovlp;                             // 2^{k_ovlp}+1 is the number of integration points in romberg method
  mesh1D om;                              // real axis mesh for SVD
  function1D<double> S, S_bose;          // eigenvalues of SVD decomposition
  function2D<double> Vt, Vt_bose;        // real axis basis functions
  function1D<double> dxi;                 // parts of fU splines stored in more efficient way for fast interpolation
  function3D<double> ff2;                 // fU splines stored in different way for fast interpolation
  double beta, L, x0;                     // parameters of the mesh should be remembered for reproducibility
  //
  SVDFunc() : lmax(0)  {  };
  //
  bool operator()() //  was it ever initialized?
  { return lmax>0; }
  double operator()(int l, int i){
    if (l<0) l=lmax-l;
    if (i<0) i=tau.size()+i;
    return fU[l][i];
  }
  double operator()(int l, int i) const{
    if (l<0) l=lmax-l;
    if (i<0) i=tau.size()+i;
    return fU[l][i];
  }
  spline1D<double>& operator[](int l){
    return fU[l];
  }

  inline vector<int> get_lll(int ii)
  { // conversion from a single combined index to the three l indices.
    //int ii = l0 + lmax*l1 + lmax*lmax*lb;
    int lb_ = ii / (lmax*lmax);
    int ir = ii % (lmax*lmax);
    int l1_ = ir / lmax;
    int l0_ = ir % lmax;
    int tmp[] = { l0_, l1_, lb_ };
    return vector<int>(tmp, tmp+3 );
  }
  void _cmp_(double beta, int& lmax, vector<spline1D<double> >& fU, function1D<double>& S, function2D<double>& Vt, function2D<double>& K, ostream& clog){
    clock_t t0 = clock();
    int Nl = min(om.size(),tau.size());
    S.resize(Nl);
    Vt.resize(om.size(),Nl);
    function2D<double> U(Nl,tau.size());

    int lwork;
    {
      int n = min(tau.size(),om.size());
      int mx = max(tau.size(),om.size());
      lwork =  4*n*n + 8*max(mx, n*(n+1));
    }
    function1D<double> work(lwork);
    function1D<int> iwork(8*min(om.size(),tau.size()));
    int info=0;
    info = xgesdd(true, tau.size(), om.size(), K.MemPt(), K.fullsize_Nd(), S.MemPt(), U.MemPt(), U.fullsize_Nd(), Vt.MemPt(), Vt.fullsize_Nd(),work.MemPt(), lwork, work.MemPt(), iwork.MemPt());
    if (info!=0){
      clog << "svd is returning "<<info<<" in svdfunc"<<endl;
    }
    double dt = static_cast<double>( clock() - t0 )/CLOCKS_PER_SEC;
    clog<<"svd time="<<dt<<endl;

    for (int l=0; l<lmax; l++)
      if (fabs(S[l])<1e-13){
	lmax=l;
	clog<<"lmax reduced to "<<lmax<<endl;
	break;
      }
    clog<<"last singular value="<<S[lmax-1]<<endl;

    for (int i=0; i<tau.size(); i++)
      for (int l=0; l<Nl; l++)
	U(l,i) *= 1./sqrt(tau.Dh(i));

    // Splines functions, which are singular values of the Kernel.
    vector<spline1D<double> > fu(lmax);
    for (int l=0; l<lmax; l++){
      fu[l].resize(tau.size());
      for (int i=0; i<tau.size(); i++) fu[l][i] = U(l,i);
      int n = tau.size();
      double x1 = 0.5*(tau[1]+tau[0]);
      double df1 = (U(l,1)-U(l,0))/(tau[1]-tau[0]);
      double x2 = 0.5*(tau[2]+tau[1]);
      double df2 = (U(l,2)-U(l,1))/(tau[2]-tau[1]);
      double df0 = df1 + (df2-df1)*(0.0-x1)/(x2-x1);
      x1 = 0.5*(tau[n-1]+tau[n-2]);
      df1 = (U(l,n-1)-U(l,n-2))/(tau[n-1]-tau[n-2]);
      x2 = 0.5*(tau[n-2]+tau[n-3]);
      df2 = (U(l,n-2)-U(l,n-3))/(tau[n-2]-tau[n-3]);
      double dfn = df1 + (df2-df1)*(beta-x1)/(x2-x1);
      fu[l].splineIt(tau, df0, dfn);
    }

    // Calculates overlap between spline interpolations
    function2D<double> overlap(lmax,lmax);
    cmpOverlap(overlap, fu, tau, beta, k_ovlp);
    // Calculates eigensystem of the overlap
    function1D<double> oE(lmax);
    SymEigensystem(overlap, oE);
    function2D<double> sou(overlap);// sou = 1/sqrt(overlap)
    // Computes  1/sqrt(overlap)
    for (int i=0; i<lmax; i++){
      for (int j=0; j<lmax; j++){
	double dsum =0.0;
	for (int k=0; k<lmax; k++) dsum += overlap(k,i)*1/sqrt(oE[k])*overlap(k,j);
	sou(i,j) = dsum;
      }
    }
    // Prepares new spline functions, which are more precisely orthonormal.
    fU.resize(lmax);
    for (int l=0; l<lmax; l++){
      fU[l].resize(tau.size());
      for (int i=0; i<tau.size(); i++){
	double ul=0;
	for (int lp=0; lp<lmax; lp++)  ul += sou(l,lp)*fu[lp][i];
	fU[l][i] = ul;
      }
      int n = tau.size();
      double x1 = 0.5*(tau[1]+tau[0]);
      double df1 = (fU[l][1]-fU[l][0])/(tau[1]-tau[0]);
      double x2 = 0.5*(tau[2]+tau[1]);
      double df2 = (fU[l][2]-fU[l][1])/(tau[2]-tau[1]);
      double df0 = df1 + (df2-df1)*(0.0-x1)/(x2-x1);
      x1 = 0.5*(tau[n-1]+tau[n-2]);
      df1 = (fU[l][n-1]-fU[l][n-2])/(tau[n-1]-tau[n-2]);
      x2 = 0.5*(tau[n-2]+tau[n-3]);
      df2 = (fU[l][n-2]-fU[l][n-3])/(tau[n-2]-tau[n-3]);
      double dfn = df1 + (df2-df1)*(beta-x1)/(x2-x1);
      fU[l].splineIt(tau, df0, dfn);
    }
  }
  //
  void cmp(const string& statistics, double beta_, int lmax_, int Ntau, double x0_, double L_, double Nw, ostream& clog, int k_ovlp_=5){
    k_ovlp=k_ovlp_;
    beta = beta_;
    L = L_;
    x0 = x0_;

    GiveTanMesh(om, x0, L, Nw);
    GiveDoubleExpMesh(tau, beta, Ntau);

    if (statistics=="fermi" || statistics=="both"){
      lmax = lmax_;
      clock_t t0 = clock();
      function2D<double> K(om.size(),tau.size());
      for (int j=0; j<om.size(); j++){
	for (int i=0; i<tau.size(); i++){
	  K(j,i) = fermi_kernel(tau[i],om[j],beta);
	  K(j,i) *= om.Dh(j)*sqrt(tau.Dh(i));
	}
      }
      double dt = static_cast<double>( clock() - t0 )/CLOCKS_PER_SEC;
      clog<<"setup time="<<dt<<endl;

      _cmp_(beta, lmax, fU, S, Vt, K, clog);
    }

    if (statistics=="bose" || statistics=="both"){
      lmax_bose = lmax_;
      clock_t t0 = clock();
      function2D<double> K(om.size(),tau.size());
      for (int j=0; j<om.size(); j++){
	for (int i=0; i<tau.size(); i++){
	  K(j,i) = bose_kernel(tau[i],om[j],beta);
	  K(j,i) *= om.Dh(j)*sqrt(tau.Dh(i));
	}
      }
      double dt = static_cast<double>( clock() - t0 )/CLOCKS_PER_SEC;
      clog<<"setup time="<<dt<<endl;

      _cmp_(beta, lmax_bose, fU_bose, S_bose, Vt_bose, K, clog);
      //cout<<"S_bose="<<S_bose<<endl;
    }
  }

  void Print(const string& filename, const string& statistics="fermi"){
    ofstream pul(filename.c_str());
    pul<<"# "<<lmax<<" "<<tau.size()<<" "<<L<<" "<<x0<<"  "<<(om.size()/2)<<endl;
    pul.precision(16);
    for (int i=0; i<tau.size(); i++){
      pul<<tau[i]<<"  ";
      if (statistics=="fermi")
	for (int l=0; l<lmax; l++) pul<<fU[l][i]<<"  ";
      else
	for (int l=0; l<lmax_bose; l++) pul<<fU_bose[l][i]<<"  ";
      pul<<endl;
    }
  }

  double CheckOverlap(/*double beta*/){
    // Now we check how well is the orthogonality obeyed after reorthogonalization
    function2D<double> overlap(lmax,lmax);
    cmpOverlap(overlap, fU, tau, beta, k_ovlp);
    double dsum=0;
    for (int l1=0; l1<lmax; l1++){
      for (int l2=0; l2<lmax; l2++){
	if (l1!=l2 && overlap(l1,l2)!=0) dsum += fabs(overlap(l1,l2));
	if (l1==l2) dsum += fabs(overlap(l1,l2)-1.0);
      }
    }
    return dsum;
  }

  void cmpOverlap(function2D<double>& overlap, const vector<spline1D<double> >& fu, const mesh1D& tau, double beta, int k=5){
    int lmax = fu.size();

    int M = static_cast<int>(pow(2.0,k))+1;    // Number of points in Romberg integration routine
    vector<double> utu(M); // storage for Romberg

    overlap.resize(lmax,lmax);
    overlap=0.0;
    int nt = tau.size();
    for (int l1=0; l1<lmax; l1++){
      double odd_1 = fu[l1][0]*fu[l1][nt-1];
      for (int l2=0; l2<=l1; l2++){
	double odd_2 = fu[l2][0]*fu[l2][nt-1];
	if (odd_1*odd_2 > 0){// both are even or odd
	  double oij=0; // overlap between u_{l1} and u_{l2} functions
	  for (int i=0; i<tau.size()-1; i++){
	    // here we integrate with a fine mesh of M points using romberg routine
	    double a = tau[i];  // integral only between t_i and t_{i+1} points
	    double b = tau[i+1];
	    double fa = fu[l1][i]   * fu[l2][i];
	    double fb = fu[l1][i+1] * fu[l2][i+1];
	    utu[0]   = fa;
	    utu[M-1] = fb;
	    tint ia=0;
	    for (int j=1; j<M-1; j++){
	      intpar p = tau.InterpLeft( a + (b-a)*j/(M-1.0), ia);
	      utu[j] = fu[l1](p) * fu[l2](p);
	    }
	    oij += romberg(utu, b-a);
	  }
	  overlap(l1,l2) = oij;
	  overlap(l2,l1) = oij;
	}
      }
    }
  }

  template <class container>
  void CreateGfromCoeff(container& gf, const functionb<double>& gl, const string& statistics="fermi"){

    vector<spline1D<double> >* pfU = (statistics=="fermi") ? &fU : &fU_bose;

    gf.resize(tau.size());
    for (int i=0; i<tau.size(); i++) gf[i] = 0.0;

    for (int l=0; l<lmax; l++)
      for (int i=0; i<tau.size(); i++)
	gf[i] += gl[l]*(*pfU)[l][i];
  }

  void CreateSplineFromCoeff(spline1D<double>& gf, const functionb<double>& gl, const string& statistics="fermi"){
    CreateGfromCoeff(gf,gl, statistics);
    {
      double x1 = 0.5*(tau[1]+tau[0]);
      double df1 = (gf[1]-gf[0])/(tau[1]-tau[0]);
      double x2 = 0.5*(tau[2]+tau[1]);
      double df2 = (gf[2]-gf[1])/(tau[2]-tau[1]);
      double df0 = df1 + (df2-df1)*(0.0-x1)/(x2-x1);
      int n = tau.size();
      x1 = 0.5*(tau[n-1]+tau[n-2]);
      df1 = (gf[n-1]-gf[n-2])/(tau[n-1]-tau[n-2]);
      x2 = 0.5*(tau[n-2]+tau[n-3]);
      df2 = (gf[n-2]-gf[n-3])/(tau[n-2]-tau[n-3]);
      double dfn = df1 + (df2-df1)*(tau[n-1]-x1)/(x2-x1);
      gf.splineIt(tau, df0, dfn);
    }
  }

  template <class container>
  void MatsubaraFrequency(container& giom, const spline1D<double>& gf, int nmax, const string& statistics="fermi"){
    giom.resize(nmax);
    //double beta=tau[tau.size()-1];
    double one = (statistics=="fermi") ? 1.0 : 0.0;
    for (int in=0; in<nmax; in++){
      double iom = (2*in+one)*M_PI/beta;
      giom[in] = gf.Fourier_(iom, tau);
    }
  }

  template <class functor>
  void ExpandAnalyticFunction(const functor& fG, function1D<double>& gl, const string& statistics="fermi") const
  {
    const vector<spline1D<double> >* pfU = (statistics=="fermi") ? &fU : &fU_bose;

    gl.resize(lmax);
    int M = static_cast<int>(pow(2.0,k_ovlp))+1;    // Number of points in Romberg integration routine
    vector<double> utu(M); // storage for Romberg
    //double beta = tau[tau.size()-1];
    for (int l=0; l<lmax; l++){
      gl[l]=0;
      for (int i=0; i<tau.size()-1; i++){
	double a = tau[i];  // integral only between t_i and t_{i+1} points
	double b = tau[i+1];
	double fa = (*pfU)[l][i]   * fG(a, beta);
	double fb = (*pfU)[l][i+1] * fG(b, beta);
	utu[0]   = fa;
	utu[M-1] = fb;
	tint ia=0;
	for (int j=1; j<M-1; j++){
	  double t = a + (b-a)*j/(M-1.0);
	  intpar p = tau.InterpLeft( t, ia);
	  utu[j] = (*pfU)[l](p) * fG(t, beta);
	}
	gl[l] += romberg(utu, b-a);
      }
    }
  }

  int l_critical(const functionb<double>& gl, double max_ratio=3., const string& statistics="fermi"){
    int nsa=5;

    int _lmax_ = statistics=="fermi" ? lmax : lmax_bose;

    if (_lmax_<=nsa) return _lmax_;

    function1D<double> gos(_lmax_);

    function1D<double>* pS = (statistics=="fermi") ? &S : &S_bose;

    for (int i=0; i<_lmax_; i++) gos[i] = fabs(gl[i]/(*pS)[i]);

    //cout<<"gos="<<gos<<endl;

    double* p = gos.begin() + nsa+1; // pointer to the fifth entry

    for (int l=nsa+1; l<_lmax_; ++l && ++p){
      double sa = *max_element(p-nsa,p);  // maximum in the past five coefficients
      //cout<<l<<" g[l]="<<gl[l]<<" gos[l]="<<gos[l]<<" gmax="<<sa<<" g/gmax="<<fabs(gos[l]/sa)<<endl;
      if (fabs(gos[l]/sa)>max_ratio){
	//cout<<"lcritical should be "<<l<<endl;
	return l;
      }
    }
    return _lmax_;
  }

  void GiveRealAxis(function2D<double>& Aw, const functionb<double>& gl, double max_ratio=3., const string& statistics="fermi")
  {
    int lcritical = l_critical(gl,max_ratio,statistics);
    Aw.resize(lcritical,om.size());

    function1D<double> gos(gl.size());

    function1D<double>* pS = (statistics=="fermi") ? &S : &S_bose;

    for (int i=0; i<gl.size(); i++) gos[i] = gl[i]/(*pS)[i];

    function2D<double>* pVt = (statistics=="fermi") ? &Vt : &Vt_bose;

    for(int i=0; i<om.size(); i++)
      Aw(0,i) = -gos[0]*(*pVt)(i,0);

    for (int l=1; l<lcritical; l++){
      for (int i=0; i<om.size(); i++)
	Aw(l,i) = Aw(l-1,i) - gos[l]*(*pVt)(i,l);
    }

    if (statistics=="bose"){
      for (int l=0; l<lcritical; l++)
	for (int iw=0; iw<om.size(); iw++)
	  Aw(l,iw) *= M_PI*om[iw];
    }
  }

  void SetUpFastInterp()
  {
    ff2.resize(tau.size(),lmax,2);
    dxi.resize(tau.size());
    for (int i=0; i<tau.size(); i++) dxi[i] = fU[0].dxi[i];
    for (int l=0; l<lmax; l++){
      for (int i=0; i<tau.size(); i++){
	ff2(i,l,0) = fU[l][i];
	ff2(i,l,1) = fU[l].f2[i];
      }
    }
  }
  void FastInterp(functionb<double>& res, const intpar& ip, double cst) const
  {
    int i= ip.i;
    double p = ip.p, q=1-ip.p;
    double dx26 = dxi[i]*dxi[i]/6.;
    double dq = dx26*q*(q*q-1);
    double dp = dx26*p*(p*p-1);
    q  *= cst;
    dq *= cst;
    p  *= cst;
    dp *= cst;
    double* __restrict__ _res = res.MemPt();
    const double* __restrict__ _ff2 = ff2[i];
    // this is fast equivalent of
    // res[l] += q * fU[l].f[i] + dq * fU[l].f2[i];
    for (int l=0; l<lmax; l++) _res[l] += q * _ff2[2*l] + dq * _ff2[2*l+1];
    _ff2 += 2*lmax;
    // this is fast equivalent of
    // res[l] += p * fu[l].f[i+1] + dp * fU[l].f2[i+1];
    for (int l=0; l<lmax; l++) _res[l] += p * _ff2[2*l] + dp * _ff2[2*l+1];
  }
  void FastInterp_(functionb<double>& res, const intpar& ip, double cst) const
  {// This is the same as FastInterp, except res is zeroth first.
    int i= ip.i;
    double p = ip.p, q=1-ip.p;
    double dx26 = dxi[i]*dxi[i]/6.;
    double dq = dx26*q*(q*q-1);
    double dp = dx26*p*(p*p-1);
    q  *= cst;
    dq *= cst;
    p  *= cst;
    dp *= cst;
    double* __restrict__ _res = res.MemPt();
    const double* __restrict__ _ff2 = ff2[i];
    for (int l=0; l<lmax; l++) _res[l] = q * _ff2[2*l] + dq * _ff2[2*l+1];
    _ff2 += 2*lmax;
    for (int l=0; l<lmax; l++) _res[l] += p * _ff2[2*l] + dp * _ff2[2*l+1];
  }

#ifdef _MPI
  void ComputeClCoefficients(function2D<double>& Cl, int my_rank, int mpi_size, int Master, int nw=0, int ntail=0, int cutoff_iom=0){
#else
  void ComputeClCoefficients(function2D<double>& Cl, int nw=0, int ntail=0, int cutoff_iom=0){
#endif
    bool BRISI = false;

    int Nt = tau.size();
    //double L = om[om.size()-1];
    //double beta = tau[tau.size()-1];
    if (nw==0)    nw = L*beta/3;      // number of Matsubara points treated exactly
    if (ntail==0) ntail=150;          // number of points used for the tail
    if (cutoff_iom==0) cutoff_iom=100;// how far the tail will extend. We will go to Matsubara index nw*cutoff_iom

    function1D<int> ind_om;
    create_log_mesh(ind_om, nw*cutoff_iom, nw, ntail); // log mesh with nw exact points in [0,...nw-1], and ntail points distributed between [nw,nw*cutoff_iom]
    mesh1D iom(ind_om.size());
    for (int in=0; in<ind_om.size(); in++)  iom[in] = ind_om[in];
    iom.SetUp(0);

    if (BRISI){
      cout.precision(11);
      cout<<"# Starting single loop for u(iw) "<<endl;
    }
    // This creates u_l(iom) from u_l(tau)
    function2D<complex<double> > u_om(lmax,iom.size());
    for (int lf=0; lf<lmax; lf++){
      for (int in=0; in<iom.size(); in++){
	double om = (2*iom[in]+1.)*M_PI/beta;
	u_om(lf,in) = fU[lf].Fourier(om, tau);
      }
    }
    if (BRISI) cout<<"# Starting double loop for w(iw) "<<endl;
    // This creates w_l(iom) from u_l(tau).
    // w_{lb,lf}(iom) = 1/beta \sum_{iOm} u^b_{lb}(iOm) * u^f_{lf}(iom+iOm)
    // which is equivalent to
    // w_{lb,lf}(iom) = Integrate[ u^b_{lb}(beta-t) * u^f_{lf}(t) e^{i*om*t}, {t,0,beta}]
    int Nl2 = lmax_bose*lmax;
#ifdef _MPI
    int ipr_proc = (Nl2 % mpi_size==0) ? Nl2/mpi_size : Nl2/mpi_size+1;
    int iistart = ipr_proc*my_rank;
    int iiend   = ipr_proc*(my_rank+1);
    if (iistart>Nl2) iistart=Nl2;
    if (iiend  >Nl2) iiend=Nl2;
    int _Nl2_ = max(Nl2, mpi_size*ipr_proc);
    if (BRISI) cout<<"istart="<<iistart<<" iend="<<iiend<<" pr_proc="<<ipr_proc<<" pr_proc*mpi_size="<<ipr_proc*mpi_size<<" Nl2="<<Nl2<<endl;
#else
    int iistart = 0;
    int iiend = Nl2;
    int ipr_proc = Nl2;
    int _Nl2_ = Nl2;
#endif
    function2D<complex<double> > w_iom(_Nl2_, iom.size());
    w_iom = 0.0;
    for (int ii=iistart; ii<iiend; ii++){
      // ii = lb * lmax + lf
      int lb = ii / lmax;
      int lf = ii % lmax;
      spline1D<double> wt(Nt);
      for (int it=0; it<tau.size(); it++)
	wt[it] = fU_bose[lb][Nt-1-it]*fU[lf][it]; // wt[t] = u^b_{lb}(beta-t) * u^f_{lf}(t)

      int n = tau.size();
      double x1 = 0.5*(tau[1]+tau[0]);
      double df1 = (wt[1]-wt[0])/(tau[1]-tau[0]);
      double x2 = 0.5*(tau[2]+tau[1]);
      double df2 = (wt[2]-wt[1])/(tau[2]-tau[1]);
      double df0 = df1 + (df2-df1)*(0.0-x1)/(x2-x1);
      x1 = 0.5*(tau[n-1]+tau[n-2]);
      df1 = (wt[n-1]-wt[n-2])/(tau[n-1]-tau[n-2]);
      x2 = 0.5*(tau[n-2]+tau[n-3]);
      df2 = (wt[n-2]-wt[n-3])/(tau[n-2]-tau[n-3]);
      double dfn = df1 + (df2-df1)*(beta-x1)/(x2-x1);
      wt.splineIt(tau, df0, dfn);
      // w(iom) = Fourier[ wt[t] ]
      for (int in=0; in<iom.size(); in++){
	double om = (2*iom[in]+1.)*M_PI/beta;
	//w_om(lb,lf,in) = wt.Fourier(om, tau);
	w_iom(ii,in) = wt.Fourier(om, tau);
      }
    }
#ifdef _MPI
    if (mpi_size>1)
      MPI_Allgather(MPI_IN_PLACE, ipr_proc*iom.size()*2, MPI_DOUBLE, w_iom.MemPt(), ipr_proc*iom.size()*2, MPI_DOUBLE, MPI_COMM_WORLD);
#endif
    if (BRISI) cout<<"# Starting last part"<<endl;
    int Nl = (lmax*lmax*lmax_bose);
    int offset=2;
    spline1D<double> ftmp(ntail+offset);
    mesh1D wom(ntail+offset);
    int ifirst = iom.size()-ntail-offset;
    for (int in=ifirst; in<iom.size(); in++) wom[in-ifirst] = iom[in];
    wom.SetUp(0);

    deque<pair<int,int> > icase;
    for (int i=0; i<Nl; i++){
      for (int j=i; j<Nl; j++){
	vector<int> li = get_lll(i);
	vector<int> lj = get_lll(j);
	if ( (li[0] + li[1] + li[2] + lj[0] + lj[1] + lj[2])%2 ) continue; // this vanishes due to symmetry
	icase.push_back( make_pair(i,j) );
      }
    }

#ifdef _MPI
    int pr_proc = (icase.size() % mpi_size==0) ? icase.size()/mpi_size : icase.size()/mpi_size+1;
    int istart = pr_proc*my_rank;
    int iend   = pr_proc*(my_rank+1);
    if (istart>icase.size()) istart=icase.size();
    if (iend  >icase.size()) iend=icase.size();
    if (BRISI) cout<<"istart="<<istart<<" iend="<<iend<<" pr_proc="<<pr_proc<<" pr_proc*mpi_size="<<pr_proc*mpi_size<<" icase.size="<<icase.size()<<endl;
#else
    int istart = 0;
    int iend = icase.size();
    int pr_proc = icase.size();
#endif
    function1D<double> Cl0(pr_proc);
    Cl0=0;
    // This loop is parallelized
    for (int ii=istart; ii<iend; ii++){
      int i = icase[ii].first;
      int j = icase[ii].second;
      vector<int> li = get_lll(i);
      vector<int> lj = get_lll(j);
      if ( (li[0] + li[1] + li[2] + lj[0] + lj[1] + lj[2])%2 ) continue; // this vanishes due to symmetry

      // lj[2],lj[1],lj[0]  :  bose,fermi,fermi
      //                       lb * lmax + lf
      //complex<double> * w_1 = w_iom[ lj[2]*lmax + li[1] ].MemPt();
      //complex<double> * w_2 = w_iom[ li[2]*lmax + lj[1] ].MemPt();
      //complex<double> * u_1 = u_om[ li[0] ].MemPt();
      //complex<double> * u_2 = u_om[ lj[0] ].MemPt();
      complex<double> * w_1 = w_iom[ lj[2]*lmax + li[0] ].MemPt();
      complex<double> * w_2 = w_iom[ li[2]*lmax + lj[0] ].MemPt();
      complex<double> * u_1 = u_om[ li[1] ].MemPt();
      complex<double> * u_2 = u_om[ lj[1] ].MemPt();

      double wsum = 0.0;
      for (int in=0; in<iom.size()-ntail; in++)
	wsum += (w_1[in] * conj(w_2[in]) * u_1[in] * conj(u_2[in])).real();
      // fill int the 1D-interpolation for the tail
      for (int in=ifirst; in<iom.size(); in++)
	ftmp[in-ifirst] = (w_1[in] * conj(w_2[in]) * u_1[in] * conj(u_2[in])).real();
      // intepolate the tail
      int n=ftmp.size();
      ftmp.splineIt(wom, (ftmp[1]-ftmp[0])/(wom[1]-wom[0]), (ftmp[n-1]-ftmp[n-2])/(wom[n-1]-wom[n-2]) );
      // Now anlytically evaluate the sum over all points, but loop only over a few points in the tail.
      // The sum of the spline in the interval [n1,n2] can be analytically evaluated:
      //  1/2*(f[n2]+f[n1])*(n2-n1)  - 1/24*(n2-n1)*((n2-n1)^2-1) * (f2[n2]+f2[n1])
      //  where f2[n1], f2[n2] is the second derivative at n1 and n2 points.
      //  Note that the first and the last point come in with the weight 1/2, hence we need to correct for that.
      wsum += 0.5*ftmp[iom.size()-ntail-ifirst] + 0.5*ftmp[iom.size()-1-ifirst];
      for (int in=iom.size()-ntail; in<iom.size()-1; in++){
	double dh = iom[in+1]-iom[in];
	wsum += 0.5*(ftmp[in-ifirst]+ftmp[in+1-ifirst])*dh - dh/24.0*(dh*dh - 1.0)*(ftmp.f2[in+1-ifirst]+ftmp.f2[in-ifirst]);
      }
      /* //The above few lines are equivalent to
	 int iom_last = iom[iom.size()-1]; tint ia=0;
	 for (int iw=iom.size()-ntail; iw<iom_last; iw++){
	 double ww = ftmp( wom.InterpLeft(iw, ia) );
	 wsum += ww;
	 }
      */
      /*
      Cl(i,j) = 2*(wsum)/beta;
      Cl(j,i) = Cl(i,j);
      */
      Cl0[ii-istart] = 2*(wsum)/beta;
      //cout<<setw(4)<<i<<" "<<setw(4)<<j<<"     "<<setw(3)<<li[2]<<" "<<setw(3)<<li[1]<<" "<<setw(3)<<li[0]<<"; "<<setw(3)<<lj[2]<<" "<<setw(3)<<lj[1]<<" "<<setw(3)<<lj[0]<<"    "<<setw(10)<<left<<Cl(i,j)<<right<<endl;
    }
#ifdef _MPI
    if (mpi_size>1){
      if (BRISI){
	cout<<"MPI_Gather"<<endl;
	cout.flush();
      }
      function1D<double> cCl0;
      cCl0.resize(pr_proc*mpi_size);
      MPI_Gather(Cl0.MemPt(), pr_proc, MPI_DOUBLE, cCl0.MemPt(), pr_proc, MPI_DOUBLE, Master, MPI_COMM_WORLD);
      if (my_rank==Master){
	Cl0.resize(icase.size());
	for (int i=0; i<icase.size(); i++) Cl0[i] = cCl0[i];
      }
    }
    if (my_rank == Master){
#endif
      Cl.resize(Nl,Nl);
      Cl=0.0;
      for (int ii=0; ii<icase.size(); ii++){
	int i = icase[ii].first;
	int j = icase[ii].second;
	Cl(i,j) = Cl0[ii];
	Cl(j,i) = Cl0[ii];
      }
#ifdef _MPI
    }
#endif
  }
};

/*
double fGs(double tau, double beta){
  // On real axis, this corresponds to
  // A(w) = 1/2*(delta(w-x0)+delta(w+x0))
  // with x0=1.
  return -exp(-beta/2.)*cosh(beta/2.-tau);
}

double chis(double tau, double beta){
// On real axis this corresponds to
// chi''(x) = pi*[delta(w-x0)-delta(w+x0)]
// with x0=1/beta
  const double bx2 = 0.5;
  return cosh(bx2*(1-2.*tau/beta))/sinh(bx2);
}

class fGspl{
public:
  spline1D<double>& fg;
  mesh1D t;
  fGspl(spline1D<double>& fg_, mesh1D& t_) : fg(fg_), t(t_) {}
  double operator()(double x, double beta) const{
    return fg(t.Interp(x));
  }
};

int main(){
  double x0=0.005;
  double L=10.;
  int Nw = 500;
  //double beta=9.35887;
  double beta=100;
  int lmax = 40;
  int k_ovlp=5;
  int Ntau = static_cast<int>(5*beta+100);
  bool readGFile=false;//true;

  spline1D<double> ginp;
  mesh1D tc;
  if (readGFile){
    ifstream  inp("../Gtau.dat_4");
    deque<double> ct, gt;
    while (inp){
      double t, g;
      inp >> t >> g;
      //cout<<t<<" "<<g<<endl;
      if (!inp) break;
      ct.push_back(t);
      gt.push_back(g);
      inp.ignore(numeric_limits<streamsize>::max(), '\n');
    }

    beta = ct.back()+ct[0];
    cout<<"beta="<<beta<<endl;
    ginp.resize(gt.size());
    tc.resize(ct.size());
    {
      for (int i=0; i<ct.size(); i++){
	tc[i] = ct[i];
	ginp[i] = gt[i];
      }
      tc.SetUp(0);
      double df0 = (ginp[1]-ginp[0])/(tc[1]-tc[0]);
      int n = tc.size();
      double dfn = (ginp[n-1]-ginp[n-2])/(tc[n-1]-tc[n-2]);
      cout<<"df0="<<df0<<" dfn="<<dfn<<endl;
      ginp.splineIt(tc, df0, dfn);
    }
  }
  fGspl Fg(ginp, tc);

  clog<<"beta="<<beta<<" Ntau="<<Ntau<<endl;

  SVDFunc svdf;

  svdf.cmp("both", beta, lmax, Ntau, x0, L, Nw, clog);

  // Print SVD functions
  svdf.Print("uls.dat",beta);
  svdf.Print("ulb.dat",beta, "bose");

  // Now we check how well is the orthogonality obeyed after reorthogonalization
  clog<<"Final overlap is smaller than "<<svdf.CheckOverlap(beta)<<endl;

  ofstream fo("cc.dat");
  mesh1D& tau = svdf.tau;
  function1D<double> Gts(tau.size());
  for (int i=0; i<tau.size(); i++){
    if (readGFile) Gts[i] = Fg(tau[i],beta);
    else Gts[i] = fGs(tau[i], beta);
    fo<<tau[i]<<" "<<Gts[i]<<" "<<svdf[0][i]<<" "<<svdf[2][i]<<endl;
  }
  fo.close();

  function1D<double> gl(lmax);
  if (readGFile) svdf.ExpandAnalyticFunction(Fg, gl);
  else svdf.ExpandAnalyticFunction(fGs, gl);

  function1D<double> gl2(lmax);
  svdf.SetUpFastInterp();
  {
    k_ovlp=5;
    gl2.resize(lmax);
    gl2 = 0.0;
    int M = pow(2,k_ovlp)+1;    // Number of points in Romberg integration routine
    function2D<double> utu_T(M,lmax); // storage for Romberg
    double beta = tau[tau.size()-1];
    for (int i=0; i<tau.size()-1; i++){
      utu_T=0;
      double a = tau[i];  // integral only between t_i and t_{i+1} points
      double b = tau[i+1];
      tint ia=0;
      for (int j=0; j<M; j++){
	double t = a + (b-a)*j/(M-1.0);
	intpar p = tau.InterpLeft( t, ia);
	double cst;
	if (readGFile) cst = Fg(t,beta);
	else cst = fGs(t, beta);
	///// Slow equivalent ////
	//for (int l=0; l<svdf.lmax; l++) utu_T(j,l) += cst * svdf.fU[l](p);
	svdf.FastInterp(utu_T[j], p, cst );
      }
      function1D<double> utu(M);
      for (int l=0; l<lmax; l++){
	for (int j=0; j<M; j++) utu[j] = utu_T(j,l);
	gl2[l] += romberg(utu, b-a);
      }
    }
  }
  cout<<"Should be equal:"<<endl;
  for (int l=0; l<lmax; l++)
    cout<<l<<" "<<gl[l]<<" "<<gl2[l]<<" "<<gl[l]-gl2[l]<<endl;

  spline1D<double> gf;
  svdf.CreateSplineFromCoeff(gf, gl);
  int nmax=5000;
  function1D<complex<double> > giom;
  svdf.MatsubaraFrequency(giom, gf, nmax);

  ofstream fig("giom.dat");
  fig.precision(16);
  for (int in=0; in<nmax; in++){
    double iom = (2*in+1)*M_PI/beta;
    double gi_exact = -iom/(iom*iom+1.);
    fig<<iom<<" "<<giom[in]<<" "<<gi_exact<<endl;
  }

  ofstream fog("gl.dat");
  fog.precision(16);
  for (int l=0; l<svdf.lmax; l++)
    if (fabs(gl[l])>1e-10)
      fog<<l<<" "<<gl[l]<<endl;
  fog.close();

  ofstream foG("Gapp.dat");
  foG.precision(16);
  for (int i=0; i<tau.size(); i++){
    double dval=0;
    for (int l=0; l<svdf.lmax; l++) dval += svdf[l][i]*gl[l];
    foG<<tau[i]<<" "<<dval<<" "<<Gts[i]<<" "<<fabs(dval-Gts[i])<<endl;
  }
  foG.close();

  //int lc = svdf.l_critical(gl);
  //cout<<lc<<endl;

  function2D<double> Aw;
  ofstream fAw("Aw.out");
  svdf.GiveRealAxis(Aw, gl, 2.0);
  for (int i=0; i<svdf.om.size(); i++){
    fAw << svdf.om[i]<<" ";
    for (int l=0; l<Aw.size_N(); l++){
      fAw << Aw(l,i) <<" ";
    }
    fAw << endl;
  }

  svdf.ExpandAnalyticFunction(chis, gl, "bose");
  svdf.CreateSplineFromCoeff(gf, gl, "bose");
  ofstream foc("Chid.dat");
  foc.precision(16);
  for (int i=0; i<svdf.tau.size(); i++){
    double val = chis(svdf.tau[i],beta);
    foc<<svdf.tau[i]<<" "<<gf[i]<<" "<<val<<" "<<fabs(val-gf[i])<<endl;
  }
  foc.close();

  svdf.MatsubaraFrequency(giom, gf, nmax, "bose");
  ofstream sig("siom.dat");
  sig.precision(16);
  for (int in=0; in<nmax; in++){
    double iom = (2*in+0.0)*M_PI/beta;
    double x0 = 1./beta;
    double gi_exact = 2*x0/(x0*x0+iom*iom);
    sig<<iom<<" "<<giom[in]<<" "<<gi_exact<<" "<<giom[in]-gi_exact<<endl;
  }

  cout<<"bose coefficients are"<<gl<<endl;

  svdf.GiveRealAxis(Aw, gl, 10.0, "bose");
  ofstream sAw("sw.out");
  for (int i=0; i<svdf.om.size(); i++){
    sAw << svdf.om[i]<<" ";
    for (int l=0; l<Aw.size_N(); l++) sAw << Aw(l,i) <<" ";
    sAw << endl;
  }

  svd.Print("usb.dat",common::beta,"bose");
  for (int l=0; l<svdf.lmax_bose; l++) svdf.MatsubaraFrequency(bUw[l], svdf.fU_bose[l], N_W);

  return 0;
}
*/


/*
 * File:   array_funcs.h
 * Author: fedro
 *
 * Created on December 10, 2014, 3:25 PM
 */

#ifndef ARRAY_FUNCS_H
#define	ARRAY_FUNCS_H

#include "array.h"
#include <assert.h>
#include <gsl/gsl_statistics_double.h>
#include <cmath>
#include <complex>
#include <fstream>

namespace neurophys {
namespace arr {

template<class T>
Array<T> zeros(const size_t size)
{
    // this assumes that whatever the type T, setting the memory byte-wise to
    // zero means the elements become 0

    Array<T> result(size);
    memset(result.data(), 0, size*sizeof(T));
    return result;
}

template<class T>
Array<T> ones(const size_t size)
{
    Array<T> result(size);
    for (int i = 0; i < size; i++)
        result[i] = 1.;
    return result;
}

template<class T>
const T sum(const Array<T>& ar)
{
    T res = 0.;
    for (int i = 0; i < ar.size(); i++)
        res += ar[i];
    return res;
}

template<class T>
const T max(const Array<T>& ar)
{
    T max = ar[0];
    for (int i = 0; i < ar.size(); i++)
        if (ar[i] > max)
            max = ar[i];
    return max;
}

template <class T>
void columns_to_stream(std::ostream& out, const Array<T>& x, const Array<T>& y)
{
    /*if (x.size() != y.size())
        throw ArraySizeMismatchException();*/
    for (int i = 0; i < std::min(x.size(),y.size()); i++)
        out << x[i] << "\t" << y[i] << "\n";
}

template <class T>
void columns_to_stream(std::ostream& out, const Array<T>& x, const Array<T>& y,
                        const Array<T>& z)
{
    /*if (x.size() != y.size())
        throw ArraySizeMismatchException();*/
    for (int i = 0; i < std::min(x.size(),y.size()); i++)
        out << x[i] << "\t" << y[i] << "\t" << z[i] << "\n";
}

template <class T>
void columns_to_file(const char filename[], const Array<T>& x, const Array<T>& y)
{
    std::ofstream of;
    of.open(filename, std::ios::out);
    columns_to_stream(of, x, y);
    of.close();
}

template <class T>
void columns_to_file(const char filename[], const Array<T>& x, const Array<T>& y,
                        const Array<T>& z)
{
    std::ofstream of;
    of.open(filename, std::ios::out);
    columns_to_stream(of, x, y, z);
    of.close();
}

template <class T>
Array<T> downsample(const Array<T>& src, const size_t destsize)
{
    assert(destsize <= src.size());
    Array<T> result = zeros<double>(destsize);
    const double f = static_cast<double>(destsize)/src.size();
    for (int i = 0; i < src.size(); i++)
    {
        result[i * f] += src[i] * f;
    }
    return result;
}

double mean(const Array<double>& ar);
double var(const Array<double>& ar);
Array<double> range(const double a, const double b, const double inc=1);
Array<double> cos(const Array<double>& x);
Array<double> sin(const Array<double>& x);
Array<std::complex<double> > conj(const Array<std::complex<double> >& x);
Array<double> real(const Array<std::complex<double> >& x);
Array<double> imag(const Array<std::complex<double> >& x);
Array<double> abs(const Array<std::complex<double> >& x);

}}

#endif	/* ARRAY_FUNCS_H */


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "eigenmap.h"
#include <cblas.h>
#include <lapacke.h>
#include <matio.h>
#include <memory.h>
#include <time.h>
/*
 * lanczos computes the smallest n_eigs eigenvalues for L and the
 * corresponding eigenvectors using the Lanczos algorithm.
 *
 * F: an array (n_patch by n_eigs) to store the eigenvectors
 * Es: an array (1 by n_eigs) to store the eigenvalues
 * L: an array (n_patch by n_patch) representing the Laplacian matrix
 * n_patch: the dimension of L
 */
/* ---- corresponding Matlab code ----
 * [F, Es] = lanczos(L, n_eigs)
 */
static double norm2(double *v, int length);
static void divide_copy(double *dest, const double *src, int length,
                        const double divisor);

void lanczos(double *F, double *Es, double *L, int n_eigs, int n_patch,
             int LANCZOS_ITR)
{
    double *b;
    double b_norm;

    double *z;
    double *alpha, *beta;
    double *q;
    int i;

    double *eigvec; // eigenvectors

    // generate random b with norm 1.
    srand((unsigned int)time(NULL));
    b = (double *)malloc(n_patch * sizeof(double));
    for (i = 0; i < n_patch; i++)
        b[i] = rand();
    b_norm = norm2(b, n_patch);
    for (i = 0; i < n_patch; i++)
        b[i] /= b_norm;

    alpha = (double *)malloc( (LANCZOS_ITR + 1) * sizeof(double) );
    beta = (double *)malloc( (LANCZOS_ITR + 1) * sizeof(double) );
    beta[0] = 0.0; // beta_0 <- 0
    z = (double *)malloc( n_patch * sizeof(double));
    q = (double *)malloc( n_patch * (LANCZOS_ITR + 2) * sizeof(double) );
    memset(&q[0], 0, n_patch * sizeof(double)); // q_0 <- 0
    memcpy(&q[n_patch], b, n_patch * sizeof(double)); // q_1 <- b

    for (i = 1; i <= LANCZOS_ITR; i++) {
        // z = L * Q(:, i)
        cblas_dsymv(CblasColMajor, CblasLower, n_patch, 1.0, L,
                    n_patch, &q[i * n_patch], 1, 0.0, z, 1);
        // alpha(i) = Q(:, i)' * z;
        alpha[i] = cblas_ddot(n_patch, &q[i * n_patch], 1, z, 1);
        // z = z - alpha(i) * Q(:, i)
        cblas_daxpy(n_patch, -alpha[i], &q[i * n_patch], 1, z, 1);
        // z = z - beta(i - 1) * Q(:, i - 1);
        cblas_daxpy(n_patch, -beta[i - 1], &q[(i - 1) * n_patch], 1, z, 1);
        /* re-orthogonalize twice */
        // b = Q(:, 1:i-1)' * z
        cblas_dgemv(CblasColMajor, CblasTrans, n_patch, i - 1, 1.0,
                    &q[n_patch], n_patch, z, 1, 0.0, b, 1);
        // z = Q(:, 1:i-1) * b + (-1) * z
        cblas_dgemv(CblasColMajor, CblasNoTrans, n_patch, i - 1, 1.0,
                    &q[n_patch], n_patch, b, 1, -1.0, z, 1);
        // b = Q(:, 1:i-1)' * z
        cblas_dgemv(CblasColMajor, CblasTrans, n_patch, i - 1, 1.0,
                    &q[n_patch], n_patch, z, 1, 0.0, b, 1);
        // z = Q(:, 1:i-1) * b + (-1) * z
        cblas_dgemv(CblasColMajor, CblasNoTrans, n_patch, i - 1, 1.0,
                    &q[n_patch], n_patch, b, 1, -1.0, z, 1);

        // beta(i) = norm(z, 2);
        beta[i] = cblas_dnrm2(n_patch, z, 1);
        // Q(:, i + 1) = z / beta(i);
        divide_copy(&q[(i + 1) * n_patch], z, n_patch, beta[i]);
    }

    // compute approximate eigensystem
    eigvec = (double *)malloc(LANCZOS_ITR * LANCZOS_ITR * sizeof(double));
    LAPACKE_dstedc(LAPACK_COL_MAJOR, 'I', LANCZOS_ITR, &alpha[1], &beta[1],
                   eigvec, LANCZOS_ITR);
    // copy specified number of eigenvalues
    memcpy(Es, &alpha[1], n_eigs * sizeof(double));

    // V = Q(:, 1:k) * U
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n_patch,
                LANCZOS_ITR, LANCZOS_ITR, 1.0, &q[n_patch], n_patch, eigvec,
                LANCZOS_ITR, 0.0, L, n_patch);
    // copy the corresponding eigenvectors
    memcpy(F, L, n_patch * n_eigs * sizeof(double));

    free(b);
    free(z);
    free(alpha);
    free(beta);
    free(q);
    free(eigvec);
}

static double norm2(double *v, int length)
{
    int i;
    double sum = 0.0;

    for (i = 0; i < length; i++)
        sum += v[i] * v[i];

    return sqrt(sum);
}

static void divide_copy(double *dest, const double *src, int length,
                        const double divisor)
{
    double factor = 1.0 / divisor;
    int i;
    for (i = 0; i < length; i++)
        dest[i] = src[i] * factor;
}


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

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

/* This is a fifth-order multiple recursive generator. The sequence is,

   x_n = (a_1 x_{n-1} + a_5 x_{n-5}) mod m

   with a_1 = 107374182, a_2 = a_3 = a_4 = 0, a_5 = 104480 and m = 2^31-1.

   We initialize the generator with x_n = s_n MOD m for n = 1..5,
   where s_n = (69069 * s_{n-1}) mod 2^32, and s_0 = s is the
   user-supplied seed.

   NOTE: According to the paper the seeds must lie in the range [0,
   2^31 - 2] with at least one non-zero value -- our seeding procedure
   satisfies these constraints.

   We then use 6 iterations of the generator to "warm up" the internal
   state.

   With this initialization procedure the theoretical value of
   z_{10006} is 2064828650 for s = 1. The subscript 10006 means (1)
   seed the generator with s = 1, (2) do the 6 warm-up iterations
   that are part of the seeding process, (3) then do 10000 actual
   iterations.

   The period of this generator is about 2^155.

   From: P. L'Ecuyer, F. Blouin, and R. Coutre, "A search for good
   multiple recursive random number generators", ACM Transactions on
   Modeling and Computer Simulation 3, 87-98 (1993). */

static inline unsigned long int mrg_get (void *vstate);
static double mrg_get_double (void *vstate);
static void mrg_set (void *state, unsigned long int s);

static const long int m = 2147483647;
static const long int a1 = 107374182, q1 = 20, r1 = 7;
static const long int a5 = 104480, q5 = 20554, r5 = 1727;

typedef struct
  {
    long int x1, x2, x3, x4, x5;
  }
mrg_state_t;

static inline unsigned long int
mrg_get (void *vstate)
{
  mrg_state_t *state = (mrg_state_t *) vstate;

  long int p1, h1, p5, h5;

  h5 = state->x5 / q5;
  p5 = a5 * (state->x5 - h5 * q5) - h5 * r5;
  if (p5 > 0)
    p5 -= m;

  h1 = state->x1 / q1;
  p1 = a1 * (state->x1 - h1 * q1) - h1 * r1;
  if (p1 < 0)
    p1 += m;

  state->x5 = state->x4;
  state->x4 = state->x3;
  state->x3 = state->x2;
  state->x2 = state->x1;

  state->x1 = p1 + p5;

  if (state->x1 < 0)
    state->x1 += m;

  return state->x1;
}

static double
mrg_get_double (void *vstate)
{
  return mrg_get (vstate) / 2147483647.0 ;
}

static void
mrg_set (void *vstate, unsigned long int s)
{
  /* An entirely adhoc way of seeding! This does **not** come from
     L'Ecuyer et al */

  mrg_state_t *state = (mrg_state_t *) vstate;

  if (s == 0)
    s = 1;	/* default seed is 1 */

#define LCG(n) ((69069 * n) & 0xffffffffUL)
  s = LCG (s);
  state->x1 = s % m;
  s = LCG (s);
  state->x2 = s % m;
  s = LCG (s);
  state->x3 = s % m;
  s = LCG (s);
  state->x4 = s % m;
  s = LCG (s);
  state->x5 = s % m;

  /* "warm it up" with at least 5 calls to go through
     all the x values */

  mrg_get (state);
  mrg_get (state);
  mrg_get (state);
  mrg_get (state);
  mrg_get (state);
  mrg_get (state);

  return;
}

static const gsl_rng_type mrg_type =
{"mrg",				/* name */
 2147483646,			/* RAND_MAX */
 0,                             /* RAND_MIN */
 sizeof (mrg_state_t),
 &mrg_set,
 &mrg_get,
 &mrg_get_double};

const gsl_rng_type *gsl_rng_mrg = &mrg_type;