#pragma once
#include "utility/scope_guard.h"
#include "utility/types.h"
#include <gsl/gsl>
#include <cfenv>
#include <cassert>

#define scoped_fesetround(mode)                                  \
    int SCOPE_GUARD_PP_UNIQUE(_rounding_mode) = fegetround();    \
    int SCOPE_GUARD_PP_UNIQUE(_rounding_ret) = fesetround(mode); \
    (void)SCOPE_GUARD_PP_UNIQUE(_rounding_ret);                  \
    assert(SCOPE_GUARD_PP_UNIQUE(_rounding_ret) == 0);           \
    SCOPE(exit) { fesetround(SCOPE_GUARD_PP_UNIQUE(_rounding_mode)); }

template <class T> inline T square(T a) { return a * a; }

template <class T> inline T cube(T a) { return a * a * a; }

template <class T> inline T clamp(T x, T min, T max)
{
    x = (x < min) ? min : x;
    x = (x > max) ? max : x;
    return x;
}

// linear interpolator
template <class R> R itp_linear(const R y[], R mu)
{
    return y[0] * (1 - mu) + y[1] * mu;
}

// Catmull-Rom interpolator
template <class R> R itp_catmull(const R y[], R mu)
{
    R mu2 = mu * mu;
    R mu3 = mu2 * mu;
    R a[] = {-R(0.5) * y[0] + R(1.5) * y[1] - R(1.5) * y[2] + R(0.5) * y[3],
             y[0] - R(2.5) * y[1] + R(2) * y[2] - R(0.5) * y[3],
             -R(0.5) * y[0] + R(0.5) * y[2], y[1]};
    return a[0] * mu3 + a[1] * mu2 + a[2] * mu + a[3];
}

//------------------------------------------------------------------------------
// Fixed point routines
//   suffix 'xN', N = bit size of the integer part
//   prefix 'l' = 64-bit operation, 32-bit otherwise
//
//   'fx' number to fixed
//   'ix' integer part, 'rx' fractional part
//   'ffx' fixed to f32, 'dfx' fixed to f64

// number to Q32,32
template <class R> inline constexpr i64 lfx32(R x)
{
    return i64(x * (i64)4294967296);
}
// number to Q16,16
template <class R> inline constexpr i32 fx16(R x)
{
    return i32(x * (i32)65536);
}
// number to Q16,48
template <class R> inline constexpr i64 lfx16(R x)
{
    return i64(x * (i64)281474976710656);
}
// number to Q8,24
template <class R> inline constexpr i32 fx8(R x)
{
    return i32(x * (i32)16777216);
}
// number to Q8,56
template <class R> inline constexpr i64 lfx8(R x)
{
    return i64(x * (i64)72057594037927936);
}

// Q32,32 integer part
inline constexpr i64 lix32(i64 x) { return x / 4294967296; }
inline constexpr u64 lix32(u64 x) { return x / 4294967296; }
template <class T> T lix32(T) = delete;
// Q16,16 integer part
inline constexpr i32 ix16(i32 x) { return x / 65536; }
inline constexpr u32 ix16(u32 x) { return x / 65536; }
template <class T> T ix16(T) = delete;
// Q16,48 integer part
inline constexpr i64 lix16(i64 x) { return x / 281474976710656; }
inline constexpr u64 lix16(u64 x) { return x / 281474976710656; }
template <class T> T lix16(T) = delete;
// Q8,24 integer part
inline constexpr i32 ix8(i32 x) { return x / 16777216; }
inline constexpr u32 ix8(u32 x) { return x / 16777216; }
template <class T> T ix8(T) = delete;
// Q8,56 integer part
inline constexpr i64 lix8(i64 x) { return x / 72057594037927936; }
inline constexpr u64 lix8(u64 x) { return x / 72057594037927936; }
template <class T> T lix8(T) = delete;

// Q32,32 fractional part
inline constexpr u64 lrx32(i64 x) { return x & 4294967295; }
inline constexpr u64 lrx32(u64 x) { return x & 4294967295; }
template <class T> T lrx32(T) = delete;
// Q16,16 fractional part
inline constexpr u32 rx16(i32 x) { return x & 65535; }
inline constexpr u32 rx16(u32 x) { return x & 65535; }
template <class T> T rx16(T) = delete;
// Q16,48 fractional part
inline constexpr u64 lrx16(i64 x) { return x & 281474976710655; }
inline constexpr u64 lrx16(u64 x) { return x & 281474976710655; }
template <class T> T lrx16(T) = delete;
// Q8,24 fractional part
inline constexpr u32 rx8(i32 x) { return x & 16777215; }
inline constexpr u32 rx8(u32 x) { return x & 16777215; }
template <class T> T rx8(T) = delete;
// Q8,56 fractional part
inline constexpr u64 lrx8(i64 x) { return x & 72057594037927935; }
inline constexpr u64 lrx8(u64 x) { return x & 72057594037927935; }
template <class T> T lrx8(T) = delete;

// Q32,32 number to real
inline constexpr f32 lffx32(i64 x) { return x / 4294967296.0f; }
inline constexpr f32 lffx32(u64 x) { return x / 4294967296.0f; }
template <class T> f32 lffx32(T x) = delete;
inline constexpr f64 ldfx32(i64 x) { return x / 4294967296.0; }
inline constexpr f64 ldfx32(u64 x) { return x / 4294967296.0; }
template <class T> f64 ldfx32(T x) = delete;
// Q16,16 number to real
inline constexpr f32 ffx16(i32 x) { return x / 65536.0f; }
inline constexpr f32 ffx16(u32 x) { return x / 65536.0f; }
template <class T> f32 ffx16(T x) = delete;
inline constexpr f64 dfx16(i32 x) { return x / 65536.0; }
inline constexpr f64 dfx16(u32 x) { return x / 65536.0; }
template <class T> f64 dfx16(T x) = delete;
// Q16,48 number to real
inline constexpr f32 lffx16(i64 x) { return x / 281474976710656.0f; }
inline constexpr f32 lffx16(u64 x) { return x / 281474976710656.0f; }
template <class T> f32 lffx16(T x) = delete;
inline constexpr f64 ldfx16(i64 x) { return x / 281474976710656.0; }
inline constexpr f64 ldfx16(u64 x) { return x / 281474976710656.0; }
template <class T> f64 ldfx16(T x) = delete;
// Q8,24 number to real
inline constexpr f32 ffx8(i32 x) { return x / 16777216.0f; }
inline constexpr f32 ffx8(u32 x) { return x / 16777216.0f; }
template <class T> f32 ffx8(T x) = delete;
inline constexpr f64 dfx8(i32 x) { return x / 16777216.0; }
inline constexpr f64 dfx8(u32 x) { return x / 16777216.0; }
template <class T> f64 dfx8(T x) = delete;
// Q8,56 number to real
inline constexpr f32 lffx8(i64 x) { return x / 72057594037927936.0f; }
inline constexpr f32 lffx8(u64 x) { return x / 72057594037927936.0f; }
template <class T> f32 lffx8(T x) = delete;
inline constexpr f64 ldfx8(i64 x) { return x / 72057594037927936.0; }
inline constexpr f64 ldfx8(u64 x) { return x / 72057594037927936.0; }
template <class T> f64 ldfx8(T x) = delete;


/* Implementation for Gillespie's direct method */

#include <math.h>
#include <stdint.h>
#include <time.h>
#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include "ssa.h"
#include "dm.h"

const gsl_rng_type *SSARNGT;
gsl_rng *SSARNG;

INDEX nextreaction(double *propensities, INDEX m)
{
	double r1 = gsl_ran_flat(SSARNG, 0.0, 1.0) * propensities[m - 1];
	INDEX i = 0;
	for(i = 0; i < m; i++)
		if(r1 <= propensities[i])
			return i;
	return -1;
}

/*
Perform one step of the Gillespie direct method, selecting a
reaction to perform and altering molecule counts and time
accordingly.
*/
void ssa_dmstep(INDEX n, INDEX m, double *propensities, INDEX **R, INDEX **P, double *k, COUNT *x, double *t)
{
	propensities[0] = ssa_h(R[0], x, n) * k[0];
	INDEX i;
	for (i = 1; i < m; i++)
		propensities[i] = ssa_h(R[i], x, n) * k[i] + propensities[i - 1];

	if (propensities[m - 1] > 0.0) {
		*t += gsl_ran_exponential(SSARNG, 1 / propensities[m - 1]);
		INDEX next = nextreaction(propensities, m);
		ssa_doreaction(R[next], P[next], x, n);
	}
}

void ssa_dm(INDEX **R, INDEX **P, INDEX n, INDEX m, double *k, COUNT *x, double T)
{
	gsl_rng_env_setup();
	SSARNGT = gsl_rng_default;
	SSARNG = gsl_rng_alloc(SSARNGT);
	gsl_rng_set(SSARNG, time(NULL));

        double t = 0.0;
        double *propensities = malloc(sizeof(double) * m);

	do {
		ssa_dmstep(n, m, propensities, R, P, k, x, &t);
		ssa_printstate(t, x, n);
	} while(t < T && propensities[m - 1] > 0.0);

	free(propensities);
}


/*
 * This file is part of MXE.
 * See index.html for further information.
 */

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

