/*
 * cube.c -- solutions of a cubic system
 *
 * (c) 2016 Prof Dr Andreas Mueller, Hochschule Rapperswil
 */
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>

int	f(double x, const double y[], double f[], void *params) {
	f[0] = (y[0] - 1) * y[0] * (y[0] + 1);
	return GSL_SUCCESS;
}

int	singlecurve(FILE *out, const char *name, double x0, double y0) {
	printf("%s: %f\n", name, y0);
	gsl_odeiv2_system	system = { f, NULL, 1, NULL };
	gsl_odeiv2_driver	*driver
		= gsl_odeiv2_driver_alloc_y_new(&system, gsl_odeiv2_step_rk8pd,
			1e-6, 1e-6, 0.0);
	fprintf(out, "path %s;\n", name);
	double	x = x0;
	double	y[1] = { y0 };
	fprintf(out, "%s = (%.3f,%.3f)", name, x, y[0]);
	for (int i = 1; i <= 400; i++) {
		double	xnext = x + 0.01;
		int	status = gsl_odeiv2_driver_apply(driver, &x, xnext, y);
		if (status != GSL_SUCCESS) {
			fprintf(stderr, "error: rc = %d\n", status);
			fprintf(out, ";\n");
			return -1;
		}
		if (fabs(y[0]) > 100) {
			goto end;
		}
		fprintf(out, "\n    --(%.3f,%.3f)", x, y[0]);
	}
end:
	fprintf(out, ";\n");
	gsl_odeiv2_driver_free(driver);
	return 1;
}

int	centercurve(FILE *out, const char *name, double y0) {
	gsl_odeiv2_system	system = { f, NULL, 1, NULL };
	gsl_odeiv2_driver	*driver
		= gsl_odeiv2_driver_alloc_y_new(&system, gsl_odeiv2_step_rk8pd,
			-1e-6, 1e-6, 0.0);
	double	x = 0;
	double	y[1] = { y0 };
	double	xnext = -2;
	// integrate backwards to the initial value
	int	status = gsl_odeiv2_driver_apply(driver, &x, xnext, y);
	gsl_odeiv2_driver_free(driver);
	driver = gsl_odeiv2_driver_alloc_y_new(&system, gsl_odeiv2_step_rk8pd,
			1e-6, 1e-6, 0.0);
/*
	if (fabs(y[0]) > 100) {
		return -1;
	}
*/
	singlecurve(out, name, -2, y[0]);
	return 1;
}

int	main(int argc, char *argv[]) {
	FILE	*out = fopen("cube.mp", "w");
	char	suffix = 'a';
	char	pathname[6];

	for (int i = 0; i < 16; i++) {
		double	y0 = -1.5 + i * 0.2;
		snprintf(pathname, sizeof(pathname), "path%c", suffix++);
		centercurve(out, pathname, y0);
	}

	return EXIT_SUCCESS;
}


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

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_siman.h>
#include <gsl/gsl_ieee_utils.h>
#include <stdio.h>

/* set up parameters for this simulated annealing run */
#define N_TRIES 200             /* how many points do we try before stepping */
#define ITERS_FIXED_T 1000      /* how many iterations for each T? */
#define STEP_SIZE 1.0           /* max step size in random walk */
#define K 1.0                   /* Boltzmann constant */
#define T_INITIAL 0.008         /* initial temperature */
#define MU_T 1.003              /* damping factor for temperature */
#define T_MIN 2.0e-6

gsl_siman_params_t params = {N_TRIES, ITERS_FIXED_T, STEP_SIZE,
                             K, T_INITIAL, MU_T, T_MIN};

double square (double x) ;
double square (double x) { return x * x ; }

double E1(void *xp);
double M1(void *xp, void *yp);
void S1(const gsl_rng * r, void *xp, double step_size);
void P1(void *xp);

/* now some functions to test in one dimension */
double E1(void *xp)
{
  double x = * ((double *) xp);

  return exp(-square(x-1))*sin(8*x) - exp(-square(x-1000))*0.89;
}

double M1(void *xp, void *yp)
{
  double x = *((double *) xp);
  double y = *((double *) yp);

  return fabs(x - y);
}

void S1(const gsl_rng * r, void *xp, double step_size)
{
  double old_x = *((double *) xp);
  double new_x;

  new_x = gsl_rng_uniform(r)*2*step_size - step_size + old_x;

  memcpy(xp, &new_x, sizeof(new_x));
}

void P1(void *xp)
{
  printf(" %12g ", *((double *) xp));
}

int main(void)
{
  double x_min = 1.36312999455315182 ;
  double x ;

  gsl_rng * r = gsl_rng_alloc (gsl_rng_env_setup()) ;

  gsl_ieee_env_setup ();

  /* The function tested here has multiple mimima.
     The global minimum is at    x = 1.36312999, (f = -0.87287)
     There is a local minimum at x = 0.60146196, (f = -0.84893) */

  x = -10.0 ;
  gsl_siman_solve(r, &x, E1, S1, M1, NULL, NULL, NULL, NULL,
                  sizeof(double), params);
  gsl_test_rel(x, x_min, 1e-3, "f(x)= exp(-(x-1)^2) sin(8x), x0=-10") ;

  x = +10.0 ;
  gsl_siman_solve(r, &x, E1, S1, M1, NULL, NULL, NULL, NULL,
                  sizeof(double), params);
  gsl_test_rel(x, x_min, 1e-3, "f(x)= exp(-(x-1)^2) sin(8x), x0=10") ;

  /* Start at the false minimum */

  x = +0.6 ;
  gsl_siman_solve(r, &x, E1, S1, M1, NULL, NULL, NULL, NULL,
                  sizeof(double), params);
  gsl_test_rel(x, x_min, 1e-3, "f(x)= exp(-(x-1)^2) sin(8x), x0=0.6") ;

  x = +0.5 ;
  gsl_siman_solve(r, &x, E1, S1, M1, NULL, NULL, NULL, NULL,
                  sizeof(double), params);
  gsl_test_rel(x, x_min, 1e-3, "f(x)= exp(-(x-1)^2) sin(8x), x0=0.5") ;

  x = +0.4 ;
  gsl_siman_solve(r, &x, E1, S1, M1, NULL, NULL, NULL, NULL,
                  sizeof(double), params);
  gsl_test_rel(x, x_min, 1e-3, "f(x)= exp(-(x-1)^2) sin(8x), x0=0.4") ;

  gsl_rng_free(r);
  exit (gsl_test_summary ());

#ifdef JUNK
  x0.D1 = 12.0;
  printf("#one dimensional problem, x0 = %f\n", x0.D1);
  gsl_siman_Usolve(r, &x0, test_E_1D, test_step_1D, distance_1D,
                   print_pos_1D, params);

  x0.D2[0] = 12.0;
  x0.D2[1] = 5.5;
  printf("#two dimensional problem, (x0,y0) = (%f,%f)\n",
         x0.D2[0], x0.D2[1]);
  gsl_siman_Usolve(r, &x0, test_E_2D, test_step_2D, distance_2D,
                   print_pos_2D, params);

  x0.D3[0] = 12.2;
  x0.D3[1] = 5.5;
  x0.D3[2] = -15.5;
  printf("#three dimensional problem, (x0,y0,z0) = (%f,%f,%f)\n",
         x0.D3[0], x0.D3[1], x0.D3[2]);
  gsl_siman_Usolve(r, &x0, test_E_3D, test_step_3D, distance_3D,
                   print_pos_3D, params);

  x0.D2[0] = 12.2;
  x0.D2[1] = 5.5;

  gsl_siman_solve(r, &x0, test_E_2D, test_step_2D, distance_2D, print_pos_2D, params);

  x0.D3[0] = 12.2;
  x0.D3[1] = 5.5;
  x0.D3[2] = -15.5;

  gsl_siman_solve(r, &x0, test_E_3D, test_step_3D, distance_3D, print_pos_3D, params);

  return 0;
#endif
}


#include <jni.h>
#include "jade_reconstruct_area_PJNI.h"
#include <gsl/gsl_linalg.h>
#include <stdio.h>

JNIEXPORT jdoubleArray JNICALL Java_jade_reconstruct_area_PJNI_matrixExp
  (JNIEnv *jenv, jobject obj, jdoubleArray arr,jint size){
	jdoubleArray ret = (*jenv)->NewDoubleArray(jenv,(jint)( size * size));
	double *OutData = (*jenv)->GetDoubleArrayElements(jenv,ret,JNI_FALSE);
	//double OutData [size*size];
	jdouble * at_data = (*jenv)->GetDoubleArrayElements(jenv, arr, NULL);
	int i,j;
	double a_data [size*size];
	for (i = 0; i < size*size; i++){
		a_data[i] = (double)at_data[i];
	}
	gsl_matrix_view m = gsl_matrix_view_array (a_data, (int)size, (int)size);
	gsl_mode_t mt = 0;
	gsl_matrix *ma = gsl_matrix_alloc (size,size);
	gsl_linalg_exponential_ss(&m.matrix, ma, mt);//m input, ma output

	int x = 0;
	for(i = 0; i < size; i++){
		for(j=0;j<size;j++){
			OutData[x] = gsl_matrix_get(ma,i,j);
			x++;
		}
	}
	//gsl_permutation_free (ma);
 	//(*jenv)->SetDoubleArrayRegion(jenv,ret,(jsize)0,(jsize)size*size,OutData);
	(*jenv)->ReleaseDoubleArrayElements(jenv,ret,OutData,0);
	(*jenv)->ReleaseDoubleArrayElements(jenv,arr,at_data,0);
	gsl_matrix_free (ma);
	return ret;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <time.h>
#define PI 3.1416
#define USAGE "./mcmc_solar.x n_steps n_burn"

void likelihood(float *chi, float *y_obs, float *y_model, int n_puntos);
void my_model(float *mine, float *anio ,float a, float b, float c, float d, int n_puntos);
void mcmc(int n_steps, float *a, float *b, float *c, float *d, float *l, float *x_obs, float *y_obs,int n_puntos, float *mine);
void min(float *mine, float *x_obs, float *a, float *b, float *c, float *d, float *l, int n_steps, int n_puntos);

void leer(float *anio, float *manc, int n_puntos);
float *reserva(int n_puntos);
void print_array(float *a, float *b, float *c, float *d , float *l, int n_steps, int n_burn);

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

	srand48(time(NULL));

	//Valores que entran
	int n_steps=atof(argv[1]);
	int n_burn=atof(argv[2]);
	//Inicializacion de los arrays que guardan los datos
	int n_puntos= 4141;
	float *manc;
	float *anio;
	anio=reserva(n_puntos);
	manc=reserva(n_puntos);

	leer(anio, manc, n_puntos);

	//Array con los resultados del ajuste
	float *mine;
	mine=reserva(n_puntos);

	//Arrays que guardan los datos de las variables
	float *a;
	float *b;
	float *c;
	float *d;
	float *l;
	a=reserva(n_steps);
	b=reserva(n_steps);
	c=reserva(n_steps);
	d=reserva(n_steps);
	l=reserva(n_steps);
	//Primer paso en la cadena
	a[0]=drand48();
	b[0]=drand48();
	c[0]=drand48();
	d[0]=drand48();
	float chi;
	my_model(mine,anio,a[0],b[0],c[0],d[0],n_puntos);
	likelihood(&chi,manc,mine,n_puntos);
	l[0]=chi;

	//Iteraciones
	mcmc(n_steps,a,b,c,d,l,anio,manc,n_puntos,mine);
	min(mine,anio,a,b,c,d,l,n_steps,n_puntos);

	print_array(a,b,c,d,l,n_steps,n_burn);

	free(mine);
	free(anio);
	free(manc);
	free(a);
	free(b);
	free(c);
	free(d);

	return 0;
}

//OBTENCION DEL MEJOR AJUSTE
void min(float *mine, float *x_obs, float *a, float *b, float *c, float *d, float *l, int n_steps, int n_puntos){
	int i;
	float temp, a2, b2, c2, d2;
	temp=l[0];
	a2=a[0];
	b2=b[0];
	c2=c[0];
	d2=d[0];
	for(i=1;i<n_steps;i++){
		if(temp > l[i] ){
			temp=l[i];
			a2=a[i];
			b2=b[i];
			c2=c[i];
			d2=d[i];
		}
	}
	my_model(mine,x_obs,a2,b2,c2,d2,n_puntos);
}

//ITERACIONES
void mcmc(int n_steps, float *a, float *b, float *c, float *d, float *l, float *x_obs, float *y_obs,int n_puntos, float *mine){
	int i;
	float a2,b2,c2,d2, chi, chi2, alpha, beta;
	const gsl_rng_type * T;
	gsl_rng * r;
	gsl_rng_env_setup();
	T = gsl_rng_default;
	r = gsl_rng_alloc (T);

    for(i=0;i<(n_steps-1);i++){
		a2 = a[i] + gsl_ran_gaussian(r, 0.1);
		b2 = b[i] + gsl_ran_gaussian(r, 0.1);
		c2 = c[i] + gsl_ran_gaussian(r, 0.1);
		d2 = c[i] + gsl_ran_gaussian(r, 0.1);

		float *y_init;
		float *y_prime;
		y_init=reserva(n_puntos);
		y_prime=reserva(n_puntos);

		my_model(y_init,x_obs,a[i],b[i],c[i],d[i],n_puntos);
		my_model(y_prime,x_obs,a2,b2,c2,d2,n_puntos);

		likelihood(&chi,y_obs,y_init,n_puntos);
		likelihood(&chi2,y_obs,y_prime,n_puntos);

		alpha = -chi2 + chi;
		if(alpha >= 0.0){
			a[i+1]=a2;
			b[i+1]=b2;
			c[i+1]=c2;
			d[i+1]=d2;
			l[i+1]=chi2;
		}
		else{
			beta = drand48();
			if(alpha < -6000.0 ){
				a[i+1]=a[i];
				b[i+1]=b[i];
				c[i+1]=c[i];
				d[i+1]=d[i];
				l[i+1]=chi;
			}
			else if(beta <= exp(alpha)){
				a[i+1]=a2;
				b[i+1]=b2;
				c[i+1]=c2;
				d[i+1]=d2;
				l[i+1]=chi2;
			}
			else{
				a[i+1]=a[i];
				b[i+1]=b[i];
				c[i+1]=c[i];
				d[i+1]=d[i];
				l[i+1]=chi;
			}
		}

		free(y_init);
		free(y_prime);
	}

  gsl_rng_free (r);
}

