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

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

/* This is a maximally equidistributed combined Tausworthe
   generator. The sequence is,

   x_n = (s1_n ^ s2_n ^ s3_n)

   s1_{n+1} = (((s1_n & 4294967294) <<12) ^ (((s1_n <<13) ^ s1_n) >>19))
   s2_{n+1} = (((s2_n & 4294967288) << 4) ^ (((s2_n << 2) ^ s2_n) >>25))
   s3_{n+1} = (((s3_n & 4294967280) <<17) ^ (((s3_n << 3) ^ s3_n) >>11))

   computed modulo 2^32. In the three formulas above '^' means
   exclusive-or (C-notation), not exponentiation. Note that the
   algorithm relies on the properties of 32-bit unsigned integers (it
   is formally defined on bit-vectors of length 32). I have added a
   bitmask to make it work on 64 bit machines.

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

   The theoretical value of x_{10007} is 2733957125. The subscript
   10007 means (1) seed the generator with s=1 (2) do six warm-up
   iterations, (3) then do 10000 actual iterations.

   The period of this generator is about 2^88.

   From: P. L'Ecuyer, "Maximally Equidistributed Combined Tausworthe
   Generators", Mathematics of Computation, 65, 213 (1996), 203--213.

   This is available on the net from L'Ecuyer's home page,

   http://www.iro.umontreal.ca/~lecuyer/myftp/papers/tausme.ps
   ftp://ftp.iro.umontreal.ca/pub/simulation/lecuyer/papers/tausme.ps

   Update: April 2002

   There is an erratum in the paper "Tables of Maximally
   Equidistributed Combined LFSR Generators", Mathematics of
   Computation, 68, 225 (1999), 261--269:
   http://www.iro.umontreal.ca/~lecuyer/myftp/papers/tausme2.ps

        ... the k_j most significant bits of z_j must be non-
        zero, for each j. (Note: this restriction also applies to the
        computer code given in [4], but was mistakenly not mentioned in
        that paper.)

   This affects the seeding procedure by imposing the requirement
   s1 > 1, s2 > 7, s3 > 15.

   The generator taus2 has been added to satisfy this requirement.
   The original taus generator is unchanged.

   Update: November 2002

   There was a bug in the correction to the seeding procedure for s2.
   It affected the following seeds 254679140 1264751179 1519430319
   2274823218 2529502358 3284895257 3539574397 (s2 < 8).

*/

static inline unsigned long int taus_get (void *vstate);
static double taus_get_double (void *vstate);
static void taus_set (void *state, unsigned long int s);

typedef struct
  {
    unsigned long int s1, s2, s3;
  }
taus_state_t;

static inline unsigned long
taus_get (void *vstate)
{
  taus_state_t *state = (taus_state_t *) vstate;

#define MASK 0xffffffffUL
#define TAUSWORTHE(s,a,b,c,d) (((s &c) <<d) &MASK) ^ ((((s <<a) &MASK)^s) >>b)

  state->s1 = TAUSWORTHE (state->s1, 13, 19, 4294967294UL, 12);
  state->s2 = TAUSWORTHE (state->s2, 2, 25, 4294967288UL, 4);
  state->s3 = TAUSWORTHE (state->s3, 3, 11, 4294967280UL, 17);

  return (state->s1 ^ state->s2 ^ state->s3);
}

static double
taus_get_double (void *vstate)
{
  return taus_get (vstate) / 4294967296.0 ;
}

static void
taus_set (void *vstate, unsigned long int s)
{
  taus_state_t *state = (taus_state_t *) vstate;

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

#define LCG(n) ((69069 * n) & 0xffffffffUL)
  state->s1 = LCG (s);
  state->s2 = LCG (state->s1);
  state->s3 = LCG (state->s2);

  /* "warm it up" */
  taus_get (state);
  taus_get (state);
  taus_get (state);
  taus_get (state);
  taus_get (state);
  taus_get (state);
  return;
}

static void
taus2_set (void *vstate, unsigned long int s)
{
  taus_state_t *state = (taus_state_t *) vstate;

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

#define LCG(n) ((69069 * n) & 0xffffffffUL)
  state->s1 = LCG (s);
  if (state->s1 < 2) state->s1 += 2UL;
  state->s2 = LCG (state->s1);
  if (state->s2 < 8) state->s2 += 8UL;
  state->s3 = LCG (state->s2);
  if (state->s3 < 16) state->s3 += 16UL;

  /* "warm it up" */
  taus_get (state);
  taus_get (state);
  taus_get (state);
  taus_get (state);
  taus_get (state);
  taus_get (state);
  return;
}

static const gsl_rng_type taus_type =
{"taus",                        /* name */
 0xffffffffUL,                  /* RAND_MAX */
 0,                             /* RAND_MIN */
 sizeof (taus_state_t),
 &taus_set,
 &taus_get,
 &taus_get_double};

const gsl_rng_type *gsl_rng_taus = &taus_type;

static const gsl_rng_type taus2_type =
{"taus2",                       /* name */
 0xffffffffUL,                  /* RAND_MAX */
 0,                             /* RAND_MIN */
 sizeof (taus_state_t),
 &taus2_set,
 &taus_get,
 &taus_get_double};

const gsl_rng_type *gsl_rng_taus2 = &taus2_type;


/*
  NAME:
     normalize_row
  PURPOSE:
     normalize a row (or column) of a matrix given as logs
  CALLING SEQUENCE:
     normalize_row(gsl_matrix * q, int row,bool isrow,bool noweight,
     double weight)
  INPUT:
     q            - matrix
     row          - row to be normalized
     isrow        - is it a row or a column
     noweight    - add a weight to all of the values?
     weight       - weight to be added to all of the values
  OUTPUT:
     normalization factor (i.e. logsum)
  REVISION HISTORY:
     2008-09-21 - Written Bovy
     2010-04-01 - Added noweight and weight inputs to allow the qij to have
                  weights - Bovy
*/
#include <math.h>
#include <gsl/gsl_matrix.h>
#include "proj_gauss_mixtures.h"

double normalize_row(gsl_matrix * q, int row, bool isrow,
		     bool noweight, double weight){
  double loglike;
  if (isrow)
    loglike = logsum(q,row,true);
  else
    loglike = logsum(q,row,false);

  int dd;
  if (isrow)
    for (dd = 0; dd != q->size2; ++dd) {
      if ( noweight )
	gsl_matrix_set(q,row,dd,gsl_matrix_get(q,row,dd)-loglike);
      else
	gsl_matrix_set(q,row,dd,gsl_matrix_get(q,row,dd)-loglike+weight);
    }
  else
    for (dd = 0; dd != q->size1; ++dd) {
      if ( noweight )
	gsl_matrix_set(q,dd,row,gsl_matrix_get(q,dd,row)-loglike);
      else
	gsl_matrix_set(q,dd,row,gsl_matrix_get(q,dd,row)-loglike+weight);
    }
  if ( ! noweight ) loglike*= exp(weight);

  return loglike;
}


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_randist.h>
#include <Python.h>

static char module_docstring[] = "GSL Statistics module";
static char t_test_docstring[] = "Perform an independent samples t-test";

double t_test(const double *data1, int n1, const double *data2, int n2)
{
    double mean1, mean2, var1, var2, num, sp, denom, t, dof, p;
    mean1 = gsl_stats_mean(data1, 1, n1);
    mean2 = gsl_stats_mean(data2, 1, n2);
    var1 = gsl_stats_variance(data1, 1, n1);
    var2 = gsl_stats_variance(data2, 1, n2);
    num = mean1 - mean2;
    sp = sqrt((((n1 - 1) * var1) + ((n2 - 1) * var2)) / (n1 + n2 - 2));
    denom = sp * sqrt((1 / (double) n1) + (1 / (double) n2));
    t = fabs(num / denom);
    dof = (double) n1 + n2 - 2;
    p = (1 - gsl_cdf_tdist_P(t, dof)) * 2;
    return p;
}

static double* PySequence_ToDoubleArray(PyObject* inp, int n)
{
    // Allocate array
    double *out;
    out = malloc(n * sizeof(double));
    for (int i = 0; i < n; i++)
    {
        PyObject *fitem;
        PyObject *item = PySequence_Fast_GET_ITEM(inp, i);
        if(!item) {
            Py_DECREF(inp);
            free(out);
            return NULL;
        }
        fitem = PyNumber_Float(item);
        if(!fitem) {
            Py_DECREF(inp);
            free(out);
            PyErr_SetString(PyExc_TypeError, "all items must be numbers");
            return NULL;
        }
        out[i] = PyFloat_AS_DOUBLE(fitem);
        Py_DECREF(fitem);
    }
    return out;
}

static PyObject *t_test_py(PyObject *self, PyObject *args)
{
    PyObject *inp1, *inp2;
    PyObject* data1;
    PyObject* data2;
    double *d1, *d2;
    int n1, n2;
    double res;

    if (!PyArg_ParseTuple(args, "OO", &inp1, &inp2))
        return 0;
    data1 = PySequence_Fast(inp1, "argument must be iterable");
    data2 = PySequence_Fast(inp2, "argument must be iterable");
    n1 = PySequence_Fast_GET_SIZE(data1);
    n2 = PySequence_Fast_GET_SIZE(data2);

    d1 = PySequence_ToDoubleArray(data1, n1);
    d2 = PySequence_ToDoubleArray(data2, n2);

    Py_DECREF(data1);
    Py_DECREF(data2);

    res = t_test(d1, n1, d2, n2);
    free(d1);
    free(d2);

    return Py_BuildValue("d", res);
}

static PyMethodDef module_methods[] = {
    {"t_test_py", t_test_py, METH_VARARGS, t_test_docstring},
    {NULL, NULL, 0, NULL}
};

static struct PyModuleDef gslstatsmodule = {
    PyModuleDef_HEAD_INIT,
    "gslstats",
    module_docstring,
    -1,
    module_methods,
};

PyMODINIT_FUNC PyInit_gslstats(void)
{
    Py_Initialize();
    return PyModule_Create(&gslstatsmodule);
}


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_vector.h>
#include "MeshAttributes.h"
#include "mathfunctions.h"
#include "oneTimeStep.h"

extern const double g;