int main(int argc, char *argv[])
{
    double x, y;

    (void)argc;
    (void)argv;

    x = 5.0;
    y = gsl_sf_bessel_J0 (x);
    printf ("J0(%g) = %.18e\n", x, y);
    return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <complex.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>

#define _USE_MATH_DEFINES

// global variables
int n = 10;

double incident(double x);
double green_function(double r1[n][3], double r2[n][3]);

int main()
{
	int i, j, k;
	double x, y, z;
	double scatterers[n][3];

	gsl_matrix * mmat = gsl_matrix_alloc (n,n);

	srand((unsigned)time(NULL));
	for (i = 1; i < n; i++)
	{
		x = 20*(double)rand()/RAND_MAX;
		y = 50*(double)rand()/RAND_MAX;
		z = 30*(double)rand()/RAND_MAX;

		scatterers[i][0] = x;
		scatterers[i][1] = y;
		scatterers[i][2] = z;

		printf("Scatterer %d position: %lf %lf %lf \n", i, scatterers[i][0], scatterers[i][1], scatterers[i][2]);

	//double field_scatterer = incident(scatterers[i][0]);

	//printf("Incident field at scatterer %d: %lf\n", i, field_scatterer);

	// fixed below
		double field_scatterer = incident(scatterers[i][0]);

	// solving the matrix equation

		gsl_matrix_set (mmat, i,i, field_scatterer);
		// continue rewriting this part

	//	gsl_vector_view b
	//		= gsl_vector_view_array (field_scatterer, n);
	// gsl vector set instead

	//	gsl_vector *total_field = gsl_vector_alloc (n);
	}
	double green = green_function(scatterers, scatterers);
	int s;
	//gsl_permutation * p = gsl_permutation_alloc (n);
	//gsl_linalg_LU_decomp (&green.matrix, p, &s);
	//gsl_linalg_LU_solve (&green.matrix, p, &b.vector, total_field);

	//printf("Total field = \n");
	//gsl_vector_fprintf (stdout, total_field, "%g");

	//gsl_permutation_free (p);

	// defining surface of sphere
	double phi[150];
	double theta[150];
	int r = 500;

	for (i=(M_PI-2); i < (M_PI+2); i++)
	{
		phi[i] =(double)(i)/((M_PI-2) - (M_PI+2));

	}
	for (i=(M_PI/2 - 2); i < (M_PI/2 +2); i++)
	{
		theta[i] = (double)(i)/((M_PI/2 - 2) - (M_PI/2 +2));
	}

	return 0;
}

double incident(double x)
{
	double E_0 = 1.0;

	return E_0*cexp(1*I*2*M_PI*x);
}

double green_function(double r1[n][3], double r2[n][3])
{
	int i, j;
	gsl_matrix * m = gsl_matrix_alloc (n,n);

	double r[n], g[n];
	for (i = 0; i < n; i++)
	{
		for (j = 0; j < n; j++)
		{
			r[n] = sqrt((r1[i][1]-r2[j][1])*(r1[i][1]-r2[j][1]) + (r1[i][2]-r2[j][2])*(r1[i][2]-r2[j][2]) + (r1[i][3] - r2[j][3])*(r1[i][3] - r2[j][3]));

			if (r[n] == 0.0)
			{
				g[n] = 1.0;
			}
			else
			{
				g[n] = exp(1*I*2*M_PI*r[n])/(4*M_PI*r[n]);
			}

			gsl_matrix_set (m, i, j, g[n]);
		}
	}

	for (i = 0; i < 100; i++)
	{
		for (j = 0; j < n; j++)
		{
			printf ("m(%d,%d) = %g\n", i, j,
					gsl_matrix_get (m, i, j));
		}
	}
	gsl_matrix_free (m);
}


/**
 */
#ifndef _CROSSDISP_UTILS_H
#define _CROSSDISP_UTILS_H

#include "spc_trace_functions.h"
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_statistics.h>
#include "aXe_grism.h"
#include "aXe_utils.h"
#include <math.h>
#include "trace_conf.h"

/**
 * Structure: drz_pars
 *  A description of an ellipse with a,b, theta.
 */
typedef struct
{
  gsl_matrix *coeffs;
  d_point     offset;
  px_point    npixels;
}
drz_pars;

int
drizzle_distort(const gsl_vector * x, void *params, gsl_vector * f);

d_point
undistort_point(gsl_matrix *coeffs, const px_point pixmax,
                  d_point xy_image);

d_point
distort_point(gsl_matrix *coeffs, const px_point pixmax, d_point xy_image);

extern gsl_matrix *
get_crossdisp_matrix(char * filename, int sci_numext);

extern d_point
get_axis_scales(beam actbeam, gsl_matrix * drzcoeffs, px_point pixmax);

extern double
get_crossdisp_scale(trace_func  *trace, d_point refpnt, gsl_matrix * drzcoeffs, px_point pixmax);

extern d_point
get_drz_position(d_point in_point, gsl_matrix * drzcoeffs, px_point pixmax);

extern d_point
get_drz_position_free(d_point in_point, gsl_matrix * drzcoeffs, px_point pix_in, px_point pix_out);

extern trace_func *
get_tracefunc_at(char *filename, d_point p);

extern double
evaln(double x, double y, gsl_matrix * drzcoeffs, int row);

extern double
devalndx(double x, double y, gsl_matrix * drzcoeffs, int row);

extern double
devalndy(double x, double y, gsl_matrix * drzcoeffs, int row);

extern gsl_matrix *
get_jacobian(int i, int j, gsl_matrix * drzcoeffs, int width, int height);

extern double
get_det_jacobian(int i, int j, gsl_matrix * drzcoeffs, int width, int height);

#endif


#include "gs2crmod_ext.h"
#include <math.h>
#include <string.h>
#include <gsl/gsl_spline.h>

VALUE gs2crmod_tensor_complexes_field_gsl_tensor_complex_2(VALUE self, VALUE options)
{
	VALUE field, field_complex, field_narray;
	VALUE field_complex_narray;
	VALUE cgsl_tensor_complex;
	VALUE ccomplex;
	VALUE shape;
	int *c_shape;
	int i, j, k;

	ccomplex = RGET_CLASS_TOP("Complex");
	field = RFCALL_11("field_gsl_tensor", options);
	cgsl_tensor_complex = RGET_CLASS(cgsl, "TensorComplex");
	shape = rb_funcall(field, rb_intern("shape"), 0);
	/*rb_p(shape);*/

	CR_INT_ARY_R2C_STACK(shape, c_shape);
	field_complex = rb_funcall3(cgsl_tensor_complex, rb_intern("alloc"), 3, RARRAY_PTR(shape));
	field_narray = RFCALL_10_ON(field, "narray");
	field_complex_narray = RFCALL_10_ON(field_complex, "narray");
	/*rb_p(field_complex);*/
	for (i=0;i<c_shape[0];i++)
	  for (j=0;j<c_shape[1];j++)
			for (k=0;k<c_shape[2];k++)
			{

				/*printf("%d %d %d\n", i, j, k);*/
				VALUE cmplex, re, im;
				re = rb_funcall(
								field_narray,
								rb_intern("[]"),
								4,
								INT2FIX(0),
								INT2FIX(k), INT2FIX(j), INT2FIX(i)
							);
				/*printf("%d %d %d re\n", i, j, k);*/
				im = rb_funcall(
								field_narray,
								rb_intern("[]"),
								4,
								INT2FIX(1),
								INT2FIX(k), INT2FIX(j), INT2FIX(i)
							);
				/*printf("%d %d %d im\n", i, j, k);*/
				cmplex =
					 	rb_funcall(
							ccomplex,
							rb_intern("rectangular"),
							2, re, im
						);
				/*printf("%d %d %d again\n", i, j, k);*/
				rb_funcall(
						field_complex_narray,
						rb_intern("[]="),
						4,
						INT2FIX(k), INT2FIX(j), INT2FIX(i),
						cmplex
					);

			}
	/*rb_p(field);*/

		/*field_complex = */
	return field_complex;
}

VALUE gs2crmod_tensor_field_gsl_tensor(VALUE self, VALUE options)
{
	VALUE cgsl_tensor;
	VALUE field, field_narray;
	VALUE field_real_space, field_real_space_narray;
	VALUE field_real_space_new, field_real_space_new_narray;
	VALUE shape, shape_real_space, shape_real_space_new;
	VALUE workspacex, workspacey;
	VALUE ccomplex, cgsl_complex;
	VALUE re, im, cmplex;
	VALUE *ary_ptr;
	VALUE range_x, range_y;
	int shape0;
	double* c_field, *c_field_real_space;
	int* c_shape, *c_shape_real_space;
	int * c_shape_real_space_new;
	int i,j,k,m;
	int kint,km,kold;
	double frac, interp_value;

	ccomplex = RGET_CLASS_TOP("Complex");
	cgsl_complex = RGET_CLASS(cgsl, "Complex");
	cgsl_tensor = RGET_CLASS(cgsl, "Tensor");
	/*cgsl_vector = RGET_CLASS(cgsl, "Vector");*/

	Check_Type(options, T_HASH);
	if(RTEST(CR_HKS(options, "field_netcdf"))){
		field = CR_HKS(options, "field_netcdf");
		CR_CHECK_CLASS(field, cgsl_tensor);
	}
	else {
	 field = RFCALL_11("field_gsl_tensor", options);
	}
	field_narray = RFCALL_10_ON(field, "narray");
	shape = RFCALL_10_ON(field, "shape");

	workspacex = RFCALL_11_ON(cgsl_vector_complex, "alloc", RARRAY_PTR(shape)[1]);
	shape0 = NUM2INT(RARRAY_PTR(shape)[0]);
	workspacey = RFCALL_11_ON(cgsl_vector, "alloc", INT2NUM(shape0 * 2 - 2 + shape0%2));

	field_real_space = rb_funcall(
			cgsl_tensor,
			rb_intern("alloc"),
			3,
			RFCALL_10_ON(workspacey, "size"),
			RARRAY_PTR(shape)[1],
			RARRAY_PTR(shape)[2]
			);
	field_real_space_narray = RFCALL_10_ON(field_real_space, "narray");
	shape_real_space = RFCALL_10_ON(field_real_space, "shape");

	CR_INT_ARY_R2C_STACK(shape, c_shape);
	CR_INT_ARY_R2C_STACK(RFCALL_10_ON(field_real_space, "shape"), c_shape_real_space);

	c_field = ALLOC_N(double, c_shape[0] * c_shape[1] * c_shape[2] * c_shape[3]);

  /*printf("Allocated stuff\n");*/
	for (j=0; j<c_shape[2]; j++) /*theta loop*/
	{
		for (i=0; i<c_shape[0]; i++) /*ky loop*/
		{
			for (k=0; k<c_shape[1]; k++) /*kx loop*/
			{
				/*First copy the field onto the
				 * x workspace, Fourier transform
				 * the workspace and copy it onto
				 * the c array*/
				re = rb_funcall(
								field_narray,
								rb_intern("[]"),
								4,
								INT2FIX(0),
								INT2FIX(j), INT2FIX(k), INT2FIX(i)
							);
				/*printf("%d %d %d re\n", i, j, k);*/
				im = rb_funcall(
								field_narray,
								rb_intern("[]"),
								4,
								INT2FIX(1),
								INT2FIX(j), INT2FIX(k), INT2FIX(i)
							);
				/*printf("%d %d %d im\n", i, j, k);*/
				cmplex =
					 	rb_funcall(
							cgsl_complex,
							rb_intern("alloc"),
							2, re, im
						);
				/*printf("%d %d %d again\n", i, j, k);*/
				/*printf("Made complex\n");*/
				rb_funcall(
					workspacex,
					rb_intern("[]="),
					2,
					INT2FIX(k),
					cmplex);
				/*printf("Added complex\n");*/

			}
			/*printf("Made complex vector\n");*/
			workspacex = RFCALL_10_ON(workspacex, "backward");
			/*printf("Done x transform\n");*/
			for (k=0;k<c_shape[1];k++){
				cmplex = RFCALL_11_ON(workspacex,"[]",INT2FIX(k));
				c_field[
					j*c_shape[0]*c_shape[1]*2 +
					i*c_shape[1]*2 +
					k*2
					] = NUM2DBL(RFCALL_10_ON(cmplex,"real"));
				c_field[
					j*c_shape[0]*c_shape[1]*2 +
					i*c_shape[1]*2 +
					k*2 + 1
					] = NUM2DBL(RFCALL_10_ON(cmplex,"imag"));

			}
		}
		/*printf("Done x\n");*/
		/* Now copy onto the y workspace,
		 * Fourier transform and copy onto
		 * the second c array*/
		for (k=0;k<c_shape[1];k++)
		{
		  m=0;
		  for (i=0;i<c_shape[0];i++)
			{
				rb_funcall(
						workspacey,
						rb_intern("[]="),
						2,
						INT2FIX(m),
						rb_float_new(c_field[
								j*c_shape[0]*c_shape[1]*2 +
								i*c_shape[1]*2 +
								k*2
								])
						);
				m++;
				/* We are converting a complex array
				 * to a half complex array in
				 * preparation for transforming
				 * it to real space, and so there
				 * are one or two elements we leave
				 * unfilled.*/
				if (i==0 || (c_shape[0]%2==0 && i == c_shape[0]/2 + 1)) continue;
				rb_funcall(
						workspacey,
						rb_intern("[]="),
						2,
						INT2FIX(m),
						rb_float_new(c_field[
								j*c_shape[0]*c_shape[1]*2 +
								i*c_shape[1]*2 +
								k*2 + 1
								])
						);
				m++;
			}
			/*printf("Made y vector\n");*/
			/*rb_p(workspacey);*/
			/*rb_p(RFCALL_10_ON(workspacey, "class"));*/
				workspacey = RFCALL_10_ON(workspacey, "backward");
				/*printf("Done y transform\n");*/
			for (i=0;i<FIX2INT(RFCALL_10_ON(workspacey, "size"));i++)
			{
				rb_funcall(
						field_real_space_narray,
						rb_intern("[]="), 4,
						INT2FIX(j), INT2FIX(k), INT2FIX(i),
						RFCALL_11_ON(workspacey, "[]", INT2FIX(i))
						);
			}
		}
	}
	/*printf("HERE!\n");*/
  range_x = CR_RANGE_INC(
				(RTEST(CR_HKS(options, "ymin")) ?
					FIX2INT(CR_HKS(options, "ymin")) :
			 		0),
				(RTEST(CR_HKS(options, "ymax")) ?
					FIX2INT(CR_HKS(options, "ymax")) :
					c_shape_real_space[0]-1)
				);
	/*rb_p(range_x);*/
	range_y = CR_RANGE_INC(
				(RTEST(CR_HKS(options, "xmin")) ?
					FIX2INT(CR_HKS(options, "xmin")) :
			 		0),
				(RTEST(CR_HKS(options, "xmax")) ?
					FIX2INT(CR_HKS(options, "xmax")) :
					c_shape_real_space[1]-1)
				);
	/*printf("Made ranges\n");*/
	/*rb_p(rb_funcall(field_real_space, rb_intern("[]"), 3, INT2FIX(0), INT2FIX(2), INT2FIX(3)));*/
	field_real_space = rb_funcall(
			field_real_space,
			rb_intern("[]"), 3,
			range_x,
			range_y,
			Qtrue);
	/*rb_p(rb_funcall(field_real_space, rb_intern("[]"), 3, INT2FIX(0), INT2FIX(2), INT2FIX(3)));*/
	/*printf("SHORTENED!\n");*/
	if (RTEST(CR_HKS(options, "interpolate_theta")))
	{
		shape_real_space_new = RFCALL_10_ON(shape_real_space, "dup");
		kint = NUM2INT(CR_HKS(options, "interpolate_theta"));
		rb_funcall(
				shape_real_space_new,
				rb_intern("[]="), 2, INT2FIX(-1),
				INT2FIX((c_shape_real_space[2] - 1)*kint+1)
				);
		CR_INT_ARY_R2C_STACK(shape_real_space_new, c_shape_real_space_new);
		ary_ptr = RARRAY_PTR(shape_real_space_new);
		field_real_space_new = rb_funcall(
				cgsl_tensor,
				rb_intern("float"),
				3,
				ary_ptr[0], ary_ptr[1], ary_ptr[2]);
		field_real_space_new_narray = RFCALL_10_ON(field_real_space_new, "narray");
    /*printf("Allocated");*/
		/*rb_p(shape_real_space_new);*/
		for (i=0;i<c_shape_real_space_new[0];i++)
		for (j=0;j<c_shape_real_space_new[1];j++)
		{
			/*printf("i %d j %d k %d\n", i, j, c_shape_real_space_new[2]-1); */
			rb_funcall(
					field_real_space_new_narray,
					rb_intern("[]="),
					4,INT2FIX(c_shape_real_space_new[2]-1),
					INT2FIX(j),INT2FIX(i),
					rb_funcall(
						field_real_space_narray,
						rb_intern("[]"), 3,
						INT2FIX(c_shape_real_space[2]-1),
						INT2FIX(j),INT2FIX(i)
						)
					);
			for (k=0;k<c_shape_real_space_new[2]-1;k++)
			{
				/*printf("i %d j %d k %d\n", i, j, k); */
				km = k%kint;
				frac = (double)km/(double)kint;
				kold = (k-km)/kint;
				interp_value = NUM2DBL(
						rb_funcall(
							field_real_space_narray,
							rb_intern("[]"),3,
							INT2FIX(kold),INT2FIX(j),INT2FIX(i)
							)
						)*(1.0-frac) + NUM2DBL(
						rb_funcall(
							field_real_space_narray,
							rb_intern("[]"),3,
							INT2FIX(kold+1),INT2FIX(j),INT2FIX(i)
							)
						) * frac;
				rb_funcall(
						field_real_space_new_narray,
						rb_intern("[]="), 4,
						INT2FIX(k),INT2FIX(j),INT2FIX(i),
						rb_float_new(interp_value)
						);
				/*if (i==0 && j==2 && k==3){*/
					/*printf("frac %f\n", frac);*/
				/*}*/
			}
		}

		field_real_space = field_real_space_new;
	}
  /*printf("Finished field_real_space_gsl_tensor\n");*/
	/*rb_p(shape_real_space_new);*/
	return field_real_space;
}

void Init_gs2crmod_ext()
{
	/*printf("HERE!!!");*/
	ccode_runner =  RGET_CLASS_TOP("CodeRunner");
	ccode_runner_gs2 =  rb_define_class_under(ccode_runner, "Gs2",
			RGET_CLASS(
				RGET_CLASS(ccode_runner, "Run"),
				"FortranNamelist"
				)
			);

	ccode_runner_gs2_gsl_tensor_complexes = rb_define_module_under(ccode_runner_gs2, "GSLComplexTensors");
	rb_include_module(ccode_runner_gs2, ccode_runner_gs2_gsl_tensor_complexes);

	ccode_runner_gs2_gsl_tensors = rb_define_module_under(ccode_runner_gs2, "GSLTensors");
	rb_include_module(ccode_runner_gs2, ccode_runner_gs2_gsl_tensors);

	cgsl = RGET_CLASS_TOP("GSL");
	cgsl_vector = RGET_CLASS(cgsl, "Vector");
	cgsl_vector_complex = RGET_CLASS(cgsl_vector, "Complex");

	rb_define_method(ccode_runner_gs2_gsl_tensor_complexes, "field_gsl_tensor_complex_2", gs2crmod_tensor_complexes_field_gsl_tensor_complex_2, 1);
	rb_define_method(ccode_runner_gs2_gsl_tensors, "field_real_space_gsl_tensor", gs2crmod_tensor_field_gsl_tensor, 1);

	/*rb_define_method(ccode_runner_ext, "hello_world", code_runner_ext_hello_world, 0);*/
}


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

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_trig.h"
#include "gsl_sf_bessel.h"

#include "error.h"

#include "bessel.h"
#include "bessel_amp_phase.h"
#include "cheb_eval.c"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* based on SLATEC besy0, 1980 version, w. fullerton */

/* chebyshev expansions

 series for by0        on the interval  0.	    to  1.60000d+01
					with weighted error   1.20e-17
					 log weighted error  16.92
			       significant figures required  16.15
				    decimal places required  17.48
*/

static double by0_data[13] = {
  -0.011277839392865573,
  -0.128345237560420350,
  -0.104378847997942490,
   0.023662749183969695,
  -0.002090391647700486,
   0.000103975453939057,
  -0.000003369747162423,
   0.000000077293842676,
  -0.000000001324976772,
   0.000000000017648232,
  -0.000000000000188105,
   0.000000000000001641,
  -0.000000000000000011
};
static cheb_series by0_cs = {
  by0_data,
  12,
  -1, 1,
  8
};

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_sf_bessel_Y0_e(const double x, gsl_sf_result * result)
{
  const double two_over_pi = 2.0/M_PI;
  const double xmax        = 1.0/GSL_DBL_EPSILON;

  /* CHECK_POINTER(result) */

  if (x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 4.0) {
    gsl_sf_result J0;
    gsl_sf_result c;
    int stat_J0 = gsl_sf_bessel_J0_e(x, &J0);
    cheb_eval_e(&by0_cs, 0.125*x*x-1.0, &c);
    result->val = two_over_pi*(-M_LN2 + log(x))*J0.val + 0.375 + c.val;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val) + c.err;
    return stat_J0;
  }
  else if(x < xmax) {
    /* Leading behaviour of phase is x, which is exact,
     * so the error is bounded.
     */
    const double z  = 32.0/(x*x) - 1.0;
    gsl_sf_result c1;
    gsl_sf_result c2;
    gsl_sf_result sp;
    const int stat_c1 = cheb_eval_e(&_gsl_sf_bessel_amp_phase_bm0_cs,  z, &c1);
    const int stat_c2 = cheb_eval_e(&_gsl_sf_bessel_amp_phase_bth0_cs, z, &c2);
    const int stat_sp = gsl_sf_bessel_sin_pi4_e(x, c2.val/x, &sp);
    const double sqrtx = sqrt(x);
    const double ampl  = (0.75 + c1.val) / sqrtx;
    result->val  = ampl * sp.val;
    result->err  = fabs(sp.val) * c1.err/sqrtx + fabs(ampl) * sp.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_3(stat_sp, stat_c1, stat_c2);
  }
  else {
    UNDERFLOW_ERROR(result);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_Y0(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_Y0_e(x, &result));
}


/* multifit/fdjac.c
 *
 * 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.
 *
 *
 * This module contains routines for approximating the Jacobian with finite
 * differences for nonlinear least-squares fitting.
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

static int fdjac(const gsl_vector *x, const gsl_vector *wts,
                 gsl_multifit_function_fdf *fdf,
                 const gsl_vector *f, gsl_matrix *J);

/*
fdjac()
  Compute approximate Jacobian using forward differences

Inputs: x   - parameter vector
        wts - data weights
        fdf - fdf struct
        f   - (input) vector of function values f_i(x)
        J   - (output) Jacobian matrix

Return: success or error
*/

static int
fdjac(const gsl_vector *x, const gsl_vector *wts,
      gsl_multifit_function_fdf *fdf, const gsl_vector *f, gsl_matrix *J)
{
  int status = 0;
  size_t i, j;
  double h;
  const double epsfcn = 0.0;
  double eps = sqrt(GSL_MAX(epsfcn, GSL_DBL_EPSILON));

  for (j = 0; j < fdf->p; ++j)
    {
      double xj = gsl_vector_get(x, j);

      /* use column j of J as temporary storage for f(x + dx) */
      gsl_vector_view v = gsl_matrix_column(J, j);

      h = eps * fabs(xj);
      if (h == 0.0)
        h = eps;

      /* perturb x_j to compute forward difference */
      gsl_vector_set((gsl_vector *) x, j, xj + h);

      status += gsl_multifit_eval_wf (fdf, x, wts, &v.vector);
      if (status)
        return status;

      /* restore x_j */
      gsl_vector_set((gsl_vector *) x, j, xj);

      h = 1.0 / h;
      for (i = 0; i < fdf->n; ++i)
        {
          double fnext = gsl_vector_get(&v.vector, i);
          double fi = gsl_vector_get(f, i);

          gsl_matrix_set(J, i, j, (fnext - fi) * h);
        }
    }

  return status;
} /* fdjac() */

/*
gsl_multifit_fdfsolver_dif_df()
  Compute approximate Jacobian using finite differences

Inputs: x   - parameter vector
        wts - data weights (set to NULL if not needed)
        fdf - fdf
        f   - (input) function values f_i(x)
        J   - (output) approximate Jacobian matrix

Return: success or error
*/

int
gsl_multifit_fdfsolver_dif_df(const gsl_vector *x, const gsl_vector *wts,
                              gsl_multifit_function_fdf *fdf,
                              const gsl_vector *f, gsl_matrix *J)
{
  return fdjac(x, wts, fdf, f, J);
} /* gsl_multifit_fdfsolver_dif_df() */

#ifndef GSL_DISABLE_DEPRECATED

/*
gsl_multifit_fdfsolver_dif_fdf()
  Compute function values (analytic) and approximate Jacobian using finite
differences

Inputs: x      - parameter vector
        fdf    - fdf
        f      - (output) function values f_i(x)
        J      - (output) approximate Jacobian matrix

Return: success or error
*/

int
gsl_multifit_fdfsolver_dif_fdf(const gsl_vector *x,
                               gsl_multifit_function_fdf *fdf,
                               gsl_vector *f, gsl_matrix *J)
{
  int status = 0;

  status = gsl_multifit_eval_wf(fdf, x, NULL, f);
  if (status)
    return status;

  status = fdjac(x, NULL, fdf, f, J);
  if (status)
    return status;

  return status;
} /* gsl_multifit_fdfsolver_dif_fdf() */

#endif /* !GSL_DISABLE_DEPRECATED */


#ifndef __MATHINT_H_
#define __MATHINT_H_

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <immintrin.h>
#include <stdbool.h>
#if (defined __AVX__ || defined __SSE4_2__)
#include "math_x86.h"
#endif
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_integration.h>

#define pi     3.14159265358979323846264338327950288
#define MAX(a, b) ((a > b) ? a : b)
#define MIN(a, b) ((a > b) ? b : a)
#define EULER  0.577215664901532860606512090082402431042
#define EPSILON __DBL_EPSILON__
#define STACK_SIZE 100000  /*FIXME: This must be adpative.*/

double IncompBesselK0_Simpson(double tol, int *cnt, double bes_a, double bes_b, int der);
double computeK0(double a, double b);
double computeINCBK0(double a, double b, int der);

typedef struct {
  double a;
  double b;
} gsl_params;
double bessel_f(double x, void * p);
double bessel_f_der(double x, void * p);
double call_gsl_bessel_integrator(double a, double b,
				  gsl_integration_workspace *w, int der);

void inline set_zero_4(double *x)
{
  x[0] = 0; x[1] = 0; x[2] = 0; x[3] = 0;
}

#endif


#include "asf.h"
#include "ceos.h"

#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_roots.h>

struct pp_erfin_params {
    int npixels,nlines;
    double satellite_height; /* earth center to satellite height (PP "rsc") */
    double slant_first; /* slant range to first pixel */
    double slant_last; /* slant range to last pixel */
    double nominal_pixsize_range;
};

static double getObjective(double R, void *params)
{
    struct pp_erfin_params *p =
        (struct pp_erfin_params *)params;

    double H=p->satellite_height;
    double sf=p->slant_first;
    double HHRR=H*H + R*R;
    /* Convert slant_first to ground range */
    double g=R*acos( (HHRR - sf*sf) / (2*H*R));
    /* Add width of CEOS image in ground range */
    g += (p->npixels-1) * p->nominal_pixsize_range;
    /* Compute slant range to last pixel */
    double slant_last_R=sqrt(HHRR - 2*H*R*cos(g/R));
    /* Compare with slant_last from CEOS */
    return slant_last_R-p->slant_last;
}

void pp_get_corrected_vals(char *sarName, double *corrected_earth_radius,
                           double *corrected_azimuth_time_per_pixel)
{
    int status;
    int iter = 0, max_iter = 100;
    const gsl_root_fsolver_type *T;
    gsl_root_fsolver *s;
    gsl_function F;
    gsl_error_handler_t *prev;
    struct pp_erfin_params params;

    double nominal_pixsize_azimuth;
    double nadir_radius; /* earth radius at nadir */
    double pp_earth_radius; /* f'd up PP Earth radius from start of swath */
    double seconds_per_azimuth_line; /* PP's real azimuth resolution */
    double lo, hi;  /* starting points for the bisection algorithm */
    struct VFDRECV facdr;

    get_asf_facdr(sarName, &facdr);

    /* Find the PP's earth radius with an iterative search */
    F.function = &getObjective;
    F.params = &params;

    params.npixels=facdr.npixels;
    params.nlines=facdr.nlines;
    params.nominal_pixsize_range=facdr.rapixspc;
    nominal_pixsize_azimuth=facdr.azpixspc;
    nadir_radius=1.0e3*facdr.eradnadr;
    params.satellite_height=1.0e3*(facdr.scalt+facdr.eradnadr);
    params.slant_first=1.0e3*facdr.sltrngfp;
    params.slant_last=1.0e3*facdr.sltrnglp;

    prev = gsl_set_error_handler_off();

    lo = nadir_radius - 2000;
    hi = nadir_radius + 2000;

    T = gsl_root_fsolver_brent;
    s = gsl_root_fsolver_alloc (T);
    gsl_root_fsolver_set (s, &F, lo, hi);

    do {
        ++iter;
        status = gsl_root_fsolver_iterate(s);
        pp_earth_radius = gsl_root_fsolver_root(s);
        status = gsl_root_test_residual(
            getObjective(pp_earth_radius, (void*)&params), 1.0e-4);
    } while (status == GSL_CONTINUE && iter < max_iter);

    if (status == GSL_SUCCESS) {
        //printf("Converged after %d iterations.\n", iter);
        //printf("PP Earth Radius: %.3f m\n",pp_earth_radius);
        //printf("   (for comparison) Nadir Earth Radius: %.3f m\n",nadir_radius);
        *corrected_earth_radius = pp_earth_radius;
    } else {
        asfPrintWarning("Failed to determine PP earth radius!\n"
                        "iter: %d, pp_earth_radius=%.3f, res=%.5f\n"
                        "Proceeding using the nadir radius: %.3f m\n"
                        "Starting points were: lo: %.3f -> %.4f\n"
                        "                      hi: %.3f -> %.4f\n",
                        iter, pp_earth_radius,
                        getObjective(pp_earth_radius, (void*)&params),
                        nadir_radius,
                        lo, getObjective(lo, (void*)&params),
                        hi, getObjective(hi, (void*)&params));
        *corrected_earth_radius = nadir_radius;
    }

    gsl_set_error_handler(prev);

    // Find the PP's per-second azimuth pixel spacing
    seconds_per_azimuth_line=nominal_pixsize_azimuth/facdr.swathvel;
    //printf("PP seconds per azimuth line: %.9f s/line\n",seconds_per_azimuth_line);
    //printf("   (for comparison) PP interpolated lines per second: %.3f lines/s\n",1.0/seconds_per_azimuth_line);
    //printf("   (for comparison) FACDR swath velocity: %.3f m/s\n",facdr.swathvel);

    //double R=pp_earth_radius;
    //double H=params.satellite_height;
    //double HHRR=H*H + R*R;
    //double slant=0.5*(params.slant_first+params.slant_last);
    //double rg_center=R*acos( (HHRR - slant*slant) / (2*H*R));
    //double vs=sqrt(facdr.scxvel*facdr.scxvel + facdr.scyvel*facdr.scyvel + facdr.sczvel*facdr.sczvel);
    //double vsg = vs * pp_earth_radius/params.satellite_height*cos(rg_center/pp_earth_radius);

    //printf("   (for comparison) PP-style recalc velocity: %.3f m/s\n",vsg);

    *corrected_azimuth_time_per_pixel = seconds_per_azimuth_line;

    // Free the solver
    gsl_root_fsolver_free(s);
}


 /**
  * File:    utils.h
  *
  * Author:  Vishal R (vishalr@pesu.pes.edu or vishalramesh01@gmail.com)
  *
  * Summary of File:
  *   Matrix operation library required for cdnn. Contains all the header files for matrix operations.
  */

#ifndef UTILS_H
#define UTILS_H

#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <omp.h>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <cblas.h>
#include <signal.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <termcap.h>

#ifdef __cplusplus
extern "C"{
#endif

  typedef struct array{
    float * matrix;
    int shape[2];
  }dARRAY;

  dARRAY * zeros(int * dims);
  dARRAY * ones(int * dims);
  dARRAY * eye(int * dims);
  dARRAY * randn(int * dims);
  dARRAY * add(dARRAY * MatrixA, dARRAY * MatrixB);
  dARRAY * addScalar(dARRAY * matrix, float scalar);
  dARRAY * subtract(dARRAY * MatrixA, dARRAY * MatrixB);
  dARRAY * subScalar(dARRAY * matrix, float scalar);
  dARRAY * sum(dARRAY * matrix, int axis);
  dARRAY * transpose(dARRAY *  Matrix);
  dARRAY * dot(dARRAY *  MatrixA, dARRAY *  MatrixB);
  dARRAY * multiply(dARRAY * MatrixA, dARRAY * MatrixB);
  dARRAY * mulScalar(dARRAY * matrix, float scalar);
  dARRAY * divison(dARRAY * MatrixA, dARRAY * MatrixB);
  dARRAY * divScalar(dARRAY * matrix, float scalar);
  dARRAY * power(dARRAY * matrix, float power);
  dARRAY * squareroot(dARRAY * matrix);
  dARRAY * exponentional(dARRAY * matrix);
  dARRAY * b_cast(dARRAY * MatrixA, dARRAY * MatrixB);
  dARRAY * reshape(dARRAY * matrix, int * dims);

  int * permutation(int length);
  float mean(dARRAY * matrix);
  float var(dARRAY * matrix, char * type);
  float std(dARRAY * matrix, char * type);
  float gaussGenerator(float * cache, int * return_cache);
  float gaussRandom();
  float rand_norm(float mu, float std);

  float frobenius_norm(dARRAY * matrix);
  float Manhattan_distance(dARRAY * matrix);

  int size(dARRAY * A);
  void shape(dARRAY * A);
  void free2d(dARRAY * matrix);
  void sleep_my(int milliseconds);
  void cleanSTDIN();
  void get_safe_nn_threads();
#ifdef __cplusplus
}
#endif
#endif //UTILS_H


#include <math.h>

#include <gsl/gsl_blas.h>

#include "qdm.h"

#define TAU_INITIAL_GUESS 0.5
#define TAU_EPS 0.001
#define TAU_ITERATION_MAX 1000

#define RESET_SIZE 30
const double RESET_VALUES[RESET_SIZE] = {
  0.010000000000000000,
  0.010100000000000000,
  0.010200000000000001,
  0.010300000000000000,
  0.010400000000000000,
  0.010500000000000001,
  0.010600000000000000,
  0.010699999999999999,
  0.010800000000000001,
  0.010900000000000000,
  0.010999999999999999,
  0.010999999999999999,
  0.150714285714285717,
  0.290428571428571425,
  0.430142857142857160,
  0.569857142857142840,
  0.709571428571428520,
  0.849285714285714310,
  0.988999999999999990,
  0.988999999999999990,
  0.989099999999999979,
  0.989199999999999968,
  0.989299999999999957,
  0.989399999999999946,
  0.989500000000000046,
  0.989600000000000035,
  0.989700000000000024,
  0.989800000000000013,
  0.989900000000000002,
  0.989999999999999991,
};

/* Helper: Compute ispline by mmm dot product. */
static
int
ispline_mmm(
    double *result,

    double tau,
    size_t spline_df,
    const gsl_vector *knots,
    const gsl_vector *mmm
)
{
  int status = 0;

  size_t m = (knots->size - spline_df) + 1;
  gsl_vector *ispline = gsl_vector_alloc(m);

  qdm_ispline_vector(ispline, tau, spline_df, knots);
  status = gsl_blas_ddot(ispline, mmm, result);
  if (status != 0) {
    goto cleanup;
  }

cleanup:
  gsl_vector_free(ispline);

  return status;
}

/* Helper: Compute mspline by mmm dot product. */
static
int
mspline_mmm(
    double *result,

    double tau,
    size_t spline_df,
    const gsl_vector *knots,
    const gsl_vector *mmm
)
{
  int status = 0;

  size_t m = (knots->size - spline_df) + 1;
  gsl_vector *mspline = gsl_vector_alloc(m);

  qdm_mspline_vector(mspline, tau, spline_df, knots);
  status = gsl_blas_ddot(mspline, mmm, result);
  if (status != 0) {
    goto cleanup;
  }

cleanup:
  gsl_vector_free(mspline);

  return status;
}

int
qdm_find_tau(
    double *result,

    double v,
    size_t spline_df,
    const gsl_vector *knots,
    const gsl_vector *mmm
)
{
  int status = 0;

  double tau = TAU_INITIAL_GUESS;
  double qi_u = 0;
  double qm_u = 0;

  size_t reset_i = 0;
  for (size_t i = 0; i < TAU_ITERATION_MAX; i++) {
    if (reset_i >= RESET_SIZE) {
      tau = TAU_INITIAL_GUESS;

      break;
    }

    /* Calculate position... */
    status = ispline_mmm(&qi_u, tau, spline_df, knots, mmm);
    if (status != 0) {
      goto cleanup;
    }

    /* Calculate slope... */
    status = mspline_mmm(&qm_u, tau, spline_df, knots, mmm);
    if (status != 0) {
      goto cleanup;
    }

    /* Check if the update is within our desired interval [0, 1]. */
    double update = tau - (qi_u - v) / qm_u;
    if (update < 0 || update > 1) {
      tau = RESET_VALUES[reset_i];
      reset_i++;

      continue;
    } else {
      tau = update;
    }

    if (fabs(qi_u - v) <= TAU_EPS) {
      break;
    }
  }

  *result = tau;

cleanup:

  return status;
}

int
qdm_logl(
    double *log_likelihood,
    double *tau,

    double v,
    size_t spline_df,
    const gsl_vector *knots,
    const gsl_vector *mmm,

    double tau_low,
    double tau_high,
    double xi_low,
    double xi_high
)
{
  int status = 0;

  double low_threshold = 0;
  double high_threshold = 0;

  status = ispline_mmm(&low_threshold, tau_low, spline_df, knots, mmm);
  if (status != 0) {
    goto cleanup;
  }

  status = ispline_mmm(&high_threshold, tau_high, spline_df, knots, mmm);
  if (status != 0) {
    goto cleanup;
  }

  if (v <= low_threshold) {
    double sigma_low = 0;

    status = mspline_mmm(&sigma_low, tau_low, spline_df, knots, mmm);
    if (status != 0) {
      goto cleanup;
    }

    sigma_low *= tau_low;

    double z = low_threshold - v;

    *log_likelihood = log(tau_low / sigma_low * pow(1 + (xi_low * z) / sigma_low, -1 / xi_low - 1));
    *tau = (1 - (1 - pow(1 + xi_low * z / sigma_low, -1 / xi_low))) * tau_low;
  } else if (v >= high_threshold) {
    double sigma_high = 0;

    status = mspline_mmm(&sigma_high, tau_high, spline_df, knots, mmm);
    if (status != 0) {
      goto cleanup;
    }

    sigma_high *= (1 - tau_high);

    double z = v - high_threshold;

    *log_likelihood = log((1 - tau_high) / sigma_high * pow(1 + (xi_high * z) / sigma_high, -1 / xi_high - 1));
    *tau = (1 - pow(1 + xi_high * z / sigma_high, -1 / xi_high)) * (1 - tau_high) + tau_high;
  } else {
    double d = 0;

    status = qdm_find_tau(tau, v, spline_df, knots, mmm);
    if (status != 0) {
      goto cleanup;
    }

    status = mspline_mmm(&d, *tau, spline_df, knots, mmm);
    if (status != 0) {
      goto cleanup;
    }

    *log_likelihood = log(1 / d);
  }

cleanup:

  return status;
}

int
qdm_logl_2(
    double *log_likelihood,
    double *tau,

    double x,
    double y,

    double tau_low,
    double tau_high,

    double xi_low,
    double xi_high,

    size_t spline_df,

    const gsl_matrix *theta,
    const gsl_vector *knots
)
{
  int status = 0;

  double xi_data[2] = {1, x};
  gsl_vector_view xi = gsl_vector_view_array(xi_data, 2);

  double mmm_data[theta->size2];
  gsl_vector_view mmm = gsl_vector_view_array(mmm_data, theta->size2);

  status = gsl_blas_dgemv(CblasTrans, 1, theta, &xi.vector, 0, &mmm.vector);
  if (status != 0) {
    return status;
  }

  status = qdm_logl(
      log_likelihood,
      tau,

      y,
      spline_df,
      knots,
      &mmm.vector,

      tau_low,
      tau_high,
      xi_low,
      xi_high
  );
  if (status != 0) {
    return status;
  }

  return status;
}

void
qdm_logl_3(
    double *log_likelihood,
    double *tau,

    double x,
    double y,

    const qdm_tau *t,
    const gsl_vector *xi,

    const gsl_matrix *theta
)
{
  double xi_intercept_data[2] = {1, x};
  gsl_vector_view xi_intercept = gsl_vector_view_array(xi_intercept_data, 2);

  double mmm_data[theta->size2];
  gsl_vector_view mmm = gsl_vector_view_array(mmm_data, theta->size2);

  gsl_blas_dgemv(CblasTrans, 1, theta, &xi_intercept.vector, 0, &mmm.vector);

  double low_threshold = qdm_tau_ispline_mmm(t, t->low, &mmm.vector);
  double high_threshold = qdm_tau_ispline_mmm(t, t->high, &mmm.vector);

  /*
  fprintf(stderr, "x    : %f\n", x);
  fprintf(stderr, "y    : %f\n", y);

  fprintf(stderr, "tl   : %f\n", t->low);
  fprintf(stderr, "th   : %f\n", t->high);

  fprintf(stderr, "low  : %f\n", low_threshold);
  fprintf(stderr, "high : %f\n", high_threshold);

  qdm_matrix_csv_fwrite(stderr, theta);
  */

  double xi_low = gsl_vector_get(xi, 0);
  double xi_high = gsl_vector_get(xi, 1);

  if (y <= low_threshold) {
    double sigma_low = qdm_tau_mspline_mmm(t, t->low, &mmm.vector) * t->low;
    double z = low_threshold - y;

    *log_likelihood = log(t->low / sigma_low * pow(1 + (xi_low * z) / sigma_low, -1 / xi_low - 1));
    *tau = (1 - (1 - pow(1 + xi_low * z / sigma_low, -1 / xi_low))) * t->low;
  } else if (y >= high_threshold) {
    double sigma_high = qdm_tau_mspline_mmm(t, t->high, &mmm.vector) * (1 - t->high);
    double z = y - high_threshold;

    *log_likelihood = log((1 - t->high) / sigma_high * pow(1 + (xi_high * z) / sigma_high, -1 / xi_high - 1));
    *tau = (1 - pow(1 + xi_high * z / sigma_high, -1 / xi_high)) * (1 - t->high) + t->high;
  } else {
    *tau = qdm_tau_find(t, y, &mmm.vector);
    *log_likelihood = log(1 / qdm_tau_mspline_mmm(t, *tau, &mmm.vector));
  }

  /*
  fprintf(stderr, "ll   : %f\n", *log_likelihood);
  fprintf(stderr, "tau  : %f\n", *tau);
  */
}


#ifndef UTILITIES_H
#define UTILITIES_H

#include <gsl/gsl_rng.h>
#include <gsl/gsl_histogram.h>
#include <fftw3.h>

gsl_rng * init_rng(void);

void allocate_arrays(double ** signal, double ** spectrum,
    const size_t length);

gsl_histogram * init_histogram(const double r_min, const double r_max,
    const size_t bin_n);

gsl_histogram * init_histogram_uniform(const double r_min, const double r_max,
    const size_t bin_n);

void load_wisdom(const char * filename);

fftw_plan make_plan(double * signal, const size_t length);

void make_fft(fftw_plan plan);

void remove_mean(double * signal, const size_t length);

void compute_spectrum(const double * signal, double * spectrum,
    const size_t length);

void smooth(const unsigned neighbors, double *spectrum, const size_t length);

void final_spectrum(double *spectrum, const size_t length,
    const unsigned count, const double delta, const unsigned neighbors);

double frequency(const size_t i, const size_t length, const double delta);

void output_spectrum(const char * filename, const double * spectrum,
    const size_t length, const double delta, const size_t n_points);

void output_signal(const char * filename, const double * signal,
    const size_t length, const double delta, const size_t n_points);

void output_signal_full(const char * filename, const double * signal,
    const size_t length, const double delta);

void output_distribution(const char * filename, const gsl_histogram * hist);

void cleanup(double * signal, double * spectrum, fftw_plan plan, gsl_rng * r,
    gsl_histogram * hist);

#endif


#ifndef _LOGIT_H
#define _LOGIT_H

#include <vector>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

class logit
{
  private:
    gsl_vector *y; // response vector
    gsl_matrix *X; // observation matrix

    size_t n; // sample size;
    size_t p; // number of parameters (including intercept)

    double psi; // dispersion

    bool free_data; // depend on different constructors

    gsl_vector *beta;
    gsl_vector *Jbeta;
    gsl_vector *pvalue;

  public:
    logit(gsl_vector *yv, gsl_matrix *Xv);
    logit(const std::vector<double> & yv, const std::vector<std::vector<double> > Xv);
    ~logit();
    void fit();
    double calculate_pi(gsl_vector* xi) const;
    double calculate_err(const gsl_vector* beta2) const;
    double calculate_err() const;
    int calculate_J(gsl_matrix* J) const;
    int calculate_U(gsl_vector* U) const;
    void display(gsl_matrix* m) const;
    void displayv(gsl_vector* v) const;
}; //logit

#endif // _LOGIT_H


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

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_interp2d.h>

#define IDX2D(i, j, w) ((j) * ((w)->xsize) + (i))

typedef struct
{
  double * zx;
  double * zy;
  double * zxy;
  size_t xsize;
  size_t ysize;
} bicubic_state_t;

static void bicubic_free (void * vstate);

static void *
bicubic_alloc(size_t xsize, size_t ysize)
{
  bicubic_state_t *state;

  state = calloc(1, sizeof (bicubic_state_t));

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

  state->zx = (double *) malloc (xsize * ysize * sizeof (double));
  if (state->zx == NULL)
    {
      bicubic_free(state);
      GSL_ERROR_NULL("failed to allocate space for zx", GSL_ENOMEM);
    }

  state->zy = (double *) malloc (xsize * ysize * sizeof (double));
  if (state->zy == NULL)
    {
      bicubic_free(state);
      GSL_ERROR_NULL("failed to allocate space for zy", GSL_ENOMEM);
    }

  state->zxy = (double *) malloc (xsize * ysize * sizeof (double));
  if (state->zxy == NULL)
    {
      bicubic_free(state);
      GSL_ERROR_NULL("failed to allocate space for zxy", GSL_ENOMEM);
    }

  state->xsize = xsize;
  state->ysize = ysize;

  return state;
} /* bicubic_alloc() */

static void
bicubic_free (void * vstate)
{
  bicubic_state_t *state = (bicubic_state_t *) vstate;

  RETURN_IF_NULL(state);

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

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

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

  free (state);
} /* bicubic_free() */

static int
bicubic_init(void * vstate, const double xa[], const double ya[],
             const double za[], size_t xsize, size_t ysize)
{
  bicubic_state_t *state = (bicubic_state_t *) vstate;

  gsl_interp_accel *acc = gsl_interp_accel_alloc();
  gsl_spline *spline;
  gsl_vector *x;
  gsl_vector *y;
  size_t i, j;

  x = gsl_vector_alloc(xsize);
  y = gsl_vector_alloc(xsize);
  spline = gsl_spline_alloc(gsl_interp_cspline, xsize);
  for (j = 0; j <= ysize - 1; j++)
    {
      for (i = 0; i <= xsize - 1; i++)
        {
          gsl_vector_set(x, i, xa[i]);
          gsl_vector_set(y, i, za[IDX2D(i, j, state)]);
        }
      gsl_spline_init(spline, x->data, y->data, xsize);
      for (i = 0; i <= xsize - 1; i++)
        {
          state->zx[IDX2D(i, j, state)] = gsl_spline_eval_deriv(spline, xa[i], acc);
        }
    }
  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_spline_free(spline);
  gsl_interp_accel_reset(acc);

  x = gsl_vector_alloc(ysize);
  y = gsl_vector_alloc(ysize);
  spline = gsl_spline_alloc(gsl_interp_cspline, ysize);
  for (i = 0; i <= xsize - 1; i++)
    {
      for (j = 0; j <= ysize - 1; j++)
        {
          gsl_vector_set(x, j, ya[j]);
          gsl_vector_set(y, j, za[IDX2D(i, j, state)]);
        }
      gsl_spline_init(spline, x->data, y->data, ysize);
      for (j = 0; j <= ysize - 1; j++)
        {
          state->zy[IDX2D(i, j, state)] = gsl_spline_eval_deriv(spline, ya[j], acc);
        }
    }
  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_spline_free(spline);
  gsl_interp_accel_reset(acc);

  x = gsl_vector_alloc(xsize);
  y = gsl_vector_alloc(xsize);
  spline = gsl_spline_alloc(gsl_interp_cspline, xsize);
  for (j = 0; j <= ysize - 1; j++)
    {
      for (i = 0; i <= xsize - 1; i++)
        {
          gsl_vector_set(x, i, xa[i]);
          gsl_vector_set(y, i, state->zy[IDX2D(i, j, state)]);
        }
      gsl_spline_init(spline, x->data, y->data, xsize);
      for (i = 0; i <= xsize - 1; i++)
        {
          state->zxy[IDX2D(i, j, state)] = gsl_spline_eval_deriv(spline, xa[i], acc);
        }
    }
  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_spline_free(spline);
  gsl_interp_accel_free(acc);

  return GSL_SUCCESS;
} /* bicubic_init() */

static int
bicubic_eval(const void * vstate, const double xarr[], const double yarr[],
             const double zarr[], size_t xsize, size_t ysize,
             double x, double y, gsl_interp_accel * xa,
             gsl_interp_accel * ya, double * z)
{
  bicubic_state_t *state = (bicubic_state_t *) vstate;

  double xmin, xmax, ymin, ymax;
  double zminmin, zminmax, zmaxmin, zmaxmax;
  double zxminmin, zxminmax, zxmaxmin, zxmaxmax;
  double zyminmin, zyminmax, zymaxmin, zymaxmax;
  double zxyminmin, zxyminmax, zxymaxmin, zxymaxmax;

  double dx, dy;   /* size of the grid cell */
  double dt, du;

  /*
   * t and u are the positions within the grid cell at which we are computing
   * the interpolation, in units of grid cell size
   */
  double t, u;
  double t0, t1, t2, t3, u0, u1, u2, u3;
  double v;
  size_t xi, yi;

  /* first compute the indices into the data arrays where we are interpolating */
  if (xa != NULL)
    xi = gsl_interp_accel_find(xa, xarr, xsize, x);
  else
    xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);

  if (ya != NULL)
    yi = gsl_interp_accel_find(ya, yarr, ysize, y);
  else
    yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);

  /* find the minimum and maximum values on the grid cell in each dimension */
  xmin = xarr[xi];
  xmax = xarr[xi + 1];
  ymin = yarr[yi];
  ymax = yarr[yi + 1];
  zminmin = zarr[IDX2D(xi, yi, state)];
  zminmax = zarr[IDX2D(xi, yi + 1, state)];
  zmaxmin = zarr[IDX2D(xi + 1, yi, state)];
  zmaxmax = zarr[IDX2D(xi + 1, yi + 1, state)];
  /* Get the width and height of the grid cell */
  dx = xmax - xmin;
  dy = ymax - ymin;
  t = (x - xmin)/dx;
  u = (y - ymin)/dy;
  dt = 1./dx; /* partial t / partial x */
  du = 1./dy; /* partial u / partial y */
  zxminmin = state->zx[IDX2D(xi, yi, state)]/dt;
  zxminmax = state->zx[IDX2D(xi, yi + 1, state)]/dt;
  zxmaxmin = state->zx[IDX2D(xi + 1, yi, state)]/dt;
  zxmaxmax = state->zx[IDX2D(xi + 1, yi + 1, state)]/dt;
  zyminmin = state->zy[IDX2D(xi, yi, state)]/du;
  zyminmax = state->zy[IDX2D(xi, yi + 1, state)]/du;
  zymaxmin = state->zy[IDX2D(xi + 1, yi, state)]/du;
  zymaxmax = state->zy[IDX2D(xi + 1, yi + 1, state)]/du;
  zxyminmin = state->zxy[IDX2D(xi, yi, state)]/(dt*du);
  zxyminmax = state->zxy[IDX2D(xi, yi + 1, state)]/(dt*du);
  zxymaxmin = state->zxy[IDX2D(xi + 1, yi, state)]/(dt*du);
  zxymaxmax = state->zxy[IDX2D(xi + 1, yi + 1, state)]/(dt*du);
  t0 = 1;
  t1 = t;
  t2 = t*t;
  t3 = t*t2;
  u0 = 1;
  u1 = u;
  u2 = u*u;
  u3 = u*u2;

  *z = 0;
  v = zminmin;
  *z += v*t0*u0;
  v = zyminmin;
  *z += v*t0*u1;
  v = -3*zminmin + 3*zminmax - 2*zyminmin - zyminmax;
  *z += v*t0*u2;
  v = 2*zminmin - 2*zminmax + zyminmin + zyminmax;
  *z += v*t0*u3;
  v = zxminmin;
  *z += v*t1*u0;
  v = zxyminmin;
  *z += v*t1*u1;
  v = -3*zxminmin + 3*zxminmax - 2*zxyminmin - zxyminmax;
  *z += v*t1*u2;
  v = 2*zxminmin - 2*zxminmax + zxyminmin + zxyminmax;
  *z += v*t1*u3;
  v = -3*zminmin + 3*zmaxmin - 2*zxminmin - zxmaxmin;
  *z += v*t2*u0;
  v = -3*zyminmin + 3*zymaxmin - 2*zxyminmin - zxymaxmin;
  *z += v*t2*u1;
  v = 9*zminmin - 9*zmaxmin + 9*zmaxmax - 9*zminmax + 6*zxminmin + 3*zxmaxmin - 3*zxmaxmax - 6*zxminmax + 6*zyminmin - 6*zymaxmin - 3*zymaxmax + 3*zyminmax + 4*zxyminmin + 2*zxymaxmin + zxymaxmax + 2*zxyminmax;
  *z += v*t2*u2;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 4*zxminmin - 2*zxmaxmin + 2*zxmaxmax + 4*zxminmax - 3*zyminmin + 3*zymaxmin + 3*zymaxmax - 3*zyminmax - 2*zxyminmin - zxymaxmin - zxymaxmax - 2*zxyminmax;
  *z += v*t2*u3;
  v = 2*zminmin - 2*zmaxmin + zxminmin + zxmaxmin;
  *z += v*t3*u0;
  v = 2*zyminmin - 2*zymaxmin + zxyminmin + zxymaxmin;
  *z += v*t3*u1;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 3*zxminmin - 3*zxmaxmin + 3*zxmaxmax + 3*zxminmax - 4*zyminmin + 4*zymaxmin + 2*zymaxmax - 2*zyminmax - 2*zxyminmin - 2*zxymaxmin - zxymaxmax - zxyminmax;
  *z += v*t3*u2;
  v = 4*zminmin - 4*zmaxmin + 4*zmaxmax - 4*zminmax + 2*zxminmin + 2*zxmaxmin - 2*zxmaxmax - 2*zxminmax + 2*zyminmin - 2*zymaxmin - 2*zymaxmax + 2*zyminmax + zxyminmin + zxymaxmin + zxymaxmax + zxyminmax;
  *z += v*t3*u3;

  return GSL_SUCCESS;
} /* bicubic_eval() */