//LECTURA DEL ARCHIVO
void leer(float *anio, float *manc, int n_puntos){

	float *mes;
	mes=reserva(n_puntos);

	FILE *in;
	int i;
	in=fopen("monthrg.dat","r");
	if(!in){
    printf("problems opening the file %s\n","monthrg.dat");
    exit(1);
	}
	float temp1;
	float temp2;
	for(i=0;i<n_puntos;i++){
	  do{
		  fscanf(in, "%f %f %f %f %f\n", &anio[i], &mes[i], &temp1, &manc[i] , &temp2);
	  }while(manc[i] == -99.0);
	}
	fclose(in);

	//Calculo del anio como decimal
	for(i=0;i<n_puntos;i++){
		anio[i] = anio[i] + (mes[i]-1.0)/12.0;
	}
	free(mes);
}

//IMPRESION DE LOS ARRAYS
void print_array(float *a, float *b, float *c, float *d , float *l, int n_steps, int n_burn){
  int i;
  for(i=n_burn;i<n_steps;i++){
    printf("%f %f %f %f %f\n", a[i], b[i], c[i] , d[i], l[i]);
  }
}

//EVALUAR LOS PUNTOS CON EL AJUSTE
void my_model(float *mine, float *anio ,float a, float b, float c, float d, int n_puntos){
	int i;
	for(i=0;i<n_puntos;i++){
		mine[i] = a * cos((2*PI/d)*anio[i] + b) + c;
	}
}

//RESERVA DE MEMORIA
float *reserva(int n_puntos){
  float *array;
  int i;
  if(!(array = malloc(n_puntos * sizeof(float)))){
    printf("Problema en reserva\n");
    exit(1);
  }
  for(i=0;i<n_puntos;i++){
    array[i] = 0.0;
  }
  return array;
}

//CALCULO DE LA VARIACION DEL AJUSTE
void likelihood(float *chi, float *y_obs, float *y_model, int n_puntos){
	int i;
	float temp=0;
	for(i=0;i<n_puntos;i++){
		temp=temp+pow(y_obs[i]-y_model[i],2.0);
	}
	*chi=(1.0/2.0)*temp;
}


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

double interpolate(interp_info *interp, double x) {
    return (gsl_interp_eval(interp->interp, interp->x, interp->y, x, interp->accel));
}


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

double myfunc(unsigned n, const double *x, double *grad, void *my_func_data)
{
    //printf("%f\t%f\t", grad[0], grad[1]);
    //printf("%f\t%f\n", x[0], x[1]);
    if (grad) {
        grad[0] = 0.0;
        grad[1] = 0.5 / sqrt(x[1]);
    }
    //printf("%f\n", sqrt(x[1]));
    //sleep(1);
    return sqrt(x[1]);
}

typedef struct {
    double a, b;
} my_constraint_data;

double myconstraint(unsigned n, const double *x, double *grad, void *data)
{
    my_constraint_data *d = (my_constraint_data *) data;
    double a = d->a, b = d->b;
    if (grad) {
        grad[0] = 3 * a * (a*x[0] + b) * (a*x[0] + b);
        grad[1] = -1.0;
    }
    return ((a*x[0] + b) * (a*x[0] + b) * (a*x[0] + b) - x[1]);
 }

int main(){
    double lb[2] = { -HUGE_VAL, 0 }; /* lower bounds */
    nlopt_opt opt;

    //opt = nlopt_create(NLOPT_LD_MMA, 2); /* algorithm and dimensionality */
    opt = nlopt_create(NLOPT_LN_COBYLA, 2); /* algorithm and dimensionality */
    nlopt_set_lower_bounds(opt, lb);
    nlopt_set_min_objective(opt, myfunc, NULL);

    my_constraint_data data[2] = { {2,0}, {-1,1} };

    nlopt_add_inequality_constraint(opt, myconstraint, &data[0], 1e-8);
    nlopt_add_inequality_constraint(opt, myconstraint, &data[1], 1e-8);

    //nlopt_set_xtol_rel(opt, 1e-4);
    nlopt_set_stopval(opt, (sqrt(8./27.) + 1e-3));

    double x[2] = { 1.234, 5.678 };  /* `*`some` `initial` `guess`*` */
    double minf; /* `*`the` `minimum` `objective` `value,` `upon` `return`*` */
    if (nlopt_optimize(opt, x, &minf) < 0) {
	printf("nlopt failed!\n");
    }
    else {
	printf("found minimum at f(%g,%g) = %0.10g\n", x[0], x[1], minf);
    }

    nlopt_destroy(opt);
}


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

#include "cosmocalc.h"
#include "weaklens.h"

double chiLim;

static void comp_lens_power_spectrum(lensPowerSpectra lps);

double lens_power_spectrum(double ell, lensPowerSpectra lps)
{
  if(lps->initFlag == 1 || lps->currCosmoNum != cosmoData.cosmoNum || lps->currWLNum != wlData.wlNum)
    {
      lps->initFlag = 0;
      lps->currCosmoNum = cosmoData.cosmoNum;
      lps->currWLNum = wlData.wlNum;

      comp_lens_power_spectrum(lps);
    }

  return exp(gsl_spline_eval(lps->spline,log(ell),lps->accel));
}

double nonlinear_powspec_for_lens(double k, double a)
{
  return nonlinear_powspec(k,a);
}

static double lenskern(double chi, double chis)
{
  if(chi > chis)
    return 0.0;
  else
    return 1.5*cosmoData.OmegaM*(100.0/CSOL)*(100.0/CSOL)/acomvdist(chi)*(chis-chi)/chis;
}

static double lenspk_integrand(double chi, void *p)
{
  lensPowerSpectra lps = (lensPowerSpectra) p;
  double sn = lps->sn;

  if(chi == 0.0 || chi < chiLim)
    return 0.0;
  else
    return lenskern(chi,lps->chis1)*lenskern(chi,lps->chis2)*(nonlinear_powspec_for_lens(lps->ell/chi,acomvdist(chi)) + sn);
}

static void comp_lens_power_spectrum(lensPowerSpectra lps)
{
#define WORKSPACE_NUM 100000
#define ABSERR 1e-12
#define RELERR 1e-12
#define TABLE_LENGTH 1000

  gsl_integration_workspace *workspace;
  gsl_function F;
  double result,abserr;
  double logltab[TABLE_LENGTH];
  double logpkltab[TABLE_LENGTH];
  double chimax;
  int i;

  //fill in bin information
  chiLim = lps->chiLim;
  if(lps->chis1 > lps->chis2)
    chimax = lps->chis1;
  else
    chimax = lps->chis2;

  fprintf(stderr,"doing lens pk - chiLim = %lf, chiMax = %lf\n",chiLim,chimax);

  //init
  workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
  F.function = &lenspk_integrand;
  F.params = lps;

  //make table
  double lnlmin = log(wlData.lmin);
  double lnlmax = log(wlData.lmax);
  for(i=0;i<TABLE_LENGTH;++i)
    {
      logltab[i] = i*(lnlmax-lnlmin)/(TABLE_LENGTH-1) + lnlmin;

      lps->ell = exp(logltab[i]);
      gsl_integration_qag(&F,0.0,chimax,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&result,&abserr);

      logpkltab[i] = log(result);
    }

  //free
  gsl_integration_workspace_free(workspace);

  //init splines and accels
  if(lps->spline != NULL)
    gsl_spline_free(lps->spline);
  lps->spline = gsl_spline_alloc(gsl_interp_akima,(size_t) (TABLE_LENGTH));
  gsl_spline_init(lps->spline,logltab,logpkltab,(size_t) (TABLE_LENGTH));
  if(lps->accel != NULL)
    gsl_interp_accel_reset(lps->accel);
  else
    lps->accel = gsl_interp_accel_alloc();

#undef TABLE_LENGTH
#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM
}

lensPowerSpectra init_lens_power_spectrum(double zs1, double zs2)
{
  lensPowerSpectra lps;

  lps = (lensPowerSpectra)malloc(sizeof(_lensPowerSpectra));
  assert(lps != NULL);

  lps->initFlag = 1;
  lps->zs1 = zs1;
  lps->zs2 = zs2;
  lps->chis1 = comvdist(1.0/(1.0 + zs1));
  lps->chis2 = comvdist(1.0/(1.0 + zs2));
  lps->spline = NULL;
  lps->accel = NULL;

  return lps;
}

void free_lens_power_spectrum(lensPowerSpectra lps)
{
  if(lps->spline != NULL)
    gsl_spline_free(lps->spline);
  if(lps->accel != NULL)
    gsl_interp_accel_free(lps->accel);
  free(lps);
}

////////////////////////////////////////
// corr. funcs!
//////////////////////////////////////

static double lenscfp_integrand(double ell, void *p)
{
  lensCorrFunc lcf = (lensCorrFunc) p;

  return ell/2.0/M_PI*lens_power_spectrum(ell,lcf->lps)*gsl_sf_bessel_J0(ell*lcf->theta/60.0/180.0*M_PI);

}

static double lenscfm_integrand(double ell, void *p)
{
  lensCorrFunc lcf = (lensCorrFunc) p;

  return ell/2.0/M_PI*lens_power_spectrum(ell,lcf->lps)*gsl_sf_bessel_Jn(4,ell*lcf->theta/60.0/180.0*M_PI);
}

static void comp_lens_corr_funcs(lensCorrFunc lcf)
{
#define WORKSPACE_NUM 100000
#define ABSERR 1e-12
#define RELERR 1e-12
#define TABLE_LENGTH 1000

  gsl_integration_workspace *workspace;
  gsl_function F;
  double result,abserr;
  double logttab[TABLE_LENGTH];
  double logcfptab[TABLE_LENGTH];
  double logcfmtab[TABLE_LENGTH];
  int i;
  double lntmin;
  double lntmax;

  //init
  workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
  F.params = lcf;
  lntmin = log(wlData.tmin);
  lntmax = log(wlData.tmax);

  //make tables
  F.function = &lenscfp_integrand;
  for(i=0;i<TABLE_LENGTH;++i)
    {
      logttab[i] = i*(lntmax-lntmin)/(TABLE_LENGTH-1) + lntmin;

      lcf->theta = exp(logttab[i]);
      gsl_integration_qag(&F,wlData.lmin,wlData.lmax,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&result,&abserr);

      logcfptab[i] = log(result);
    }

  F.function = &lenscfm_integrand;
  for(i=0;i<TABLE_LENGTH;++i)
    {
      logttab[i] = i*(lntmax-lntmin)/(TABLE_LENGTH-1) + lntmin;

      lcf->theta = exp(logttab[i]);
      gsl_integration_qag(&F,wlData.lmin,wlData.lmax,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&result,&abserr);

      logcfmtab[i] = log(result);
    }

  //free
  gsl_integration_workspace_free(workspace);

  //init splines and accels
  if(lcf->splineP != NULL)
    gsl_spline_free(lcf->splineP);
  lcf->splineP = gsl_spline_alloc(gsl_interp_akima,(size_t) (TABLE_LENGTH));
  gsl_spline_init(lcf->splineP,logttab,logcfptab,(size_t) (TABLE_LENGTH));
  if(lcf->accelP != NULL)
    gsl_interp_accel_reset(lcf->accelP);
  else
    lcf->accelP = gsl_interp_accel_alloc();

  if(lcf->splineM != NULL)
    gsl_spline_free(lcf->splineM);
  lcf->splineM = gsl_spline_alloc(gsl_interp_akima,(size_t) (TABLE_LENGTH));
  gsl_spline_init(lcf->splineM,logttab,logcfmtab,(size_t) (TABLE_LENGTH));
  if(lcf->accelM != NULL)
    gsl_interp_accel_reset(lcf->accelM);
  else
    lcf->accelM = gsl_interp_accel_alloc();

#undef TABLE_LENGTH
#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM
}

double lens_corr_func_minus(double theta, lensCorrFunc lcf)
{
  if(lcf->initFlag == 1 || lcf->currCosmoNum != cosmoData.cosmoNum || lcf->currWLNum != wlData.wlNum)
    {
      lcf->initFlag = 0;
      lcf->currCosmoNum = cosmoData.cosmoNum;
      lcf->currWLNum = wlData.wlNum;

      comp_lens_corr_funcs(lcf);
    }

  return exp(gsl_spline_eval(lcf->splineM,log(theta),lcf->accelM));
}

double lens_corr_func_plus(double theta, lensCorrFunc lcf)
{
  if(lcf->initFlag == 1 || lcf->currCosmoNum != cosmoData.cosmoNum || lcf->currWLNum != wlData.wlNum)
    {
      lcf->initFlag = 0;
      lcf->currCosmoNum = cosmoData.cosmoNum;
      lcf->currWLNum = wlData.wlNum;

      comp_lens_corr_funcs(lcf);
    }

  return exp(gsl_spline_eval(lcf->splineP,log(theta),lcf->accelP));
}

lensCorrFunc init_lens_corr_func(lensPowerSpectra lps)
{
  lensCorrFunc lcf;

  lcf = (lensCorrFunc)malloc(sizeof(_lensCorrFunc));
  assert(lcf != NULL);

  lcf->initFlag = 1;
  lcf->lps = lps;
  lcf->splineM = NULL;
  lcf->accelM = NULL;
  lcf->splineP = NULL;
  lcf->accelP = NULL;

  return lcf;
}

