/* interpolation/steffen.c

 * 

 * Copyright (C) 2014 Jean-François Caron

 * 

 * This program is free software; you can redistribute it and/or modify

 * it under the terms of the GNU General Public License as published by

 * the Free Software Foundation; either version 3 of the License, or (at

 * your option) any later version.

 * 

 * This program is distributed in the hope that it will be useful, but

 * WITHOUT ANY WARRANTY; without even the implied warranty of

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

 * General Public License for more details.

 * 

 * You should have received a copy of the GNU General Public License

 * along with this program; if not, write to the Free Software

 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

 */

/* Author:  J.-F. Caron

 *

 * This interpolation method is taken from 

 * M.Steffen, "A simple method for monotonic interpolation in one dimension",

 * Astron. Astrophys. 239, 443-450 (1990).

 *

 * This interpolation method guarantees monotonic interpolation functions between

 * the given data points.  A consequence of this is that extremal values can only

 * occur at the data points.  The interpolating function and its first derivative

 * are guaranteed to be continuous, but the second derivative is not.

 *

 * The implementation is modelled on the existing Akima interpolation method

 * previously included in GSL by Gerard Jungman.

 */

#include <config.h>

#include <stdlib.h>

#include <math.h>

#include <gsl/gsl_math.h>

#include <gsl/gsl_errno.h>

#include "integ_eval.h"

#include <gsl/gsl_interp.h>

typedef struct

{

  double * a;       /* eqs 2-5 of paper */

  double * b;

  double * c;

  double * d;

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

} steffen_state_t;

static void steffen_free (void * vstate);

static double steffen_copysign(const double x, const double y);

static void *

steffen_alloc (size_t size)

{

  steffen_state_t *state;

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

  if (state == NULL)

    {

      GSL_ERROR_NULL("failed to allocate space for state", GSL_ENOMEM);

    }

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

  if (state->a == NULL)

    {

      steffen_free(state);

      GSL_ERROR_NULL("failed to allocate space for a", GSL_ENOMEM);

    }

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

  if (state->b == NULL)

    {

      steffen_free(state);

      GSL_ERROR_NULL("failed to allocate space for b", GSL_ENOMEM);

    }

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

  if (state->c == NULL)

    {

      steffen_free(state);

      GSL_ERROR_NULL("failed to allocate space for c", GSL_ENOMEM);

    }

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

  if (state->d == NULL)

    {

      steffen_free(state);

      GSL_ERROR_NULL("failed to allocate space for d", GSL_ENOMEM);

    }

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

  if (state->y_prime == NULL)

    {

      steffen_free(state);

      GSL_ERROR_NULL("failed to allocate space for y_prime", GSL_ENOMEM);

    }

  return state;

}

static int

steffen_init (void * vstate, const double x_array[],

              const double y_array[], size_t size)

{

  steffen_state_t *state = (steffen_state_t *) vstate;

  size_t i;

  double *a = state->a;

  double *b = state->b;

  double *c = state->c;

  double *d = state->d;

  double *y_prime = state->y_prime;

  /*

   * first assign the interval and slopes for the left boundary.

   * We use the "simplest possibility" method described in the paper

   * in section 2.2

   */

  double h0 = (x_array[1] - x_array[0]);

  double s0 = (y_array[1] - y_array[0]) / h0;

  y_prime[0] = s0;

  /* Now we calculate all the necessary s, h, p, and y' variables 

     from 1 to N-2 (0 to size - 2 inclusive) */

  for (i = 1; i < (size - 1); i++)

    {

      double pi;

      /* equation 6 in the paper */

      double hi = (x_array[i+1] - x_array[i]);

      double him1 = (x_array[i] - x_array[i - 1]);

      /* equation 7 in the paper */

      double si = (y_array[i+1] - y_array[i]) / hi;

      double sim1 = (y_array[i] - y_array[i - 1]) / him1;

      /* equation 8 in the paper */

      pi = (sim1*hi + si*him1) / (him1 + hi);

      /* This is a C equivalent of the FORTRAN statement below eqn 11 */

      y_prime[i] = (steffen_copysign(1.0,sim1) + steffen_copysign(1.0,si)) *

                    GSL_MIN(fabs(sim1),

                            GSL_MIN(fabs(si), 0.5*fabs(pi))); 

    }

  /*

   * we also need y' for the rightmost boundary; we use the

   * "simplest possibility" method described in the paper in

   * section 2.2

   *

   * y' = s_{n-1}

   */

  y_prime[size-1] = (y_array[size - 1] - y_array[size - 2]) /

                    (x_array[size - 1] - x_array[size - 2]);

  /* Now we can calculate all the coefficients for the whole range. */

  for (i = 0; i < (size - 1); i++)

    {

      double hi = (x_array[i+1] - x_array[i]);

      double si = (y_array[i+1] - y_array[i]) / hi;

      /* These are from equations 2-5 in the paper. */

      a[i] = (y_prime[i] + y_prime[i+1] - 2*si) / hi / hi;

      b[i] = (3*si - 2*y_prime[i] - y_prime[i+1]) / hi;

      c[i] = y_prime[i];

      d[i] = y_array[i];

    }

  return GSL_SUCCESS;

}