static int
bicubic_deriv_x(const void * vstate, const double xarr[], const double yarr[],
                const double zarr[], size_t xsize, size_t ysize,
                double x, double y,
                gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_p)
{
  bicubic_state_t *state = (bicubic_state_t *) vstate;

  double xmin, xmax, ymin, ymax;
  double zminmin, zminmax, zmaxmin, zmaxmax;
  double zxminmin, zxminmax, zxmaxmin, zxmaxmax;
  double zyminmin, zyminmax, zymaxmin, zymaxmax;
  double zxyminmin, zxyminmax, zxymaxmin, zxymaxmax;
  double dx, dy; /* size of the grid cell */
  double dt, du;

  /*
   * t and u are the positions within the grid cell at which we are computing
   * the interpolation, in units of grid cell size
   */
  double t, u;
  double t0, t1, t2, u0, u1, u2, u3;
  double v;
  size_t xi, yi;

  /* first compute the indices into the data arrays where we are interpolating */
  if (xa != NULL)
    xi = gsl_interp_accel_find(xa, xarr, xsize, x);
  else
    xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);

  if (ya != NULL)
    yi = gsl_interp_accel_find(ya, yarr, ysize, y);
  else
    yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);

  /* find the minimum and maximum values on the grid cell in each dimension */
  xmin = xarr[xi];
  xmax = xarr[xi + 1];
  ymin = yarr[yi];
  ymax = yarr[yi + 1];
  zminmin = zarr[IDX2D(xi, yi, state)];
  zminmax = zarr[IDX2D(xi, yi + 1, state)];
  zmaxmin = zarr[IDX2D(xi + 1, yi, state)];
  zmaxmax = zarr[IDX2D(xi + 1, yi + 1, state)];

  /* get the width and height of the grid cell */
  dx = xmax - xmin;
  dy = ymax - ymin;
  t = (x - xmin)/dx;
  u = (y - ymin)/dy;
  dt = 1./dx; /* partial t / partial x */
  du = 1./dy; /* partial u / partial y */

  zxminmin = state->zx[IDX2D(xi, yi, state)]/dt;
  zxminmax = state->zx[IDX2D(xi, yi + 1, state)]/dt;
  zxmaxmin = state->zx[IDX2D(xi + 1, yi, state)]/dt;
  zxmaxmax = state->zx[IDX2D(xi + 1, yi + 1, state)]/dt;
  zyminmin = state->zy[IDX2D(xi, yi, state)]/du;
  zyminmax = state->zy[IDX2D(xi, yi + 1, state)]/du;
  zymaxmin = state->zy[IDX2D(xi + 1, yi, state)]/du;
  zymaxmax = state->zy[IDX2D(xi + 1, yi + 1, state)]/du;
  zxyminmin = state->zxy[IDX2D(xi, yi, state)]/(dt*du);
  zxyminmax = state->zxy[IDX2D(xi, yi + 1, state)]/(dt*du);
  zxymaxmin = state->zxy[IDX2D(xi + 1, yi, state)]/(dt*du);
  zxymaxmax = state->zxy[IDX2D(xi + 1, yi + 1, state)]/(dt*du);

  t0 = 1;
  t1 = t;
  t2 = t*t;
  u0 = 1;
  u1 = u;
  u2 = u*u;
  u3 = u*u2;

  *z_p = 0;
  v = zxminmin;
  *z_p += v*t0*u0;
  v = zxyminmin;
  *z_p += v*t0*u1;
  v = -3*zxminmin + 3*zxminmax - 2*zxyminmin - zxyminmax;
  *z_p += v*t0*u2;
  v = 2*zxminmin - 2*zxminmax + zxyminmin + zxyminmax;
  *z_p += v*t0*u3;
  v = -3*zminmin + 3*zmaxmin - 2*zxminmin - zxmaxmin;
  *z_p += 2*v*t1*u0;
  v = -3*zyminmin + 3*zymaxmin - 2*zxyminmin - zxymaxmin;
  *z_p += 2*v*t1*u1;
  v = 9*zminmin - 9*zmaxmin + 9*zmaxmax - 9*zminmax + 6*zxminmin + 3*zxmaxmin - 3*zxmaxmax - 6*zxminmax + 6*zyminmin - 6*zymaxmin - 3*zymaxmax + 3*zyminmax + 4*zxyminmin + 2*zxymaxmin + zxymaxmax + 2*zxyminmax;
  *z_p += 2*v*t1*u2;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 4*zxminmin - 2*zxmaxmin + 2*zxmaxmax + 4*zxminmax - 3*zyminmin + 3*zymaxmin + 3*zymaxmax - 3*zyminmax - 2*zxyminmin - zxymaxmin - zxymaxmax - 2*zxyminmax;
  *z_p += 2*v*t1*u3;
  v = 2*zminmin - 2*zmaxmin + zxminmin + zxmaxmin;
  *z_p += 3*v*t2*u0;
  v = 2*zyminmin - 2*zymaxmin + zxyminmin + zxymaxmin;
  *z_p += 3*v*t2*u1;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 3*zxminmin - 3*zxmaxmin + 3*zxmaxmax + 3*zxminmax - 4*zyminmin + 4*zymaxmin + 2*zymaxmax - 2*zyminmax - 2*zxyminmin - 2*zxymaxmin - zxymaxmax - zxyminmax;
  *z_p += 3*v*t2*u2;
  v = 4*zminmin - 4*zmaxmin + 4*zmaxmax - 4*zminmax + 2*zxminmin + 2*zxmaxmin - 2*zxmaxmax - 2*zxminmax + 2*zyminmin - 2*zymaxmin - 2*zymaxmax + 2*zyminmax + zxyminmin + zxymaxmin + zxymaxmax + zxyminmax;
  *z_p += 3*v*t2*u3;
  *z_p *= dt;

  return GSL_SUCCESS;
} /* bicubic_deriv_x() */

