#pragma once

/*** mathematics.h:

     Mathematical and statistical functions. ***/

/** Avoid including this file twice. **/

#ifndef MATHEMATICS_H
#define MATHEMATICS_H

/** Libraries. **/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <complex.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/** Constants. **/

#define MATH_PI 3.1415926535897932385 /* Pi. */
#define MATH_2PI 6.2831853071795864770 /* 2 pi. */
#define MATH_PI_2 1.57079632679489661925 /* Pi / 2. */

/* Coordinates / axes. */

#define MATH_X 0 /* Castesian. */
#define MATH_Y 1
#define MATH_Z 2
#define MATH_RADIUS 0 /* Spherical. */
#define MATH_POLAR 1
#define MATH_AZIMUTH 2
#define MATH_MU 1 /* Pseudospherical (mu = cos(polar) replacing polar). */

/** Variables. **/

/* Random numbers. */

gsl_rng *RandomNumberGenerator; /* Random number generator. */
unsigned RandomSeed; /* Random seed. */
unsigned SortComponent = 0; /* Component used for sorting vectors. */

/** Function prototypes. **/

int SortAscending(const void *, const void *);
int SortIndexesAscending(const void *, const void *);
double ScalarProduct(double *, double *, unsigned);
double Norm(double *, unsigned);
double NormSquared(double *, unsigned);
double AngleCosine(double *, double *);

void SphericalToCartesian(double *, double *);
void PseudoSphericalToCartesian(double *, double *);
void CartesianToSpherical(double *, double *);
void CartesianToPseudoSpherical(double *, double *);
void Rotate(double *, double *, unsigned, double);
unsigned GetRandomSeed();
int InitialiseRandom(const gsl_rng_type *);
int FinaliseRandom();
double MetropolisHastings(double *, double *, double *, double *, unsigned, unsigned, unsigned, unsigned, double *, double *, double *,
                          void (*)(double *, double *, double *, double *, double *, unsigned), double (*)(double *, double *, double *, double *, double *, unsigned),
                          double (*)(double *));
double ParallelTempering(double *, double *, double *, double *, unsigned, unsigned, unsigned, unsigned, double *, double *, double *,
                         void (*)(double *, double *, double *, double *, double *, unsigned), double (*)(double *, double *, double *, double *, double *, unsigned),
                         double (*)(double *), unsigned, double *, double);
void TanWeights(double *, double *, double *, double *, unsigned, unsigned, unsigned, double *, double *, double *,
                double (*)(double *, double *, double *, double *, double *, unsigned), unsigned);
double TanNormalisation(double *, unsigned, double *);
double WeightAndSum(double *, double *, unsigned);
void UniformTransition(double *, double *, double *, double *, double *, unsigned);
double UniformTransitionPDF(double *, double *, double *, double *, double *, unsigned);
double PowerLawDeviate(double, double, double);
void IsotropicDirection(double, double *);
void StandardGaussianDeviates(double *);
void GaussianDeviates(double *, double, double);
complex ComplexGaussianDeviates(complex, complex);

/** SortAscending.

    - Function designed to be an argument of qsort().
    - Sorts the elements of a vector in ascending order. The vector elements are reorganised. **/

int SortAscending(const void *x, const void *y)
{
  double difference;

  difference = *((double *)x) - *((double *)y);
  if (difference > 0.0)
    return 1;
  else if (difference < 0.0)
    return -1;
  else
    return 0;
}

/** SortIndexesAscending.

    - Function designed to be an argument of qsort().
    - Sorts the indexes of a vector, in such a way that its elements are arranged in ascending order. The vector remains unchanged.
    - 'SortComponent' indicates the component used to sort, in the case that the vector elements have more than one dimension. **/

int SortIndexesAscending(const void *x, const void *y)
{
  double difference;

  difference = *(*(double **)x + SortComponent) - *(*(double **)y + SortComponent);
  if (difference > 0.0)
    return 1;
  else if (difference < 0.0)
    return -1;
  else
    return 0;
}

/** ScalarProduct.

    - Computes the scalar product between two vectors. **/

double ScalarProduct(double *vector_1, double *vector_2, unsigned vector_size)
{
  unsigned i;
  double product;

  /* Compute components product and sum. */

  product = 0.0;
  for (i = 0; i < vector_size; i++)
    product += vector_1[i] * vector_2[i];
  return product;
}

/** Norm.

    - Computes the norm of a vector. **/