void oneTimeStep2D (double time, double dt)
{

	// store H, Qx, Qy
	gsl_vector *gzeta = gsl_vector_alloc(3*NumEl);
	gsl_vector *gQx = gsl_vector_alloc(3*NumEl);
	gsl_vector *gQy = gsl_vector_alloc(3*NumEl);

	int j = 0;
	for (int i=0; i < NumEl; ++i) {
		for (int k=0; k < 3; ++k){
			gsl_vector_set(gzeta,j,zeta[index(i,k,3)]);
			gsl_vector_set(gQx,j, Qx[index(i,k,3)]);
			gsl_vector_set(gQy,j, Qy[index(i,k,3)]);
			++j;
		}
	}

	/************ print out zeta, Qx and  Qy for debugging ********************/
	#ifdef DEBUG
	printf("Initial values:\n zeta \n");
	gsl_vector_fprintf(stdout,gzeta,"%e");
	printf("\n Qx \n");
	gsl_vector_fprintf(stdout,gQx,"%e");
	printf("\n Qy \n");
	gsl_vector_fprintf(stdout,gQy,"%e");
	printf("\n");
	#endif
	/***************************************************************************/

	// Calculate the wet-dry status of the elements
//	wetDryStatus2D();

	// Intermediate RK step
	compute2DL(time);

	gsl_vector *gRHSZeta = gsl_vector_alloc(3*NumEl);
	gsl_vector *gRHSQx = gsl_vector_alloc(3*NumEl);
	gsl_vector *gRHSQy = gsl_vector_alloc(3*NumEl);

	j = 0;
	for (int i=0; i < NumEl; ++i) {
		for (int k=0; k < 3; ++k){
			gsl_vector_set(gRHSZeta,j,RHSZeta[index(i,k,3)]);
			gsl_vector_set(gRHSQx,j,RHSQx[index(i,k,3)]);
			gsl_vector_set(gRHSQy,j,RHSQy[index(i,k,3)]);
			++j;
		}
	}

	/************ print out RHSZeta, RHSQx and RHSQy for debugging ********************/
	#ifdef DEBUG
	printf("After first computeL:\n RHSZeta \n");
	gsl_vector_fprintf(stdout,gRHSZeta,"%e");
	printf("\n RHSQx \n");
	gsl_vector_fprintf(stdout,gRHSQx,"%e");
	printf("\n RHSQy \n");
	gsl_vector_fprintf(stdout,gRHSQy,"%e");
	printf("\n");
	#endif
	/***************************************************************************/

	// w_1 = w + dt*L;
	gsl_vector *gzeta_1 = gsl_vector_alloc(3*NumEl);
	gsl_vector *gQx_1 = gsl_vector_alloc(3*NumEl);
	gsl_vector *gQy_1 = gsl_vector_alloc(3*NumEl);
	for (int i=0; i<3*NumEl; ++i)
	{
		gsl_vector_set(gzeta_1,i,gsl_vector_get(gzeta,i));
		gsl_vector_set(gQx_1,i,gsl_vector_get(gQx,i));
		gsl_vector_set(gQy_1,i,gsl_vector_get(gQy,i));
	}

	gsl_vector_scale(gRHSZeta,dt);
	gsl_vector_scale(gRHSQx,dt);
	gsl_vector_scale(gRHSQy,dt);
	gsl_vector_add(gzeta_1,gRHSZeta);
	gsl_vector_add(gQx_1, gRHSQx);
	gsl_vector_add(gQy_1, gRHSQy);

	/************ print out zeta, Qx and  Qy for debugging ********************/
	#ifdef DEBUG
	printf("After first computeL:\n Zeta \n");
	gsl_vector_fprintf(stdout,gzeta_1,"%e");
	printf("\n Qx \n");
	gsl_vector_fprintf(stdout,gQx_1,"%e");
	printf("\n Qy \n");
	gsl_vector_fprintf(stdout,gQy_1,"%e");
	printf("\n");
	#endif
	/***************************************************************************/

	j = 0;
	for (int i=0; i<NumEl; ++i){
		for (int k =0; k < 3; ++k){
			zeta[index(i,k,3)] = gsl_vector_get(gzeta_1,j);
			Qx[index(i,k,3)] = gsl_vector_get(gQx_1,j);
			Qy[index(i,k,3)] = gsl_vector_get(gQy_1,j);
			++j;
		}
	}

/*	printf("zeta\n");
	for (int i=0; i < NumEl; ++i)
	{
		for (int k =0; k <3; ++k)
		{
			printf("%1.15f\t",junc->zeta[index(i,k,3)]);
		}
		printf("\n");

	}

	printf("Qx\n");
	for (int i=0; i < NumEl; ++i)
	{
		for (int k =0; k <3; ++k)
		{
			printf("%1.15f\t",junc->Qx[index(i,k,3)]);
		}
		printf("\n");

	}

	printf("Qy\n");
	for (int i=0; i < NumEl; ++i)
	{
		for (int k =0; k <3; ++k)
		{
			printf("%1.15f\t",junc->Qy[index(i,k,3)]);
		}
		printf("\n");

	}
*/
	// Apply minmod slope limiter on w_1
	SlopeLimiter();

	// Apply the positive-depth operator on w_1
//	PDop2D();

	j = 0;
	for (int i=0; i<NumEl; ++i)
	{
		for(int k=0; k <3; ++k)
		{
			gsl_vector_set(gzeta_1,j,zeta[index(i,k,3)]);
			gsl_vector_set(gQx_1,j,Qx[index(i,k,3)]);
			gsl_vector_set(gQy_1,j,Qy[index(i,k,3)]);
			++j;
		}
	}

	/************ print out zeta, Qx and  Qy for debugging ********************/
	#ifdef DEBUG
	printf("After first slope limiting:\n zeta \n");
	gsl_vector_fprintf(stdout,gzeta_1,"%e");
	printf("\n Qx \n");
	gsl_vector_fprintf(stdout,gQx_1,"%e");
	printf("\n Qy \n");
	gsl_vector_fprintf(stdout,gQy_1,"%e");
	printf("\n");
	#endif
	/***************************************************************************/

	// w_1 = w_1 + w;
	gsl_vector_add(gzeta_1,gzeta);
	gsl_vector_add(gQx_1,gQx);
	gsl_vector_add(gQy_1,gQy);

	// Calcualte the wet-dry status of the elements again
//	wetDryStatus2D();

	// Compute w
	compute2DL(time);

	j = 0;
	for (int i=0; i < NumEl; ++i) {
		for (int k=0; k < 3; ++k){
			gsl_vector_set(gRHSZeta,j,RHSZeta[index(i,k,3)]);
			gsl_vector_set(gRHSQx,j,RHSQx[index(i,k,3)]);
			gsl_vector_set(gRHSQy,j,RHSQy[index(i,k,3)]);
			++j;
		}
	}

	/************ print out zeta, Qx and  Qy for debugging ********************/
	#ifdef DEBUG
	printf("After second computeL:\n RHSZeta \n");
	gsl_vector_fprintf(stdout,gRHSZeta,"%e");
	printf("\n RHSQx \n");
	gsl_vector_fprintf(stdout,gRHSQx,"%e");
	printf("\n RHSQy \n");
	gsl_vector_fprintf(stdout,gRHSQy,"%e");
	printf("\n");
	#endif
	/***************************************************************************/

	gsl_vector_scale(gzeta_1,0.5);
	gsl_vector_scale(gQx_1,0.5);
	gsl_vector_scale(gQy_1,0.5);
	gsl_vector_scale(gRHSZeta,dt/2);
	gsl_vector_scale(gRHSQx,dt/2);
	gsl_vector_scale(gRHSQy,dt/2);

	gsl_vector_add(gzeta_1,gRHSZeta);
	gsl_vector_add(gQx_1,gRHSQx);
	gsl_vector_add(gQy_1,gRHSQy);

	j = 0;
	for (int i=0; i<NumEl; ++i){
		for (int k =0; k < 3; ++k){
			zeta[index(i,k,3)] = gsl_vector_get(gzeta_1,j);
			Qx[index(i,k,3)] = gsl_vector_get(gQx_1,j);
			Qy[index(i,k,3)] = gsl_vector_get(gQy_1,j);
			++j;
		}
	}

	// Apply minmod slope limiter to w
	SlopeLimiter();

	// Apply the positive depth operator on w
//	PDop2D();

	gsl_vector_free(gzeta);
	gsl_vector_free(gQx);
	gsl_vector_free(gQy);
	gsl_vector_free(gzeta_1);
	gsl_vector_free(gQx_1);
	gsl_vector_free(gQy_1);
	gsl_vector_free(gRHSZeta);
	gsl_vector_free(gRHSQx);
	gsl_vector_free(gRHSQy);

}

void time_evolution(double FinalTime)
{

	// output the file with x y and z information for the elements
	boundary_conditions();

	double time = 0;
	double dt = 1e-7;
	int Nstep = 0;
	int fileNumber = 1;

	// write out initial conditions
	FILE* Initfile1;
	FILE* Initfile2;
	FILE* Initfile3;
//	FILE* Initfile4;

	char flname1[100];
	char flname2[100];
	char flname3[100];
//	char flname4[100];
	sprintf(flname1, "000_zeta.dat");
	sprintf(flname2, "000_Qx.dat");
	sprintf(flname3, "000_Qy.dat");
//	sprintf(flname4, "000_Qdotn.dat");
	Initfile1 = fopen(flname1, "w");
	Initfile2 = fopen(flname2, "w");
	Initfile3 = fopen(flname3, "w");
//	Initfile4 = fopen(flname4, "w");

/*	fprintf(Initfile1, "H at time 0\n");
	fprintf(Initfile2, "Qx at time 0 \n");
	fprintf(Initfile3, "Qy at time 0 \n");
	fprintf(Initfile4, "Qdotn at time 0 \n");
*/
	int k;
	for (int j=0; j<NumEl; ++j)
	{
		for(k=0; k<3; ++k)
		{
			double zeta_val = zeta[index(j,k,3)];
			double zval = (z[EltoVert[j*3+k]]+z[EltoVert[j*3+((k+1)%3)]])/2;
			double H = zeta_val + zval;
			double Qx_val = Qx[index(j,k,3)];
			double Qy_val = Qy[index(j,k,3)];
		//	double Qdotn = Qx_val*normvecx[index(j,k,3)] + Qy_val*normvecy[index(j,k,3)];
			fprintf(Initfile1, "%1.15f\t\t",zeta_val);
			fprintf(Initfile2, "%1.15f\t\t",Qx_val);
			fprintf(Initfile3, "%1.15f\t\t",Qy_val);
		//	fprintf(Initfile4, "%1.15f\t\t",Qdotn);
		}
		fprintf(Initfile1, "\n");
		fprintf(Initfile2, "\n");
		fprintf(Initfile3, "\n");
//		fprintf(Initfile4, "\n");
	}
	fclose(Initfile1);
	fclose(Initfile2);
	fclose(Initfile3);
//	fclose(Initfile4);

	//wetDryStatus2D();
	while (time<FinalTime)
	{
		time = time + dt;
		printf("dt = %e\n", dt);
		printf("time = %e\n", time);

		oneTimeStep2D(time,dt);

		boundary_conditions();

		/********* Output data every twentienth step *************/
		Nstep = Nstep + 1;
		int NstepinSec = floor(1./dt);
		if (time == FinalTime)
	//	if (Nstep%500 == 0 || time == FinalTime)
//		if ((Nstep%(10*NstepinSec) == 0) || time == FinalTime)
		{

			FILE* file3;
			FILE* file4;
			FILE* file5;
//			FILE* file6;

			char filename1[100];
			char filename2[100];
			char filename3[100];
//			char filename4[100];

/*			sprintf(filename1, "Zeta%d", NumEl);
			sprintf(filename2, "Qx%d", NumEl);
			sprintf(filename3, "Qy%d", NumEl);
*/

			sprintf(filename1, "%3.3d_H.dat", fileNumber);
			sprintf(filename2, "%3.3d_Qx.dat", fileNumber);
			sprintf(filename3, "%3.3d_Qy.dat", fileNumber);

		//	sprintf(filename1, "%3.3d_H.dat", fileNumber);
		//	sprintf(filename2, "%3.3d_Qx.dat", fileNumber);
		//	sprintf(filename3, "%3.3d_Qy.dat", fileNumber);
//			sprintf(filename4, "%3.3d_Q.n.dat", fileNumber);
			file3 = fopen(filename1, "w");
			file4 = fopen(filename2, "w");
			file5 = fopen(filename3, "w");
//			file6 = fopen(filename4, "w");

			//fprintf(file3, "H at time %e\n", time);
			//fprintf(file4, "Qx at time %e \n", time);
			//fprintf(file5, "Qy at time %e \n", time);
//			fprintf(file6, "Qdotn at time %e \n", time);

			int k;
			for (int j=0; j<NumEl; ++j)
			{
				for(k=0; k<3; ++k)
				{
					double zeta_val = zeta[index(j,k,3)];
					double Qx_val = Qx[index(j,k,3)];
					double Qy_val = Qy[index(j,k,3)];
					double zval = (z[EltoVert[j*3+k]]+z[EltoVert[j*3+((k+1)%3)]])/2;
					double hval = zeta_val + zval;

//					double Qdotn = Qx_val*normvecx[index(j,k,3)] + Qy_val*normvecy[index(j,k,3)];
					fprintf(file3, "%1.15f\t\t",zeta_val);
					fprintf(file4, "%1.15f\t\t",Qx_val);
					fprintf(file5, "%1.15f\t\t",Qy_val);
//					fprintf(file6, "%1.15f\t\t",Qdotn);
				}
				fprintf(file3, "\n");
				fprintf(file4, "\n");
				fprintf(file5, "\n");
//				fprintf(file6, "\n");
			}
			fclose(file3);
			fclose(file4);
			fclose(file5);
//			fclose(file6);

			++fileNumber;
		}

		/**************** Set next time step ****************/

		// set next time step
	//	printf("max_lambda = %e\n", max_lambda);
	//	printf("minEdgLength = %e\n", minEdgLength);
		dt = 0.1*minEdgLength/max_lambda;
		if ((time + dt) > FinalTime)
			dt = FinalTime - time;

	}
}


/*
 * Licensed to the OpenAirInterface (OAI) Software Alliance under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The OpenAirInterface Software Alliance licenses this file to You under
 * the OAI Public License, Version 1.0  (the "License"); you may not use this file
 * except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.openairinterface.org/?page_id=698
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *-------------------------------------------------------------------------------
 * For more information about the OpenAirInterface (OAI) Software Alliance:
 *      contact@openairinterface.org
 */

#include <math.h>
#include <cblas.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>

#include "PHY/TOOLS/defs.h"
#include "defs.h"

// NEW code with lookup table for sin/cos based on delay profile (TO BE TESTED)

double **cos_lut=NULL,**sin_lut=NULL;

//#if 1

int init_freq_channel(channel_desc_t *desc,uint16_t nb_rb,int16_t n_samples)
{

  double delta_f,freq;  // 90 kHz spacing
  double delay;
  int16_t f;
  uint8_t l;

  if ((n_samples&1)==0) {
    fprintf(stderr, "freq_channel_init: n_samples has to be odd\n");
    return(-1);
  }

  cos_lut = (double **)malloc(n_samples*sizeof(double*));
  sin_lut = (double **)malloc(n_samples*sizeof(double*));

  delta_f = nb_rb*180000/(n_samples-1);

  for (f=-(n_samples>>1); f<=(n_samples>>1); f++) {
    freq=delta_f*(double)f*1e-6;// due to the fact that delays is in mus

    cos_lut[f+(n_samples>>1)] = (double *)malloc((int)desc->nb_taps*sizeof(double));
    sin_lut[f+(n_samples>>1)] = (double *)malloc((int)desc->nb_taps*sizeof(double));

    for (l=0; l<(int)desc->nb_taps; l++) {
      if (desc->nb_taps==1)
        delay = desc->delays[l];
      else
        delay = desc->delays[l]+NB_SAMPLES_CHANNEL_OFFSET/desc->sampling_rate;

      cos_lut[f+(n_samples>>1)][l] = cos(2*M_PI*freq*delay);
      sin_lut[f+(n_samples>>1)][l] = sin(2*M_PI*freq*delay);
      //printf("values cos:%d, sin:%d\n", cos_lut[f][l], sin_lut[f][l]);

    }
  }

  return(0);
}