void free_lens_corr_func(lensCorrFunc lcf)
{
  if(lcf->splineM != NULL)
    gsl_spline_free(lcf->splineM);
  if(lcf->accelM != NULL)
    gsl_interp_accel_free(lcf->accelM);

  if(lcf->splineP != NULL)
    gsl_spline_free(lcf->splineP);
  if(lcf->accelP != NULL)
    gsl_interp_accel_free(lcf->accelP);

  free(lcf);
}


/** @file
 * Based on the paper:
 * Quintana, F.A. (1998) ``Nonparametric Bayesian analysis for assessing homogeneity in $ k\times l$
 * contingency tables with fixed right margin totals''.
 * Also as Technical Report PUC/FM-96/7. Journal of the American Statistical Association, 93(443), 1140-1149.
 * Available as compressed postscript.
 * http://www.mat.puc.cl/~quintana/hcttr.ps.gz
 */

#ifndef BIO_CONTINGENCY_HOMOGENEITY_H_
#define BIO_CONTINGENCY_HOMOGENEITY_H_

#include "bio/defs.h"
#include "bio/partition.h"

#include <boost/assign/list_of.hpp>

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

#include <numeric>
#include <stdexcept>

BIO_NS_START

typedef std::vector<double> gamma_vec_t;
extern gamma_vec_t contingency_gamma_exponential_prior;

template <class LambdaIt>
double
calculate_ln_D(
	LambdaIt lambda_begin,
	LambdaIt lambda_end)
{
	double result = gsl_sf_lngamma(std::accumulate(lambda_begin, lambda_end, double(0)));
	for (LambdaIt i = lambda_begin; lambda_end != i; ++i)
	{
		result -= gsl_sf_lngamma(*i);
	}

	return result;
}

/** Calculate natural logarithm of a term for one partition in Lm(n). */
template <class PartitionIt, class CountIt, class LambdaIt>
double
contingency_calculate_ln_L_term(
	PartitionIt part_it,
	unsigned m,
	CountIt n_begin,
	CountIt n_end,
	LambdaIt lambda_begin,
	LambdaIt lambda_end)
{
	double ln_result = 1.0;

#ifdef BIO_CONTINGENCY_PRINT_PARTITIONS
	//can only do this for smallish numbers
	if (26 >= part_it.M.size())
	{
		std::vector<char> v;
		for (unsigned i = 0; i != part_it.M.size(); ++i)
		{
			v.push_back('a' + i);
		}
		std::cout << *part_it[v] << '\n';
	}
#endif //BIO_CONTINGENCY_PRINT_PARTITIONS

	//for each set in the partition
	for (unsigned i = 0; i < m; ++i)
	{
		//calculate lambda + n
		typedef std::vector<double> lambda_vec_t;
		lambda_vec_t lambda_plus_n;
		{
			std::copy(lambda_begin, lambda_end, std::inserter(lambda_plus_n, lambda_plus_n.begin()));
			unsigned e_i = 0;
			unsigned j = 0;
			//for each index
			for (CountIt n = n_begin; n_end != n; ++j, ++n)
			{
				//is this j in this partition?
				if (i == part_it.M[j])
				{
					for (unsigned k = 0; k != (*n).size(); ++k)
					{
						lambda_plus_n[k] += (*n)[k];
					}
					if (1 < e_i)
					{
						ln_result += gsl_sf_log(double(e_i)); //the factorial term
					}
					++e_i;
				}
			}
		}

		ln_result += calculate_ln_D(lambda_begin, lambda_end);
		ln_result -= calculate_ln_D(lambda_plus_n.begin(), lambda_plus_n.end());
	}

	return ln_result;
}

/** Calculate Lm(n). */
template <class CountIt, class LambdaIt>
double
contingency_calculate_L(
	unsigned m,
	CountIt n_begin,
	CountIt n_end,
	LambdaIt lambda_begin,
	LambdaIt lambda_end)
{
	unsigned set_size = unsigned(n_end - n_begin);
	BOOST_ASSERT(unsigned(lambda_end - lambda_begin) == set_size);

	double result = 0;

	//iterate over all the partitions
	partition::iterator_k part_it(set_size, m);
	try
	{
		while (true)
		{
			const double ln_L_term =
				contingency_calculate_ln_L_term(
					part_it,
					m,
					n_begin,
					n_end,
					lambda_begin,
					lambda_end);

			//check for underflow - in which case it will be 0.
			if (ln_L_term > GSL_LOG_DBL_MIN)
			{
				result += gsl_sf_exp(ln_L_term);
			}

			++part_it;
		}
	}
	catch (std::overflow_error&) //how the partition code determines there are no more partitions!
	{
	}

	return result;
}

template <class Value>
Value
factorial(Value value)
{
	Value result = Value(1);
	while (value != 0)
	{
		result *= value;
		--value;
	}
	return result;
}

typedef double(* contingency_cluster_prior)(double c, void * void_params);

struct ContingencyClusterPrior
{
	virtual double operator()(double c) = 0;
	virtual double get_ln_prior(double c);
};

struct ExponentialPrior : ContingencyClusterPrior
{
	double mean;
	ExponentialPrior(double mean = 1.0);
	double operator()(double c);
	double get_ln_prior(double c);
};

struct GammaParams
{
	unsigned m;
	unsigned k;
	ContingencyClusterPrior * prior;
};

double
gamma_exponential_prior_integrand(double c, void * void_params);

double
contingency_calculate_gamma(
	unsigned m,
	unsigned k,
	ContingencyClusterPrior * prior);

template <class GammaIt, class CountIt, class LambdaIt>
double
contingency_calculate_bayes_factor(
	GammaIt gamma_begin,
	GammaIt gamma_end,
	CountIt n_begin,
	CountIt n_end,
	LambdaIt lambda_begin,
	LambdaIt lambda_end)
{
	const unsigned k = unsigned(n_end == n_begin ? 0 : n_begin->end() - n_begin->begin());

	double result = contingency_calculate_L(1, n_begin, n_end, lambda_begin, lambda_end);

	result *= (1 - factorial(k-1) * (*gamma_begin));

	result /= factorial(k-1);

	double sum = 0;
	for (unsigned i = 2; k + 1 != i; ++i)
	{
		sum += *(gamma_begin + i - 1) * contingency_calculate_L(i, n_begin, n_end, lambda_begin, lambda_end);
	}
	result /= sum;

	return result;
}

/** Contains the data for a contingency table and defines types. */
template <unsigned K, unsigned L>
struct ContingencyTable
{
	typedef boost::array<unsigned, K> row_t;
	typedef boost::array<row_t, L> table_t;
	typedef boost::array<double, L> lambda_vec_t;

	table_t data;

	static unsigned get_k() { return K; }
	static unsigned get_l() { return L; }
};

template <unsigned K, unsigned L>
std::ostream &
operator<<(std::ostream & os, const ContingencyTable<K, L> & table)
{
	for (unsigned l = 0; L != l; ++l)
	{
		for (unsigned k = 0; K != k; ++k)
		{
			os << table.data[l][k] << '\t';
		}
		os << '\n';
	}
	return os;
}

BIO_NS_END

#endif //BIO_CONTINGENCY_HOMOGENEITY_H_


/* W. H. Bell
** A program to generate random numbers obeying a Landau distribution
** using the GNU scientific libraries.
*/

#include <stdio.h>

/* Header files needed for the Landau distribution generator */
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

int main() {
  int i;
  double ran_num;

  const gsl_rng_type * T;
  gsl_rng * r;

  /* Initialise the random number generator */
  gsl_rng_env_setup();
  T = gsl_rng_default;
  r = gsl_rng_alloc (T);
  printf("Loop Num | Value\n");
  printf("----------------\n");
  for (i=0;i<2500;i++) {

    /* Generate a random number obeying the Landau distribution */
    ran_num=gsl_ran_landau (r);

    printf("%.4d     | %lf\n", i, ran_num);
  }

  return 0;
}


/*
 * Gates.h
 *
 *  Created on: 28 Jan 2019
 *      Author: jobinrjohnson
 */

#ifndef IMPLEMENTATION_GATES_H_
#define IMPLEMENTATION_GATES_H_

#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>

#define ccout std::cout
#define cendl std::endl

#include <qubit.h>

#define GATE_HARDAMARD 1
#define GATE_PAULI_X 2
#define GATE_PAULI_Y 6
#define GATE_PAULI_Z 7
#define GATE_CNOT 3
#define GATE_SWAP 4
#define GATE_T 5

#include <iostream>

class Gates
{

  protected:
	gsl_matrix_complex *get_gate_matrix(int);

  public:
	Gates();
	virtual ~Gates();

	static bool get_measure(Qubit &q)
	{

		// TODO Better random and measure implementation

		gsl_vector_complex *state = q.get_state();
		double amp_0 = gsl_complex_abs2(gsl_vector_complex_get(state, 0));

		const gsl_rng_type *T;
		gsl_rng *r;
		gsl_rng_env_setup();
		gsl_rng_default_seed = time(NULL);
		T = gsl_rng_default;
		r = gsl_rng_alloc(T);

		double u = gsl_rng_uniform(r);

		gsl_rng_free(r);

		if (u >= amp_0)
		{
			return true;
		}

		return false;
	}
};

#endif /* IMPLEMENTATION_GATES_H_ */


//
//  her2.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 12/23/11.
//  Copyright (c) 2011 University of Colorado Denver. All rights reserved.
//

#ifndef _her2_h
#define _her2_h

/// @file her2.h Performs complex outer product of two vectors.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief Performs a vector outer product of two complex vectors.
   ///
   /// For a complex Hermitian matrix A, complex vectors x and y, and complex scalar alpha,
   ///
   ///        A := alpha * x * y' + conj(alpha) * y * x' + A
   ///
   /// is computed. The result is Hermitian and is stored as either upper or lower triangular in A.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param uplo Specifies whether A is stored as upper or lower triangular.
   ///
   ///        if uplo = 'U' or 'u' then A is upper triangular
   ///        if uplo = 'L' or 'l' then A is lower triangular
   ///
   /// @param n Specifies the order of the matrix A.  n>=0
   /// @param alpha Complex scalar.
   /// @param x Pointer to complex vector x.
   /// @param incx Increment of the vector x.  x!=0
   /// @param y Pointer to complex vector y.
   /// @param incy Increment of the vector y.  y!=0
   /// @param A Pointer to complex Hermitian n-by-n matrix A.If uplo = 'U' or 'u' then only the upper
   /// triangular part of A is referenced and the lower part is not referenced.  If uplo = 'L' or 'l' then
   /// only the lower triangular part of A is referenced and the upper part is not referenced.
   /// @param ldA Column length of matrix A.  ldA>=n.
   /// @ingroup BLAS

   template <typename real_t>
   int HER2(char uplo, int_t n, complex<real_t> alpha, complex<real_t> *x, int_t incx, complex<real_t> *y, int_t incy, complex<real_t> *A, int_t ldA)
   {
      using std::conj;
      using std::real;
      using std::toupper;
      const complex<real_t> zero(0.0,0.0);
      complex<real_t> tx,ty;
      int_t i,j,kx,ky,jx,ix,jy,iy;

      uplo=toupper(uplo);

      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(incy==0)
         return -7;
      else if(ldA<n)
         return -9;
      else if((n==0)||(alpha==zero))
         return 0;

      if((incx==1)&&(incy==1))
      {
         if(uplo=='U')
         {
            for(j=0;j<n;j++)
            {
               tx=conj(alpha*x[j]);
               ty=alpha*conj(y[j]);
               for(i=0;i<j;i++)
                  A[i]+=x[i]*ty+y[i]*tx;
               A[j]=real(A[j])+real(x[j]*ty+y[j]*tx);
               A+=ldA;
            }
         }
         else
         {
            for(j=0;j<n;j++)
            {
               tx=conj(alpha*x[j]);
               ty=alpha*conj(y[j]);
               A[j]=real(A[j])+real(x[j]*ty+y[j]*tx);
               for(i=j+1;i<n;i++)
                  A[i]+=x[i]*ty+y[i]*tx;
               A+=ldA;
            }
         }
      }
      else
      {
         kx=(incx>0)?0:(1-n)*incx;
         ky=(incy>0)?0:(1-n)*incy;
         if(uplo=='U')
         {
            jx=kx;
            jy=ky;
            for(j=0;j<n;j++)
            {
               tx=conj(alpha*x[jx]);
               ty=alpha*conj(y[jy]);
               ix=kx;
               iy=ky;
               for(i=0;i<j;i++)
               {
                  A[i]+=x[ix]*ty+y[iy]*tx;
                  ix+=incx;
                  iy+=incy;
               }
               A[j]=real(A[j])+real(x[jx]*ty+y[jy]*tx);
               A+=ldA;
               jx+=incx;
               jy+=incy;
            }
         }
         else
         {
            jx=kx;
            jy=ky;
            for(j=0;j<n;j++)
            {
               tx=conj(alpha*x[jx]);
               ty=alpha*conj(y[jy]);
               ix=jx;
               iy=jy;
               A[j]=real(A[j])+real(x[jx]*ty+y[jy]*tx);
               for(i=j+1;i<n;i++)
               {
                  ix+=incx;
                  iy+=incy;
                  A[i]+=x[ix]*ty+y[iy]*tx;
               }
               jx+=incx;
               jy+=incy;
               A+=ldA;
            }
         }
      }
      return 0;
   }
#ifdef __latl_cblas
#include <cblas.h>

   template <> int HER2<float>(char uplo, int_t n, complex<float> alpha, complex<float> *x, int_t incx, complex<float> *y, int_t incy, complex<float> *A, int_t ldA)
   {
      uplo=std::toupper(uplo);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(ldA<n)
         return -7;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_cher2(CblasColMajor,Uplo,n,&alpha,x,incx,y,incy,A,ldA);

      return 0;
   }

   template <> int HER2<double>(char uplo, int_t n, complex<double> alpha, complex<double> *x, int_t incx, complex<double> *y, int_t incy, complex<double> *A, int_t ldA)
   {
      uplo=std::toupper(uplo);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(ldA<n)
         return -7;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_zher2(CblasColMajor,Uplo,n,&alpha,x,incx,y,incy,A,ldA);

      return 0;
   }