double Norm(double *vector, unsigned vector_size)
{
  unsigned i;
  double *component_sq, max_component_sq, norm;

  /* Generate an auxiliary vector of component squares, in ascending order. */

  component_sq = malloc(vector_size * sizeof(double));
  for (i = 0; i < vector_size; i++)
    component_sq[i] = vector[i] * vector[i];
  qsort(component_sq, vector_size, sizeof(double), SortAscending);

  /* Divide component squares by the largest one to avoid overflow. */

  max_component_sq = component_sq[vector_size-1];
  if (max_component_sq <= 0.0) /* All components are null. */
    return 0.0;
  vector_size--;
  for (i = 0; i < vector_size; i++)
    component_sq[i] /= max_component_sq;

  /* Sum component squares in ascending order, to avoid loss of significance. */

  norm = 0.0;
  for (i = 0; i < vector_size; i++)
    norm += component_sq[i];
  norm += 1.0;

  /* Renormalise. */

  norm = sqrt(max_component_sq * norm);
  free(component_sq);
  return norm;
}

/** NormSquared.

    - Computes the square of the norm of a vector. **/

double NormSquared(double *vector, unsigned vector_size)
{
  unsigned i;
  double *component_sq, max_component_sq, norm;

  /* Generate an auxiliary vector of component squares, in ascending order. */

  component_sq = malloc(vector_size * sizeof(double));
  for (i = 0; i < vector_size; i++)
    component_sq[i] = vector[i] * vector[i];
  qsort(component_sq, vector_size, sizeof(double), SortAscending);

  /* Divide component squares by the largest one to avoid overflow. */

  max_component_sq = component_sq[vector_size-1];
  if (max_component_sq <= 0.0) /* All components are null. */
    return 0.0;
  vector_size--;
  for (i = 0; i < vector_size; i++)
    component_sq[i] /= max_component_sq;

  /* Sum component squares in ascending order, to avoid loss of significance. */

  norm = 0.0;
  for (i = 0; i < vector_size; i++)
    norm += component_sq[i];
  norm += 1.0;

  /* Renormalise. */

  norm *= max_component_sq;
  free(component_sq);
  return norm;
}

/** AngleCosine.

    - Computes the cosine of the angle between two 3D vectors. **/

double AngleCosine(double *vector_1, double *vector_2)
{
  double norm_1, norm_2, cosine;

  norm_1 = Norm(vector_1, 3);
  norm_2 = Norm(vector_2, 3);
  if (norm_1 <= 0.0 || norm_2 <= 0.0) /* At least one vector is null. */
    return 0.0;
  cosine = ScalarProduct(vector_1, vector_2, 3) / (norm_1 * norm_2);
  if (cosine > 1.0) /* Correct eventual round-off errors. */
    cosine = 1.0;
  else if (cosine < -1.0)
    cosine = -1.0;
  return cosine;
}

/** SphericalToCartesian.

    - Converts a 3D vector from spherical (radius, polar angle, azimuth) to Cartesian form.
    - Initial and final vectors may be the same. **/

void SphericalToCartesian(double *vec_in, double *vec_out)
{
  double sin_polar, cos_polar, radius, azimuth;

  radius = vec_in[MATH_RADIUS];
  sin_polar = sin(vec_in[MATH_POLAR]);
  cos_polar = cos(vec_in[MATH_POLAR]);
  azimuth = vec_in[MATH_AZIMUTH];
  vec_out[MATH_X] = radius * sin_polar * cos(azimuth);
  vec_out[MATH_Y] = radius * sin_polar * sin(azimuth);
  vec_out[MATH_Z] = radius * cos_polar;
}

/** PseudoSphericalToCartesian.

    - Converts a 3D vector from pseudo spherical (radius, polar angle cosine, azimuth) to Cartesian form.
    - Initial and final vectors may be the same. **/

void PseudoSphericalToCartesian(double *vec_in, double *vec_out)
{
  double cos_polar, sin_polar, radius, azimuth;

  radius = vec_in[MATH_RADIUS];
  azimuth = vec_in[MATH_AZIMUTH];
  cos_polar = vec_in[MATH_MU];
  sin_polar = 1.0 - cos_polar * cos_polar;
  if (sin_polar <= 0.0) /* Correct eventual round-off errors. */
    sin_polar = 0.0;
  else
    sin_polar = sqrt(sin_polar);
  vec_out[MATH_X] = radius * sin_polar * cos(azimuth);
  vec_out[MATH_Y] = radius * sin_polar * sin(azimuth);
  vec_out[MATH_Z] = radius * cos_polar;
}

/** CartesianToSpherical.

    - Converts a 3D vector from Cartesian to spherical (radius, polar angle, azimuth) form.
    - Initial and final vectors may be the same. **/

void CartesianToSpherical(double *vec_in, double *vec_out)
{
  double radius, azimuth, polar;

  radius = Norm(vec_in, 3);
  if (radius > 0.0)
    polar = acos(vec_in[MATH_Z] / radius);
  else
    polar = 0.0;
  azimuth = atan2(vec_in[MATH_Y], vec_in[MATH_X]);
  vec_out[MATH_RADIUS] = radius;
  vec_out[MATH_POLAR] = polar;
  vec_out[MATH_AZIMUTH] = azimuth;
}