int freq_channel(channel_desc_t *desc,uint16_t nb_rb,int16_t n_samples)
{

  int16_t f,f2,d;
  uint8_t aarx,aatx,l;
  double *clut,*slut;
  static int freq_channel_init=0;
  static int n_samples_max=0;

  // do some error checking
  // n_samples has to be a odd number because we assume the spectrum is symmetric around the DC and includes the DC
  if ((n_samples&1)==0) {
    fprintf(stderr, "freq_channel: n_samples has to be odd\n");
    return(-1);
  }

  // printf("no of taps:%d,",(int)desc->nb_taps);

  if (freq_channel_init == 0) {
    // we are initializing the lut for the largets possible n_samples=12*nb_rb+1
    // if called with n_samples<12*nb_rb+1, we decimate the lut
    n_samples_max=12*nb_rb+1;
    if (init_freq_channel(desc,nb_rb,n_samples_max)==0)
      freq_channel_init=1;
    else
      return(-1);
  }

  d=(n_samples_max-1)/(n_samples-1);

  //printf("no_samples=%d, n_samples_max=%d, d=%d\n",n_samples,n_samples_max,d);

  start_meas(&desc->interp_freq);

  for (f=-n_samples_max/2,f2=-n_samples/2; f<n_samples_max/2; f+=d,f2++) {
    clut = cos_lut[n_samples_max/2+f];
    slut = sin_lut[n_samples_max/2+f];

    for (aarx=0; aarx<desc->nb_rx; aarx++) {
      for (aatx=0; aatx<desc->nb_tx; aatx++) {
        desc->chF[aarx+(aatx*desc->nb_rx)][n_samples/2+f2].x=0.0;
        desc->chF[aarx+(aatx*desc->nb_rx)][n_samples/2+f2].y=0.0;

        for (l=0; l<(int)desc->nb_taps; l++) {

          desc->chF[aarx+(aatx*desc->nb_rx)][n_samples/2+f2].x+=(desc->a[l][aarx+(aatx*desc->nb_rx)].x*clut[l]+
              desc->a[l][aarx+(aatx*desc->nb_rx)].y*slut[l]);
          desc->chF[aarx+(aatx*desc->nb_rx)][n_samples/2+f2].y+=(-desc->a[l][aarx+(aatx*desc->nb_rx)].x*slut[l]+
              desc->a[l][aarx+(aatx*desc->nb_rx)].y*clut[l]);
        }
      }
    }
  }

  stop_meas(&desc->interp_freq);

  return(0);
}

double compute_pbch_sinr(channel_desc_t *desc,
                         channel_desc_t *desc_i1,
                         channel_desc_t *desc_i2,
                         double snr_dB,double snr_i1_dB,
                         double snr_i2_dB,
                         uint16_t nb_rb)
{

  double avg_sinr,snr=pow(10.0,.1*snr_dB),snr_i1=pow(10.0,.1*snr_i1_dB),snr_i2=pow(10.0,.1*snr_i2_dB);
  uint16_t f;
  uint8_t aarx,aatx;
  double S;
  struct complex S_i1;
  struct complex S_i2;

  avg_sinr=0.0;

  //  printf("nb_rb %d\n",nb_rb);
  for (f=(nb_rb-6); f<(nb_rb+6); f++) {
    S = 0.0;
    S_i1.x =0.0;
    S_i1.y =0.0;
    S_i2.x =0.0;
    S_i2.y =0.0;

    for (aarx=0; aarx<desc->nb_rx; aarx++) {
      for (aatx=0; aatx<desc->nb_tx; aatx++) {
        S    += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc->chF[aarx+(aatx*desc->nb_rx)][f].x +
                 desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc->chF[aarx+(aatx*desc->nb_rx)][f].y);
        //  printf("%d %d chF[%d] => (%f,%f)\n",aarx,aatx,f,desc->chF[aarx+(aatx*desc->nb_rx)][f].x,desc->chF[aarx+(aatx*desc->nb_rx)][f].y);

        if (desc_i1) {
          S_i1.x += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].x +
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].y);
          S_i1.y += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].y -
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].x);
        }

        if (desc_i2) {
          S_i2.x += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].x +
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].y);
          S_i2.y += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].y -
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].x);
        }
      }
    }

    //    printf("snr %f f %d : S %f, S_i1 %f, S_i2 %f\n",snr,f-nb_rb,S,snr_i1*sqrt(S_i1.x*S_i1.x + S_i1.y*S_i1.y),snr_i2*sqrt(S_i2.x*S_i2.x + S_i2.y*S_i2.y));
    avg_sinr += (snr*S/(desc->nb_tx+snr_i1*sqrt(S_i1.x*S_i1.x + S_i1.y*S_i1.y)+snr_i2*sqrt(S_i2.x*S_i2.x + S_i2.y*S_i2.y)));
  }

  //  printf("avg_sinr %f (%f,%f,%f)\n",avg_sinr/12.0,snr,snr_i1,snr_i2);
  return(10*log10(avg_sinr/12.0));
}

double compute_sinr(channel_desc_t *desc,
                    channel_desc_t *desc_i1,
                    channel_desc_t *desc_i2,
                    double snr_dB,double snr_i1_dB,
                    double snr_i2_dB,
                    uint16_t nb_rb)
{

  double avg_sinr,snr=pow(10.0,.1*snr_dB),snr_i1=pow(10.0,.1*snr_i1_dB),snr_i2=pow(10.0,.1*snr_i2_dB);
  uint16_t f;
  uint8_t aarx,aatx;
  double S;
  struct complex S_i1;
  struct complex S_i2;

  DevAssert( nb_rb > 0 );

  avg_sinr=0.0;

  //  printf("nb_rb %d\n",nb_rb);
  for (f=0; f<2*nb_rb; f++) {
    S = 0.0;
    S_i1.x =0.0;
    S_i1.y =0.0;
    S_i2.x =0.0;
    S_i2.y =0.0;

    for (aarx=0; aarx<desc->nb_rx; aarx++) {
      for (aatx=0; aatx<desc->nb_tx; aatx++) {
        S    += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc->chF[aarx+(aatx*desc->nb_rx)][f].x +
                 desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc->chF[aarx+(aatx*desc->nb_rx)][f].y);

        if (desc_i1) {
          S_i1.x += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].x +
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].y);
          S_i1.y += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].y -
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i1->chF[aarx+(aatx*desc->nb_rx)][f].x);
        }

        if (desc_i2) {
          S_i2.x += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].x +
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].y);
          S_i2.y += (desc->chF[aarx+(aatx*desc->nb_rx)][f].x*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].y -
                     desc->chF[aarx+(aatx*desc->nb_rx)][f].y*desc_i2->chF[aarx+(aatx*desc->nb_rx)][f].x);
        }
      }
    }

    //        printf("f %d : S %f, S_i1 %f, S_i2 %f\n",f-nb_rb,snr*S,snr_i1*sqrt(S_i1.x*S_i1.x + S_i1.y*S_i1.y),snr_i2*sqrt(S_i2.x*S_i2.x + S_i2.y*S_i2.y));
    avg_sinr += (snr*S/(desc->nb_tx+snr_i1*sqrt(S_i1.x*S_i1.x + S_i1.y*S_i1.y)+snr_i2*sqrt(S_i2.x*S_i2.x + S_i2.y*S_i2.y)));
  }

  //  printf("avg_sinr %f (%f,%f,%f)\n",avg_sinr/12.0,snr,snr_i1,snr_i2);
  return(10*log10(avg_sinr/(nb_rb*2)));
}

int pbch_polynomial_degree=6;
double pbch_awgn_polynomial[7]= {-7.2926e-05, -2.8749e-03, -4.5064e-02, -3.5301e-01, -1.4655e+00, -3.6282e+00, -6.6907e+00};

void load_pbch_desc(FILE *pbch_file_fd)
{

  int i, ret;
  char dummy[25];

  ret = fscanf(pbch_file_fd,"%d",&pbch_polynomial_degree);

  if (ret < 0) {
    printf("fscanf failed: %s\n", strerror(errno));
    exit(EXIT_FAILURE);
  }

  if (pbch_polynomial_degree>6) {
    printf("Illegal degree for pbch interpolation polynomial %d\n",pbch_polynomial_degree);
    exit(-1);
  }

  printf("PBCH polynomial : ");

  for (i=0; i<=pbch_polynomial_degree; i++) {
    ret = fscanf(pbch_file_fd,"%s",dummy);

    if (ret < 0) {
      printf("fscanf failed: %s\n", strerror(errno));
      exit(EXIT_FAILURE);
    }

    pbch_awgn_polynomial[i] = strtod(dummy,NULL);
    printf("%f ",pbch_awgn_polynomial[i]);
  }

  printf("\n");
}

double pbch_bler(double sinr)
{

  int i;
  double log10_bler=pbch_awgn_polynomial[pbch_polynomial_degree];
  double sinrpow=sinr;
  double bler=0.0;

  //  printf("log10bler %f\n",log10_bler);
  if (sinr<-10.0)
    bler= 1.0;
  else if (sinr>=0.0)
    bler=0.0;
  else  {
    for (i=1; i<=pbch_polynomial_degree; i++) {
      //    printf("sinrpow %f\n",sinrpow);
      log10_bler += (pbch_awgn_polynomial[pbch_polynomial_degree-i]*sinrpow);
      sinrpow *= sinr;
      //    printf("log10bler %f\n",log10_bler);
    }

    bler = pow(10.0,log10_bler);
  }

  //printf ("sinr %f bler %f\n",sinr,bler);
  return(bler);

}


/* interpolation/steffen.c
 *
 * Copyright (C) 2014 Jean-François Caron
 *
 * 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:  J.-F. Caron
 *
 * This interpolation method is taken from
 * M.Steffen, "A simple method for monotonic interpolation in one dimension",
 * Astron. Astrophys. 239, 443-450 (1990).
 *
 * This interpolation method guarantees monotonic interpolation functions between
 * the given data points.  A consequence of this is that extremal values can only
 * occur at the data points.  The interpolating function and its first derivative
 * are guaranteed to be continuous, but the second derivative is not.
 *
 * The implementation is modelled on the existing Akima interpolation method
 * previously included in GSL by Gerard Jungman.
 */

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "integ_eval.h"
#include <gsl/gsl_interp.h>

typedef struct
{
  double * a;       /* eqs 2-5 of paper */
  double * b;
  double * c;
  double * d;

  double * y_prime; /* eq 11 of paper */
} steffen_state_t;

static void steffen_free (void * vstate);
static double steffen_copysign(const double x, const double y);

static void *
steffen_alloc (size_t size)
{
  steffen_state_t *state;

  state = (steffen_state_t *) calloc (1, sizeof (steffen_state_t));

  if (state == NULL)
    {
      GSL_ERROR_NULL("failed to allocate space for state", GSL_ENOMEM);
    }

  state->a = (double *) malloc (size * sizeof (double));

  if (state->a == NULL)
    {
      steffen_free(state);
      GSL_ERROR_NULL("failed to allocate space for a", GSL_ENOMEM);
    }

  state->b = (double *) malloc (size * sizeof (double));

  if (state->b == NULL)
    {
      steffen_free(state);
      GSL_ERROR_NULL("failed to allocate space for b", GSL_ENOMEM);
    }

  state->c = (double *) malloc (size * sizeof (double));

  if (state->c == NULL)
    {
      steffen_free(state);
      GSL_ERROR_NULL("failed to allocate space for c", GSL_ENOMEM);
    }

  state->d = (double *) malloc (size * sizeof (double));

  if (state->d == NULL)
    {
      steffen_free(state);
      GSL_ERROR_NULL("failed to allocate space for d", GSL_ENOMEM);
    }

  state->y_prime = (double *) malloc (size * sizeof (double));
  if (state->y_prime == NULL)
    {
      steffen_free(state);
      GSL_ERROR_NULL("failed to allocate space for y_prime", GSL_ENOMEM);
    }

  return state;
}