#endif

}
#endif


/*****************************************************************\
           __
          / /
		 / /                     __  __
		/ /______    _______    / / / / ________   __       __
	   / ______  \  /_____  \  / / / / / _____  | / /      / /
	  / /      | / _______| / / / / / / /____/ / / /      / /
	 / /      / / / _____  / / / / / / _______/ / /      / /
	/ /      / / / /____/ / / / / / / |______  / |______/ /
   /_/      /_/ |________/ / / / /  \_______/  \_______  /
                          /_/ /_/                     / /
			                                         / /
		       High Level Game Framework            /_/

  ---------------------------------------------------------------

  Copyright (c) 2007-2011 - Rodrigo Braz Monteiro.
  This file is subject to the terms of halley_license.txt.

\*****************************************************************/

#pragma once

#include <cmath>
#include <iostream>
#include "angle.h"
#include <halley/utils/utils.h>
#include "halley/text/string_converter.h"
#include <gsl/gsl>

#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif

namespace Halley {
	//////////////////////////////
	// Vector2D class declaration
	template <typename T=float, class U=Angle<float>>
	class Vector2D {
	private:
		constexpr T mod(T a, T b) const { return a % b; }

	public:
		T x,y;

		// Constructors
		constexpr Vector2D () : x(0), y(0) {}

		constexpr Vector2D (T _x, T _y) : x(_x), y(_y) {}

		template <typename V>
		constexpr explicit Vector2D (Vector2D<V> vec) : x(T(vec.x)), y(T(vec.y)) {}

		Vector2D (T length, U angle)
		{
			float s, c;
			angle.sincos(s, c);
			x = s*length;
			y = c*length;
		}

		// Getter
		constexpr inline T& operator[](size_t n)
		{
			Expects(n <= 1);
			return (&x)[n];
		}
		constexpr inline T operator[](size_t n) const
		{
			Expects(n <= 1);
			return (&x)[n];
		}

		// Assignment and comparison
		constexpr inline Vector2D& operator = (Vector2D param) { x = param.x; y = param.y; return *this; }
		constexpr inline Vector2D& operator = (T param) { x = param; y = param; return *this; }
		constexpr inline bool operator == (Vector2D param) const { return x == param.x && y == param.y; }
		constexpr inline bool operator != (Vector2D param) const { return x != param.x || y != param.y; }
		constexpr inline bool operator < (Vector2D param) const { return y != param.y ? y < param.y : x < param.x; }

		// Basic algebra
		constexpr inline Vector2D operator + (Vector2D param) const { return Vector2D(x + param.x,y + param.y); }
		constexpr inline Vector2D operator - (Vector2D param) const { return Vector2D(x - param.x,y - param.y); }
		constexpr inline Vector2D operator * (Vector2D param) const { return Vector2D(x * param.x,y * param.y); }
		constexpr inline Vector2D operator / (Vector2D param) const { return Vector2D(x / param.x,y / param.y); }
		constexpr inline Vector2D operator % (Vector2D param) const { return Vector2D(mod(x, param.x), mod(y, param.y)); }

		constexpr inline Vector2D modulo(Vector2D param) const { return Vector2D(Halley::modulo<T>(x, param.x), Halley::modulo<T>(y, param.y)); }
		constexpr inline Vector2D floorDiv(Vector2D param) const { return Vector2D(Halley::floorDiv(x, param.x), Halley::floorDiv(y, param.y)); }

		constexpr inline Vector2D operator - () const { return Vector2D(-x,-y); }

		template <typename V>
		constexpr inline Vector2D operator * (V param) const { return Vector2D(T(x * param), T(y * param)); }

		template <typename V>
		constexpr inline Vector2D operator / (V param) const { return Vector2D(T(x / param), T(y / param)); }

		// In-place operations
		constexpr inline Vector2D& operator += (Vector2D param) { x += param.x; y += param.y; return *this; }
		constexpr inline Vector2D& operator -= (Vector2D param) { x -= param.x; y -= param.y; return *this; }
		constexpr inline Vector2D& operator *= (const T param) { x *= param; y *= param; return *this; }
		constexpr inline Vector2D& operator /= (const T param) { x /= param; y /= param; return *this; }

		// Get the normalized vector (unit vector)
		constexpr inline Vector2D unit () const
		{
			float len = length();
			if (len != 0) {
				return (*this) / len;
			} else {
				return Vector2D(0, 0);
			}
		}
		constexpr inline Vector2D normalized() const
		{
			return unit();
		}
		constexpr inline void normalize()
		{
			*this = unit();
		}

		// Get the orthogonal vector
		constexpr inline Vector2D orthoLeft () const { return Vector2D(-y, x); }
		constexpr inline Vector2D orthoRight () const { return Vector2D(y, -x); }

		// Cross product (the Z component of it)
		constexpr inline T cross (Vector2D param) const { return x * param.y - y * param.x; }

		// Dot product
		constexpr inline T dot (Vector2D param) const { return (x * param.x) + (y * param.y); }

		// Length
		constexpr inline T length () const { return static_cast<T>(std::sqrt(squaredLength())); }
		constexpr inline T len () const { return length(); }
		constexpr inline T manhattanLength() const { return std::abs(x) + std::abs(y); }

		// Squared length, often useful and much faster
		constexpr inline T squaredLength () const { return x*x+y*y; }

		// Projection on another vector
		inline Vector2D projection (Vector2D param) const
		{
			Vector2D unit = param.unit();
			return this->dot(unit) * unit;
		}
		inline T projectionLength (Vector2D param) const
		{
			return this->dot(param.unit());
		}

		// Rounding
		constexpr inline Vector2D floor() const { return Vector2D(std::floor(x), std::floor(y)); }
		constexpr inline Vector2D ceil() const { return Vector2D(std::ceil(x), std::ceil(y)); }
		constexpr inline Vector2D round() const { return Vector2D(std::round(x), std::round(y)); }

		// Gets the angle that this vector is pointing to
		inline U angle () const
		{
			U angle;
			angle.setRadians(atan2(y, x));
			return angle;
		}

		// Rotates vector by an angle
		inline Vector2D rotate (U angle) const
		{
			T cos, sin;
			angle.sincos(sin, cos);
			return Vector2D(x*cos - y*sin, x*sin + y*cos);
		}

		// Rotates vector by sine and cosine
		inline Vector2D rotate (T sine, T cosine) const
		{
			return Vector2D(x*cosine - y*sine, x*sine + y*cosine);
		}

		// Removes the length of the vector along the given axis
		constexpr inline Vector2D neutralize (Vector2D param) const
		{
			return *this - dot(param)*param;
		}

		String toString() const
		{
			return String("(") + x + ", " + y + ")";
		}

		constexpr Vector2D abs() const
		{
			return Vector2D(std::abs(x), std::abs(y));
		}

		constexpr inline static Vector2D<T,U> min(Vector2D<T,U> a, Vector2D<T,U> b)
		{
			return Vector2D<T,U>(std::min(a.x, b.x), std::min(a.y, b.y));
		}

		constexpr inline static Vector2D<T,U> max(Vector2D<T,U> a, Vector2D<T,U> b)
		{
			return Vector2D<T,U>(std::max(a.x, b.x), std::max(a.y, b.y));
		}
	};

	////////////////////
	// Global operators
	template <typename T,class U,typename V>
	inline Vector2D<T,U> operator * (V f, const Vector2D<T,U> &v)
	{
		return Vector2D<T,U>(T(v.x * f),T(v.y * f));
	}

	template <typename T,class U>
	std::ostream& operator<< (std::ostream& ostream, const Vector2D<T, U>& v)
	{
		ostream << "(" << v.x << "," << v.y << ")" ; return ostream;
	}

	////////////
	// Typedefs
	typedef Vector2D<double,Angle<double> > Vector2d;
	typedef Vector2D<> Vector2f;
	typedef Vector2D<int> Vector2i;
	typedef Vector2D<short> Vector2s;
	typedef Vector2D<char> Vector2c;
	typedef Vector2f Position;
	typedef Vector2f Size;
}

namespace std {
	template<>
	struct hash<Halley::Vector2i>
	{
		size_t operator()(const Halley::Vector2i& v) const
		{
			return std::hash<long long>()(*reinterpret_cast<const long long*>(&v));
		}
	};

	template<>
	struct hash<Halley::Vector2f>
	{
		size_t operator()(const Halley::Vector2f& v) const
		{
			return std::hash<long long>()(*reinterpret_cast<const long long*>(&v));
		}
	};
}


#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include "../globals.h"
#include "galaxy_cluster.h"

#define NTABLE 256
#define GSL_SPLINE gsl_interp_cspline

#define Zero 1
#define Infinity 1e25

struct HaloProperties Halo = { 0 };

void setup_GalaxyCluster()
{
    Problem.Boxsize[0] = Problem.Boxsize[1] = Problem.Boxsize[2] = 1000;  // kpc
    Halo.Rho0 = 1e-26;  // g/cm^3
    Halo.Beta = 2.0 / 3.0;
    Halo.Rcore = 20;    // kpc
    Halo.R_Sample = Problem.Boxsize[0];

    sprintf ( Problem.Name, "IC_GalaxyCluster" );

    Problem.Rho_Max = Halo.Rho0;

    Density_Func_Ptr = &GalaxyCluster_Density;
    U_Func_Ptr = &GalaxyCluster_U;
    Velocity_Func_Ptr = &GalaxyCluster_Velocity;
}

float betamodel ( const float r )
{
    float rho0 = Halo.Rho0;
    float beta = Halo.Beta;
    float rc = Halo.Rcore;

    return rho0 * pow ( 1 + p2 ( r / rc ), -1.5 * beta );
}

float GalaxyCluster_Density ( const int ipart , const double bias )
{
    const float x = P[ipart].Pos[0] - 0.5 * Problem.Boxsize[0];
    const float y = P[ipart].Pos[1] - 0.5 * Problem.Boxsize[1];;
    const float z = P[ipart].Pos[2] - 0.5 * Problem.Boxsize[2];;

    const float r = sqrt ( x * x + y * y + z * z );

    return betamodel ( r );
}

void GalaxyCluster_Velocity ( const int ipart, float out[3] )
{
    out[0] = 0.0;
    out[1] = 0.0;
    out[2] = 0.0;
}

float GalaxyCluster_U ( const int ipart )
{
    return 0;
}


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

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_const.h>
#include <gsl/gsl_test.h>

#include <gsl/gsl_ieee_utils.h>

int
main (void)
{
  gsl_ieee_env_setup ();

  /* Basic check to make sure the header files are functioning */

  {
    double c = GSL_CONST_MKS_SPEED_OF_LIGHT;
    double eps = GSL_CONST_MKS_VACUUM_PERMITTIVITY;
    double mu = GSL_CONST_MKS_VACUUM_PERMEABILITY;

    gsl_test_rel (c, 1.0/sqrt(eps*mu), 1e-6, "speed of light (mks)");
  }

  {
    double ly = GSL_CONST_CGS_LIGHT_YEAR;
    double c = GSL_CONST_CGS_SPEED_OF_LIGHT;
    double y = 365.2425 * GSL_CONST_CGS_DAY;

    gsl_test_rel (ly, c * y, 1e-6, "light year (cgs)");
  }

  {
    double c = GSL_CONST_MKSA_SPEED_OF_LIGHT;
    double eps = GSL_CONST_MKSA_VACUUM_PERMITTIVITY;
    double mu = GSL_CONST_MKSA_VACUUM_PERMEABILITY;

    gsl_test_rel (c, 1.0/sqrt(eps*mu), 1e-6, "speed of light (mksa)");
  }

  {
    double ly = GSL_CONST_CGSM_LIGHT_YEAR;
    double c = GSL_CONST_CGSM_SPEED_OF_LIGHT;
    double y = 365.2425 * GSL_CONST_CGSM_DAY;

    gsl_test_rel (ly, c * y, 1e-6, "light year (cgsm)");
  }

  {
    double micro = GSL_CONST_NUM_MICRO;
    double mega = GSL_CONST_NUM_MEGA;
    double kilo = GSL_CONST_NUM_KILO;

    gsl_test_rel (mega/kilo, 1/(micro*kilo), 1e-10, "kilo (mega/kilo, 1/(micro*kilo))");
  }

  {
    double d = GSL_CONST_MKSA_DEBYE;
    double c = GSL_CONST_MKSA_SPEED_OF_LIGHT;
    double desu = d * c * 1000.0;

    gsl_test_rel (desu, 1e-18, 1e-10, "debye (esu)");
  }

  {
    double k = GSL_CONST_MKSA_BOLTZMANN;
    double c = GSL_CONST_MKSA_SPEED_OF_LIGHT;
    double h = GSL_CONST_MKSA_PLANCKS_CONSTANT_H;
    double s = 2 * pow(M_PI, 5.0) * pow(k, 4.0) / (15 * pow(c, 2.0) * pow(h, 3.0));
    double sigma = GSL_CONST_MKSA_STEFAN_BOLTZMANN_CONSTANT;

    gsl_test_rel(s, sigma, 1e-10, "stefan boltzmann constant");
  }

  exit (gsl_test_summary ());
}


/* =======================================
 * Code for 8.06 Term Paper (Spring 2015)
 *
 * Author : To Chin Yu (ytc@mit.edu)
 * Purpose: 1D lattice simulation
 * V(x) = alpha*x^2 + lambda*x^4
 * =======================================
 */
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#define sq(x) pow((x),2.0)
#define cb(x) pow((x),3.0)
#define qd(x) pow((x),4.0)

// GSL Random Number Generator (https://www.gnu.org/software/gsl/)
#include <gsl/gsl_rng.h>
const gsl_rng_type * T; gsl_rng * r;