/** CartesianToPseudoSpherical.

    - Converts a 3D vector from Cartesian to pseudo spherical (radius, polar angle cosine, azimuth) form.
    - Initial and final vectors may be the same. **/

void CartesianToPseudoSpherical(double *vec_in, double *vec_out)
{
  double radius, cos_polar, azimuth;

  radius = Norm(vec_in, 3);
  if (radius > 0.0)
    cos_polar = vec_in[MATH_Z] / radius;
  else
    cos_polar = 1.0;
  azimuth = atan2(vec_in[MATH_Y], vec_in[MATH_X]);
  vec_out[MATH_RADIUS] = radius;
  vec_out[MATH_MU] = cos_polar;
  vec_out[MATH_AZIMUTH] = azimuth;
}

/** Rotate.

    - Rotates a vector.
    - Vector is rotated counterclockwise (or axes clockwise) as seen from the tip of the rotation axis specified by argument 'axis'.
    - Initial and final vectors may be the same. **/

void Rotate(double *vec_in, double *vec_out, unsigned axis, double angle)
{
  unsigned i, j;
  double cos_angle, sin_angle, x, y;

  cos_angle = cos(angle);
  sin_angle = sin(angle);
  axis %= 3;
  vec_out[axis] = vec_in[axis];
  i = (axis + 1) % 3;
  j = (axis + 2) % 3;
  x = vec_in[i];
  y = vec_in[j];
  vec_out[i] = x * cos_angle - y * sin_angle;
  vec_out[j] = x * sin_angle + y * cos_angle;
}

/** GetRandomSeed.

    - Provides a random seed by reading garbage from /dev/urandom. **/

unsigned GetRandomSeed()
{
  FILE *random_file;
  unsigned seed;
  int error;

  seed = 0;
  random_file = fopen("/dev/urandom", "rb");
  if (!random_file)
    return 0;
  do
  {
    error = fread(&seed, 1, sizeof(unsigned int), random_file);
    if (error != sizeof(unsigned int))
      return 0;
  }
  while (!seed);
  fclose(random_file);
  return seed;
}

/** InitialiseRandom.

    - Initialises the random number generator. **/

int InitialiseRandom(const gsl_rng_type *random_generator)
{
  RandomSeed = GetRandomSeed();
  if (!RandomSeed)
    return EXIT_FAILURE;
  RandomNumberGenerator = gsl_rng_alloc(random_generator);
  if (!RandomNumberGenerator)
    return EXIT_FAILURE;
  gsl_rng_set(RandomNumberGenerator, RandomSeed);
  return EXIT_SUCCESS;
}

/** FinaliseRandom.

    - Finalises the random number generator. **/

int FinaliseRandom()
{
  /* Release random number generator memory. */

  if (RandomNumberGenerator)
  {
    gsl_rng_free(RandomNumberGenerator);
    return EXIT_SUCCESS;
  }
  return EXIT_FAILURE;
}

/** MetropolisHastings.

    - Constructs a Markov chain using Metropolis-Hastings sampling from a given PDF ('sampling_pdf').
    - 'states' contains the actual Markov chain, comprising 'chain_size' elements. Each element contains 'state_size' doubles. The initial state must be stored in
      the first element of 'states'. 'states' must be allocated by the calling function.
    - 'proposals' is the chain of proposals derived from each state (same length and element size as 'states'). It must be allocated by the calling function.
    - 'lower_bound' and 'upper_bound' must contain the absolute bounds for the variables defining the state.
    - 'step' must contain the parameter defining the size of the transition step.
    - 'sampling_pdf' must point to a function computing the PDF sampled by the Markov chain. Its only argument is a state of the chain; any other parameter must be
      passed through global variables.
    - 'transition_pdf' must point to a function computing the PDF of a Metropolis-Hastings transition, given a Markov chain state. Its arguments are the final and
      initial states of the transition, in this order. Any other parameter must be passed through global variables.
    - 'transition' must point to a function computing the proposed final state of a Metropolis-Hastings transition, given a Markov chain state. Its arguments are the
      final and initial states of the transition, in this order. Any other parameter must be passed through global variables.
    - 'proposals_PDF' is an array of doubles of the same length of the Markov chain. It must be allocated by the calling function if storage of sampling PDF values
      for the proposals is desired, or set to NULL otherwise.
    - 'states_PDF' is an array of doubles of the same length of the Markov chain. It must be allocated by the calling function if storage of sampling PDF values
      for the states is desired, or set to NULL otherwise.
    - If the initial state set in 'states' has a null value of the sampling PDF, the first transition probability is set to unity. Once a state with a non-null value
      of the sampling PDF is reached, the sampling PDF values do not vanish over the rest of the Markov chain by construction.
    - The order of the state and proposal chains is the following: any element of 'proposals' is sampled from the corresponding element of 'states', whereas any
      element of 'states' (except the first one) is the previous element of either 'states' or 'proposals'.
    - If 'save_states' is set to null, only the final state and proposal are given. Otherwise, the full chains are saved.
    - If 'log_flag' is non null, 'sampling_pdf' is assumed to give the natural logarithm of the PDF.
    - Returns the acceptance ratio of the chain. **/