static int
steffen_init (void * vstate, const double x_array[],
              const double y_array[], size_t size)
{
  steffen_state_t *state = (steffen_state_t *) vstate;
  size_t i;
  double *a = state->a;
  double *b = state->b;
  double *c = state->c;
  double *d = state->d;
  double *y_prime = state->y_prime;

  /*
   * first assign the interval and slopes for the left boundary.
   * We use the "simplest possibility" method described in the paper
   * in section 2.2
   */
  double h0 = (x_array[1] - x_array[0]);
  double s0 = (y_array[1] - y_array[0]) / h0;

  y_prime[0] = s0;

  /* Now we calculate all the necessary s, h, p, and y' variables
     from 1 to N-2 (0 to size - 2 inclusive) */
  for (i = 1; i < (size - 1); i++)
    {
      double pi;

      /* equation 6 in the paper */
      double hi = (x_array[i+1] - x_array[i]);
      double him1 = (x_array[i] - x_array[i - 1]);

      /* equation 7 in the paper */
      double si = (y_array[i+1] - y_array[i]) / hi;
      double sim1 = (y_array[i] - y_array[i - 1]) / him1;

      /* equation 8 in the paper */
      pi = (sim1*hi + si*him1) / (him1 + hi);

      /* This is a C equivalent of the FORTRAN statement below eqn 11 */
      y_prime[i] = (steffen_copysign(1.0,sim1) + steffen_copysign(1.0,si)) *
                    GSL_MIN(fabs(sim1),
                            GSL_MIN(fabs(si), 0.5*fabs(pi)));
    }

  /*
   * we also need y' for the rightmost boundary; we use the
   * "simplest possibility" method described in the paper in
   * section 2.2
   *
   * y' = s_{n-1}
   */
  y_prime[size-1] = (y_array[size - 1] - y_array[size - 2]) /
                    (x_array[size - 1] - x_array[size - 2]);

  /* Now we can calculate all the coefficients for the whole range. */
  for (i = 0; i < (size - 1); i++)
    {
      double hi = (x_array[i+1] - x_array[i]);
      double si = (y_array[i+1] - y_array[i]) / hi;

      /* These are from equations 2-5 in the paper. */
      a[i] = (y_prime[i] + y_prime[i+1] - 2*si) / hi / hi;
      b[i] = (3*si - 2*y_prime[i] - y_prime[i+1]) / hi;
      c[i] = y_prime[i];
      d[i] = y_array[i];
    }

  return GSL_SUCCESS;
}

static void
steffen_free (void * vstate)
{
  steffen_state_t *state = (steffen_state_t *) vstate;

  RETURN_IF_NULL(state);

  if (state->a)
    free (state->a);

  if (state->b)
    free (state->b);

  if (state->c)
    free (state->c);

  if (state->d)
    free (state->d);

  if (state->y_prime)
    free (state->y_prime);

  free (state);
}

static int
steffen_eval (const void * vstate,
              const double x_array[], const double y_array[], size_t size,
              double x, gsl_interp_accel * a, double *y)
{
  const steffen_state_t *state = (const steffen_state_t *) vstate;

  size_t index;

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  {
    const double x_lo = x_array[index];
    const double delx = x - x_lo;
    const double a = state->a[index];
    const double b = state->b[index];
    const double c = state->c[index];
    const double d = state->d[index];
    /* Use Horner's scheme for efficient evaluation of polynomials. */
    /* *y = a*delx*delx*delx + b*delx*delx + c*delx + d; */
    *y = d + delx*(c + delx*(b + delx*a));

    return GSL_SUCCESS;
  }
}

static int
steffen_eval_deriv (const void * vstate,
                    const double x_array[], const double y_array[], size_t size,
                    double x, gsl_interp_accel * a, double *dydx)
{
  const steffen_state_t *state = (const steffen_state_t *) vstate;

  size_t index;

  /* DISCARD_POINTER(y_array); /\* prevent warning about unused parameter *\/ */

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  {
    double x_lo = x_array[index];
    double delx = x - x_lo;
    double a = state->a[index];
    double b = state->b[index];
    double c = state->c[index];
    /*double d = state->d[index];*/
    /* *dydx = 3*a*delx*delx*delx + 2*b*delx + c; */
    *dydx = c + delx*(2*b + delx*3*a);
    return GSL_SUCCESS;
  }
}

static int
steffen_eval_deriv2 (const void * vstate,
                     const double x_array[], const double y_array[], size_t size,
                     double x, gsl_interp_accel * a, double *y_pp)
{
  const steffen_state_t *state = (const steffen_state_t *) vstate;

  size_t index;

  /* DISCARD_POINTER(y_array); /\* prevent warning about unused parameter *\/ */

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  {
    const double x_lo = x_array[index];
    const double delx = x - x_lo;
    const double a = state->a[index];
    const double b = state->b[index];
    *y_pp = 6*a*delx + 2*b;
    return GSL_SUCCESS;
  }
}

static int
steffen_eval_integ (const void * vstate,
                    const double x_array[], const double y_array[], size_t size,
                    gsl_interp_accel * acc, double a, double b,
                    double * result)
{
  /* a and b are the boundaries of the integration. */

  const steffen_state_t *state = (const steffen_state_t *) vstate;

  size_t i, index_a, index_b;

  /* Find the data points in the x_array that are nearest to the desired */
  /* a and b integration boundaries. */

  if (acc != 0)
    {
      index_a = gsl_interp_accel_find (acc, x_array, size, a);
      index_b = gsl_interp_accel_find (acc, x_array, size, b);
    }
  else
    {
      index_a = gsl_interp_bsearch (x_array, a, 0, size - 1);
      index_b = gsl_interp_bsearch (x_array, b, 0, size - 1);
    }

  *result = 0.0;

  /* Iterate over all the segments between data points and sum the */
  /* contributions into result. */
  for(i=index_a; i<=index_b; i++)
    {
      const double x_hi = x_array[i + 1];
      const double x_lo = x_array[i];
      const double dx = x_hi - x_lo;
      if(dx != 0.0)
        {
          /*
           * check if we are at a boundary point, so take the
           * a and b parameters instead of the data points.
           */
          double x1 = (i == index_a) ? a-x_lo : 0.0;
          double x2 = (i == index_b) ? b-x_lo : x_hi-x_lo;

          *result += (1.0/4.0)*state->a[i]*(x2*x2*x2*x2 - x1*x1*x1*x1)
                    +(1.0/3.0)*state->b[i]*(x2*x2*x2 - x1*x1*x1)
                    +(1.0/2.0)*state->c[i]*(x2*x2 - x1*x1)
                    +state->d[i]*(x2-x1);
        }
      else /* if the interval was zero, i.e. consecutive x values in data. */
        {
          *result = 0.0;
          return GSL_EINVAL;
        }
    }

  return GSL_SUCCESS;
}

static double
steffen_copysign(const double x, const double y)
{
  if ((x < 0 && y > 0) || (x > 0 && y < 0))
    return -x;

  return x;
}

static const gsl_interp_type steffen_type =
{
  "steffen",
  3,
  &steffen_alloc,
  &steffen_init,
  &steffen_eval,
  &steffen_eval_deriv,
  &steffen_eval_deriv2,
  &steffen_eval_integ,
  &steffen_free
};

const gsl_interp_type * gsl_interp_steffen = &steffen_type;


/* cdf/tdist.c
 *
 * Copyright (C) 2002 Jason H. Stover.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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.
 */

/*
 * Computes the Student's t cumulative distribution function using
 * the method detailed in
 *
 * W.J. Kennedy and J.E. Gentle, "Statistical Computing." 1980.
 * Marcel Dekker. ISBN 0-8247-6898-1.
 *
 * G.W. Hill and A.W. Davis. "Generalized asymptotic expansions
 * of Cornish-Fisher type." Annals of Mathematical Statistics,
 * vol. 39, 1264-1273. 1968.
 *
 * G.W. Hill. "Algorithm 395: Student's t-distribution," Communications
 * of the ACM, volume 13, number 10, page 617. October 1970.
 *
 * G.W. Hill, "Remark on algorithm 395: Student's t-distribution,"
 * Transactions on Mathematical Software, volume 7, number 2, page
 * 247. June 1982.
 */

#include <config.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>

#include "beta_inc.c"

static double
poly_eval (const double c[], unsigned int n, double x)
{
  unsigned int i;
  double y = c[0] * x;

  for (i = 1; i < n; i++)
    {
      y = x * (y + c[i]);
    }

  y += c[n];

  return y;
}

/*
 * Use the Cornish-Fisher asymptotic expansion to find a point u such
 * that gsl_cdf_gauss(y) = tcdf(t).
 *
 */

static double
cornish_fisher (double t, double n)
{
  const double coeffs6[10] = {
    0.265974025974025974026,
    5.449696969696969696970,
    122.20294372294372294372,
    2354.7298701298701298701,
    37625.00902597402597403,
    486996.1392857142857143,
    4960870.65,
    37978595.55,
    201505390.875,
    622437908.625
  };
  const double coeffs5[8] = {
    0.2742857142857142857142,
    4.499047619047619047619,
    78.45142857142857142857,
    1118.710714285714285714,
    12387.6,
    101024.55,
    559494.0,
    1764959.625
  };
  const double coeffs4[6] = {
    0.3047619047619047619048,
    3.752380952380952380952,
    46.67142857142857142857,
    427.5,
    2587.5,
    8518.5
  };
  const double coeffs3[4] = {
    0.4,
    3.3,
    24.0,
    85.5
  };

  double a = n - 0.5;
  double b = 48.0 * a * a;

  double z2 = a * log1p (t * t / n);
  double z = sqrt (z2);

  double p5 = z * poly_eval (coeffs6, 9, z2);
  double p4 = -z * poly_eval (coeffs5, 7, z2);
  double p3 = z * poly_eval (coeffs4, 5, z2);
  double p2 = -z * poly_eval (coeffs3, 3, z2);
  double p1 = z * (z2 + 3.0);
  double p0 = z;

  double y = p5;
  y = (y / b) + p4;
  y = (y / b) + p3;
  y = (y / b) + p2;
  y = (y / b) + p1;
  y = (y / b) + p0;

  if (t < 0)
    y *= -1;

  return y;
}

#if 0
/*
 * Series approximation for t > 4.0. This needs to be fixed;
 * it shouldn't subtract the result from 1.0. A better way is
 * to use two different series expansions. Figuring this out
 * means rummaging through Fisher's paper in Metron, v5, 1926,
 * "Expansion of Student's integral in powers of n^{-1}."
 */

#define MAXI 40

static double
normal_approx (const double x, const double nu)
{
  double y;
  double num;
  double diff;
  double q;
  int i;
  double lg1, lg2;

  y = 1 / sqrt (1 + x * x / nu);
  num = 1.0;
  q = 0.0;
  diff = 2 * GSL_DBL_EPSILON;
  for (i = 2; (i < MAXI) && (diff > GSL_DBL_EPSILON); i += 2)
    {
      diff = q;
      num *= y * y * (i - 1) / i;
      q += num / (nu + i);
      diff = q - diff;
    }
  q += 1 / nu;
  lg1 = gsl_sf_lngamma (nu / 2.0);
  lg2 = gsl_sf_lngamma ((nu + 1.0) / 2.0);

  diff = lg2 - lg1;
  q *= pow (y, nu) * exp (diff) / sqrt (M_PI);

  return q;
}
#endif

double
gsl_cdf_tdist_P (const double x, const double nu)
{
  double P;

  double x2 = x * x;

  if (nu > 30 && x2 < 10 * nu)
    {
      double u = cornish_fisher (x, nu);
      P = gsl_cdf_ugaussian_P (u);

      return P;
    }

  if (x2 < nu)
    {
      double u = x2 / nu;
      double eps = u / (1 + u);

      if (x >= 0)
        {
          P = beta_inc_AXPY (0.5, 0.5, 0.5, nu / 2.0, eps);
        }
      else
        {
          P = beta_inc_AXPY (-0.5, 0.5, 0.5, nu / 2.0, eps);
        }
    }
  else
    {
      double v = nu / (x * x);
      double eps = v / (1 + v);

      if (x >= 0)
        {
          P = beta_inc_AXPY (-0.5, 1.0, nu / 2.0, 0.5, eps);
        }
      else
        {
          P = beta_inc_AXPY (0.5, 0.0, nu / 2.0, 0.5, eps);
        }
    }

  return P;
}

double
gsl_cdf_tdist_Q (const double x, const double nu)
{
  double Q;

  double x2 = x * x;

  if (nu > 30 && x2 < 10 * nu)
    {
      double u = cornish_fisher (x, nu);
      Q = gsl_cdf_ugaussian_Q (u);

      return Q;
    }

  if (x2 < nu)
    {
      double u = x2 / nu;
      double eps = u / (1 + u);

      if (x >= 0)
        {
          Q = beta_inc_AXPY (-0.5, 0.5, 0.5, nu / 2.0, eps);
        }
      else
        {
          Q = beta_inc_AXPY (0.5, 0.5, 0.5, nu / 2.0, eps);
        }
    }
  else
    {
      double v = nu / (x * x);
      double eps = v / (1 + v);

      if (x >= 0)
        {
          Q = beta_inc_AXPY (0.5, 0.0, nu / 2.0, 0.5, eps);
        }
      else
        {
          Q = beta_inc_AXPY (-0.5, 1.0, nu / 2.0, 0.5, eps);
        }
    }

  return Q;
}


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