// Parameters
#define N_lat   5000  // Lattice size
#define a       0.1   // Lattice spacing
#define N_therm 500   // Thermalization
#define N_meas  1000  // Number of configuration to measure
#define N_skip  100   // Decorrelation
#define alpha   0     // Strength of potential (harmonic)
#define lambda  1     // Strength of potential (anharmonic)
#define epsilon 0.2   // Size of fluctuation

#define N_bin 1000
#define dx 0.01
#define x_range 5.0 // x in [-range, range]

// Globals
double x[N_lat];      // Lattice
int acc,tot;          // Acceptance rate = acc/tot
double E0;            // Ground state energy
double C[N_lat];      // Correlator
double psi[N_bin];    // Wavefunction

// Output Files
FILE *fE0, *fCorRatio, *fpsi;

// Prototypes
void setup();
void cleanup();
void update();

int main(void){
  setup();

  // Thermalize
  for(int n=0;n<N_therm;n++){
    update();

    // Plot E0 during thermalization
    if(0){
      E0=0;
      for(int i=0;i<N_lat;i++){
        E0 += ( 2*x[i]*lambda*cb(x[i]) + lambda*qd(x[i]) )/N_lat;
      }
      fprintf(fE0,"%f\n", E0);
    }
  }

  // Do updates and measurements
  for(int n=0;n<N_meas;n++){
    // Decorrelate
    for(int m=0;m<N_skip;m++){ update(); }
    // Measurements:
    // 1. E0

    // Plot E0
    E0=0;
    for(int i=0;i<N_lat;i++){
      E0 += ( 2*x[i]*lambda*cb(x[i]) + lambda*qd(x[i]) )/N_lat;
    }
    fprintf(fE0,"%f\n", E0);

    // 2. Correlator
    for(int i=0;i<N_lat;i++){
      for(int T=0;T<N_lat;T++){
        C[T] += x[i]*x[(i+T)%N_lat] / (N_lat*N_meas);
      }
    }

    // 3. Wavefunction
    for(int i=0;i<N_lat;i++){
      if(abs(x[i])<x_range){
        psi[(int)floor((x[i]+x_range)/dx)]+=1;
      }
    }

  }

  // Plot correlator ratio
  for(int T=0;T<N_lat;T++){
    fprintf(fCorRatio,"%f \n", C[T]/C[(T+1)%N_lat]); // for large T, this gives a(E1-E0)
  }

  // Plot wavefunction
  for(int i=0;i<N_bin;i++){
    fprintf(fpsi,"%f, %f\n", i*dx-x_range, sqrt(psi[i]/(dx*N_meas*N_lat)));
  }

  printf("Acceptance rate: %f \n", (double)acc/tot);

  cleanup();
  return 0;
}

// Calculate difference in action
double dS(int i, double x_old){
  return (x[i]*(x[i]-x[(i+1)%N_lat]-x[(i-1+N_lat)%N_lat])/a
          + alpha*a*sq(x[i]) + lambda*a*qd(x[i]))
          - (x_old*(x_old-x[(i+1)%N_lat]-x[(i-1+N_lat)%N_lat])/a
               + alpha*a*sq(x_old) + lambda*a*qd(x_old));
}

void update(){
  double x_old;
  double d; // Difference in action
  for(int i=0;i<N_lat;i++){
    x_old=x[i];
    x[i]=x[i] + epsilon * (gsl_rng_uniform(r)-0.5)*2;
    acc++; tot++;
    d=dS(i,x_old);
    if(d>0){
      if(exp(-d)<gsl_rng_uniform(r)){ x[i]=x_old; acc--; } // Revert to old value
    }
  }
}

void setup(){
  // Cold start
  for(int i=0;i<N_lat;i++){ x[i]=0; C[i]=0; }

  // Initialize variables
  acc = 0;
  tot = 0;
  E0 = 0;
  for(int i=0;i<N_bin;i++){ psi[i]=0; }

  // Create Files
  fE0=fopen("E0.txt", "w+");
  fCorRatio=fopen("CorRatio.txt", "w+");
  fpsi=fopen("psi.txt", "w+");

  // Setup random environment
  gsl_rng_env_setup();
  T = gsl_rng_mt19937;
  r = gsl_rng_alloc (T);
}

void cleanup(){
  gsl_rng_free (r);

  // Close Files
  fclose(fE0);
  fclose(fCorRatio);
  fclose(fpsi);
}


#include <gsl/gsl_rng.h>
#include <gsl/gsl_sort_double.h>

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

  size_t i, k = 5, N = 100000;

  double * x = malloc (N * sizeof(double));
  double * small = malloc (k * sizeof(double));

  gsl_rng_env_setup();

  T = gsl_rng_default;
  r = gsl_rng_alloc (T);

  for (i = 0; i < N; i++)
    {
      x[i] = gsl_rng_uniform(r);
    }

  gsl_sort_smallest (small, k, x, 1, N);

  printf ("%d smallest values from %d\n", k, N);

  for (i = 0; i < k; i++)
    {
      printf ("%d: %.18f\n", i, small[i]);
    }

  free (x);
  free (small);
  gsl_rng_free (r);
  return 0;
}


/*
 * Gsl_blas_dgemm_matrix_view: test gsl_blas_dgemm (matrix . matrix)
 *
 * Copyright (c) 2012 Jérémie Decock
 *
 * Required: GSL library (libgsl0-dev)
 * Usage: gcc gsl_blas_dgemm_matrix_view.c -lgsl -lgslcblas -lm
 *    or: gcc gsl_blas_dgemm_matrix_view.c $(pkg-config --libs gsl)
 */

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

int main (void)
{
    double a[] = { 0.11, 0.12, 0.13,
                   0.21, 0.22, 0.23 };

    double b[] = { 1011, 1012,
                   1021, 1022,
                   1031, 1032 };

    double c[] = { 0.00, 0.00,
                   0.00, 0.00 };

    gsl_matrix_view A = gsl_matrix_view_array(a, 2, 3);
    gsl_matrix_view B = gsl_matrix_view_array(b, 3, 2);
    gsl_matrix_view C = gsl_matrix_view_array(c, 2, 2);

    /* Compute C = A B */

    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans,
                   1.0, &A.matrix, &B.matrix,
                   0.0, &C.matrix);

    printf("[ %g, %g\n", c[0], c[1]);
    printf("  %g, %g ]\n", c[2], c[3]);

    return 0;
}


#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

struct data
{
  double *t;
  double *y;
  size_t n;
};

/* model function: a * exp( -1/2 * [ (t - b) / c ]^2 ) */
double
gaussian(const double a, const double b, const double c, const double t)
{
  const double z = (t - b) / c;
  return (a * exp(-0.5 * z * z));
}

int
func_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  struct data *d = (struct data *) params;
  double a = gsl_vector_get(x, 0);
  double b = gsl_vector_get(x, 1);
  double c = gsl_vector_get(x, 2);
  size_t i;

  for (i = 0; i < d->n; ++i)
    {
      double ti = d->t[i];
      double yi = d->y[i];
      double y = gaussian(a, b, c, ti);

      gsl_vector_set(f, i, yi - y);
    }

  return GSL_SUCCESS;
}

int
func_df (const gsl_vector * x, void *params, gsl_matrix * J)
{
  struct data *d = (struct data *) params;
  double a = gsl_vector_get(x, 0);
  double b = gsl_vector_get(x, 1);
  double c = gsl_vector_get(x, 2);
  size_t i;

  for (i = 0; i < d->n; ++i)
    {
      double ti = d->t[i];
      double zi = (ti - b) / c;
      double ei = exp(-0.5 * zi * zi);

      gsl_matrix_set(J, i, 0, -ei);
      gsl_matrix_set(J, i, 1, -(a / c) * ei * zi);
      gsl_matrix_set(J, i, 2, -(a / c) * ei * zi * zi);
    }

  return GSL_SUCCESS;
}

int
func_fvv (const gsl_vector * x, const gsl_vector * v,
          void *params, gsl_vector * fvv)
{
  struct data *d = (struct data *) params;
  double a = gsl_vector_get(x, 0);
  double b = gsl_vector_get(x, 1);
  double c = gsl_vector_get(x, 2);
  double va = gsl_vector_get(v, 0);
  double vb = gsl_vector_get(v, 1);
  double vc = gsl_vector_get(v, 2);
  size_t i;

  for (i = 0; i < d->n; ++i)
    {
      double ti = d->t[i];
      double zi = (ti - b) / c;
      double ei = exp(-0.5 * zi * zi);
      double Dab = -zi * ei / c;
      double Dac = -zi * zi * ei / c;
      double Dbb = a * ei / (c * c) * (1.0 - zi*zi);
      double Dbc = a * zi * ei / (c * c) * (2.0 - zi*zi);
      double Dcc = a * zi * zi * ei / (c * c) * (3.0 - zi*zi);
      double sum;

      sum = 2.0 * va * vb * Dab +
            2.0 * va * vc * Dac +
                  vb * vb * Dbb +
            2.0 * vb * vc * Dbc +
                  vc * vc * Dcc;

      gsl_vector_set(fvv, i, sum);
    }

  return GSL_SUCCESS;
}

void
callback(const size_t iter, void *params,
         const gsl_multifit_nlinear_workspace *w)
{
  gsl_vector *f = gsl_multifit_nlinear_residual(w);
  gsl_vector *x = gsl_multifit_nlinear_position(w);
  double avratio = gsl_multifit_nlinear_avratio(w);
  double rcond;

  (void) params; /* not used */

  /* compute reciprocal condition number of J(x) */
  gsl_multifit_nlinear_rcond(&rcond, w);

  fprintf(stderr, "iter %2zu: a = %.4f, b = %.4f, c = %.4f, |a|/|v| = %.4f cond(J) = %8.4f, |f(x)| = %.4f\n",
          iter,
          gsl_vector_get(x, 0),
          gsl_vector_get(x, 1),
          gsl_vector_get(x, 2),
          avratio,
          1.0 / rcond,
          gsl_blas_dnrm2(f));
}

void
solve_system(gsl_vector *x, gsl_multifit_nlinear_fdf *fdf,
             gsl_multifit_nlinear_parameters *params)
{
  const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust;
  const size_t max_iter = 200;
  const double xtol = 1.0e-8;
  const double gtol = 1.0e-8;
  const double ftol = 1.0e-8;
  const size_t n = fdf->n;
  const size_t p = fdf->p;
  gsl_multifit_nlinear_workspace *work =
    gsl_multifit_nlinear_alloc(T, params, n, p);
  gsl_vector * f = gsl_multifit_nlinear_residual(work);
  gsl_vector * y = gsl_multifit_nlinear_position(work);
  int info;
  double chisq0, chisq, rcond;

  /* initialize solver */
  gsl_multifit_nlinear_init(x, fdf, work);

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

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

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

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

  gsl_vector_memcpy(x, y);

  /* print summary */

  fprintf(stderr, "NITER         = %zu\n", gsl_multifit_nlinear_niter(work));
  fprintf(stderr, "NFEV          = %zu\n", fdf->nevalf);
  fprintf(stderr, "NJEV          = %zu\n", fdf->nevaldf);
  fprintf(stderr, "NAEV          = %zu\n", fdf->nevalfvv);
  fprintf(stderr, "initial cost  = %.12e\n", chisq0);
  fprintf(stderr, "final cost    = %.12e\n", chisq);
  fprintf(stderr, "final x       = (%.12e, %.12e, %12e)\n",
          gsl_vector_get(x, 0), gsl_vector_get(x, 1), gsl_vector_get(x, 2));
  fprintf(stderr, "final cond(J) = %.12e\n", 1.0 / rcond);

  gsl_multifit_nlinear_free(work);
}

int
main (void)
{
  const size_t n = 300;  /* number of data points to fit */
  const size_t p = 3;    /* number of model parameters */
  const double a = 5.0;  /* amplitude */
  const double b = 0.4;  /* center */
  const double c = 0.15;  /* width */
  const gsl_rng_type * T = gsl_rng_default;
  gsl_vector *f = gsl_vector_alloc(n);
  gsl_vector *x = gsl_vector_alloc(p);
  gsl_multifit_nlinear_fdf fdf;
  gsl_multifit_nlinear_parameters fdf_params =
    gsl_multifit_nlinear_default_parameters();
  struct data fit_data;
  gsl_rng * r;
  size_t i;

  gsl_rng_env_setup ();
  r = gsl_rng_alloc (T);

  fit_data.t = malloc(n * sizeof(double));
  fit_data.y = malloc(n * sizeof(double));
  fit_data.n = n;

  /* generate synthetic data with noise */
  for (i = 0; i < n; ++i)
    {
      double t = (double)i / (double) n;
      double y0 = gaussian(a, b, c, t);
      double dy = gsl_ran_gaussian (r, 0.1 * y0);

      fit_data.t[i] = t;
      fit_data.y[i] = y0 + dy;
    }

  /* define function to be minimized */
  fdf.f = func_f;
  fdf.df = func_df;
  fdf.fvv = func_fvv;
  fdf.n = n;
  fdf.p = p;
  fdf.params = &fit_data;

  /* starting point */
  gsl_vector_set(x, 0, 1.0);
  gsl_vector_set(x, 1, 0.0);
  gsl_vector_set(x, 2, 1.0);

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

  /* print data and model */
  {
    double A = gsl_vector_get(x, 0);
    double B = gsl_vector_get(x, 1);
    double C = gsl_vector_get(x, 2);

    for (i = 0; i < n; ++i)
      {
        double ti = fit_data.t[i];
        double yi = fit_data.y[i];
        double fi = gaussian(A, B, C, ti);

        printf("%f %f %f\n", ti, yi, fi);
      }
  }

  gsl_vector_free(f);
  gsl_vector_free(x);
  gsl_rng_free(r);

  return 0;
}


/* randist/chisq.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 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 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* The chisq distribution has the form

   p(x) dx = (1/(2*Gamma(nu/2))) (x/2)^(nu/2 - 1) exp(-x/2) dx

   for x = 0 ... +infty */