static void

steffen_free (void * vstate)

{

  steffen_state_t *state = (steffen_state_t *) vstate;

  RETURN_IF_NULL(state);

  if (state->a)

    free (state->a);

  if (state->b)

    free (state->b);

  if (state->c)

    free (state->c);

  if (state->d)

    free (state->d);

  if (state->y_prime)

    free (state->y_prime);

  free (state);

}

static int

steffen_eval (const void * vstate,

              const double x_array[], const double y_array[], size_t size,

              double x, gsl_interp_accel * a, double *y)

{

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

  size_t index;

  if (a != 0)

    {

      index = gsl_interp_accel_find (a, x_array, size, x);

    }

  else

    {

      index = gsl_interp_bsearch (x_array, x, 0, size - 1);

    }

  /* evaluate */

  {

    const double x_lo = x_array[index];

    const double delx = x - x_lo;

    const double a = state->a[index];

    const double b = state->b[index];

    const double c = state->c[index];

    const double d = state->d[index];

    /* Use Horner's scheme for efficient evaluation of polynomials. */

    /* *y = a*delx*delx*delx + b*delx*delx + c*delx + d; */

    *y = d + delx*(c + delx*(b + delx*a));

    return GSL_SUCCESS;

  }

}

static int

steffen_eval_deriv (const void * vstate,

                    const double x_array[], const double y_array[], size_t size,

                    double x, gsl_interp_accel * a, double *dydx)

{

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

  size_t index;

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

  if (a != 0)

    {

      index = gsl_interp_accel_find (a, x_array, size, x);

    }

  else

    {

      index = gsl_interp_bsearch (x_array, x, 0, size - 1);

    }

  /* evaluate */

  {

    double x_lo = x_array[index];

    double delx = x - x_lo;

    double a = state->a[index];

    double b = state->b[index];

    double c = state->c[index];

    /*double d = state->d[index];*/

    /* *dydx = 3*a*delx*delx*delx + 2*b*delx + c; */

    *dydx = c + delx*(2*b + delx*3*a);

    return GSL_SUCCESS;

  }

}

static int

steffen_eval_deriv2 (const void * vstate,

                     const double x_array[], const double y_array[], size_t size,

                     double x, gsl_interp_accel * a, double *y_pp)

{

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

  size_t index;

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

  if (a != 0)

    {

      index = gsl_interp_accel_find (a, x_array, size, x);

    }

  else

    {

      index = gsl_interp_bsearch (x_array, x, 0, size - 1);

    }

  /* evaluate */

  {

    const double x_lo = x_array[index];

    const double delx = x - x_lo;

    const double a = state->a[index];

    const double b = state->b[index];

    *y_pp = 6*a*delx + 2*b;

    return GSL_SUCCESS;

  }

}

static int

steffen_eval_integ (const void * vstate,

                    const double x_array[], const double y_array[], size_t size,

                    gsl_interp_accel * acc, double a, double b,

                    double * result)

{

  /* a and b are the boundaries of the integration. */

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

  size_t i, index_a, index_b;

  /* Find the data points in the x_array that are nearest to the desired */

  /* a and b integration boundaries. */

  if (acc != 0)

    {

      index_a = gsl_interp_accel_find (acc, x_array, size, a);

      index_b = gsl_interp_accel_find (acc, x_array, size, b);

    }

  else

    {

      index_a = gsl_interp_bsearch (x_array, a, 0, size - 1);

      index_b = gsl_interp_bsearch (x_array, b, 0, size - 1);

    }

  *result = 0.0;

  /* Iterate over all the segments between data points and sum the */

  /* contributions into result. */

  for(i=index_a; i<=index_b; i++) 

    {

      const double x_hi = x_array[i + 1];

      const double x_lo = x_array[i];

      const double dx = x_hi - x_lo;

      if(dx != 0.0) 

        {

          /*

           * check if we are at a boundary point, so take the

           * a and b parameters instead of the data points.

           */

          double x1 = (i == index_a) ? a-x_lo : 0.0;

          double x2 = (i == index_b) ? b-x_lo : x_hi-x_lo;

          *result += (1.0/4.0)*state->a[i]*(x2*x2*x2*x2 - x1*x1*x1*x1)

                    +(1.0/3.0)*state->b[i]*(x2*x2*x2 - x1*x1*x1)

                    +(1.0/2.0)*state->c[i]*(x2*x2 - x1*x1)

                    +state->d[i]*(x2-x1);

        }

      else /* if the interval was zero, i.e. consecutive x values in data. */

        {

          *result = 0.0;

          return GSL_EINVAL;

        }

    }

  return GSL_SUCCESS;

}

static double

steffen_copysign(const double x, const double y)

{

  if ((x < 0 && y > 0) || (x > 0 && y < 0))

    return -x;

  return x;

}

static const gsl_interp_type steffen_type = 

{

  "steffen", 

  3,

  &steffen_alloc,

  &steffen_init,

  &steffen_eval,

  &steffen_eval_deriv,

  &steffen_eval_deriv2,

  &steffen_eval_integ,

  &steffen_free

};

const gsl_interp_type * gsl_interp_steffen = &steffen_type;