#ifndef __GSL_MULTIFIT_H__
#define __GSL_MULTIFIT_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_types.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
{
  size_t nmax;         /* maximum number of observations */
  size_t pmax;         /* maximum number of parameters */
  size_t n;            /* number of observations in current SVD decomposition */
  size_t p;            /* number of parameters in current SVD decomposition */
  gsl_matrix * A;      /* least squares matrix for SVD, n-by-p */
  gsl_matrix * Q;
  gsl_matrix * QSI;
  gsl_vector * S;
  gsl_vector * t;
  gsl_vector * xt;
  gsl_vector * D;
  double rcond;        /* reciprocal condition number */
}
gsl_multifit_linear_workspace;

GSL_FUN gsl_multifit_linear_workspace *
gsl_multifit_linear_alloc (const size_t n, const size_t p);

GSL_FUN void
gsl_multifit_linear_free (gsl_multifit_linear_workspace * w);

GSL_FUN int
gsl_multifit_linear (const gsl_matrix * X,
                     const gsl_vector * y,
                     gsl_vector * c,
                     gsl_matrix * cov,
                     double * chisq,
                     gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_tsvd (const gsl_matrix * X,
                          const gsl_vector * y,
                          const double tol,
                          gsl_vector * c,
                          gsl_matrix * cov,
                          double * chisq,
                          size_t * rank,
                          gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_svd (const gsl_matrix * X,
                         gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_bsvd (const gsl_matrix * X,
                          gsl_multifit_linear_workspace * work);

GSL_FUN size_t
gsl_multifit_linear_rank(const double tol, const gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_solve (const double lambda,
                           const gsl_matrix * X,
                           const gsl_vector * y,
                           gsl_vector * c,
                           double *rnorm,
                           double *snorm,
                           gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_applyW(const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           gsl_matrix * WX,
                           gsl_vector * Wy);

GSL_FUN int
gsl_multifit_linear_stdform1 (const gsl_vector * L,
                              const gsl_matrix * X,
                              const gsl_vector * y,
                              gsl_matrix * Xs,
                              gsl_vector * ys,
                              gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_wstdform1 (const gsl_vector * L,
                               const gsl_matrix * X,
                               const gsl_vector * w,
                               const gsl_vector * y,
                               gsl_matrix * Xs,
                               gsl_vector * ys,
                               gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_L_decomp (gsl_matrix * L, gsl_vector * tau);

GSL_FUN int
gsl_multifit_linear_stdform2 (const gsl_matrix * LQR,
                              const gsl_vector * Ltau,
                              const gsl_matrix * X,
                              const gsl_vector * y,
                              gsl_matrix * Xs,
                              gsl_vector * ys,
                              gsl_matrix * M,
                              gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_wstdform2 (const gsl_matrix * LQR,
                               const gsl_vector * Ltau,
                               const gsl_matrix * X,
                               const gsl_vector * w,
                               const gsl_vector * y,
                               gsl_matrix * Xs,
                               gsl_vector * ys,
                               gsl_matrix * M,
                               gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_genform1 (const gsl_vector * L,
                              const gsl_vector * cs,
                              gsl_vector * c,
                              gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_genform2 (const gsl_matrix * LQR,
                              const gsl_vector * Ltau,
                              const gsl_matrix * X,
                              const gsl_vector * y,
                              const gsl_vector * cs,
                              const gsl_matrix * M,
                              gsl_vector * c,
                              gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_wgenform2 (const gsl_matrix * LQR,
                               const gsl_vector * Ltau,
                               const gsl_matrix * X,
                               const gsl_vector * w,
                               const gsl_vector * y,
                               const gsl_vector * cs,
                               const gsl_matrix * M,
                               gsl_vector * c,
                               gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_lreg (const double smin, const double smax,
                          gsl_vector * reg_param);

GSL_FUN int
gsl_multifit_linear_lcurve (const gsl_vector * y,
                            gsl_vector * reg_param,
                            gsl_vector * rho, gsl_vector * eta,
                            gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_lcurvature (const gsl_vector * y,
                                const gsl_vector * reg_param,
                                const gsl_vector * rho,
                                const gsl_vector * eta,
                                gsl_vector * kappa,
                                gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_lcorner(const gsl_vector *rho,
                            const gsl_vector *eta,
                            size_t *idx);

GSL_FUN int
gsl_multifit_linear_lcorner2(const gsl_vector *reg_param,
                             const gsl_vector *eta,
                             size_t *idx);

GSL_FUN int
gsl_multifit_linear_Lk(const size_t p, const size_t k, gsl_matrix *L);

GSL_FUN int
gsl_multifit_linear_Lsobolev(const size_t p, const size_t kmax,
                             const gsl_vector *alpha, gsl_matrix *L,
                             gsl_multifit_linear_workspace *work);

GSL_FUN int
gsl_multifit_wlinear (const gsl_matrix * X,
                      const gsl_vector * w,
                      const gsl_vector * y,
                      gsl_vector * c,
                      gsl_matrix * cov,
                      double * chisq,
                      gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_wlinear_tsvd (const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           const double tol,
                           gsl_vector * c,
                           gsl_matrix * cov,
                           double * chisq,
                           size_t * rank,
                           gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_wlinear_svd (const gsl_matrix * X,
                          const gsl_vector * w,
                          const gsl_vector * y,
                          double tol,
                          size_t * rank,
                          gsl_vector * c,
                          gsl_matrix * cov,
                          double *chisq,
                          gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_wlinear_usvd (const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           double tol,
                           size_t * rank,
                           gsl_vector * c,
                           gsl_matrix * cov,
                           double *chisq,
                           gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_est (const gsl_vector * x,
                         const gsl_vector * c,
                         const gsl_matrix * cov, double *y, double *y_err);

GSL_FUN double
gsl_multifit_linear_rcond (const gsl_multifit_linear_workspace * w);

GSL_FUN int
gsl_multifit_linear_residuals (const gsl_matrix *X, const gsl_vector *y,
                               const gsl_vector *c, gsl_vector *r);

/* gcv.c */
GSL_FUN int
gsl_multifit_linear_gcv_init(const gsl_vector * y,
                             gsl_vector * reg_param,
                             gsl_vector * UTy,
                             double * delta0,
                             gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_gcv_curve(const gsl_vector * reg_param,
                              const gsl_vector * UTy,
                              const double delta0,
                              gsl_vector * G,
                              gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_gcv_min(const gsl_vector * reg_param,
                            const gsl_vector * UTy,
                            const gsl_vector * G,
                            const double delta0,
                            double * lambda,
                            gsl_multifit_linear_workspace * work);

GSL_FUN double
gsl_multifit_linear_gcv_calc(const double lambda,
                             const gsl_vector * UTy,
                             const double delta0,
                             gsl_multifit_linear_workspace * work);

GSL_FUN int
gsl_multifit_linear_gcv(const gsl_vector * y,
                        gsl_vector * reg_param,
                        gsl_vector * G,
                        double * lambda,
                        double * G_lambda,
                        gsl_multifit_linear_workspace * work);

typedef struct
{
  const char * name;     /* method name */
  int (*wfun)(const gsl_vector *r, gsl_vector *w);
  int (*psi_deriv)(const gsl_vector *r, gsl_vector *dpsi);
  double tuning_default; /* default tuning constant */
} gsl_multifit_robust_type;

typedef struct
{
  double sigma_ols;    /* OLS estimate of sigma */
  double sigma_mad;    /* MAD estimate of sigma */
  double sigma_rob;    /* robust estimate of sigma */
  double sigma;        /* final estimate of sigma */
  double Rsq;          /* R^2 coefficient of determination */
  double adj_Rsq;      /* degree of freedom adjusted R^2 */
  double rmse;         /* root mean squared error */
  double sse;          /* residual sum of squares */
  size_t dof;          /* degrees of freedom */
  size_t numit;        /* number of iterations */
  gsl_vector *weights; /* final weights */
  gsl_vector *r;       /* final residuals y - X c */
} gsl_multifit_robust_stats;

typedef struct
{
  size_t n;            /* number of observations */
  size_t p;            /* number of parameters */
  size_t numit;        /* number of iterations */
  size_t maxiter;      /* maximum iterations */
  const gsl_multifit_robust_type *type;
  double tune;         /* tuning parameter */

  gsl_vector *r;       /* residuals at current iteration */
  gsl_vector *weights; /* weights at current iteration */
  gsl_vector *c_prev;  /* coefficients from previous iteration */
  gsl_vector *resfac;  /* multiplicative factors for residuals */

  gsl_vector *psi;     /* psi(r) */
  gsl_vector *dpsi;    /* psi'(r) */

  gsl_matrix *QSI;     /* Q S^{-1} of original matrix X */
  gsl_vector *D;       /* balancing parameters of original matrix X */

  gsl_vector *workn;   /* workspace of length n */

  gsl_multifit_robust_stats stats; /* various statistics */

  gsl_multifit_linear_workspace *multifit_p;
} gsl_multifit_robust_workspace;

/* available types */
GSL_VAR const gsl_multifit_robust_type * gsl_multifit_robust_default;
GSL_VAR const gsl_multifit_robust_type * gsl_multifit_robust_bisquare;
GSL_VAR const gsl_multifit_robust_type * gsl_multifit_robust_cauchy;
GSL_VAR const gsl_multifit_robust_type * gsl_multifit_robust_fair;
GSL_VAR const gsl_multifit_robust_type * gsl_multifit_robust_huber;
GSL_VAR const gsl_multifit_robust_type * gsl_multifit_robust_ols;
GSL_VAR const gsl_multifit_robust_type * gsl_multifit_robust_welsch;

GSL_FUN gsl_multifit_robust_workspace *gsl_multifit_robust_alloc(const gsl_multifit_robust_type *T,
                                                         const size_t n, const size_t p);
GSL_FUN void gsl_multifit_robust_free(gsl_multifit_robust_workspace *w);
GSL_FUN int gsl_multifit_robust_tune(const double tune,
                             gsl_multifit_robust_workspace *w);
GSL_FUN int gsl_multifit_robust_maxiter(const size_t maxiter,
                                gsl_multifit_robust_workspace *w);
GSL_FUN const char *gsl_multifit_robust_name(const gsl_multifit_robust_workspace *w);
GSL_FUN gsl_multifit_robust_stats gsl_multifit_robust_statistics(const gsl_multifit_robust_workspace *w);
GSL_FUN int gsl_multifit_robust_weights(const gsl_vector *r, gsl_vector *wts,
                                gsl_multifit_robust_workspace *w);
GSL_FUN int gsl_multifit_robust(const gsl_matrix * X, const gsl_vector * y,
                        gsl_vector * c, gsl_matrix *cov,
                        gsl_multifit_robust_workspace *w);
GSL_FUN int gsl_multifit_robust_est(const gsl_vector * x, const gsl_vector * c,
                            const gsl_matrix * cov, double *y, double *y_err);
GSL_FUN int gsl_multifit_robust_residuals(const gsl_matrix * X,
                                  const gsl_vector * y,
                                  const gsl_vector * c, gsl_vector * r,
                                  gsl_multifit_robust_workspace * w);

__END_DECLS

#endif /* __GSL_MULTIFIT_H__ */


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

#ifndef _geru_h
#define _geru_h

/// @file geru.h Performs complex vector outer product.

#include "latl.h"

namespace LATL
{
   /// @brief Performs a vector outer product of two complex vectors.
   ///
   /// For a complex matrix A, complex vectors x and y, and complex scalar alpha,
   ///
   ///        A := alpha * x * y.' + A
   ///
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param m Specifies the number of rows of the matrix A.  m>=0
   /// @param n Specifies the number of coumns 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 m-by-n matrix A.
   /// @param ldA Column length of matrix A.  ldA>=m.
   /// @ingroup BLAS

   template <typename real_t>
   int GERU(int_t m, 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;

      const complex<real_t> zero(0.0,0.0);
      int_t i,j,kx,jy,ix;

      if(m<0)
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(incy==0)
         return -7;
      else if(ldA<m)
         return -9;
      else if((m==0)||(n==0)||(alpha==zero))
         return 0;

      jy=(incy>0)?0:(1-n)*incy;
      kx=(incx>0)?0:(1-m)*incx;

      if(incx==1)
      {
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               A[i]+=x[i]*alpha*y[jy];
            jy+=incy;
            A+=ldA;
         }
      }
      else
      {
         for(j=0;j<n;j++)
         {
            ix=kx;
            for(i=0;i<m;i++)
            {
               A[i]+=x[ix]*alpha*y[jy];
               ix+=incx;
            }
            A+=ldA;
            jy+=incy;
         }
      }
      return 0;
   }
#ifdef __latl_cblas
#include <cblas.h>

   template <> int GERU<float>(int_t m, int_t n, complex<float> alpha, complex<float> *x, int_t incx, complex<float> *y, int_t incy, complex<float> *A, int_t ldA)
   {
      if(m<0)
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(incy==0)
         return -7;
      else if(ldA<m)
         return -9;

      cblas_cgeru(CblasColMajor,m,n,&alpha,x,incx,y,incy,A,ldA);

      return 0;
   }

   template <> int GERU<double>(int_t m, int_t n, complex<double> alpha, complex<double> *x, int_t incx, complex<double> *y, int_t incy, complex<double> *A, int_t ldA)
   {
      if(m<0)
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(incy==0)
         return -7;
      else if(ldA<m)
         return -9;

      cblas_cgeru(CblasColMajor,m,n,&alpha,x,incx,y,incy,A,ldA);

      return 0;
   }

#endif
}
#endif


/* linalg/exponential.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2001, 2002 Gerard Jungman, 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.
 */

/* Author:  G. Jungman */

/* Calculate the matrix exponential, following
 * Moler + Van Loan, SIAM Rev. 20, 801 (1978).
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_mode.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>

#include "gsl_linalg.h"

/* store one of the suggested choices for the
 * Taylor series / square  method from Moler + VanLoan
 */
struct moler_vanloan_optimal_suggestion
{
  int k;
  int j;
};
typedef  struct moler_vanloan_optimal_suggestion  mvl_suggestion_t;

/* table from Moler and Van Loan
 * mvl_tab[gsl_mode_t][matrix_norm_group]
 */
static mvl_suggestion_t mvl_tab[3][6] =
{
  /* double precision */
  {
    { 5, 1 }, { 5, 4 }, { 7, 5 }, { 9, 7 }, { 10, 10 }, { 8, 14 }
  },

  /* single precision */
  {
    { 2, 1 }, { 4, 0 }, { 7, 1 }, { 6, 5 }, { 5, 9 }, { 7, 11 }
  },

  /* approx precision */
  {
    { 1, 0 }, { 3, 0 }, { 5, 1 }, { 4, 5 }, { 4, 8 }, { 2, 11 }
  }
};

inline
static double
sup_norm(const gsl_matrix * A)
{
  double min, max;
  gsl_matrix_minmax(A, &min, &max);
  return GSL_MAX_DBL(fabs(min), fabs(max));
}

static
mvl_suggestion_t
obtain_suggestion(const gsl_matrix * A, gsl_mode_t mode)
{
  const unsigned int mode_prec = GSL_MODE_PREC(mode);
  const double norm_A = sup_norm(A);
  if(norm_A < 0.01) return mvl_tab[mode_prec][0];
  else if(norm_A < 0.1) return mvl_tab[mode_prec][1];
  else if(norm_A < 1.0) return mvl_tab[mode_prec][2];
  else if(norm_A < 10.0) return mvl_tab[mode_prec][3];
  else if(norm_A < 100.0) return mvl_tab[mode_prec][4];
  else if(norm_A < 1000.0) return mvl_tab[mode_prec][5];
  else
  {
    /* outside the table we simply increase the number
     * of squarings, bringing the reduced matrix into
     * the range of the table; this is obviously suboptimal,
     * but that is the price paid for not having those extra
     * table entries
     */
    const double extra = log(1.01*norm_A/1000.0) / M_LN2;
    const int extra_i = (unsigned int) ceil(extra);
    mvl_suggestion_t s = mvl_tab[mode][5];
    s.j += extra_i;
    return s;
  }
}

/* use series representation to calculate matrix exponential;
 * this is used for small matrices; we use the sup_norm
 * to measure the size of the terms in the expansion
 */
static void
matrix_exp_series(
  const gsl_matrix * B,
  gsl_matrix * eB,
  int number_of_terms
  )
{
  int count;
  gsl_matrix * temp = gsl_matrix_calloc(B->size1, B->size2);

  /* init the Horner polynomial evaluation,
   * eB = 1 + B/number_of_terms; we use
   * eB to collect the partial results
   */
  gsl_matrix_memcpy(eB, B);
  gsl_matrix_scale(eB, 1.0/number_of_terms);
  gsl_matrix_add_diagonal(eB, 1.0);
  for(count = number_of_terms-1; count >= 1; --count)
  {
    /*  mult_temp = 1 + B eB / count  */
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, B, eB, 0.0, temp);
    gsl_matrix_scale(temp, 1.0/count);
    gsl_matrix_add_diagonal(temp, 1.0);

    /*  transfer partial result out of temp */
    gsl_matrix_memcpy(eB, temp);
  }

  /* now eB holds the full result; we're done */
  gsl_matrix_free(temp);
}

int
gsl_linalg_exponential_ss(
  const gsl_matrix * A,
  gsl_matrix * eA,
  gsl_mode_t mode
  )
{
  if(A->size1 != A->size2)
  {
    GSL_ERROR("cannot exponentiate a non-square matrix", GSL_ENOTSQR);
  }
  else if(A->size1 != eA->size1 || A->size2 != eA->size2)
  {
    GSL_ERROR("exponential of matrix must have same dimension as matrix", GSL_EBADLEN);
  }
  else
  {
    int i;
    const mvl_suggestion_t sugg = obtain_suggestion(A, mode);
    const double divisor = exp(M_LN2 * sugg.j);

    gsl_matrix * reduced_A = gsl_matrix_alloc(A->size1, A->size2);

    /*  decrease A by the calculated divisor  */
    gsl_matrix_memcpy(reduced_A, A);
    gsl_matrix_scale(reduced_A, 1.0/divisor);

    /*  calculate exp of reduced matrix; store in eA as temp  */
    matrix_exp_series(reduced_A, eA, sugg.k);

    /*  square repeatedly; use reduced_A for scratch */
    for(i = 0; i < sugg.j; ++i)
    {
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, eA, eA, 0.0, reduced_A);
      gsl_matrix_memcpy(eA, reduced_A);
    }

    gsl_matrix_free(reduced_A);

    return GSL_SUCCESS;
  }
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdarg.h>
#include <gbpLib.h>
#include <gbpMath.h>
#include <gbpCosmo_mass_functions.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

// Compute comoving mass function
double mass_function(double M_interp, double z, cosmo_info **cosmo, int mode, ...) {
    va_list vargs;
    va_start(vargs, mode);

    // Get passed parameters (if mode specifies they are there).
    double *P = NULL;
    if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS))
        P = (double *)va_arg(vargs, double *);

    // Initialize/compute some misc. cosmology things
    double sigma_interp = sqrt(power_spectrum_variance(k_of_M(M_interp, *cosmo), z, cosmo, PSPEC_LINEAR_TF, PSPEC_ALL_MATTER));
    double Omega_M      = ((double *)ADaPS_fetch(*cosmo, "Omega_M"))[0];
    double rho_o        = Omega_M * rho_crit_z(0, *cosmo);

    // Compute mass function (eqn 41 of Lukic et al, 2007, for example)
    double dlnInvs_dlogM = dln_Inv_sigma_dlogM(cosmo, M_interp, PSPEC_LINEAR_TF, PSPEC_ALL_MATTER);
    double rval          = rho_o * scaled_mass_function(sigma_interp, mode, P) * dlnInvs_dlogM / M_interp;
    va_end(vargs);
    return (rval);
}


/*
VEGAS_TABLE Version 1.  5/27/16
report issues to Tim Waters,
waters@lanl.gov

**** Basic code description:
Input: 	An ascii table with entries
		on a uniform grid of (x,y) values
		and a (x,y) value for which an
    interpolated value is sought
Output: The interpolated value
     corresponding to that (x,y) pair

**** Detailed description:

>> Step 1. Instantiation:
The class constructor calls a method
to parse an ascii text file which contains
an (N+1)x(M+1) table:
The first row contains the M values of x.
The first column contains the N values of y.
The remainder of the table contains the NxM
values of z.

Here is a simple example of an input file:

4	1e0     1e1     1e2     1e3
1e4	1e-20	2e-20	3e-20	4e-20
1e5	5e-20	6e-20	7e-20	8e-20
1e6	1e-19	2e-19	3e-19	4e-19
1e7	5e-19	6e-19	7e-19	8e-19
1e8	1e-18	2e-18	3e-18	4e-18

In this example, N = 5 and M = 4.
M must be specified as the first element.
The remaining elements in row 1 are values
of the photoionization parameter, xi.
The left column below M are the temperature
values.  The remaining elements are net heating
rates that are functions of T and xi.

By default, the input file is limited to size
1000 x 1000.  To increase this size limit,
increase Nmax in the default constructor.

>> Step 2. Table initialization:
The file parsed above is read into a
table, but this is not the actual table
used.  The actual table is made upon calling
an initialization function, and optionally
array arguments can be passed specifying the
min/max of both T and xi. This may be useful
when combining multiple overlapping tables
and will allow for flexibility if tables
generated from non-uniform grids are needed,
although a table-search algorithm has not
been implemented.  The input table is then
reduced to a size that contains only the
values between these limits.

>> Step 3. Invocation:
The code returns the interpolated value
of whatever rate was in the input file.
The code uses the bicubic and bilinear
interpolation algorithms from the GSL
library.  By default, bicubic is used.

Importantly, the code assumes that a
nearly uniform grid was produced via
x = j*(xmax-xmin)/(M-1) + xmin
y = i*(ymax-ymin)/(N-1) + ymin
This allows avoiding an expensive
table search for the lookup operation.
For a uniform grid generated from the
above formulas, the table indices
for a desired value of (x,y) are
i_x = (M-1)*(x - xmin)/(xmax - xmin)
i_y = (N-1)*(y - ymin)/(ymax - ymin)
Since xstar may slightly modify the
grid locations (x,y) originally
specified, our lookup operation
consists of first guessing that the
lookup value indices are (i_x,i_y)
and then allowing for some wiggle
room by searching 1 index left, right,
above, and below upon finding that our
guess was incorrect.  The code will
terminate if this search fails.

**** Example usages:
1. Bicubic interpolation on a table that was
generated from a log-log spaced grid
VEGAS_LUT hc_xstar("input_table.tab","bicubic");
hc_xstar.initialize_table();
hc_rate = hc_xstar.get_rate(T,xi);

1a. If lin-lin spacing was used instead, use
VEGAS_LUT hc_xstar("input_table.tab","bicubic",false,false);

1c. To apply a bounding box, use instead
const double T_bbox[2] = {1e5,1e7};
const double xi_bbox[2] = {1e1,5e2};
hc_xstar.initialize_table(T_bbox,xi_bbox);
*/

#ifndef H_VEGAS_TABLE
#define H_VEGAS_TABLE

#include <iostream>		// std::cout, std::endl
#include <iomanip>      // std::setprecision
#include <fstream>		// std::ifstream
#include <string>
#include <vector>
#include <time.h>
#include <cmath>

#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>

class VEGAS_LUT
{
public:
    //default constructor will parse the input file
    //log-log spacing for (T,xi) is assumed by default
	VEGAS_LUT(	const std::string a="VEGAS_table.tab",
				const std::string b="bicubic",
				bool c=true, bool d=true):
	filename(a),gsl_routine(b),logT(c),logxi(d)
	{
	 // allocate a (Nmax x Nmax) table for reading in the file
	  int Nmax = 1000;
	  input_table = new double*[Nmax];
	  for (int i = 0; i < Nmax; i++)
    	input_table[i] = new double[Nmax];
      xivals = new double[Nmax];
      Tvals = new double[Nmax];

      gsl_init();
      parse_file();
	}
	~VEGAS_LUT()  { free_memory(); }

	//user must call the initialize_table method
	void initialize_table(const double*, const double*);

	//user function that returns an interpolated value
	double get_rate(double, double);

	// actual table (T,xi) bounds:
	// hydro code needs to know Temp bounds when doing implicit root finding
	double y_min,y_max,x_min,x_max;

private:
	bool logT,logxi;					// determines log gridding
	double* xlim;						// input table xi bounds
	double* ylim;						// input table T bounds
	const double* T_bounds;				// user specified T bounds
	const double* xi_bounds;			// user specified xi bounds
	double** 	input_table; 			// pointer to input table
	double** 	table;					// pointer to actual table
	double* 	xivals; 				// xi-vals from input table
	double*		Tvals; 					// T-vals from input table
	double* 	xvals; 					// xi-vals for actual table
	double*		yvals; 					// T-vals for actual table
	unsigned int 	NT,	  // # rows of input table (# of Ts)
					Nxi,  // # cols of input table (# of xis)
					N,	  // # rows of actual table (# of Ts)
					M,	  // # cols of actual table (# of xis)
					i_T,  // T-offset between the input and actual tables
					i_xi; // xi-offset between the input and actual tables
	typedef std::vector<double> array_t;	//dynamic array data type
	std::string filename, gsl_routine;		//input strings

	//member functions for handling the input file
	void parse_file();
	bool read_value(std::ifstream&, const int, array_t&);
	void free_input_table();
	void free_memory();

	//other member functions
	void reduce_input_table();
	void make_table(unsigned int, unsigned int);
	void determine_box_indices(double, double, unsigned int&, unsigned int&);

	//GSL variables and member functions
	bool use_gsl_bicubic;
	const gsl_interp2d_type *gsl_scheme;
	typedef gsl_spline2d* spline_t;
	spline_t **spline_table;
    gsl_interp_accel *xacc, *yacc;
    size_t nx,ny;
    void gsl_init();
    void make_spline_table();
};

void VEGAS_LUT::gsl_init()
{
    if (gsl_routine == "bilinear")
    {
      gsl_scheme = gsl_interp2d_bilinear;
      use_gsl_bicubic = false;
      nx = 2;
      ny = 2;
    }
    else if (gsl_routine == "bicubic")
    {
      gsl_scheme = gsl_interp2d_bicubic;
      use_gsl_bicubic = true;
      nx = 4;
      ny = 4;
    }
    else
    {
      std::cout << "ERROR: GSL interpolation scheme " << gsl_routine
      << " not recognized. Choose from:\n"
      << "bilinear\n"
      << "bicubic\n"
      << std::endl;
      exit(0);
    }
}

bool VEGAS_LUT::read_value(std::ifstream& fin, const int j, array_t& input_data)
{
  double value;
  if (fin >> value)
  {
    input_data[j]=value;
    return true;
  }
  else return false;
}

void VEGAS_LUT::parse_file()
{
  std::ifstream fin(filename.c_str());
  if (!fin)
  {
    std::cout << "File " << filename << " does not exist!" << std::endl;
	exit(0);
  }
  bool file_is_open = false;
  bool end_of_line = false;

  // read in first value: Nxi
  array_t val1(1);
  file_is_open = read_value(fin,0,val1);
  Nxi = (unsigned int) val1[0];

  // read in remainder of first row: all xi-values
  array_t this_row(Nxi+1);
  int i=1,j;
  while(!end_of_line)
  {
    file_is_open = read_value(fin,i,this_row);
    xivals[i-1] = this_row[i];
    if( i!=0 && i%Nxi == 0 )
        end_of_line = true;
    else i++; //next row element
  }

  // read in 1st element of 2nd row: the 1st T value
  file_is_open = read_value(fin,0,this_row);
  Tvals[0] = this_row[0];

  // read in the rest of the file
  end_of_line = false;
  i = 1; j=0;
  while(file_is_open)
  {
    file_is_open = read_value(fin,i,this_row);

    if( i!=0 && i%Nxi == 0 )
    {
        end_of_line = true;
        i=0; // j++; //reset column, increment row
    }
    else i++; //next row element

    if(end_of_line)
    {
      // 1st element of this_row is T
      Tvals[j] = this_row[0];

      // remaining elements are rates
      for (int ii=0; ii<Nxi; ii++)
        //std::cout << "this_row[" << ii << "] = " << this_row[ii] << std::endl;
        input_table[j][ii] = this_row[ii+1];

      j++;
    }
    end_of_line = false;
  }

  NT = j; //number of temperature values
  fin.close();

  if (Nxi < 2 || NT < 2)
    {
    std::cout << "ERROR: Table dimensions must be at least 2x2!" << std::endl;
    exit(0);
    }
}

void VEGAS_LUT::make_table(unsigned int i0, unsigned int j0)
{

    // allocate memory
	table = new double*[N];
	for (int j = 0; j < N; j++)
    	table[j] = new double[M];

    // assign values
	for (int j = 0; j < N; j++)
	  for (int i = 0; i < M; i++)
	    table[j][i] = input_table[j0+j][i0+i];
}

void VEGAS_LUT::make_spline_table()
{

    // allocate memory for spline_table
	spline_table = new spline_t*[N];
	for (int j = 0; j < N; j++)
    	spline_table[j] = new spline_t[M];

    /* Each location in our table is to be thought of as a 4-corner box.
     * The corners have locations (xb[], yb[]) and function values (zb[])
     * and this data is collected ahead of time so that GSL only needs to
     * access it to carry out the interpolation.  For the case of bicubic
     * interpolation, GSL will also take numerical derivatives and store
     * all that data, so a lot of expense is saved by collecting this at
     * initialization. */
    double *xb = new double[nx];
	double *yb = new double[ny];
	double *zb = new double[nx*ny];

	/* Associations:
	T  = y(j), with j from (0,N-1)
	xi = x(i), with i from (0,M-1)
	*/
	for (int j=1; j < (N-2); j++)
      for (int i=1; i < (M-2); i++)
      {
      	// allocate storage for interpolation data
        gsl_spline2d *spline = gsl_spline2d_alloc(gsl_scheme, nx, ny);

        // evaluate position data (xb[],yb[]) and store function values (zb[]) to spline
	    for (int jb=0; jb < nx; jb++)
		  for (int ib=0; ib < ny; ib++)
		  {
		    if (use_gsl_bicubic)
		    {
		      xb[ib] = xvals[i+ib-1];
		      yb[jb] = yvals[j+jb-1];
			  gsl_spline2d_set(spline, zb, ib, jb, table[j+jb-1][i+ib-1]);
		    }
		    else
		    {
		      xb[ib] = xvals[i+ib];
		      yb[jb] = yvals[j+jb];
		      gsl_spline2d_set(spline, zb, ib, jb, table[j+jb][i+ib]);
		    }
		  }

          /* add position data
          * In the case of bicubic, this also internally calculates
          * derivative data and stores that as well. */
          gsl_spline2d_init(spline, xb, yb, zb, nx, ny);

          // store this interpolation data in our spline table
          spline_table[j][i] = spline;
      } //end loop

    /* The above loop excludes all boxes on table edges, which always require
     * doing bilinear interpolation, so add that data now */
    int j_edges[2] = {0, N-2};
    int i_edges[2] = {0, M-2};
    int i,j;

    for (int jj=0; jj < 2; jj++)
      for (int i=0; i < (M-1); i++)
      {
        j = j_edges[jj];

		gsl_spline2d *spline = gsl_spline2d_alloc(gsl_interp2d_bilinear, 2, 2);

		// evaluate position data (xb[],yb[]) and store function values (zb[]) to spline
		  for (int jb=0; jb < 2; jb++)
			for (int ib=0; ib < 2; ib++)
			{
				xb[ib] = xvals[i+ib];
				yb[jb] = yvals[j+jb];
				gsl_spline2d_set(spline, zb, ib, jb, table[j+jb][i+ib]);
			}

		/* add position data
        * In the case of bicubic, this also internally calculates
        * derivative data and stores that as well. */
        gsl_spline2d_init(spline, xb, yb, zb, 2, 2);

        // store this interpolation data in our spline table
        //std::cout<<  "(i,j) = (" << i << ", " << j << ")" << std::endl;
        spline_table[j][i] = spline;
      }

    for (int j=0; j < (N-1); j++)
      for (int ii=0; ii < 2; ii++)
      {
        i = i_edges[ii];

		gsl_spline2d *spline = gsl_spline2d_alloc(gsl_interp2d_bilinear, 2, 2);

		// evaluate position data (xb[],yb[]) and store function values (zb[]) to spline
		  for (int jb=0; jb < 2; jb++)
			for (int ib=0; ib < 2; ib++)
			{
				xb[ib] = xvals[i+ib];
				yb[jb] = yvals[j+jb];
				gsl_spline2d_set(spline, zb, ib, jb, table[j+jb][i+ib]);
			}

		/* add position data
        * In the case of bicubic, this also internally calculates
        * derivative data and stores that as well. */
        gsl_spline2d_init(spline, xb, yb, zb, 2, 2);

        // store this interpolation data in our spline table
        //std::cout<<  "(i,j) = (" << i << ", " << j << ")" << std::endl;
        spline_table[j][i] = spline;
      }

    // These I think are used when a lookup table must be searched
    // but since we know the lookup locations there is no point
    xacc = NULL; //gsl_interp_accel_alloc();
    yacc = NULL; //gsl_interp_accel_alloc();

}

void VEGAS_LUT::free_memory()
{
  for (int i = 0; i < N; i++)
  {
    delete[] table[i];
    delete[] spline_table[i];
  }

  //gsl_interp_accel_free(xacc);
  //gsl_interp_accel_free(yacc);

}

void VEGAS_LUT::free_input_table()
{
  for (int i = 0; i < N; i++)
    delete[] input_table[i];
}

void VEGAS_LUT::reduce_input_table()
{

    double Tmin,Tmax,ximin,ximax;
    Tmin = T_bounds[0];
    Tmax = T_bounds[1];
    ximin = xi_bounds[0];
    ximax = xi_bounds[1];

    if (Tmin < Tvals[0] || Tmax > Tvals[NT-1])
    {
    std::cout << "ERROR: Tmin or Tmax is not contained within file "
    << filename << "!"
    << std::endl;
    exit(0);
    }

    if (ximin < xivals[0] || ximax > xivals[Nxi-1])
    {
    std::cout << "ERROR: x_min or x_max is not contained within file "
    << filename << "!"
    << std::endl;
    exit(0);
    }

    // find the indices of the table corresponding to input min/max values
    int i = 0;
    while (Tvals[i] < Tmax && i < NT)
      i++;
    y_max = Tvals[i];  //so y_max is slightly greater than Tmax

    int ii = 0;
    while (Tvals[ii] <= Tmin && ii < i)
      ii++;
    y_min = Tvals[ii-1]; //so y_min is slightly less than Tmin

    // assign private variables
    i_T = ii-1; // start position in input table
    N = i - ii + 2; // # of T-vals in desired table

    i = 0;
    while (xivals[i] < ximax && i < Nxi)
      i++;
    x_max = xivals[i]; //so x_max is slightly greater than ximax

    ii = 0;
    while (xivals[ii] <= ximin && ii < i)
      ii++;
    x_min = xivals[ii-1]; //so x_min is slightly less than ximin

    // assign private variables
    i_xi = ii-1; // start position in input table
    M = i - ii + 2; // # of xi-vals in desired table
}

void VEGAS_LUT::initialize_table(const double * T_bbox = NULL, const double * xi_bbox = NULL)
{
	int i0,j0;

	if (T_bbox != NULL || xi_bbox != NULL)
	{
	  T_bounds = T_bbox;
	  xi_bounds = xi_bbox;
	  reduce_input_table(); // this sets i_xi, i_T, N, M, y_min, Tmax, x_min, x_max
	  i0 = i_xi;
	  j0 = i_T;
	}
	else // the actual table will be the same size as the input table
	{
	  y_min = Tvals[0];
	  y_max = Tvals[NT-1];
	  x_min = xivals[0];
	  x_max = xivals[Nxi-1];
	  N = NT;
	  M = Nxi;
	  i0 = 0;
	  j0 = 0;
	}

	// store the values of xi and T that will be used
    // they way they will be used (i.e. log or lin)
    xvals = new double[M];
    yvals = new double[N];
    double x,y;
    for (int i=0; i<M; i++)
    {
      if(logxi)	x = log10(xivals[i0+i]);
      else 		x = xivals[i0+i];
      xvals[i] = x;
    }
    for (int j=0; j<N; j++)
    {
      if (logT)	y = log10(Tvals[j0+j]);
      else 		y = Tvals[j0+j];
      yvals[j] = y;
    }

    // generate the actual table of H/C rates
	make_table(i0,j0);

	// alloc storage for max/min vals of T,xi
	xlim = new double[2]; // xi limits
  ylim = new double[2]; // T limits

    // set any log gridding: use limits of input table
	 if (logT) {
	   ylim[0] = log10(Tvals[0]);
	   ylim[1] = log10(Tvals[NT-1]); }
	  else {
	   ylim[0] = Tvals[0];
	   ylim[1] = Tvals[NT-1]; }

     if (logxi) {
       xlim[0] = log10(xivals[0]);
       xlim[1] = log10(xivals[Nxi-1]); }
     else {
       xlim[0] = xivals[0];
       xlim[1] = xivals[Nxi-1]; }

    // generate the spline table for GSL
	make_spline_table();

	// delete input_table
	free_input_table();

}

void VEGAS_LUT::determine_box_indices(double x, double y, unsigned int &i_x, unsigned int &i_y)
{
  unsigned int i,j;
  bool correct_guess = false;

  /* First guess the indices assuming the input table was
   * generated from a uniform grid: the actual xi and T values
   * that xstar produces data for may be slightly different
   * as xstar's stopping criteria algorithms may slightly adjust
   * the input values.  On rare occasions this guess can be off
   * by 1 index, so here we allow for that. */
  i = (Nxi-1) * (x - xlim[0]) / (xlim[1] - xlim[0]) - i_xi;
  j = (NT-1) * (y - ylim[0]) / (ylim[1] - ylim[0]) - i_T ;

  /* now we check our guess in x and if needed, adjust one box left/right */
  if (x > xvals[i] && x < xvals[i+1])  //then we guessed right
  {
    i_x = i;
    correct_guess = true;
  }

  if (!correct_guess)
  {
    if (i==0 || i==(M-1))
    {
      if (i==0 && x < xvals[2])
        i_x = 1;
      else if (i==(M-1) && x > xvals[M-2])
        i_x = M-2;
    }
    else if (x < xvals[i]) //then move to the left 1
      i_x = i-1;
    else if (x > xvals[i+1]) //then move to the right 1
      i_x = i+1;
    else // we give up
    {
      std::cout << "ERROR: search for the table xi-index failed!"
      			<< "\nDouble check inputs or try debugging "
      			<< "function determine_box_indices()"
      			<< std::endl;
      exit(0);
    }
  }

  /* now we check our guess in y and if needed, adjust one box up/down */
  correct_guess = false; // reset

  if (y > yvals[j] && y < yvals[j+1])  //then we guessed right
  {
    i_y = j;
    correct_guess = true;
  }

  if (!correct_guess)
  {
    if (j==0 || j==(N-1))
    {
      if (j==0 && y < yvals[2])
        i_y = 1;
      else if (j==(N-1) && y > yvals[N-2])
        i_y = N-2;
    }
    else if (y < yvals[j]) //then move down 1
      i_y = j-1;
    else if (y > yvals[j+1]) //then move up 1
      i_y = j+1;
    else // we give up
    {
      std::cout << "ERROR: search for the table T-index failed!"
      			<< "\nDouble check inputs or try debugging "
      			<< "function determine_box_indices()"
      			<< std::endl;
      exit(0);
    }
  }

}

double VEGAS_LUT::get_rate(double y_val, double x_val)
{
	unsigned int i_x,i_y;
	double x,y;

	/* terminate program if values exceed bounding box
    if (y_val < y_min || y_val > y_max || x_val < x_min || x_val > x_max)
    {
      std::cout
      << "\nFATAL ERROR: (y_val,x_val) = (" << y_val << ", " << x_val << ") "
      << "lies outside of table's bounding box!\n"
      << ">> Bounding Box:\n"
      << "(y_min,y_max) = (" << y_min << ", " << y_max << ")\n"
      << "(x_min,x_max) = (" << x_min << ", " << x_max << ")\n"
      << std::endl;
	  exit(0);
    }
    */

    /* use boundary rates for values outside bounding box */
    double eps = 1e-10;
    if (y_val < y_min) y_val = y_min*(1. + eps);
    if (y_val > y_max) y_val = y_max*(1. - eps);
    if (x_val < x_min) x_val = x_min*(1. + eps);
    if (x_val > x_max) x_val = x_max*(1. - eps);

	// apply any log scaling
	if (logT)  y = log10(y_val);
	else       y = y_val;
  if (logxi) x = log10(x_val);
  else       x = x_val;

	 determine_box_indices(x,y,i_x,i_y);

	return gsl_spline2d_eval(spline_table[i_y][i_x], x, y, xacc, yacc);
}

#endif //H_VEGAS_TABLE


/*
 Copyright (C) 2002 M. Marques, A. Castro, A. Rubio, G. Bertsch, M. Oliveira

 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, 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.

 $Id: minimizer_low.c 11822 2014-02-25 21:18:45Z dstrubbe $
*/

#include <config.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_multimin.h>

#include "string_f.h"

/* A. First, the interface to the gsl function that calculates the minimum
   of an N-dimensional function, with the knowledge of the function itself
   and its gradient. */

/* This is a type used to communicate with Fortran; func_d is the type of the
   interface to a Fortran subroutine that calculates the function and its
   gradient. */
typedef void (*func_d)(const int*, const double*, double*, const int*, double*);
typedef struct{
  func_d func;
} param_fdf_t;

/* Compute both f and df together. */
static void
my_fdf (const gsl_vector *v, void *params, double *f, gsl_vector *df)
{
  double *x, *gradient, ff2;
  int i, dim, getgrad;
  param_fdf_t * p;

  p = (param_fdf_t *) params;

  dim = v->size;
  x = (double *)malloc(dim*sizeof(double));
  gradient = (double *)malloc(dim*sizeof(double));

  for(i=0; i<dim; i++) x[i] = gsl_vector_get(v, i);
  getgrad = (df == NULL) ? 0 : 1;

  p->func(&dim, x, &ff2, &getgrad, gradient);

  if(f != NULL)
    *f = ff2;

  if(df != NULL){
    for(i=0; i<dim; i++)
      gsl_vector_set(df, i, gradient[i]);
  }

  free(x); free(gradient);
}

static double
my_f (const gsl_vector *v, void *params)
{
  double val;

  my_fdf(v, params, &val, NULL);
  return val;
}

/* The gradient of f, df = (df/dx, df/dy). */
static void
my_df (const gsl_vector *v, void *params, gsl_vector *df)
{
  my_fdf(v, params, NULL, df);
}

typedef void (*print_f_ptr)(const int*, const int*, const double*, const double*, const double*, const double*);

int FC_FUNC_(oct_minimize, OCT_MINIMIZE)
     (const int *method, const int *dim, double *point, const double *step, const double *line_tol,
      const double *tolgrad, const double *toldr, const int *maxiter, func_d f,
      const print_f_ptr write_info, double *minimum)
{
  int iter = 0;
  int status;
  double maxgrad, maxdr;
  int i;
  double * oldpoint;
  double * grad;

  const gsl_multimin_fdfminimizer_type *T = NULL;
  gsl_multimin_fdfminimizer *s;
  gsl_vector *x;
  gsl_vector *absgrad, *absdr;
  gsl_multimin_function_fdf my_func;

  param_fdf_t p;

  p.func = f;

  oldpoint = (double *) malloc(*dim * sizeof(double));
  grad     = (double *) malloc(*dim * sizeof(double));

  my_func.f = &my_f;
  my_func.df = &my_df;
  my_func.fdf = &my_fdf;
  my_func.n = *dim;
  my_func.params = (void *) &p;

  /* Starting point */
  x = gsl_vector_alloc (*dim);
  for(i=0; i<*dim; i++) gsl_vector_set (x, i, point[i]);

  /* Allocate space for the gradient */
  absgrad = gsl_vector_alloc (*dim);
  absdr = gsl_vector_alloc (*dim);

  //GSL recommends line_tol = 0.1;
  switch(*method){
  case 1:
    T = gsl_multimin_fdfminimizer_steepest_descent;
    break;
  case 2:
    T = gsl_multimin_fdfminimizer_conjugate_fr;
    break;
  case 3:
    T = gsl_multimin_fdfminimizer_conjugate_pr;
    break;
  case 4:
    T = gsl_multimin_fdfminimizer_vector_bfgs;
    break;
  case 5:
    T = gsl_multimin_fdfminimizer_vector_bfgs2;
    break;
  }

  s = gsl_multimin_fdfminimizer_alloc (T, *dim);

  gsl_multimin_fdfminimizer_set (s, &my_func, x, *step, *line_tol);
  do
    {
      iter++;
      for(i=0; i<*dim; i++) oldpoint[i] = point[i];

      /* Iterate */
      status = gsl_multimin_fdfminimizer_iterate (s);

      /* Get current minimum, point and gradient */
      *minimum = gsl_multimin_fdfminimizer_minimum(s);
      for(i=0; i<*dim; i++) point[i] = gsl_vector_get(gsl_multimin_fdfminimizer_x(s), i);
      for(i=0; i<*dim; i++) grad[i] = gsl_vector_get(gsl_multimin_fdfminimizer_gradient(s), i);

      /* Compute convergence criteria */
      for(i=0; i<*dim; i++) gsl_vector_set(absdr, i, fabs(point[i]-oldpoint[i]));
      maxdr = gsl_vector_max(absdr);
      for(i=0; i<*dim; i++) gsl_vector_set(absgrad, i, fabs(grad[i]));
      maxgrad = gsl_vector_max(absgrad);

      /* Print information */
      write_info(&iter, dim, minimum, &maxdr, &maxgrad, point);

      /* Store infomation for next iteration */
      for(i=0; i<*dim; i++) oldpoint[i] = point[i];

      if (status)
        break;

      if ( (maxgrad <= *tolgrad) || (maxdr <= *toldr) ) status = GSL_SUCCESS;
      else status = GSL_CONTINUE;
    }
  while (status == GSL_CONTINUE && iter <= *maxiter);

  if(status == GSL_CONTINUE) status = 1025;

  gsl_multimin_fdfminimizer_free (s);
  gsl_vector_free (x); gsl_vector_free(absgrad); gsl_vector_free(absdr);

  free(oldpoint);
  free(grad);

  return status;
}

/* B. Second, the interface to the gsl function that calculates the minimum
   of a one-dimensional function, with the knowledge of the function itself,
   but not its gradient. */

/* This is a type used to communicate with Fortran; func1 is the type of the
   interface to a Fortran subroutine that calculates the function and its
   gradient. */
typedef void (*func1)(const double*, double*);
typedef struct{
  func1 func;
} param_f1_t;

double fn1(double x, void * params)
{
  param_f1_t * p = (param_f1_t* ) params;
  double fx;
  p->func(&x, &fx);
  return fx;
}

void FC_FUNC_(oct_1dminimize, OCT_1DMINIMIZE)(double *a, double *b, double *m, func1 f, int *status)
{
  int iter = 0;
  int max_iter = 100;
  const gsl_min_fminimizer_type *T;
  gsl_min_fminimizer *s;
  gsl_function F;
  param_f1_t p;

  p.func = f;

  F.function = &fn1;
  F.params = (void *) &p;

  T = gsl_min_fminimizer_brent;
  s = gsl_min_fminimizer_alloc (T);

  *status = gsl_min_fminimizer_set (s, &F, *m, *a, *b);

  gsl_set_error_handler_off();

  do
    {
      iter++;
      *status = gsl_min_fminimizer_iterate (s);

      *m = gsl_min_fminimizer_x_minimum (s);
      *a = gsl_min_fminimizer_x_lower (s);
      *b = gsl_min_fminimizer_x_upper (s);

      *status = gsl_min_test_interval (*a, *b, 0.00001, 0.0);

      /*if (*status == GSL_SUCCESS) printf ("Converged:\n");*/
      /*printf ("%5d [%.7f, %.7f] %.7f \n", iter, *a, *b,*m);*/
    }
  while (*status == GSL_CONTINUE && iter < max_iter);
  gsl_min_fminimizer_free(s);

}

/* C. Third, the interface to the gsl function that calculates the minimum
   of an N-dimensional function, with the knowledge of the function itself,
   but not its gradient. */

/* This is a type used to communicate with Fortran; funcn is the type of the
   interface to a Fortran subroutine that calculates the function and its
   gradient. */
typedef void (*funcn)(int*, double*, double*);
typedef struct{
  funcn func;
} param_fn_t;

double fn(const gsl_vector *v, void * params)
{
  double val;
  double *x;
  int i, dim;
  param_fn_t * p;

  p = (param_fn_t *) params;
  dim = v->size;
  x = (double *)malloc(dim*sizeof(double));

  for(i=0; i<dim; i++) x[i] = gsl_vector_get(v, i);
  p->func(&dim, x, &val);

  free(x);
  return val;
}

typedef void (*print_f_fn_ptr)(const int*, const int*, const double*, const double*, const double*);

int FC_FUNC_(oct_minimize_direct, OCT_MINIMIZE_DIRECT)
     (const int *method, const int *dim, double *point, const double *step,
      const double *toldr, const int *maxiter, funcn f,
      const print_f_fn_ptr write_info, double *minimum)
{
  int iter = 0, status, i;
  double size;

  const gsl_multimin_fminimizer_type *T = NULL;
  gsl_multimin_fminimizer *s = NULL;
  gsl_vector *x, *ss;
  gsl_multimin_function my_func;

  param_fn_t p;
  p.func = f;

  my_func.f = &fn;
  my_func.n = *dim;
  my_func.params = (void *) &p;

  /* Set the initial vertex size vector */
  ss = gsl_vector_alloc (*dim);
  gsl_vector_set_all (ss, *step);

  /* Starting point */
  x = gsl_vector_alloc (*dim);
  for(i=0; i<*dim; i++) gsl_vector_set (x, i, point[i]);

  switch(*method){
  case 6:
    T = gsl_multimin_fminimizer_nmsimplex;
    break;
  }

  s = gsl_multimin_fminimizer_alloc (T, *dim);
  gsl_multimin_fminimizer_set (s, &my_func, x, ss);

  do
    {
      iter++;
      status = gsl_multimin_fminimizer_iterate(s);

      if(status) break;

      *minimum = gsl_multimin_fminimizer_minimum(s);
      for(i=0; i<*dim; i++) point[i] = gsl_vector_get(gsl_multimin_fminimizer_x(s), i);

      size = gsl_multimin_fminimizer_size (s);
      status = gsl_multimin_test_size (size, *toldr);

      write_info(&iter, dim, minimum, &size, point);

    }
  while (status == GSL_CONTINUE && iter < *maxiter);

  if(status == GSL_CONTINUE) status = 1025;

  gsl_vector_free(x);
  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free(s);
  return status;
}