static int
bicubic_deriv_y(const void * vstate, const double xarr[], const double yarr[],
                const double zarr[], size_t xsize, size_t ysize,
                double x, double y,
                gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_p)
{
  bicubic_state_t *state = (bicubic_state_t *) vstate;

  double xmin, xmax, ymin, ymax;
  double zminmin, zminmax, zmaxmin, zmaxmax;
  double zxminmin, zxminmax, zxmaxmin, zxmaxmax;
  double zyminmin, zyminmax, zymaxmin, zymaxmax;
  double zxyminmin, zxyminmax, zxymaxmin, zxymaxmax;
  /* dx and dy are the size of the grid cell */
  double dx, dy;
  double dt, du;
  /* t and u are the positions within the grid cell at which we are
   * computing the interpolation, in units of grid cell size */
  double t, u;
  double t0, t1, t2, t3, u0, u1, u2;
  double v;
  size_t xi, yi;

  /* first compute the indices into the data arrays where we are interpolating */
  if (xa != NULL)
    xi = gsl_interp_accel_find(xa, xarr, xsize, x);
  else
    xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);

  if (ya != NULL)
    yi = gsl_interp_accel_find(ya, yarr, ysize, y);
  else
    yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);

  /* find the minimum and maximum values on the grid cell in each dimension */
  xmin = xarr[xi];
  xmax = xarr[xi + 1];
  ymin = yarr[yi];
  ymax = yarr[yi + 1];
  zminmin = zarr[IDX2D(xi, yi, state)];
  zminmax = zarr[IDX2D(xi, yi + 1, state)];
  zmaxmin = zarr[IDX2D(xi + 1, yi, state)];
  zmaxmax = zarr[IDX2D(xi + 1, yi + 1, state)];

  /* get the width and height of the grid cell */
  dx = xmax - xmin;
  dy = ymax - ymin;
  t = (x - xmin)/dx;
  u = (y - ymin)/dy;
  dt = 1./dx; /* partial t / partial x */
  du = 1./dy; /* partial u / partial y */

  zxminmin = state->zx[IDX2D(xi, yi, state)]/dt;
  zxminmax = state->zx[IDX2D(xi, yi + 1, state)]/dt;
  zxmaxmin = state->zx[IDX2D(xi + 1, yi, state)]/dt;
  zxmaxmax = state->zx[IDX2D(xi + 1, yi + 1, state)]/dt;
  zyminmin = state->zy[IDX2D(xi, yi, state)]/du;
  zyminmax = state->zy[IDX2D(xi, yi + 1, state)]/du;
  zymaxmin = state->zy[IDX2D(xi + 1, yi, state)]/du;
  zymaxmax = state->zy[IDX2D(xi + 1, yi + 1, state)]/du;
  zxyminmin = state->zxy[IDX2D(xi, yi, state)]/(dt*du);
  zxyminmax = state->zxy[IDX2D(xi, yi + 1, state)]/(dt*du);
  zxymaxmin = state->zxy[IDX2D(xi + 1, yi, state)]/(dt*du);
  zxymaxmax = state->zxy[IDX2D(xi + 1, yi + 1, state)]/(dt*du);

  t0 = 1;
  t1 = t;
  t2 = t*t;
  t3 = t*t2;
  u0 = 1;
  u1 = u;
  u2 = u*u;

  *z_p = 0;
  v = zyminmin;
  *z_p += v*t0*u0;
  v = -3*zminmin + 3*zminmax - 2*zyminmin - zyminmax;
  *z_p += 2*v*t0*u1;
  v = 2*zminmin-2*zminmax + zyminmin + zyminmax;
  *z_p += 3*v*t0*u2;
  v = zxyminmin;
  *z_p += v*t1*u0;
  v = -3*zxminmin + 3*zxminmax - 2*zxyminmin - zxyminmax;
  *z_p += 2*v*t1*u1;
  v = 2*zxminmin - 2*zxminmax + zxyminmin + zxyminmax;
  *z_p += 3*v*t1*u2;
  v = -3*zyminmin + 3*zymaxmin - 2*zxyminmin - zxymaxmin;
  *z_p += v*t2*u0;
  v = 9*zminmin - 9*zmaxmin + 9*zmaxmax - 9*zminmax + 6*zxminmin + 3*zxmaxmin - 3*zxmaxmax - 6*zxminmax + 6*zyminmin - 6*zymaxmin - 3*zymaxmax + 3*zyminmax + 4*zxyminmin + 2*zxymaxmin + zxymaxmax + 2*zxyminmax;
  *z_p += 2*v*t2*u1;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 4*zxminmin - 2*zxmaxmin + 2*zxmaxmax + 4*zxminmax - 3*zyminmin + 3*zymaxmin + 3*zymaxmax - 3*zyminmax - 2*zxyminmin - zxymaxmin - zxymaxmax - 2*zxyminmax;
  *z_p += 3*v*t2*u2;
  v = 2*zyminmin - 2*zymaxmin + zxyminmin + zxymaxmin;
  *z_p += v*t3*u0;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 3*zxminmin - 3*zxmaxmin + 3*zxmaxmax + 3*zxminmax - 4*zyminmin + 4*zymaxmin + 2*zymaxmax - 2*zyminmax - 2*zxyminmin - 2*zxymaxmin - zxymaxmax - zxyminmax;
  *z_p += 2*v*t3*u1;
  v = 4*zminmin - 4*zmaxmin + 4*zmaxmax - 4*zminmax + 2*zxminmin + 2*zxmaxmin - 2*zxmaxmax - 2*zxminmax + 2*zyminmin - 2*zymaxmin - 2*zymaxmax + 2*zyminmax + zxyminmin + zxymaxmin + zxymaxmax + zxyminmax;
  *z_p += 3*v*t3*u2;
  *z_p *= du;

  return GSL_SUCCESS;
}