double MetropolisHastings(double *states, double *proposals, double *proposals_pdf, double *states_pdf, unsigned state_size, unsigned chain_size, unsigned save_states,
                          unsigned log_flag, double *lower_bound, double *upper_bound, double *step,
			  void (*transition)(double *, double *, double *, double *, double *, unsigned),
                          double (*transition_pdf)(double *, double *, double *, double *, double *, unsigned), double (*sampling_pdf)(double *))
{
  unsigned i, accepted_transitions, state_size_bytes;
  double direct_transition_pdf, inverse_transition_pdf, pdf_ratio, state_pdf, proposal_pdf, *proposal, *state, *old_state, acceptance_ratio;

  /* Set initial stuff. */

  state_size_bytes = state_size * sizeof(double);
  proposal = proposals;
  state = states;
  state_pdf = sampling_pdf(state);
  if (states_pdf)
    states_pdf[0] = state_pdf;

  /* Construct the Markov chain. */

  accepted_transitions = 0;
  chain_size--;
  for (i = 0; i < chain_size; i++)
  {
    /* Set new proposal. */

    transition(proposal, state, lower_bound, upper_bound, step, state_size);
    proposal_pdf = sampling_pdf(proposal);
    if (proposals_pdf && save_states)
      proposals_pdf[i] = proposal_pdf;

    /* Compute transition probabilities. */

    direct_transition_pdf = transition_pdf(proposal, state, lower_bound, upper_bound, step, state_size);
    inverse_transition_pdf = transition_pdf(state, proposal, lower_bound, upper_bound, step, state_size);
    if (!log_flag && state_pdf <= 0.0)
      pdf_ratio = 1.1;
    else if (direct_transition_pdf <= 0.0)
      pdf_ratio = -0.1;
    else if (log_flag)
      pdf_ratio = exp(proposal_pdf - state_pdf) * inverse_transition_pdf / direct_transition_pdf;
    else
      pdf_ratio = proposal_pdf * inverse_transition_pdf / (state_pdf * direct_transition_pdf);

    /* Accept of reject transition. */

    if (save_states)
    {
      old_state = state;
      state += state_size;
    }
    if (gsl_rng_uniform(RandomNumberGenerator) < pdf_ratio)
    {
      memcpy(state, proposal, state_size_bytes);
      state_pdf = proposal_pdf;
      accepted_transitions++;
    }
    else if (save_states)
      memcpy(state, old_state, state_size_bytes);
    if (save_states)
    {
      proposal += state_size;
      if (states_pdf)
        states_pdf[i+1] = state_pdf;
    }
  }

  /* Compute final stuff. */

  transition(proposal, state, lower_bound, upper_bound, step, state_size);
  acceptance_ratio = accepted_transitions / (double) chain_size;
  if (proposals_pdf && save_states)
    proposals_pdf[chain_size] = sampling_pdf(proposal);
  return acceptance_ratio;
}

/** ParallelTempering.

    - Constructs a Markov chain using Metropolis-Hastings sampling from a given PDF ('sampling_pdf'), with parallel tempering.
    - 'states' contains the cold Markov chain, comprising 'chain_size' elements. Each element contains 'state_size' doubles. The initial state must be stored in
      the first element of 'states'. 'states' must be allocated by the calling function.
    - 'proposals' is the chain of proposals derived from each state (same length and element size as 'states'). It must be allocated by the calling function.
    - 'lower_bound' and 'upper_bound' must contain the absolute bounds for the variables defining the state.
    - 'step' must contain the parameter defining the size of the transition step.
    - 'sampling_pdf' must point to a function computing the PDF sampled by the cold Markov chain. Its only argument is a state of the chain; any other parameter must
      be passed through global variables.
    - 'transition_pdf' must point to a function computing the PDF of a Metropolis-Hastings transition, given a Markov chain state. Its arguments are the final and
      initial states of the transition, in this order. Any other parameter must be passed through global variables.
    - 'transition' must point to a function computing the proposed final state of a Metropolis-Hastings transition, given a Markov chain state. Its arguments are the
      final and initial states of the transition, in this order. Any other parameter must be passed through global variables.
    - 'proposals_PDF' is an array of doubles of the same length of the Markov chain. It must be allocated by the calling function if storage of sampling PDF values
      for the proposals is desired, or set to NULL otherwise.
    - 'states_PDF' is an array of doubles of the same length of the Markov chain. It must be allocated by the calling function if storage of sampling PDF values
      for the states is desired, or set to NULL otherwise.
    - If the initial state set in 'states' has a null value of the sampling PDF, the first transition probability is set to unity. Once a state with a non-null value
      of the sampling PDF is reached, the sampling PDF values do not vanish over the rest of the Markov chain by construction.
    - The order of the state and proposal chains is the following: any element of 'proposals' is sampled from the corresponding element of 'states', whereas any
      element of 'states' (except the first one) is the previous element of either 'states' or 'proposals'.
    - If 'save_states' is set to null, only the final state and proposal are given. Otherwise, the full chains are saved.
    - If 'log_flag' is non null, 'sampling_pdf' is assumed to give the natural logarithm of the PDF.
    - The number of hot chains in the parallel-tempering scheme is 'hot_chains_count', which have tempering factors given by 'tempering_factors'.
    - 'exchange_probability' gives the probability that two chains are interchanged in a given step.
    - Returns the acceptance ratio of the chain. **/