double
gsl_ran_chisq (const gsl_rng * r, const double nu)
{
  double chisq = 2 * gsl_ran_gamma (r, nu / 2, 1.0);
  return chisq;
}

double
gsl_ran_chisq_pdf (const double x, const double nu)
{
  if (x < 0)
    {
      return 0 ;
    }
  else
    {
      if(nu == 2.0)
        {
          return exp(-x/2.0) / 2.0;
        }
      else
        {
          double p;
          double lngamma = gsl_sf_lngamma (nu / 2);

          p = exp ((nu / 2 - 1) * log (x/2) - x/2 - lngamma) / 2;
          return p;
        }
    }
}


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

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

/**
 * Spline wrapper
 * Used to take care of evaluations outside the supported range
 */
typedef struct {
  gsl_interp_accel *intacc; //GSL spline
  gsl_spline *spline;
  double x0,xf; //Interpolation limits
  double y0,yf; //Constant values to use beyond interpolation limit
} SplPar;

SplPar *ccl_spline_init(int n,double *x,double *y,double y0,double yf);

double ccl_spline_eval(double x,SplPar *spl);

void ccl_spline_free(SplPar *spl);

CCL_END_DECLS

#endif


#ifndef __calcAinvb__
#define __calcAinvb__

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix_double.h>
//#include <gsl/gsl_multimin.h>
#include <gsl/gsl_multiroots.h>
//#include "my_misc.c"
//#define GSL 1
#define GSL 1
int GSLtag;
void least_gsl_error_handler( const char * reason,
			      const char * file,
			      int line,
			      int gsl_errno)
{
  //  printf("%s:%d: %s (gsl_error: %s)\n",
  //	 file, line, reason, gsl_strerror(gsl_errno));
  printf("%s:%d: %s (gsl_error %d)\n",
	 file, line, reason,gsl_errno);
  GSLtag=1;
}
double VarY;
void calc_Ainvb(double *M, double *A_data, double *b_data, int nx, int ndata,int nxmarume)
{
  /* b= A M  --> M= Ainv b
     b  in R^{ndata x 1}
     A  in R^{ndata x nx}   (ndata>nx)
     M  in R^{nx    x 1}
   */
  int i;
  gsl_vector *S;
  gsl_vector *work;
  gsl_vector *x;
  gsl_vector *b;
  gsl_matrix *V;
  gsl_matrix *A;
  if(ndata<=0){
    M[0]=1;
    for(i=1;i<nx;i++) M[i]=1./nx;
  }
  else{
    A =gsl_matrix_alloc(ndata,nx);
    b =gsl_vector_alloc(ndata);b->data=b_data;
    A->data=A_data;
    //  for(i=0;i<ndata;i++){printf("%e %e %d #bi,Ai\n",A->data[i],b->data[i],i); }

    V = gsl_matrix_alloc (nx,nx);
    S = gsl_vector_alloc (nx);
    work = gsl_vector_alloc(nx);
    x = gsl_vector_alloc(nx);

    gsl_set_error_handler( &least_gsl_error_handler );GSLtag=0;

#if GSL == 1
    gsl_linalg_SV_decomp_jacobi (A, V, S); //higer accuracy than Golub-Reinsh
#elif GSL == 2
    gsl_linalg_SV_decomp (A, V, S, work); //Golub-Reinsh
#elif GSL == 3
    {//faster
      gsl_matrix *X;
      X = gsl_matrix_alloc (nx,nx);
      gsl_linalg_SV_decomp_mod (A, X, V, S, work);
      gsl_matrix_free(X);
    }
#endif
    //    for(i=nxmarume;i<nx;i++){

    fprintf(stdout,"Lambda");
    for(i=0;i<nx;i++){
      //      if(nx>40) break;
      //      fprintf(stdout,"S->data[%d]=%e;",i,S->data[i]);
      fprintf(stdout,"(%d)%e",i,S->data[i]);
    }
    fprintf(stdout,"\n");
    VarY=0;
    if(GSLtag==1){
      fprintf(stderr,"\nEigenValue=");for(i=0;i<nx;i++) fprintf(stderr,"%e ",S->data[i]); fprintf(stderr,"\n");
      for(i=0;i<nx;i++) M[i]=1./nx;
    }
    else{
      if(nxmarume>=0){//Principal Component Analysis
	for(i=nxmarume;i<nx;i++) S->data[i]=0;
	for(i=0;i<nxmarume;i++) VarY+=S->data[i];
	gsl_linalg_SV_solve(A,V,S,b,x);
	for(i=0;i<nx;i++) M[i]=gsl_vector_get(x,i);
	//    {//check
	//      double err=0,yhat;
	//      int j;
	//      for(i=0;i<ndata;i++){
	//	if(i>10) break;
	//	yhat=b_data[i];
	//	for(j=0;j<nx;j++){
	//	  yhat-= M[j]*A_data[i*nx+j];
	//	}
	//	err+=square(yhat);
	//	fprintf(stderr,"err=%e=%e/%d. yhat=%e=(%e",err/(i+1.),err,i,yhat,b_data[i]);
	//	for(j=0;j<nx;j++) fprintf(stderr,"-(%e)*(%e)",M[j],A_data[i*nx+j]);
	//	fprintf(stderr,")^2\n");
	//      }
	//    }
      }
      else{//Minor Component Analysis
	int ii=0;
	int im=-nxmarume;
	for(i=nx-1;i>=0;i--){
	  if(S->data[i]>1e-20){
	    ii++;
	    VarY+=S->data[i];
	    if(ii>=im) break;
	  }
	}
	int j;

	double M0=-V->data[i];
	for(j=1;j<nx;j++){
	  M[j-1]=V->data[j*nx+i]/M0;
	}
//	double M0=-V->data[i*nx];
//	for(j=1;j<nx;j++){
//	  M[j-1]=V->data[i*nx+j]/M0;
//	}

      }
    }
    gsl_vector_free(S);
    gsl_vector_free(work);
    gsl_vector_free(x);
    gsl_matrix_free(V);
    gsl_matrix_free(A);
    gsl_vector_free(b);
  }
}
void calc_AtinvbMCA(double *M, double *At_data, double *b_data, int nx, int ndata,int nxmarume)
{
  /* b= A M  --> M= Ainv b
     b  in R^{ndata x 1}
     A=At  in R^{ndata x nx}
     M  in R^{nx    x 1}
   */
  int nx1=nx+1,i,j,jnx1;
  gsl_matrix *A  =gsl_matrix_alloc(ndata,nx1);
  for(j=0;j<ndata;j++){
    jnx1=j*nx1;
    A->data[jnx1]=b_data[j];
    for(i=0;i<nx;i++){
      A->data[jnx1+i+1]=At_data[i*ndata+j];
    }
  }
  calc_Ainvb(M, (double *)(A->data), b_data, nx1, ndata, -nxmarume);

  gsl_matrix_free(A);
}
void calc_Atinvb(double *M, double *At_data, double *b_data, int nx, int ndata,int nxmarume)
{
  /* b= A M  --> M= Ainv b
     b  in R^{ndata x 1}
     A=At  in R^{ndata x nx}
     M  in R^{nx    x 1}
   */
  gsl_matrix *At =gsl_matrix_alloc(nx,ndata);
  gsl_matrix *A  =gsl_matrix_alloc(ndata,nx);
  At->data=At_data;
  gsl_matrix_transpose_memcpy(A, At);
  calc_Ainvb(M, (double *)(A->data), b_data, nx, ndata, nxmarume);
  gsl_matrix_free(A);
  gsl_matrix_free(At);
}

#endif // __calcAinvb__


/* -*- Mode: c++ -*- */
// copyright (c) 2006-2010 by Christos Dimitrakakis
// <christos.dimitrakakis@gmail.com>
/***************************************************************************
 *                                                                         *
 *   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.                                   *
 *                                                                         *
 ***************************************************************************/

#ifndef MATRIX_H
#define MATRIX_H

#include <gsl/gsl_cblas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <exception>
#include <iostream>
#include <stdexcept>
#include <vector>
#include "Vector.h"
#include "real.h"
//#include <atlas/cblas.h>

#undef REFERENCE_ACCESS

#define ACCURACY_LIMIT 1e-12

/** \brief An n-by-m dimensional matrix.

    TODO: Use the BLAS interface for some / most routines.
 */
class Matrix {
 public:
  /// Check bounds?
  enum BoundsCheckingStatus { NO_CHECK_BOUNDS = 0, CHECK_BOUNDS = 1 };
#ifdef NDEBUG
  static Matrix Null(int rows, int columns,
                     enum BoundsCheckingStatus check = NO_CHECK_BOUNDS);
  static Matrix Unity(int rows, int columns,
                      enum BoundsCheckingStatus check = NO_CHECK_BOUNDS);

  Matrix(int rows = 1, int columns = 1,
         enum BoundsCheckingStatus check = NO_CHECK_BOUNDS);
  Matrix(int rows, int columns, real* y,
         enum BoundsCheckingStatus check = NO_CHECK_BOUNDS);
  explicit Matrix(const Vector& v,
                  enum BoundsCheckingStatus check_ = NO_CHECK_BOUNDS);
#else
  static Matrix Null(int rows, int columns,
                     enum BoundsCheckingStatus check = CHECK_BOUNDS);
  static Matrix Unity(int rows, int columns,
                      enum BoundsCheckingStatus check = CHECK_BOUNDS);

  Matrix(int rows = 1, int columns = 1,
         enum BoundsCheckingStatus check = CHECK_BOUNDS);
  Matrix(int rows, int columns, real* y,
         enum BoundsCheckingStatus check = CHECK_BOUNDS);
  explicit Matrix(const Vector& v,
                  enum BoundsCheckingStatus check_ = CHECK_BOUNDS);
#endif
  Matrix(const Matrix& rhs, bool clone = true);
  ~Matrix();
  void Resize(int rows_, int columns_);
  Matrix AddRow(const Vector& rhs);
  Matrix AddColumn(const Vector& rhs);
  Matrix& operator=(const Matrix& rhs);
  bool operator==(const Matrix& rhs) const;
  bool operator!=(const Matrix& rhs) const;
  Matrix operator+(const Matrix& rhs);
  Matrix& operator+=(const Matrix& rhs);
  Matrix operator-(const Matrix& rhs);
  Matrix& operator-=(const Matrix& rhs);
  Matrix& operator*=(const real& rhs);
  Matrix operator*(const Matrix& rhs);
  Matrix operator*(const real& rhs);
  Matrix operator+(const real& rhs);
  Matrix operator-(const real& rhs);
  Matrix operator/(const real& rhs);
  /// Matrix inversion (defaults to GSL with LU)
  Matrix Inverse(real epsilon = ACCURACY_LIMIT) const {
    return GSL_Inverse();
    // return Inverse_LU();
  }
  Vector SVD_Solve(const Vector& b) const;

  Matrix GSL_Inverse() const;

  /** Matrix inversion using the Cholesky decomposition.

      First call Cholesky with accuracy epsilon to obtain $\fL,
      L'\f$ such that \f$A = LL'\f$.  Then solve for \f$X = A^{-1}\f$:
      \f[
      LL'X = I,
      \f]
      by dynamic programming.
  */
  Matrix Inverse_Cholesky(real epsilon = ACCURACY_LIMIT) const {
    Matrix U = Cholesky(epsilon);
    Matrix L(U, false);
    L.Transpose();
    return Inverse(L, U);
  }
  /** Matrix inversion using the LU decomposition.

      First call LUDecomposition with accuracy epsilon to obtain $\fL,
      U\f$ such that \f$A = LU\f$.  Then solve for \f$X = A^{-1}\f$:
      \f[
      LUX = I,
      \f]
      by dynamic programming.
  */
  Matrix Inverse_LU(real epsilon = ACCURACY_LIMIT) const {
    real det;
    Matrix tmp(*this);
    std::vector<Matrix> A = tmp.LUDecomposition(det, epsilon);
    return Inverse(A[0], A[1]);
  }

  Matrix Inverse(const Matrix& L, const Matrix& U) const;
  bool isSymmetric() const;
  bool isTriangular() const;
  bool isUpperTriangular() const;
  bool isLowerTriangular() const;
  real det() const;
  real tr() const;
  real L1Norm() const;
  real L2Norm() const;
  Matrix Multiple(const Matrix& rhs) const;
  Matrix Kron(const Matrix& rhs) const;
  real ColumnSum(int c) const;
  real RowSum(int r) const;
  real compute_det_triangular() const;
  real gaussian_elimination_forward(real epsilon = ACCURACY_LIMIT);
  std::vector<Matrix> QRDecomposition() const;
  std::vector<Matrix> LUDecomposition(real& determinant,
                                      real epsilon = ACCURACY_LIMIT);
  Matrix Cholesky(real epsilon = ACCURACY_LIMIT) const;
  void Cholesky(Matrix& chol, real epsilon = ACCURACY_LIMIT) const;
  void Clear();
  void Transpose();
  Vector getColumn(int c) const;
  Vector getRow(int r) const;
  void setColumn(int c, const Vector& x);
  void setRow(int r, const Vector& x);
  void SortRow(int r);
  void SortColumn(int r);
  int Rows() const;
  int Columns() const;
  Vector Vec() const;
  void Vec(const Vector& x);
  Vector RowMax() const;
  Vector ColumnMax() const;
  Vector RowMin() const;
  Vector ColumnMin() const;
  real& operator()(int i, int j);
  const real& operator()(int i, int j) const;
  void print(FILE* f) const;
  friend Matrix operator*(const real& lhs, const Matrix& rhs);
  friend Matrix operator*(const Vector& lhs, const Matrix& rhs);
  friend Vector operator*(const Matrix& lhs, const Vector& rhs);

 protected:
  int rows;     ///< number of rows in the matrix
  int columns;  ///< number of columns in the matrix
  real* x;      ///< data
#ifdef REFERENCE_ACCESS
  real** x_list;  ///< data pointers
  void MakeReferences();
#endif
  const enum BoundsCheckingStatus checking_bounds;
  bool transposed;
  bool clear_data;
  const real& qGet(int i, int j);
  void qSet(int i, int j, real v);
};