static int
bicubic_deriv_xx(const void * vstate, const double xarr[], const double yarr[],
                 const double zarr[], size_t xsize, size_t ysize,
                 double x, double y,
                 gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_pp)
{
  bicubic_state_t *state = (bicubic_state_t *) vstate;

  double xmin, xmax, ymin, ymax;
  double zminmin, zminmax, zmaxmin, zmaxmax;
  double zxminmin, zxminmax, zxmaxmin, zxmaxmax;
  double zyminmin, zyminmax, zymaxmin, zymaxmax;
  double zxyminmin, zxyminmax, zxymaxmin, zxymaxmax;

  double dx, dy; /* size of the grid cell */
  double dt, du;

  /*
   * t and u are the positions within the grid cell at which we are computing
   * the interpolation, in units of grid cell size
   */
  double t, u;
  double t0, t1, u0, u1, u2, u3;
  double v;
  size_t xi, yi;

  /* first compute the indices into the data arrays where we are interpolating */
  if (xa != NULL)
    xi = gsl_interp_accel_find(xa, xarr, xsize, x);
  else
    xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);

  if (ya != NULL)
    yi = gsl_interp_accel_find(ya, yarr, ysize, y);
  else
    yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);

  /* find the minimum and maximum values on the grid cell in each dimension */
  xmin = xarr[xi];
  xmax = xarr[xi + 1];
  ymin = yarr[yi];
  ymax = yarr[yi + 1];
  zminmin = zarr[IDX2D(xi, yi, state)];
  zminmax = zarr[IDX2D(xi, yi + 1, state)];
  zmaxmin = zarr[IDX2D(xi + 1, yi, state)];
  zmaxmax = zarr[IDX2D(xi + 1, yi + 1, state)];

  /* get the width and height of the grid cell */
  dx = xmax - xmin;
  dy = ymax - ymin;
  t = (x - xmin)/dx;
  u = (y - ymin)/dy;
  dt = 1./dx; /* partial t / partial x */
  du = 1./dy; /* partial u / partial y */

  zxminmin = state->zx[IDX2D(xi, yi, state)]/dt;
  zxminmax = state->zx[IDX2D(xi, yi + 1, state)]/dt;
  zxmaxmin = state->zx[IDX2D(xi + 1, yi, state)]/dt;
  zxmaxmax = state->zx[IDX2D(xi + 1, yi + 1, state)]/dt;
  zyminmin = state->zy[IDX2D(xi, yi, state)]/du;
  zyminmax = state->zy[IDX2D(xi, yi + 1, state)]/du;
  zymaxmin = state->zy[IDX2D(xi + 1, yi, state)]/du;
  zymaxmax = state->zy[IDX2D(xi + 1, yi + 1, state)]/du;
  zxyminmin = state->zxy[IDX2D(xi, yi, state)]/(dt*du);
  zxyminmax = state->zxy[IDX2D(xi, yi + 1, state)]/(dt*du);
  zxymaxmin = state->zxy[IDX2D(xi + 1, yi, state)]/(dt*du);
  zxymaxmax = state->zxy[IDX2D(xi + 1, yi + 1, state)]/(dt*du);

  t0 = 1;
  t1 = t;
  u0 = 1;
  u1 = u;
  u2 = u*u;
  u3 = u*u2;

  *z_pp = 0;
  v = -3*zminmin + 3*zmaxmin - 2*zxminmin - zxmaxmin;
  *z_pp += 2*v*t0*u0;
  v = -3*zyminmin + 3*zymaxmin - 2*zxyminmin - zxymaxmin;
  *z_pp += 2*v*t0*u1;
  v = 9*zminmin - 9*zmaxmin + 9*zmaxmax - 9*zminmax + 6*zxminmin + 3*zxmaxmin - 3*zxmaxmax - 6*zxminmax + 6*zyminmin - 6*zymaxmin - 3*zymaxmax + 3*zyminmax + 4*zxyminmin + 2*zxymaxmin + zxymaxmax + 2*zxyminmax;
  *z_pp += 2*v*t0*u2;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 4*zxminmin - 2*zxmaxmin + 2*zxmaxmax + 4*zxminmax - 3*zyminmin + 3*zymaxmin + 3*zymaxmax - 3*zyminmax - 2*zxyminmin - zxymaxmin - zxymaxmax - 2*zxyminmax;
  *z_pp += 2*v*t0*u3;
  v = 2*zminmin - 2*zmaxmin + zxminmin + zxmaxmin;
  *z_pp += 6*v*t1*u0;
  v = 2*zyminmin - 2*zymaxmin + zxyminmin + zxymaxmin;
  *z_pp += 6*v*t1*u1;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 3*zxminmin - 3*zxmaxmin + 3*zxmaxmax + 3*zxminmax - 4*zyminmin + 4*zymaxmin + 2*zymaxmax - 2*zyminmax - 2*zxyminmin - 2*zxymaxmin - zxymaxmax - zxyminmax;
  *z_pp += 6*v*t1*u2;
  v = 4*zminmin - 4*zmaxmin + 4*zmaxmax - 4*zminmax + 2*zxminmin + 2*zxmaxmin - 2*zxmaxmax - 2*zxminmax + 2*zyminmin - 2*zymaxmin - 2*zymaxmax + 2*zyminmax + zxyminmin + zxymaxmin + zxymaxmax + zxyminmax;
  *z_pp += 6*v*t1*u3;
  *z_pp *= dt*dt;

  return GSL_SUCCESS;
}