double ParallelTempering(double *states, double *proposals, double *proposals_pdf, double *states_pdf, unsigned state_size, unsigned chain_size, unsigned save_states,
                         unsigned log_flag, double *lower_bound, double *upper_bound, double *step,
                         void (*transition)(double *, double *, double *, double *, double *, unsigned),
                         double (*transition_pdf)(double *, double *, double *, double *, double *, unsigned), double (*sampling_pdf)(double *),
                         unsigned hot_chains_count, double *tempering_factors, double exchange_probability)
{
  unsigned i, j, accepted_transitions, state_size_bytes;
  double direct_transition_pdf, inverse_transition_pdf, pdf_ratio, state_pdf, proposal_pdf, *proposal, *state, *old_state, acceptance_ratio, x, *aux_state;
  double *cool_state, hot_proposals_pdf, factor, *hot_proposals, *hot_states, *hot_states_pdf, *cool_state_pdf;

  /* Set initial stuff. */

  state_size_bytes = state_size * sizeof(double);
  proposal = proposals;
  state = states;
  state_pdf = sampling_pdf(state);
  if (states_pdf)
    states_pdf[0] = state_pdf;
  hot_proposals = malloc(hot_chains_count * state_size_bytes);
  hot_states = malloc(hot_chains_count * state_size_bytes);
  aux_state = malloc(state_size_bytes);
  for (i = 0; i < hot_chains_count; i++)
    memcpy(hot_states + i * state_size, state, state_size_bytes); /* Copy initial state of cold chain to all hot chains. */
  hot_states_pdf = malloc(hot_chains_count * sizeof(double));
  for (i = 0; i < hot_chains_count; i++)
  {
    if (log_flag)
      hot_states_pdf[i] = state_pdf * tempering_factors[i];
    else
      hot_states_pdf[i] = pow(state_pdf, tempering_factors[i]);
  }

  /* Construct the Markov chains. */

  accepted_transitions = 0;
  chain_size--;
  for (i = 0; i < chain_size; i++)
  {
    /* Set new proposal for cold chain. */

    transition(proposal, state, lower_bound, upper_bound, step, state_size);
    proposal_pdf = sampling_pdf(proposal);
    if (proposals_pdf && save_states)
      proposals_pdf[i] = proposal_pdf;

    /* Compute transition probabilities for cold chain. */

    direct_transition_pdf = transition_pdf(proposal, state, lower_bound, upper_bound, step, state_size);
    inverse_transition_pdf = transition_pdf(state, proposal, lower_bound, upper_bound, step, state_size);
    if (!log_flag && state_pdf <= 0.0)
      pdf_ratio = 1.1;
    else if (direct_transition_pdf <= 0.0)
      pdf_ratio = -0.1;
    else if (log_flag)
      pdf_ratio = exp(proposal_pdf - state_pdf) * inverse_transition_pdf / direct_transition_pdf;
    else
      pdf_ratio = proposal_pdf * inverse_transition_pdf / (state_pdf * direct_transition_pdf);

    /* Accept of reject transition for cold chain. */

    if (save_states)
    {
      old_state = state;
      state += state_size;
    }
    if (gsl_rng_uniform(RandomNumberGenerator) < pdf_ratio)
    {
      memcpy(state, proposal, state_size_bytes);
      state_pdf = proposal_pdf;
      accepted_transitions++;
    }
    else if (save_states)
      memcpy(state, old_state, state_size_bytes);
    if (save_states)
    {
      proposal += state_size;
      if (states_pdf)
        states_pdf[i+1] = state_pdf;
    }

    /* Hot chains. */

    for (j = 0; j < hot_chains_count; j++)
    {
      /* Set new proposal. */

      transition(hot_proposals + j * state_size, hot_states + j * state_size, lower_bound, upper_bound, step, state_size);
      hot_proposals_pdf = sampling_pdf(hot_proposals + j * state_size);
      if (log_flag)
        hot_proposals_pdf *= tempering_factors[j];
      else
        hot_proposals_pdf = pow(hot_proposals_pdf, tempering_factors[j]);

      /* Compute transition probabilities. */

      direct_transition_pdf = transition_pdf(hot_proposals + j * state_size, hot_states + j * state_size, lower_bound, upper_bound, step, state_size);
      inverse_transition_pdf = transition_pdf(hot_states + j * state_size, hot_proposals + j * state_size, lower_bound, upper_bound, step, state_size);
      if (!log_flag && hot_states_pdf[j] <= 0.0)
        pdf_ratio = 1.1;
      else if (direct_transition_pdf <= 0.0)
        pdf_ratio = -0.1;
      else if (log_flag)
        pdf_ratio = exp(hot_proposals_pdf - hot_states_pdf[j]) * inverse_transition_pdf / direct_transition_pdf;
      else
        pdf_ratio = hot_proposals_pdf * inverse_transition_pdf / (hot_states_pdf[j] * direct_transition_pdf);

      /* Accept of reject transition. */

      if (gsl_rng_uniform(RandomNumberGenerator) < pdf_ratio)
      {
        memcpy(hot_states + j * state_size, hot_proposals + j * state_size, state_size_bytes);
        hot_states_pdf[j] = hot_proposals_pdf;
      }
    }

    /* Exchange chain states. */

    if (gsl_rng_uniform(RandomNumberGenerator) < exchange_probability)
    {
      j = gsl_rng_uniform_int(RandomNumberGenerator, hot_chains_count);
      factor = tempering_factors[j];
      if (j)
      {
        factor /= tempering_factors[j-1];
        cool_state_pdf = hot_states_pdf + j - 1;
        cool_state = hot_states + (j - 1) * state_size;
      }
      else
      {
        cool_state_pdf = &state_pdf;
        cool_state = state;
      }
      if (log_flag)
        pdf_ratio = exp(*cool_state_pdf * (factor - 1.0) + hot_states_pdf[j] * (1.0 / factor - 1.0));
      else
        pdf_ratio = pow(*cool_state_pdf, factor - 1.0) * pow(hot_states_pdf[j], 1.0 / factor - 1.0);
      if (gsl_rng_uniform(RandomNumberGenerator) < pdf_ratio)
      {
        if (log_flag)
        {
          x = hot_states_pdf[j];
          hot_states_pdf[j] = *cool_state_pdf * factor;
          *cool_state_pdf = x / factor;
        }
        else
        {
          x = hot_states_pdf[j];
          hot_states_pdf[j] = pow(*cool_state_pdf, factor);
          *cool_state_pdf = pow(x, 1.0 / factor);
        }
        memcpy(aux_state, hot_states + j * state_size, state_size_bytes);
        memcpy(hot_states + j * state_size, cool_state, state_size_bytes);
        memcpy(cool_state, aux_state, state_size_bytes);
      }
    }
  }
  transition(proposal, state, lower_bound, upper_bound, step, state_size);
  acceptance_ratio = accepted_transitions / (double) chain_size;
  if (proposals_pdf && save_states)
    proposals_pdf[chain_size] = sampling_pdf(proposal);
  free(hot_proposals);
  free(hot_states);
  free(hot_states_pdf);
  free(aux_state);
  return acceptance_ratio;
}