Matrix operator*(const real& lhs, const Matrix& rhs);
Matrix operator*(const Vector& lhs, const Matrix& rhs);
Vector operator*(const Matrix& lhs, const Vector& rhs);
real Mahalanobis2(const Vector& x, const Matrix& A, const Vector& y);

/// Kronecker product
inline Matrix Kron(const Matrix& lhs, const Matrix& rhs) {
  return lhs.Kron(rhs);
}

inline Matrix OuterProduct(const Vector& lhs, const Vector& rhs) {
  Matrix R(lhs.n, rhs.n);
  MatrixProduct(lhs, rhs, R);
  return R;
}

Matrix Transpose(const Matrix& rhs);

/// In-place transpose matrix
inline void Matrix::Transpose() { transposed = !transposed; }

inline int Matrix::Rows() const {
  if (transposed) {
    return columns;
  }
  return rows;
}

inline int Matrix::Columns() const {
  if (transposed) {
    return rows;
  }
  return columns;
}

inline real& Matrix::operator()(int i, int j) {
  if (transposed) {
    int tmp = i;
    i = j;
    j = tmp;
  }

  if (checking_bounds) {
    if (i < 0 || j < 0 || i >= rows || j >= columns) {
      fprintf(stderr, "bad access: (%d, %d) on a %d x %d matrix\n", i, j, rows,
              columns);
      fflush(stderr);
      throw std::out_of_range("matrix index out of range");
    }
  }
#ifdef REFERENCE_ACCESS
  return x_list[i][j];
#else
  return x[i * columns + j];
#endif
}

inline const real& Matrix::operator()(int i, int j) const {
  if (transposed) {
    int tmp = i;
    i = j;
    j = tmp;
  }

  if (checking_bounds) {
    if (i < 0 || j < 0 || i >= rows || j >= columns) {
      fprintf(stderr, "bad access: (%d, %d) on a %d x %d matrix\n", i, j, rows,
              columns);
      fflush(stderr);
      throw std::out_of_range("matrix index out of range");
    }
  }
#ifdef REFERENCE_ACCESS
  return x_list[i][j];
#else
  return x[i * columns + j];
#endif
}

#if 0
inline const real& Matrix::qGet(int i, int j) {
#ifdef REFERENCE_ACCESS
  return x_list[i][j];
#else
  return x[i * columns + j];
#endif
}

inline void Matrix::qSet(int i, int j, real v) {
#ifdef REFERENCE_ACCESS
  x_list[i][j] = v;
#else
  x[i * columns + j] = v;
#endif
}
#endif

#endif


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

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_elljac.h>

/* GJ: See [Thompson, Atlas for Computing Mathematical Functions] */

/* BJG 2005-07: New algorithm based on Algorithm 5 from Numerische
   Mathematik 7, 78-90 (1965) "Numerical Calculation of Elliptic
   Integrals and Elliptic Functions" R. Bulirsch.

   Minor tweak is to avoid division by zero when sin(x u_l) = 0 by
   computing reflected values sn(K-u) cn(K-u) dn(K-u) and using
   transformation from Abramowitz & Stegun table 16.8 column "K-u"*/

int
gsl_sf_elljac_e(double u, double m, double * sn, double * cn, double * dn)
{
  if(fabs(m) > 1.0) {
    *sn = 0.0;
    *cn = 0.0;
    *dn = 0.0;
    GSL_ERROR ("|m| > 1.0", GSL_EDOM);
  }
  else if(fabs(m) < 2.0*GSL_DBL_EPSILON) {
    *sn = sin(u);
    *cn = cos(u);
    *dn = 1.0;
    return GSL_SUCCESS;
  }
  else if(fabs(m - 1.0) < 2.0*GSL_DBL_EPSILON) {
    *sn = tanh(u);
    *cn = 1.0/cosh(u);
    *dn = *cn;
    return GSL_SUCCESS;
  }
  else {
    int status = GSL_SUCCESS;
    const int N = 16;
    double mu[16];
    double nu[16];
    double c[16];
    double d[16];
    double sin_umu, cos_umu, t, r;
    int n = 0;

    mu[0] = 1.0;
    nu[0] = sqrt(1.0 - m);

    while( fabs(mu[n] - nu[n]) > 4.0 * GSL_DBL_EPSILON * fabs(mu[n]+nu[n])) {
      mu[n+1] = 0.5 * (mu[n] + nu[n]);
      nu[n+1] = sqrt(mu[n] * nu[n]);
      ++n;
      if(n >= N - 1) {
        status = GSL_EMAXITER;
        break;
      }
    }

    sin_umu = sin(u * mu[n]);
    cos_umu = cos(u * mu[n]);

    /* Since sin(u*mu(n)) can be zero we switch to computing sn(K-u),
       cn(K-u), dn(K-u) when |sin| < |cos| */

    if (fabs(sin_umu) < fabs(cos_umu))
      {
        t = sin_umu / cos_umu;

        c[n] = mu[n] * t;
        d[n] = 1.0;

        while(n > 0) {
          n--;
          c[n] = d[n+1] * c[n+1];
          r = (c[n+1] * c[n+1]) / mu[n+1];
          d[n] = (r + nu[n]) / (r + mu[n]);
          }

        *dn = sqrt(1.0-m) / d[n];
        *cn = (*dn) * GSL_SIGN(cos_umu) / gsl_hypot(1.0, c[n]);
        *sn = (*cn) * c[n] /sqrt(1.0-m);
      }
    else
      {
        t = cos_umu / sin_umu;

        c[n] = mu[n] * t;
        d[n] = 1.0;

        while(n > 0) {
          --n;
          c[n] = d[n+1] * c[n+1];
          r = (c[n+1] * c[n+1]) / mu[n+1];
          d[n] = (r + nu[n]) / (r + mu[n]);
        }

        *dn = d[n];
        *sn = GSL_SIGN(sin_umu) / gsl_hypot(1.0, c[n]);
        *cn = c[n] * (*sn);
      }

    return status;
  }
}


#ifndef AVERAGER_H_
#define AVERAGER_H_

#include <gsl/gsl_statistics_double.h>
#include <string.h>
#include <iostream>

class Averager
{
public:
    virtual ~Averager() {};
    virtual void reset() = 0;
    virtual void feed(const double x) = 0;
    virtual double get_mean() const = 0;
    virtual double get_var() const = 0;
    virtual unsigned int get_n() const = 0;
};

class SimpleAverager: public Averager
{
public:
    SimpleAverager():
        _n(0), _sum(0), _sum_of_squares(0) {}
    virtual void reset() { _n = 0; _sum = 0; _sum_of_squares = 0; }
    virtual void feed(const double x)
    {
        _n++; _sum += x;
        _sum_of_squares += x * x;
    }
    virtual double get_mean() const { return _sum / _n; }
    virtual double get_var() const
    {
        return 1./(_n - 1) * (_sum_of_squares - _n * get_mean()*get_mean());
    }
    virtual unsigned int get_n() const { return _n; }
private:
    unsigned int _n;
    double _sum;
    double _sum_of_squares;
};

class BufferAverager: public Averager
{
public:
    BufferAverager(const size_t bufsize):
        bufsize_(bufsize), n_(0), i_(0)
    {
        buf_ = new double[bufsize];
    }
    virtual ~BufferAverager()
    {
        delete[] buf_;
    }
    virtual void feed(const double x)
    {
        if (i_ == bufsize_)
        {
            i_ = 0;
        }
        if (i_ < bufsize_)
        {
            buf_[i_] = x;
            i_++;
            if (i_ > n_)
                n_++;
        }
    }
    virtual double get_mean() const
    {
        return gsl_stats_mean(buf_, 1, n_);
    }
    virtual double get_var() const
    {
        return gsl_stats_variance(buf_, 1, n_);
    }
    virtual unsigned int get_n() const
    {
        return n_;
    }
    virtual void reset()
    {
        memset(buf_, 0, bufsize_ * sizeof(double));
        n_ = 0;
        i_ = 0;
    }
    void dump(std::ostream& out)
    {
        for (int j = 0; j < n_; j++)
            out << buf_[j] << "\n";
    }

private:
    const size_t bufsize_;
    double* buf_;
    unsigned int n_;
    unsigned int i_;
};

class FancyAverager: public Averager
{
public:
    FancyAverager():
        _mean_avg(), _sq_avg(), _cub_avg() {}
    virtual void reset()
    {
        _mean_avg.reset();
        _sq_avg.reset();
    }
    virtual void feed(const double x)
    {
        _mean_avg.feed(x);
        _sq_avg.feed(x*x);
        _cub_avg.feed(x*x*x);
    }
    virtual unsigned int get_n() const { return _mean_avg.get_n(); }
    virtual double get_mean() const { return _mean_avg.get_mean(); }
    virtual double get_var() const { return _mean_avg.get_var(); }
    double get_mean_of_square() const { return _sq_avg.get_mean(); }
    double get_var_of_square() const { return _sq_avg.get_var(); }
    double get_mean_of_cube() const { return _cub_avg.get_mean(); }
    double get_var_of_cube() const { return _cub_avg.get_var(); }
private:
    SimpleAverager _mean_avg;
    SimpleAverager _sq_avg;
    SimpleAverager _cub_avg;
};

#endif // AVERAGER_H_


static char help[] =
"Solve the linear biharmonic equation in 2D.  Equation is\n"
"  Lap^2 u = f\n"
"where Lap = - grad^2 is the positive Laplacian, equivalently\n"
"  u_xxxx + 2 u_xxyy + u_yyyy = f(x,y)\n"
"Domain is unit square  S = (0,1)^2.   Boundary conditions are homogeneous\n"
"simply-supported:  u = 0,  Lap u = 0.  The equation is rewritten as a\n"
"2x2 block system with SPD Laplacian blocks on the diagonal:\n"
"   | Lap |  0  |  | v |   | f | \n"
"   |-----|-----|  |---| = |---| \n"
"   | -I  | Lap |  | u |   | 0 | \n"
"Includes manufactured, polynomial exact solution.  The discretization is\n"
"structured-grid (DMDA) finite differences.  Includes analytical Jacobian.\n"
"Recommended preconditioning combines fieldsplit:\n"
"   -pc_type fieldsplit -pc_fieldsplit_type multiplicative|additive \n"
"with multigrid as the preconditioner for the diagonal blocks:\n"
"   -fieldsplit_v_pc_type mg|gamg -fieldsplit_u_pc_type mg|gamg\n"
"(GMG requires setting levels and Galerkin coarsening.)  One can also do\n"
"monolithic multigrid (-pc_type mg|gamg).\n\n";

#include <petsc.h>

typedef struct {
    PetscReal  v, u;
} Field;

typedef struct {
    PetscReal  (*f)(PetscReal x, PetscReal y);  // right-hand side
} BiharmCtx;

static PetscReal c(PetscReal x) {
    return x*x*x * (1.0-x)*(1.0-x)*(1.0-x);
}

static PetscReal ddc(PetscReal x) {
    return 6.0 * x * (1.0-x) * (1.0 - 5.0 * x + 5.0 * x*x);
}

static PetscReal d4c(PetscReal x) {
    return - 72.0 * (1.0 - 5.0 * x + 5.0 * x*x);
}

static PetscReal u_exact_fcn(PetscReal x, PetscReal y) {
    return c(x) * c(y);
}

static PetscReal lap_u_exact_fcn(PetscReal x, PetscReal y) {
    return - ddc(x) * c(y) - c(x) * ddc(y);  // Lap u = - grad^2 u
}

static PetscReal f_fcn(PetscReal x, PetscReal y) {
    return d4c(x) * c(y) + 2.0 * ddc(x) * ddc(y) + c(x) * d4c(y);  // Lap^2 u = grad^4 u
}

extern PetscErrorCode FormExactWLocal(DMDALocalInfo*, Field**, BiharmCtx*);
extern PetscErrorCode FormFunctionLocal(DMDALocalInfo*, Field**, Field **FF, BiharmCtx*);
extern PetscErrorCode FormJacobianLocal(DMDALocalInfo*, Field**, Mat, Mat, BiharmCtx*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    DM             da;
    SNES           snes;
    Vec            w, w_initial, w_exact;
    BiharmCtx      user;
    Field          **aW;
    PetscReal      normv, normu, errv, erru;
    DMDALocalInfo  info;

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

    user.f = &f_fcn;
    ierr = DMDACreate2d(PETSC_COMM_WORLD,
                        DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DMDA_STENCIL_STAR,
                        3,3,PETSC_DECIDE,PETSC_DECIDE,
                        2,1,              // degrees of freedom, stencil width
                        NULL,NULL,&da); CHKERRQ(ierr);
    ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);  // this must be called BEFORE SetUniformCoordinates
    ierr = DMDASetUniformCoordinates(da,0.0,1.0,0.0,1.0,-1.0,-1.0); CHKERRQ(ierr);
    ierr = DMDASetFieldName(da,0,"v"); CHKERRQ(ierr);
    ierr = DMDASetFieldName(da,1,"u"); 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 = DMDASNESSetJacobianLocal(da,
               (DMDASNESJacobian)FormJacobianLocal,&user); CHKERRQ(ierr);
    ierr = SNESSetType(snes,SNESKSPONLY); CHKERRQ(ierr);
    ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);

    ierr = DMGetGlobalVector(da,&w_initial); CHKERRQ(ierr);
    ierr = VecSet(w_initial,0.0); CHKERRQ(ierr);
    ierr = SNESSolve(snes,NULL,w_initial); CHKERRQ(ierr);
    ierr = DMRestoreGlobalVector(da,&w_initial); CHKERRQ(ierr);
    ierr = DMDestroy(&da); CHKERRQ(ierr);

    ierr = SNESGetSolution(snes,&w); CHKERRQ(ierr);
    ierr = SNESGetDM(snes,&da); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);

    ierr = DMCreateGlobalVector(da,&w_exact); CHKERRQ(ierr);
    ierr = DMDAVecGetArray(da,w_exact,&aW); CHKERRQ(ierr);
    ierr = FormExactWLocal(&info,aW,&user); CHKERRQ(ierr);
    ierr = DMDAVecRestoreArray(da,w_exact,&aW); CHKERRQ(ierr);
    ierr = VecStrideNorm(w_exact,0,NORM_INFINITY,&normv); CHKERRQ(ierr);
    ierr = VecStrideNorm(w_exact,1,NORM_INFINITY,&normu); CHKERRQ(ierr);
    ierr = VecAXPY(w,-1.0,w_exact); CHKERRQ(ierr);
    ierr = VecStrideNorm(w,0,NORM_INFINITY,&errv); CHKERRQ(ierr);
    ierr = VecStrideNorm(w,1,NORM_INFINITY,&erru); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,
        "done on %d x %d grid ...\n"
        "  errors |v-vex|_inf/|vex|_inf = %.5e, |u-uex|_inf/|uex|_inf = %.5e\n",
        info.mx,info.my,errv/normv,erru/normu); CHKERRQ(ierr);

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