static int
bicubic_deriv_xy(const void * vstate, const double xarr[], const double yarr[],
                 const double zarr[], size_t xsize, size_t ysize,
                 double x, double y,
                 gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_pp)
{
  bicubic_state_t *state = (bicubic_state_t *) vstate;

  double xmin, xmax, ymin, ymax;
  double zminmin, zminmax, zmaxmin, zmaxmax;
  double zxminmin, zxminmax, zxmaxmin, zxmaxmax;
  double zyminmin, zyminmax, zymaxmin, zymaxmax;
  double zxyminmin, zxyminmax, zxymaxmin, zxymaxmax;

  double dx, dy; /* size of the grid cell */
  double dt, du;

  /*
   * t and u are the positions within the grid cell at which we are computing
   * the interpolation, in units of grid cell size
   */
  double t, u;
  double t0, t1, t2, u0, u1, u2;
  double v;
  size_t xi, yi;

  /* first compute the indices into the data arrays where we are interpolating */
  if (xa != NULL)
    xi = gsl_interp_accel_find(xa, xarr, xsize, x);
  else
    xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);

  if (ya != NULL)
    yi = gsl_interp_accel_find(ya, yarr, ysize, y);
  else
    yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);

  /* find the minimum and maximum values on the grid cell in each dimension */
  xmin = xarr[xi];
  xmax = xarr[xi + 1];
  ymin = yarr[yi];
  ymax = yarr[yi + 1];
  zminmin = zarr[IDX2D(xi, yi, state)];
  zminmax = zarr[IDX2D(xi, yi + 1, state)];
  zmaxmin = zarr[IDX2D(xi + 1, yi, state)];
  zmaxmax = zarr[IDX2D(xi + 1, yi + 1, state)];

  /* get the width and height of the grid cell */
  dx = xmax - xmin;
  dy = ymax - ymin;
  t = (x - xmin)/dx;
  u = (y - ymin)/dy;
  dt = 1./dx; /* partial t / partial x */
  du = 1./dy; /* partial u / partial y */

  zxminmin = state->zx[IDX2D(xi, yi, state)]/dt;
  zxminmax = state->zx[IDX2D(xi, yi + 1, state)]/dt;
  zxmaxmin = state->zx[IDX2D(xi + 1, yi, state)]/dt;
  zxmaxmax = state->zx[IDX2D(xi + 1, yi + 1, state)]/dt;
  zyminmin = state->zy[IDX2D(xi, yi, state)]/du;
  zyminmax = state->zy[IDX2D(xi, yi + 1, state)]/du;
  zymaxmin = state->zy[IDX2D(xi + 1, yi, state)]/du;
  zymaxmax = state->zy[IDX2D(xi + 1, yi + 1, state)]/du;
  zxyminmin = state->zxy[IDX2D(xi, yi, state)]/(dt*du);
  zxyminmax = state->zxy[IDX2D(xi, yi + 1, state)]/(dt*du);
  zxymaxmin = state->zxy[IDX2D(xi + 1, yi, state)]/(dt*du);
  zxymaxmax = state->zxy[IDX2D(xi + 1, yi + 1, state)]/(dt*du);

  t0 = 1;
  t1 = t;
  t2 = t*t;
  u0 = 1;
  u1 = u;
  u2 = u*u;

  *z_pp = 0;
  v = zxyminmin;
  *z_pp += v*t0*u0;
  v = -3*zxminmin + 3*zxminmax - 2*zxyminmin - zxyminmax;
  *z_pp += 2*v*t0*u1;
  v = 2*zxminmin - 2*zxminmax + zxyminmin + zxyminmax;
  *z_pp += 3*v*t0*u2;
  v = -3*zyminmin + 3*zymaxmin - 2*zxyminmin - zxymaxmin;
  *z_pp += 2*v*t1*u0;
  v = 9*zminmin - 9*zmaxmin + 9*zmaxmax - 9*zminmax + 6*zxminmin + 3*zxmaxmin - 3*zxmaxmax - 6*zxminmax + 6*zyminmin - 6*zymaxmin - 3*zymaxmax + 3*zyminmax + 4*zxyminmin + 2*zxymaxmin + zxymaxmax + 2*zxyminmax;
  *z_pp += 4*v*t1*u1;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 4*zxminmin - 2*zxmaxmin + 2*zxmaxmax + 4*zxminmax - 3*zyminmin + 3*zymaxmin + 3*zymaxmax - 3*zyminmax - 2*zxyminmin - zxymaxmin - zxymaxmax - 2*zxyminmax;
  *z_pp += 6*v*t1*u2;
  v = 2*zyminmin - 2*zymaxmin + zxyminmin + zxymaxmin;
  *z_pp += 3*v*t2*u0;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 3*zxminmin - 3*zxmaxmin + 3*zxmaxmax + 3*zxminmax - 4*zyminmin + 4*zymaxmin + 2*zymaxmax - 2*zyminmax - 2*zxyminmin - 2*zxymaxmin - zxymaxmax - zxyminmax;
  *z_pp += 6*v*t2*u1;
  v = 4*zminmin - 4*zmaxmin + 4*zmaxmax - 4*zminmax + 2*zxminmin + 2*zxmaxmin - 2*zxmaxmax - 2*zxminmax + 2*zyminmin - 2*zymaxmin - 2*zymaxmax + 2*zyminmax + zxyminmin + zxymaxmin + zxymaxmax + zxyminmax;
  *z_pp += 9*v*t2*u2;
  *z_pp *= dt*du;

  return GSL_SUCCESS;
}