/** TanWeights.

    - Computes the weights proposed by Tan (2006, J.Comp.Graph.Stat., 15, 735) to use Metropolis-Hastings sampling for the evaluation of integrals via the Monte
      Carlo method.
    - 'states' contains the actual Markov chain, comprising 'chain_size' elements. Each element contains 'state_size' doubles.
    - 'proposals' contains the chain of proposals derived from each state (same length and element size as 'states').
    - 'proposals_pdf' is an array of doubles of the same length of the Markov chain, containing the sampling PDF values for the proposals.
    - If the present chain is an extension of an old one, 'old_chain_size' is the size of the latter.
    - 'weights' is an array of 'chain_size' elements for storing the resulting weights. It must be allocated by the calling function. Elements of 'weights' between
      'old_chain_size' and 'chain_size' are erased before computing.
    - 'lower_bound' and 'upper_bound' must contain the absolute bounds for the variables defining the state.
    - 'step' must contain the parameter defining the size of the transition step.
    - 'sort_component' indicates the component of the state used for sorting them, to reduce the computational cost if the PDF is bound.
    - 'transition_pdf' must point to a function computing the PDF of a Metropolis-Hastings transition, given a Markov chain state. Its arguments are the final and
      initial states of the transition, in this order. Any other parameter must be passed through global variables. **/