PetscErrorCode FormExactWLocal(DMDALocalInfo *info, Field **aW, BiharmCtx *user) {
    PetscErrorCode ierr;
    PetscInt   i, j;
    PetscReal  xymin[2], xymax[2], hx, hy, x, y;
    ierr = DMGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = j * hy;
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = i * hx;
            aW[j][i].u = u_exact_fcn(x,y);
            aW[j][i].v = lap_u_exact_fcn(x,y);
        }
    }
    return 0;
}

PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, Field **aW,
                                 Field **FF, BiharmCtx *user) {
    PetscErrorCode ierr;
    PetscInt   i, j;
    PetscReal  xymin[2], xymax[2], hx, hy, darea, scx, scy, scdiag, x, y,
               ve, vw, vn, vs, ue, uw, un, us;
    ierr = DMGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    darea = hx * hy;               // multiply FD equations by this
    scx = hy / hx;
    scy = hx / hy;
    scdiag = 2.0 * (scx + scy);    // diagonal scaling
    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) {
                FF[j][i].v = scdiag * aW[j][i].v;
                FF[j][i].u = scdiag * aW[j][i].u;
            } else {
                ve = (i+1 == info->mx-1) ? 0.0 : aW[j][i+1].v;
                vw = (i-1 == 0)          ? 0.0 : aW[j][i-1].v;
                vn = (j+1 == info->my-1) ? 0.0 : aW[j+1][i].v;
                vs = (j-1 == 0)          ? 0.0 : aW[j-1][i].v;
                FF[j][i].v = scdiag * aW[j][i].v - scx * (vw + ve) - scy * (vs + vn)
                             - darea * (*(user->f))(x,y);
                ue = (i+1 == info->mx-1) ? 0.0 : aW[j][i+1].u;
                uw = (i-1 == 0)          ? 0.0 : aW[j][i-1].u;
                un = (j+1 == info->my-1) ? 0.0 : aW[j+1][i].u;
                us = (j-1 == 0)          ? 0.0 : aW[j-1][i].u;
                FF[j][i].u = - darea * aW[j][i].v
                             + scdiag * aW[j][i].u - scx * (uw + ue) - scy * (us + un);
            }
        }
    }
    ierr = PetscLogFlops(18.0*info->xm*info->ym);CHKERRQ(ierr);
    return 0;
}

PetscErrorCode FormJacobianLocal(DMDALocalInfo *info, Field **aW,
                                 Mat J, Mat Jpre, BiharmCtx *user) {
    PetscErrorCode ierr;
    PetscInt     i, j, c, ncol;
    PetscReal    xymin[2], xymax[2], hx, hy, darea, scx, scy, scdiag, val[6];
    MatStencil   col[6], row;

    ierr = DMGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    darea = hx * hy;               // multiply FD equations by this
    scx = hy / hx;
    scy = hx / hy;
    scdiag = 2.0 * (scx + scy);    // diagonal scaling
    for (j = info->ys; j < info->ys + info->ym; j++) {
        row.j = j;
        for (i = info->xs; i < info->xs + info->xm; i++) {
            row.i = i;
            for (c = 0; c < 2; c++) { // v,u equations are c=0,1
                row.c = c;
                col[0].c = c;     col[0].i = i;       col[0].j = j;
                val[0] = scdiag;
                if (i==0 || i==info->mx-1 || j==0 || j==info->my-1) {
                    ierr = MatSetValuesStencil(Jpre,1,&row,1,col,val,INSERT_VALUES);
                        CHKERRQ(ierr);
                } else {
                    ncol = 1;
                    if (i+1 < info->mx-1) {
                        col[ncol].c = c;  col[ncol].i = i+1;  col[ncol].j = j;
                        val[ncol++] = -scx;
                    }
                    if (i-1 > 0) {
                        col[ncol].c = c;  col[ncol].i = i-1;  col[ncol].j = j;
                        val[ncol++] = -scx;
                    }
                    if (j+1 < info->my-1) {
                        col[ncol].c = c;  col[ncol].i = i;    col[ncol].j = j+1;
                        val[ncol++] = -scy;
                    }
                    if (j-1 > 0) {
                        col[ncol].c = c;  col[ncol].i = i;    col[ncol].j = j-1;
                        val[ncol++] = -scy;
                    }
                    if (c == 1) {  // u equation;  has off-diagonal block entry
                        col[ncol].c = 0;
                        col[ncol].i = i;  col[ncol].j = j;
                        val[ncol++] = - darea;
                    }
                    ierr = MatSetValuesStencil(Jpre,1,&row,ncol,col,val,INSERT_VALUES);
                        CHKERRQ(ierr);
                }
            }
        }
    }

    ierr = MatAssemblyBegin(Jpre,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(Jpre,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != Jpre) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}


//
// Created by karl_ on 2020-5-3.
//

#include <stdlib.h>
#include <math.h>
#include <cblas.h>
#include <core/color-space.h>
#include <imgproc/histogram-equalization.h>
#include "color-convert.h"

// ref: https://www.math.uci.edu/icamp/courses/math77c/demos/hist_eq.pdf
static void
histeq(double const *src, unsigned src_stride, double *dst, unsigned dst_stride, size_t area, unsigned range) {
    unsigned *cdf;
    size_t i;
    cdf = calloc(range, sizeof(unsigned));
    for (i = 0; i < area; ++i) {
        cdf[(int) (src[i * src_stride] * range)] += 1;
    }
    for (i = 1; i < range; ++i) {
        cdf[i] += cdf[i - 1];
    }
    for (i = 0; i < area; ++i) {
        dst[i * dst_stride] = floor((double) (range - 1) * cdf[(int) (src[i * src_stride] * range)] / area) / range;
    }
    free(cdf);
}

static gboolean histeq_grayscale(CoreImage *src, CoreImage **dst) {
    gdouble *src_data_cast = NULL;
    gdouble *dst_data_cast = NULL;
    gpointer src_data, dst_data;
    CoreSize *size = NULL;
    guint8 channel;
    gsize i, area;
    CoreColorSpace color_space;
    CorePixelType pixel_type;
    gdouble range, inv_range;
    g_return_val_if_fail(src != NULL, FALSE);
    g_return_val_if_fail(dst != NULL, FALSE);

    channel = core_image_get_channel(src);
    g_return_val_if_fail(channel == 1, FALSE);

    src = g_object_ref(src);
    src_data = core_image_get_data(src);
    size = core_image_get_size(src);
    area = core_size_get_area(size);
    color_space = core_image_get_color_space(src);
    pixel_type = core_image_get_pixel_type(src);
    range = core_pixel_get_range(pixel_type);
    inv_range = 1.0 / range;

    /* cast to double and normalize pixel data to [0, 1] */
    if (core_pixel_is_double(pixel_type)) {
        src_data_cast = src_data;
    } else if (core_pixel_is_uint8(pixel_type)) {
        src_data_cast = g_malloc(sizeof(gdouble) * area);
        for (i = 0; i < area; ++i) {
            src_data_cast[i] = ((guint8 *) src_data)[i] * inv_range;
        }
    } else {
        g_return_val_if_fail(FALSE, FALSE);
    }
    dst_data_cast = g_malloc(sizeof(gdouble) * area);

    /* run the algorithm */
    histeq(src_data_cast, 1, dst_data_cast, 1, area, 256);

    /* cast back to uchar */
    if (core_pixel_is_double(pixel_type)) {
        /* exactly do nothing */
    } else if (core_pixel_is_uint8(pixel_type)) {
        /* reuse allocated memory */
        for (i = 0; i < area; ++i) {
            ((guint8 *) dst_data_cast)[i] = dst_data_cast[i] * 255.0;
        }
    } else {
        g_return_val_if_fail(FALSE, FALSE);
    }
    dst_data = dst_data_cast;
    if (core_pixel_is_uint8(pixel_type)) {
        /* release useless memory, can be removed for speed */
        dst_data = g_realloc(dst_data, sizeof(guint8) * area);
    }

    if (*dst == NULL) {
        *dst = core_image_new_with_data(dst_data, color_space, pixel_type, size, FALSE);
    } else {
        core_image_assign_data(*dst, dst_data, color_space, pixel_type, size, FALSE);
    }

    /* free allocated additional memory */
    if (core_pixel_is_uint8(pixel_type)) {
        g_free(src_data_cast);
    }
    g_object_unref(size);
    g_object_unref(src);
    return TRUE;
}

static gboolean histeq_hsl(CoreImage *src, CoreImage **dst) {
    gdouble *src_data, *dst_data;
    CoreSize *size;
    gsize area, block_size;

    src_data = core_image_get_data(src);
    size = core_image_get_size(src);
    area = core_size_get_area(size);
    block_size = area * 3 * sizeof(gdouble);
    dst_data = g_malloc(block_size);
    memcpy(dst_data, src_data, block_size);
    histeq(src_data + 2, 3, dst_data + 2, 3, area, 255);

    if (*dst == NULL) {
        *dst = core_image_new_with_data(dst_data, CORE_COLOR_SPACE_HSL, CORE_PIXEL_D3, size, FALSE);
    } else {
        core_image_assign_data(*dst, dst_data, CORE_COLOR_SPACE_HSL, CORE_PIXEL_D3, size, FALSE);
    }

    g_object_unref(size);
}

gboolean imgproc_histogram_equalization(CoreImage *src, CoreImage **dst) {
    CoreColorSpace src_color_space = core_image_get_color_space(src);
    CoreImage *dummy = NULL;
    gboolean ret;

    if (src_color_space == CORE_COLOR_SPACE_GRAY_SCALE) {
        ret = histeq_grayscale(src, dst);
    } else {
        if (src_color_space == CORE_COLOR_SPACE_RGB) {
            imgproc_to_HSL(src, &dummy);
        } else {
            dummy = g_object_ref(src);
        }
        ret = histeq_hsl(dummy, dst);
        g_object_unref(dummy);
        if (src_color_space == CORE_COLOR_SPACE_RGB) {
            imgproc_to_RGB(*dst, dst);
        }
    }
    return ret;
}


#include <gsl/gsl_randist.h>
#include <time.h>

gsl_rng *r;

void initialiseStatistics()
{
  r = gsl_rng_alloc(gsl_rng_ranlxs0);
  gsl_rng_set(r, time(NULL));
}

void cleanupStatistics() { free(r); }

double denormalise(double standardNormalValue, double mean, double stddev)
{
  return (standardNormalValue * stddev) + mean;
}

double getRandomGaussian(double mean, double stddev, double *min, double *max)
{
  double v = gsl_ran_gaussian(r, 1.0);
  double d = denormalise(v, mean, stddev);

  if (min != NULL)
  {
    if (d <= *min)
    {
      return getRandomGaussian(mean, stddev, min, max);
    }
  }
  if (max != NULL)
  {
    if (d >= *max)
    {
      return getRandomGaussian(mean, stddev, min, max);
    }
  }
  return d;
}


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

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_histogram.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>

#include "urand.c"

void
test1d_resample (void)
{
  size_t i;
  int status = 0;

  gsl_histogram *h;

  gsl_ieee_env_setup ();

  h = gsl_histogram_calloc_uniform (10, 0.0, 1.0);

  gsl_histogram_increment (h, 0.1);
  gsl_histogram_increment (h, 0.2);
  gsl_histogram_increment (h, 0.2);
  gsl_histogram_increment (h, 0.3);

  {
    gsl_histogram_pdf *p = gsl_histogram_pdf_alloc (10);

    gsl_histogram *hh = gsl_histogram_calloc_uniform (100, 0.0, 1.0);

    gsl_histogram_pdf_init (p, h);

    for (i = 0; i < 100000; i++)
      {
        double u = urand();
        double x = gsl_histogram_pdf_sample (p, u);
        gsl_histogram_increment (hh, x);
      }

    for (i = 0; i < 100; i++)
      {
        double y = gsl_histogram_get (hh, i) / 2500;
        double x, xmax;
        size_t k;
        double ya;

        gsl_histogram_get_range (hh, i, &x, &xmax);

        gsl_histogram_find (h, x, &k);
        ya = gsl_histogram_get (h, k);

        if (ya == 0)
          {
            if (y != 0)
              {
                printf ("%d: %g vs %g\n", (int) i, y, ya);
                status = 1;
              }
          }
        else
          {
            double err = 1 / sqrt (gsl_histogram_get (hh, i));
            double sigma = fabs ((y - ya) / (ya * err));
            if (sigma > 3)
              {
                status = 1;
                printf ("%g vs %g err=%g sigma=%g\n", y, ya, err, sigma);
              }
          }
      }

    gsl_histogram_pdf_free (p) ;
    gsl_histogram_free (hh);

    gsl_test (status, "gsl_histogram_pdf_sample within statistical errors");
  }

  gsl_histogram_free (h);
}