static int
bicubic_deriv_yy(const void * vstate, const double xarr[], const double yarr[],
                 const double zarr[], size_t xsize, size_t ysize,
                 double x, double y,
                 gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_pp)
{
  bicubic_state_t *state = (bicubic_state_t *) vstate;

  double xmin, xmax, ymin, ymax;
  double zminmin, zminmax, zmaxmin, zmaxmax;
  double zxminmin, zxminmax, zxmaxmin, zxmaxmax;
  double zyminmin, zyminmax, zymaxmin, zymaxmax;
  double zxyminmin, zxyminmax, zxymaxmin, zxymaxmax;

  double dx, dy; /* size of the grid cell */
  double dt, du;

  /*
   * t and u are the positions within the grid cell at which we are computing
   * the interpolation, in units of grid cell size
   */
  double t, u;
  double t0, t1, t2, t3, u0, u1;
  double v;
  size_t xi, yi;

  /* first compute the indices into the data arrays where we are interpolating */
  if (xa != NULL)
    xi = gsl_interp_accel_find(xa, xarr, xsize, x);
  else
    xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);

  if (ya != NULL)
    yi = gsl_interp_accel_find(ya, yarr, ysize, y);
  else
    yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);

  /* find the minimum and maximum values on the grid cell in each dimension */
  xmin = xarr[xi];
  xmax = xarr[xi + 1];
  ymin = yarr[yi];
  ymax = yarr[yi + 1];
  zminmin = zarr[IDX2D(xi, yi, state)];
  zminmax = zarr[IDX2D(xi, yi + 1, state)];
  zmaxmin = zarr[IDX2D(xi + 1, yi, state)];
  zmaxmax = zarr[IDX2D(xi + 1, yi + 1, state)];

  /* get the width and height of the grid cell */
  dx = xmax - xmin;
  dy = ymax - ymin;
  t = (x - xmin)/dx;
  u = (y - ymin)/dy;
  dt = 1./dx; /* partial t / partial x */
  du = 1./dy; /* partial u / partial y */

  zxminmin = state->zx[IDX2D(xi, yi, state)]/dt;
  zxminmax = state->zx[IDX2D(xi, yi + 1, state)]/dt;
  zxmaxmin = state->zx[IDX2D(xi + 1, yi, state)]/dt;
  zxmaxmax = state->zx[IDX2D(xi + 1, yi + 1, state)]/dt;
  zyminmin = state->zy[IDX2D(xi, yi, state)]/du;
  zyminmax = state->zy[IDX2D(xi, yi + 1, state)]/du;
  zymaxmin = state->zy[IDX2D(xi + 1, yi, state)]/du;
  zymaxmax = state->zy[IDX2D(xi + 1, yi + 1, state)]/du;
  zxyminmin = state->zxy[IDX2D(xi, yi, state)]/(dt*du);
  zxyminmax = state->zxy[IDX2D(xi, yi + 1, state)]/(dt*du);
  zxymaxmin = state->zxy[IDX2D(xi + 1, yi, state)]/(dt*du);
  zxymaxmax = state->zxy[IDX2D(xi + 1, yi + 1, state)]/(dt*du);

  t0 = 1;
  t1 = t;
  t2 = t*t;
  t3 = t*t2;
  u0 = 1;
  u1 = u;

  *z_pp = 0;
  v = -3*zminmin + 3*zminmax - 2*zyminmin - zyminmax;
  *z_pp += 2*v*t0*u0;
  v = 2*zminmin-2*zminmax + zyminmin + zyminmax;
  *z_pp += 6*v*t0*u1;
  v = -3*zxminmin + 3*zxminmax - 2*zxyminmin - zxyminmax;
  *z_pp += 2*v*t1*u0;
  v = 2*zxminmin - 2*zxminmax + zxyminmin + zxyminmax;
  *z_pp += 6*v*t1*u1;
  v = 9*zminmin - 9*zmaxmin + 9*zmaxmax - 9*zminmax + 6*zxminmin + 3*zxmaxmin - 3*zxmaxmax - 6*zxminmax + 6*zyminmin - 6*zymaxmin - 3*zymaxmax + 3*zyminmax + 4*zxyminmin + 2*zxymaxmin + zxymaxmax + 2*zxyminmax;
  *z_pp += 2*v*t2*u0;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 4*zxminmin - 2*zxmaxmin + 2*zxmaxmax + 4*zxminmax - 3*zyminmin + 3*zymaxmin + 3*zymaxmax - 3*zyminmax - 2*zxyminmin - zxymaxmin - zxymaxmax - 2*zxyminmax;
  *z_pp += 6*v*t2*u1;
  v = -6*zminmin + 6*zmaxmin - 6*zmaxmax + 6*zminmax - 3*zxminmin - 3*zxmaxmin + 3*zxmaxmax + 3*zxminmax - 4*zyminmin + 4*zymaxmin + 2*zymaxmax - 2*zyminmax - 2*zxyminmin - 2*zxymaxmin - zxymaxmax - zxyminmax;
  *z_pp += 2*v*t3*u0;
  v = 4*zminmin - 4*zmaxmin + 4*zmaxmax - 4*zminmax + 2*zxminmin + 2*zxmaxmin - 2*zxmaxmax - 2*zxminmax + 2*zyminmin - 2*zymaxmin - 2*zymaxmax + 2*zyminmax + zxyminmin + zxymaxmin + zxymaxmax + zxyminmax;
  *z_pp += 6*v*t3*u1;
  *z_pp *= du*du;

  return GSL_SUCCESS;
}