void TanWeights(double *states, double *proposals, double *proposals_pdf, double *weights, unsigned state_size, unsigned chain_size, unsigned old_chain_size,
                double *lower_bound, double *upper_bound, double *step, double (*transition_pdf)(double *, double *, double *, double *, double *, unsigned),
                unsigned sort_component)
{
  unsigned i, j, k, m, n;
  double *proposal, **pointers, pdf;

  if (sort_component >= state_size)
    SortComponent = 0;
  else
    SortComponent = sort_component;
  memset(weights + old_chain_size, 0, (chain_size - old_chain_size) * sizeof(double));
  pointers = malloc(chain_size * sizeof(double *));
  for (i = 0; i < chain_size; i++)
    pointers[i] = states + i * state_size;
  qsort(pointers, chain_size, sizeof(double *), SortIndexesAscending);
  for (i = 0; i < chain_size; i++)
  {
    k = pointers[i] - states;
    m = k / state_size;
    proposal = proposals + k;
    for (j = i; j < chain_size; j++)
    {
      n = (pointers[j] - states) / state_size;
      if (m < old_chain_size && n < old_chain_size)
        continue; /* Avoid re-computation of weights already computed. */
      pdf = transition_pdf(proposal, pointers[j], lower_bound, upper_bound, step, state_size);
      if (pdf <= 0.0)
        break;
      weights[m] += pdf;
    }
    for (j = 1; j <= i; j++)
    {
      n = (pointers[i-j] - states) / state_size;
      if (m < old_chain_size && n < old_chain_size)
        continue;
      pdf = transition_pdf(proposal, pointers[i-j], lower_bound, upper_bound, step, state_size);
      if (pdf <= 0.0)
        break;
      weights[m] += pdf;
    }
    if (weights[m] > 0.0)
      weights[m] = proposals_pdf[m] / weights[m];
  }
  free(pointers);
}

/** TanNormalisation.

    - Computes the PDF normalisation proposed by Tan (2006, J.Comp.Graph.Stat., 15, 735), together with an estimate of its undertainty ('error').
    - 'weights' is the array of weights (of size 'element_count') computed by 'TanWeights'. **/

double TanNormalisation(double *weights, unsigned element_count, double *error)
{
  unsigned i;
  double norm, **pointers, x;

  /* Perform weighted sum. */

  pointers = malloc(element_count * sizeof(double *));
  for (i = 0; i < element_count; i++)
    pointers[i] = weights + i;
  qsort(pointers, element_count, sizeof(double *), SortIndexesAscending);
  norm = 0.0;
  *error = 0.0;
  for (i = 0; i < element_count; i++)
  {
    x = pointers[i][0];
    norm += x;
    *error += x * x;
  }
  *error = sqrt(*error - norm * norm / element_count);
  free(pointers);
  return norm;
}

/** TanMean.

    - Computes the mean of a function as proposed by Tan (2006, J.Comp.Graph.Stat., 15, 735), together with an estimate of its undertainty ('error').
    - 'weights' is the array of weights (of size 'element_count') computed by 'TanWeights'.
    - 'norm' is the PDF normalisation computed by 'TanNormalisation'.
    - 'function' is the array of function values (of size 'element_count') at the proposal states. **/

double TanMean(double *weights, double *function, double norm, unsigned element_count, double *error)
{
  unsigned i;
  double mean, x;

  /* Perform weighted sum. */

  mean = 0.0;
  *error = 0.0;
  for (i = 0; i < element_count; i++)
    mean += weights[i] * function[i];
  mean /= norm;
  for (i = 0; i < element_count; i++)
  {
    x = (function[i] - mean) * weights[i];
    *error += x * x;
  }
  *error = sqrt(*error) / norm;
  return mean;
}

/** UniformTransition.

    - Samples an n-dimensional transition in a Markov chain, from a uniform PDF.
    - 'state' contains the present state of the Markov chain, comprising 'state_size' doubles.
    - 'proposal' will store the resulting proposed state (same size as 'state').
    - 'lower_bound' and 'upper_bound' must contain the absolute bounds for the variables defining the state.
    - 'step' must contain the parameter defining the size of the transition step. **/

void UniformTransition(double *proposal, double *state, double *lower_bound, double *upper_bound, double *step, unsigned state_size)
{
  unsigned i;
  double min, max;

  for (i = 0; i < state_size; i++)
  {
    if (step[i] <= 0.0)
      proposal[i] = state[i];
    else
    {
      min = state[i] - 0.5 * step[i];
      max = min + step[i];
      if (min < lower_bound[i])
        min = lower_bound[i];
      if (max > upper_bound[i])
        max = upper_bound[i];
      proposal[i] = min + (max - min) * gsl_rng_uniform(RandomNumberGenerator);
    }
  }
}

/** UniformTransitionPDF.

    - Computes the n-dimensional PDF value of a Markov chain transition, using a uniform PDF.
    - 'state' contains the state of the Markov chain from which the transition occurs, comprising 'state_size' doubles.
    - 'proposal' contains the proposed state of the chain after the transition (same size as 'state').
    - 'lower_bound' and 'upper_bound' must contain the absolute bounds for the variables defining the state.
    - 'step' must contain the parameter defining the size of the transition step. **/

double UniformTransitionPDF(double *proposal, double *state, double *lower_bound, double *upper_bound, double *step, unsigned state_size)
{
  unsigned i;
  double min, max, density;

  density = 1.0;
  for (i = 0; i < state_size; i++)
  {
    if (step[i] <= 0.0)
      continue;
    min = state[i] - 0.5 * step[i];
    max = min + step[i];
    if (min < lower_bound[i])
      min = lower_bound[i];
    if (max > upper_bound[i])
      max = upper_bound[i];
    if (proposal[i] < min || proposal[i] > max)
      return 0.0;
    density /= max - min;
  }
  return density;
}

/** PowerLawDeviate.

    - Generates a power-law deviate with an index 'index' in any closed interval ['min', 'max']. **/

double PowerLawDeviate(double min, double max, double index)
{
  double deviate, std_deviate;

  std_deviate = gsl_rng_uniform(RandomNumberGenerator);
  if (index == -1.0)
    deviate = min * exp(std_deviate * log(max / min));
  else if (index == 0.0)
    deviate = min + (max - min) * std_deviate;
  else
  {
    index += 1.0;
    min = pow(min, index);
    max = pow(max, index);
    deviate = pow(min + (max - min) * std_deviate, 1.0 / index);
  }
  return deviate;
}

/** IsotropicDirection.

    - Generates a random direction isotropically distributed within a cone of a given semi-aperture 'semi_aperture'.
    - The output direction has unit norm, by construction.
    - Relies on the fact that, for an isotropic distribution of directions in three dimensions, the projection onto any axis is distributed uniformly in [-1,1]. **/

void IsotropicDirection(double semi_aperture, double *direction)
{
  double cos_polar, sin_polar, azimuth, cos_polar_max;

  /* Sample polar angle cosine with uniform PDF in [cos_polar_max, 1]. */

  cos_polar_max = cos(semi_aperture);
  cos_polar = cos_polar_max + gsl_rng_uniform(RandomNumberGenerator) * (1.0 - cos_polar_max);
  sin_polar = 1.0 - cos_polar * cos_polar;
  if (sin_polar > 0.0) /* Correct eventual round-off errors. */
    sin_polar = sqrt(sin_polar);
  else
    sin_polar = 0.0;

  /* Sample azimuth with uniform PDF in [0, 2 pi). */

  azimuth = MATH_2PI * gsl_rng_uniform(RandomNumberGenerator);

  /* Compute direction. */

  direction[MATH_X] = sin_polar * cos(azimuth);
  direction[MATH_Y] = sin_polar * sin(azimuth);
  direction[MATH_Z] = cos_polar;
}

/** StandardGaussianDeviates.

    - Generates a couple of standard gaussian deviates.
    - Deviates are stored in 'deviates', which must be allocated by the calling program. **/

void StandardGaussianDeviates(double *deviates)
{
  double x, y, r;

  y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
  do
  {
    x = y;
    y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
    r = x * x + y * y;
  }
  while (r == 0.0 || r >= 1.0);
  r = sqrt(-2.0 * log(r) / r);
  deviates[0] = x * r;
  deviates[1] = y * r;
}

/** GaussianDeviates.

    - Generates a couple of gaussian deviates with mean 'mean' and standard deviation 'std_dev'.
    - Deviates are stored in 'deviates', which must be allocated by the calling program. **/

void GaussianDeviates(double *deviates, double mean, double std_dev)
{
  double x, y, r;

  y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
  do
  {
    x = y;
    y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
    r = x * x + y * y;
  }
  while (r == 0.0 || r >= 1.0);
  r = sqrt(-2.0 * log(r) / r) * std_dev;
  deviates[0] = x * r + mean;
  deviates[1] = y * r + mean;
}

/** ComplexGaussianDeviates.

    - Generates a gaussian deviate with mean 'mean' and standard deviation 'std_dev' in the complex plane. **/

complex ComplexGaussianDeviates(complex mean, complex std_dev)
{
  double x, y, r;
  complex deviate;

  y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
  do
  {
    x = y;
    y = 2.0 * gsl_rng_uniform(RandomNumberGenerator) - 1.0;
    r = x * x + y * y;
  }
  while (r == 0.0 || r >= 1.0);
  r = sqrt(-2.0 * log(r) / r);
  deviate = mean + r * (x * creal(std_dev) + I * y * cimag(std_dev));
  return deviate;
}

#endif

/*** End of 'mathematics.h'. ***/


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


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


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


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


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