static const gsl_interp2d_type bicubic_type = {
  "bicubic",
  4,
  &bicubic_alloc,
  &bicubic_init,
  &bicubic_eval,
  &bicubic_deriv_x,
  &bicubic_deriv_y,
  &bicubic_deriv_xx,
  &bicubic_deriv_xy,
  &bicubic_deriv_yy,
  &bicubic_free
};

const gsl_interp2d_type * gsl_interp2d_bicubic = &bicubic_type;

#undef IDX2D


#ifndef QDM_ISPLINE_H
#define QDM_ISPLINE_H 1

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

void
qdm_ispline_vector(
    gsl_vector *result,
    const double tau,
    const size_t spline_df,
    const gsl_vector *knots
);

void
qdm_ispline_matrix(
    gsl_matrix *result,
    const gsl_vector *taus,
    const size_t spline_df,
    const gsl_vector *knots
);

#endif /* QDM_ISPLINE_H */


#ifndef H_MATH_UTILS_GSL
#define H_MATH_UTILS_GSL

#include <gsl/gsl_math.h>
//#include <egsl/egsl.h>
#include </home/alecone/catkin_graph/src/csm/sm/lib/egsl/egsl.h>

#include "laser_data.h"

#define gvg gsl_vector_get
#define gvs gsl_vector_set

/* GSL stuff */
	const char* gsl_friendly_pose(gsl_vector*v);
	gsl_vector * vector_from_array(unsigned int n, double *x);
	void vector_to_array(const gsl_vector*v, double*);
	void copy_from_array(gsl_vector*v, double*);

	void oplus(const gsl_vector*x1,const gsl_vector*x2, gsl_vector*res);
	void ominus(const gsl_vector*x, gsl_vector*res);
	void pose_diff(const gsl_vector*pose2,const gsl_vector*pose1,gsl_vector*res);

	void transform(const gsl_vector* point2d, const gsl_vector* pose, gsl_vector*result2d);
	void gsl_vector_set_nan(gsl_vector*v);

	double distance(const gsl_vector* a,const gsl_vector* b);
	double distance_squared(const gsl_vector* a,const gsl_vector* b);

	/** Returns norm of 2D point p */
	double norm(const gsl_vector*p);
	const char* egsl_friendly_pose(val pose);
	const char* egsl_friendly_cov(val cov);

/** Returns Fisher's information matrix. You still have to multiply
    it by (1/sigma^2). */
val ld_fisher0(LDP ld);

#endif


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

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_sf_gamma.h>

/* Generate a random matrix from a Wishart distribution using the Bartlett
 * decomposition, following Smith and Hocking, Journal of the Royal Statistical
 * Society. Series C (Applied Statistics), Vol. 21, No. 3 (1972), pp. 341-345.
 *
 * df      degrees of freedom
 * L       matrix resulting from the Cholesky decomposition of
 *         the scale matrix V = L L^T (dimension d x d)
 * result  output matrix (dimension d x d)
 * work    matrix used for intermediate computations (dimension d x d)
 */
int
gsl_ran_wishart (const gsl_rng * r,
                 const double df,
                 const gsl_matrix * L,
                 gsl_matrix * result,
                 gsl_matrix * work)
{
  if (L->size1 != L->size2)
    {
      GSL_ERROR("L should be a square matrix", GSL_ENOTSQR);
    }
  else if (result->size1 != result->size2)
    {
      GSL_ERROR("result should be a square matrix", GSL_ENOTSQR);
    }
  else if (work->size1 != work->size2)
    {
      GSL_ERROR("work should be a square matrix", GSL_ENOTSQR);
    }
  else if (result->size1 != L->size1)
    {
      GSL_ERROR("incompatible dimensions of result matrix", GSL_EBADLEN);
    }
  else if (work->size1 != L->size1)
    {
      GSL_ERROR("incompatible dimensions of work matrix", GSL_EBADLEN);
    }
  else if (df <= L->size1 - 1)
    {
      GSL_ERROR("incompatible degrees of freedom", GSL_EDOM);
    }
  else
    {
      /* result: X = L A A^T L^T */

      size_t d = L->size1, i, j;

      /* insure the upper part of A is zero before filling its lower part */
      gsl_matrix_set_zero(work);
      for (i = 0; i < d; ++i)
        {
          gsl_matrix_set(work, i, i, sqrt(gsl_ran_chisq(r, df - i)));

          for (j = 0; j < i; ++j)
            {
              gsl_matrix_set(work, i, j, gsl_ran_ugaussian(r));
            }
        }

      /* compute L * A */
      gsl_blas_dtrmm(CblasLeft, CblasLower, CblasNoTrans, CblasNonUnit, 1.0,
                     L, work);

      /* compute (L * A) * (L * A)^T */
      gsl_blas_dsyrk(CblasUpper, CblasNoTrans, 1.0, work, 0.0, result);
      for (i = 0; i < d; ++i)
        {
          for (j = 0; j < i; ++j)
            {
              gsl_matrix_set(result, i, j, gsl_matrix_get(result, j, i));
            }
        }

      return GSL_SUCCESS;
    }
}

/* Compute the log of the probability density function at a given quantile
 * matrix for a Wishart distribution using the Cholesky decomposition of the
 * scale matrix.
 *
 * X       quantile matrix at which to evaluate the PDF (dimension d x d)
 * L_X     matrix resulting from the Cholesky decomposition of
 *         of the quantile matrix at which to evaluate the PDF
 *         X = L_X L_X^T (dimension d x d)
 * df      degrees of freedom
 * L       matrix resulting from the Cholesky decomposition of
 *         the scale matrix V = L L^T (dimension d x d)
 * result  output of the density (dimension 1)
 * work    matrix used for intermediate computations (dimension d x d)
 */
int
gsl_ran_wishart_log_pdf (const gsl_matrix * X,
                         const gsl_matrix * L_X,
                         const double df,
                         const gsl_matrix * L,
                         double * result,
                         gsl_matrix * work)
{
  if (L->size1 != L->size2)
    {
      GSL_ERROR("L should be a square matrix", GSL_ENOTSQR);
    }
  else if (X->size1 != X->size2)
    {
      GSL_ERROR("X should be a square matrix", GSL_ENOTSQR);
    }
  else if (L_X->size1 != L_X->size2)
    {
      GSL_ERROR("L_X should be a square matrix", GSL_ENOTSQR);
    }
  else if (X->size1 != L->size1)
    {
      GSL_ERROR("incompatible dimensions of X matrix", GSL_EBADLEN);
    }
  else if (L_X->size1 != L->size1)
    {
      GSL_ERROR("incompatible dimensions of L_X matrix", GSL_EBADLEN);
    }
  else if (df <= L->size1 - 1)
    {
      GSL_ERROR("incompatible degrees of freedom", GSL_EDOM);
    }
  else
    {
      size_t d = L->size1, i;
      int status;
      double log_mv_Ga, log_det_V, log_det_X, tr_Vinv_X;

      /* compute the log of the multivariate Gamma */
      log_mv_Ga = d * (d-1) * 0.25 * log(M_PI);
      for (i = 0; i < d; ++i)
        {
          log_mv_Ga += gsl_sf_lngamma((df - i + 1) * 0.5);
        }

      /* compute the log of the determinant of the scale matrix */
      log_det_V = log(gsl_matrix_get(L, 0, 0));
      for (i = 1; i < d; ++i)
        {
          log_det_V += log(gsl_matrix_get(L, i, i));
        }
      log_det_V = 2 * log_det_V;

      /* compute the log of the determinant of the quantile matrix */
      log_det_X = log(gsl_matrix_get(L_X, 0, 0));
      for (i = 1; i < d; ++i)
        {
          log_det_X += log(gsl_matrix_get(L_X, i, i));
        }
      log_det_X = 2 * log_det_X;

      /* compute the trace of V^(-1) X */
      status = gsl_linalg_cholesky_solve_mat(L, X, work);
      if (status)
        return status;
      tr_Vinv_X = gsl_matrix_get(work, 0, 0);
      for (i = 1; i < d; ++i)
        {
          tr_Vinv_X += gsl_matrix_get(work, i, i);
        }

      /* add all to get the log of the PDF */
      *result = - (0.5 * df * d) * log(2.0)
                - (0.5 * df) * log_det_V
                - log_mv_Ga
                + 0.5 * (df - d - 1) * log_det_X
                - 0.5 * tr_Vinv_X;

      return GSL_SUCCESS;
    }
}

int
gsl_ran_wishart_pdf (const gsl_matrix * X,
                     const gsl_matrix * L_X,
                     const double df,
                     const gsl_matrix * L,
                     double * result,
                     gsl_matrix * work)
{
  double logpdf;
  int status = gsl_ran_wishart_log_pdf(X, L_X, df, L, &logpdf, work);

  if (status == GSL_SUCCESS)
    *result = exp(logpdf);

  return status;
}


#ifndef __INC_MULTIMODELSTRUCT__
#define __INC_MULTIMODELSTRUCT__

#include "modelstruct.h"
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

struct modelstruct;

/**
 * \struct multimodelstruct
 * handle a model with t-variate output i.e y_m = {y_1, ..., y_t}^{T}
 *
 * this contains some r <= t modelstructs representing the emulators in the PCA projected
 * space
 */
typedef struct multi_modelstruct{
	int nt; // number of observables in OBSERVED space
	int nr; // number of pca-observables kept in the ROTATED space
	int nparams; // number of dimensions in the PARAMETER SPACE
	int nmodel_points;  // number of points in the design
	int cov_fn_index;
	int regression_order;

	/**
	 * the design (rows:nmodel_points) (cols:nparams)
	 */
	gsl_matrix *xmodel;
	/**
	 * the full training set in the original space
	 * (rows:nmodel_points) (cols:nt)
	 */
	gsl_matrix *training_matrix;
	/**
	 * a t length vector of the the mean values of the cols of training_matrix
	 */
	gsl_vector *training_mean;

	/** array of (r) pca decomposed models
	 *
	 * pointers from this back to xmodel, is this bad?
	 */
	modelstruct** pca_model_array;

	/**
	 * the eigenvalues and vectors from the pca decomp (t xt) , not saved for now...
	 */
	//gsl_vector *pca_eigenvalues;
	//gsl_matrix *pca_eigenvectors;

	gsl_vector *pca_evals_r;	/** just the first r evals */
	gsl_matrix *pca_evecs_r;  /** the first r evecs (t x nr)*/

	/**
	 * an (r x nmodel_points) matrix of the pca-transformed training points, these are
	 * used to init each of the r entries of pca_model_array */
	gsl_matrix *pca_zmatrix; /** z <- (1/sqrt(pca_evals_r)) %*% t(pca_evecs_r) %*% (training_matrix - training_mean) */

} multi_modelstruct;

multi_modelstruct* alloc_multimodelstruct(gsl_matrix *xmodel_in, gsl_matrix *training_matrix_in,
																				 int cov_fn_index, int regression_order, double varfrac);

void gen_pca_decomp(multi_modelstruct *m, double vfrac);
void gen_pca_model_array(multi_modelstruct *m);

void dump_multi_modelstruct(FILE* fptr, multi_modelstruct *m );
multi_modelstruct *load_multi_modelstruct(FILE* fptr);

double vector_elt_sum(gsl_vector* vec, int nstop);

void free_multimodelstruct(multi_modelstruct *m);

#endif


#ifndef _TEST_H_
#define _TEST_H_

#define _GNU_SOURCE

#include <time.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include <omp.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_legendre.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_vegas.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_sf_expint.h>
#include <ctype.h>
#include "cuba.h"

#define PSC             101L
#define ST              102L
#define TK              103L

#define CO10            131L
#define CO21            132L
#define CO32            133L
#define CO43            134L
#define CO54            135L
#define CO65            136L
#define CII             137L

#define NPARS     6

extern struct globals gb;

#include "../Class/include/class.h"
#include "global_structs.h"
#include "cosmology.h"

typedef struct initialize_struct{
      double Mh_min;
      long   mode_mf;
      size_t ninterp;
      int    nlines;
      int    lines[7];
}initialize_struct;

void initialize(char *argv[], struct initialize_struct *init_struct);
void  cleanup();

int Cosmology_init(struct Cosmology *Cx, double pk_kmax, double pk_zmax,
            int nlines, int * line_types, size_t npoints_interp, double M_min, long mode_mf);

double PS_line_HM(struct Cosmology *Cx, double k, double z, double M_min, long mode_mf,  long line_type, int line_id);
double PS_shot_HM(struct Cosmology *Cx, double k, double z, double M_min, double *input, long mode_mf, long line_type, int line_id);
double b22_ls(struct Cosmology *Cx, double z);

double PS_shot(struct Cosmology *Cx, double z, size_t line_id);
double Tbar_line(struct Cosmology *Cx, size_t line_id, double z);
void   line_bias(struct Line *Lx, double z, double *result);

double rhoc(struct Cosmology *Cx, double z);

#endif