
/*********************************************************************
 * E.S.O. - VLT project
 *
 * "@(#) $Id: kmclipm_priv_splines.c,v 1.19 2013-10-09 12:19:49 aagudo Exp $"
 *
 * Functions that are implemented as recipes. They are also useedd in KMOS
 * data reduction pipeline.
 *
 * who       when        what
 * --------  ----------  ----------------------------------------------
 */

/**
    @internal
    @defgroup kmclipm_priv_splines PRIVATE: Spline functions

    This module provides splines interploation methods
    @ref kmclipm_priv_splines kmclipm_priv_splines

    @par Synopsis:
    @code
      #include <kmclipm_priv_splines.h>
    @endcode
    @{
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#define _ISOC99_SOURCE

/*------------------------------------------------------------------------------
 *                                  Includes
 *----------------------------------------------------------------------------*/

#include <cpl.h>
#include <math.h>
#include <float.h>

#include "kmclipm_compatibility_cpl.h"
#include "kmclipm_constants.h"
#include "kmclipm_math.h"
#include "kmclipm_priv_splines.h"
#include "kmclipm_priv_constants.h"
#include "kmclipm_priv_error.h"

double polynomial_interpolation (double xi[], double yi[], int n, double x, double *dy);
int hunt_for_index(double xx[], unsigned long n, double x, unsigned int *jlo);
void report_error(const char *func, cpl_error_code code, const char *msg);

/*------------------------------------------------------------------------------
 *                                  Definitions
 *----------------------------------------------------------------------------*/

/**
    @brief  Bicubic splines interpolation with a regular array as input and output

    @param      nxi     number of points along the input x axis
    @param      xi0     start value for the input x axis
    @param      dxi     increment value for the input x axis
    @param      nyi     number of points along the input y axis
    @param      yi0     start value for the input y axis
    @param      dyi     increment value for the input y axis

    @param      **vi    values for the two dimensional input array

    @param      nxo     number of points along the output x axis
    @param      xo0     start value for the output x axis
    @param      dxo     increment value for the output x axis
    @param      nyo     number of points along the output y axis
    @param      yo0     start value for the output y axis
    @param      dyo     increment value for the output y axis

    @param      mode    mode for boundary estimate (NATURAL, ESTIMATED1 or ESTIMATED2)

    @return     **vo    values for the two dimensional output array

    @warning The memory for the output array **vo is allocated by this function. To free the memory again please use
    a call to free_matrix(vo, nyo)
*/
double **bicubicspline_reg_reg(
        int nxi, double xi0, double dxi,
        int nyi, double yi0, double dyi,
        double **vi,
        int nxo, double xo0, double dxo,
        int nyo, double yo0, double dyo,
        enum boundary_mode mode)
{
    double **vo = matrix(nxo, nyo);
    double **v2 = blank_matrix(nxi);
    double *vtmp = NULL;
    double *vvtmp = NULL;
    double y1start = 0.0;  /* not used for bicubicsplines */
    double y1end = 0.0;    /* not used for bicubicsplines */
    int ix = 0, iy = 0, k = 0;

    for (ix=0; ix<nxi; ix++) {
        v2[ix] = spline_reg_init(nyi, dyi, vi[ix], mode, y1start, y1end);
    }

    for (ix=0; ix<nxo; ix++) {
        for (iy=0; iy<nyo; iy++) {
            vvtmp = vector(nxi);
            for (k=0; k<nxi; k++) {
                vvtmp[k] = spline_reg_interpolate(nyi, yi0, dyi, vi[k], v2[k], yo0 + iy*dyo);
            }
            vtmp = spline_reg_init(nxi, dxi, vvtmp, mode, y1start, y1end);
            vo[ix][iy] = spline_reg_interpolate(nxi, xi0, dxi, vvtmp, vtmp, xo0 + ix*dxo);
            free_vector(vtmp);
            free_vector(vvtmp);
       }
    }
    free_matrix(v2,nxi);

    return vo;
}

/**
    @brief  Bicubic splines interpolation with a regular array as input and an irregular spaced output array

    @param      nxi     number of points along the input x axis
    @param      xi0     start value for the input x axis
    @param      dxi     increment value for the input x axis
    @param      nyi     number of points along the input y axis
    @param      yi0     start value for the input y axis
    @param      dyi     increment value for the input y axis

    @param      **vi    values for the two dimensional input array

    @param      nxo     number of points along the output x axis
    @param      *xo     values on the output x axis
    @param      nyo     number of points along the output y axis
    @param      *yo     values on the output y axis

    @param      mode    mode for boundary estimate (NATURAL, ESTIMATED1 or ESTIMATED2)

    @return     **vo    values for the two dimensional output array

    @warning The memory for the output array **vo is allocated by this function. To free the memory again please use
    a call to free_matrix(vo, nyo)

*/
double **bicubicspline_reg_irreg(
        int nxi, double xi0, double dxi,
        int nyi, double yi0, double dyi,
        double **vi,
        int nxo, double *xo,
        int nyo, double *yo,
        enum boundary_mode mode)
{
    double **vo = matrix(nxo, nyo);
    double **v2 = blank_matrix(nxi);
    double *vtmp = NULL;
    double *vvtmp = NULL;
    double y1start = 0.0;  /* not used for bicubicsplines */
    double y1end = 0.0;    /* not used for bicubicsplines */
    int ix = 0, iy = 0, k = 0;

    for (ix=0; ix<nxi; ix++) {
        v2[ix] = spline_reg_init(nyi, dyi, vi[ix], mode, y1start, y1end);
    }

    for (ix=0; ix<nxo; ix++) {
        for (iy=0; iy<nyo; iy++) {
            vvtmp = vector(nxi);
            for (k=0; k<nxi; k++) {
                vvtmp[k] = spline_reg_interpolate(nyi, yi0, dyi, vi[k], v2[k], yo[iy]);
            }
            vtmp = spline_reg_init(nxi, dxi, vvtmp, mode, y1start, y1end);
            vo[ix][iy] = spline_reg_interpolate(nxi, xi0, dxi, vvtmp, vtmp, xo[ix]);
            free_vector(vtmp);
            free_vector(vvtmp);
        }
    }
    free_matrix(v2,nxi);

    return vo;
}

/**
    @brief  Bicubic splines interpolation with a regular array as input and a set of points for the output array

    @param      nxi     number of points along the input x axis
    @param      xi0     start value for the input x axis
    @param      dxi     increment value for the input x axis
    @param      nyi     number of points along the input y axis
    @param      yi0     start value for the input y axis
    @param      dyi     increment value for the input y axis

    @param      **vi    values for the two dimensional input array

    @param      nso     number of points in the output set
    @param      *xo     values on the x axis for each point in the output set
    @param      *yo     values on the y axis for each point in the output set

    @param      mode    mode for boundary estimate (NATURAL, ESTIMATED1 or ESTIMATED2)

    @return     *vo    set of values for the each xo[x],yo[x]

    @warning The memory for the output array **vo is allocated by this function. To free the memory again please use
    a call to free_vector(vo)

*/
double *bicubicspline_reg_set(
        int nxi, double xi0, double dxi,
        int nyi, double yi0, double dyi,
        double **vi,
        int nso, double *xo, double *yo,
        enum boundary_mode mode)
{
    double *vo = vector(nso);
    double **v2 = blank_matrix(nxi);
    double *vtmp = NULL;
    double *vvtmp = NULL;
    double y1start = 0.0;  /* not used for bicubicsplines */
    double y1end = 0.0;    /* not used for bicubicsplines */
    int ix = 0, is = 0, k = 0;


    for (ix=0; ix<nxi; ix++) {
        v2[ix] = spline_reg_init(nyi, dyi, vi[ix], mode, y1start, y1end);
    }

    for (is=0; is<nso; is++) {
        vvtmp = vector(nxi);
        for (k=0; k<nxi; k++) {
            vvtmp[k] = spline_reg_interpolate(nyi, yi0, dyi, vi[k], v2[k], yo[is]);
        }
        vtmp = spline_reg_init(nxi, dxi, vvtmp, mode, y1start, y1end);
        vo[is] = spline_reg_interpolate(nxi, xi0, dxi, vvtmp, vtmp, xo[is]);
        free_vector(vtmp);
        free_vector(vvtmp);
    }
    free_matrix(v2,nxi);

    return vo;
}

/**
    @brief  Bicubic splines interpolation with a irregular array as input and a regular output array

    @param      nxi     number of points along the input x axis
    @param      *xi     values on the input x axis
    @param      nyi     number of points along the input y axis
    @param      *yi     values on the input y axis

    @param      **vi    values for the two dimensional input array

    @param      nxo     number of points along the output x axis
    @param      xo0     start value for the output x axis
    @param      dxo     increment value for the output x axis
    @param      nyo     number of points along the output y axis
    @param      yo0     start value for the output y axis
    @param      dyo     increment value for the output y axis

    @param      mode    mode for boundary estimate (NATURAL, ESTIMATED1 or ESTIMATED2)

    @return     **vo    values for the two dimensional output array

    @warning The memory for the output array **vo is allocated by this function. To free the memory again please use
    a call to free_matrix(vo, nyo)

*/
double **bicubicspline_irreg_reg(
        int nxi, double *xi,
        int nyi, double *yi,
        double **vi,
        int nxo, double xo0, double dxo,
        int nyo, double yo0, double dyo,
        enum boundary_mode mode)
{
    double **vo = matrix(nxo, nyo);
    double **v2 = blank_matrix(nxi);
    double *vtmp = NULL;
    double *vvtmp = NULL;
    double y1start = 0.0;  /* not used for bicubicsplines */
    double y1end = 0.0;    /* not used for bicubicsplines */
    int ix = 0, iy = 0, k = 0;


    for (ix=0; ix<nxi; ix++) {
        v2[ix] = spline_irreg_init(nyi, yi, vi[ix], mode, y1start, y1end);
    }

    for (ix=0; ix<nxo; ix++) {
        for (iy=0; iy<nyo; iy++) {
            vvtmp = vector(nxi);
            for (k=0; k<nxi; k++) {
                vvtmp[k] = spline_irreg_interpolate(nyi, yi, vi[k], v2[k], yo0 + iy*dyo);
            }
            vtmp = spline_irreg_init(nxi, xi, vvtmp, mode, y1start, y1end);
            vo[ix][iy] = spline_irreg_interpolate(nxi, xi, vvtmp, vtmp, xo0 + ix*dxo);
            free_vector(vtmp);
            free_vector(vvtmp);
        }
    }
    free_matrix(v2,nxi);

    return vo;
}

/**
    @brief  Bicubic splines interpolation with a irregular arrays as input and output

    @param      nxi     number of points along the input x axis
    @param      *xi     values on the input x axis
    @param      nyi     number of points along the input y axis
    @param      *yi     values on the input y axis

    @param      **vi    values for the two dimensional input array

    @param      nxo     number of points along the output x axis
    @param      *xo     values on the output x axis
    @param      nyo     number of points along the output y axis
    @param      *yo     values on the output y axis

    @param      mode    mode for boundary estimate (NATURAL, ESTIMATED1 or ESTIMATED2)

    @return     **vo    values for the two dimensional output array

    @warning The memory for the output array **vo is allocated by this function. To free the memory again please use
    a call to free_matrix(vo, nyo)

*/
double **bicubicspline_irreg_irreg(
        int nxi, double *xi,
        int nyi, double *yi,
        double **vi,
        int nxo, double *xo,
        int nyo, double *yo,
        enum boundary_mode mode)
{
    double **vo = matrix(nxo, nyo);
    double **v2 = blank_matrix(nxi);
    double *vtmp = NULL;
    double *vvtmp = NULL;
    double y1start = 0.0;  /* not used for bicubicsplines */
    double y1end = 0.0;    /* not used for bicubicsplines */
    int ix = 0, iy = 0, k = 0;

    for (ix=0; ix<nxi; ix++) {
        v2[ix] = spline_irreg_init(nyi, yi, vi[ix], mode, y1start, y1end);
    }

    for (ix=0; ix<nxo; ix++) {
        for (iy=0; iy<nyo; iy++) {
            vvtmp = vector(nxi);
            for (k=0; k<nxi; k++) {
                vvtmp[k] = spline_irreg_interpolate(nyi, yi, vi[k], v2[k], yo[iy]);
            }
            vtmp = spline_irreg_init(nxi, xi, vvtmp, mode, y1start, y1end);
            vo[ix][iy] = spline_irreg_interpolate(nxi, xi, vvtmp, vtmp, xo[ix]);
            free_vector(vtmp);
            free_vector(vvtmp);
        }
    }
    free_matrix(v2,nxi);

    return vo;
}

/**
    @brief  Bicubic splines interpolation with a irregular arrays as input and a set of points for the output array

    @param      nxi     number of points along the input x axis
    @param      *xi     values on the input x axis
    @param      nyi     number of points along the input y axis
    @param      *yi     values on the input y axis

    @param      **vi    values for the two dimensional input array

    @param      nso     number of points in the output set
    @param      *xo     values on the x axis for each point in the output set
    @param      *yo     values on the y axis for each point in the output set

    @param      mode    mode for boundary estimate (NATURAL, ESTIMATED1 or ESTIMATED2)

    @return     *vo    set of values for the each xo[x],yo[x]

    @warning The memory for the output array **vo is allocated by this function. To free the memory again please use
    a call to free_vector(vo)

*/
 double *bicubicspline_irreg_set(
         int nxi, double *xi,
         int nyi, double *yi,
         double **vi,
         int nso, double *xo, double *yo,
         enum boundary_mode mode)
 {
     double *vo = vector(nso);
     double **v2 = blank_matrix(nxi);
     double *vtmp = NULL;
     double *vvtmp = NULL;
     double y1start = 0.0;  /* not used for bicubicsplines */
     double y1end = 0.0;    /* not used for bicubicsplines */
     int ix = 0, is = 0, k = 0;

     for (ix=0; ix<nxi; ix++) {
         v2[ix] = spline_irreg_init(nyi, yi, vi[ix], mode, y1start, y1end);
     }

     for (is=0; is<nso; is++) {
             vvtmp = vector(nxi);
             for (k=0; k<nxi; k++) {
                 vvtmp[k] = spline_irreg_interpolate(nyi, yi, vi[k], v2[k], yo[is]);
             }
             vtmp = spline_irreg_init(nxi, xi, vvtmp, mode, y1start, y1end);
             vo[is] = spline_irreg_interpolate(nxi, xi, vvtmp, vtmp, xo[is]);
             free_vector(vtmp);
             free_vector(vvtmp);
     }
     free_matrix(v2,nxi);

     return vo;
 }

/**
    @brief  Cubic splines interpolation with a regular array as input and output

    @param      nin     number of points along the input axis
    @param      xi0     start value for the input axis
    @param      dxi     increment value for the input axis

    @param      *yi    values for the one dimensional input array

    @param      nout    number of points along the output axis
    @param      xo0     start value for the output axis
    @param      dxo     increment value for the output axis

    @param      mode    mode for boundary estimate (NATURAL, ESTIMATED1 or ESTIMATED2)

    @return     *vo    values for the one dimensional output array

    @warning The memory for the output array **vo is allocated by this function. To free the memory again please use
    a call to free_vector(vo)

*/
double *cubicspline_reg_reg(
        int nin, double xi0, double dxi,
        double *yi,
        int nout, double xo0, double dxo,
        enum boundary_mode mode, ...)
{
    va_list ap;
    int ix = 0;
    double y1start = 0.0;
    double y1end = 0.0;
    double *y2 = NULL;
    double *yout = NULL;

    if (mode == EXACT) {
        va_start(ap, mode);
        y1start = va_arg(ap, double);
        y1end = va_arg(ap, double);
        va_end(ap);
    }

    y2 = spline_reg_init (nin, dxi, yi, mode, y1start, y1end);

    yout = vector(nout);
    for (ix=0; ix<nout; ix++) {
        yout[ix] = spline_reg_interpolate(nin, xi0, dxi, yi, y2, xo0+ix*dxo);
    }

    free_vector(y2);
    return yout;

}

/**
    @brief  Cubic splines interpolation with a irregular array as input and a regular output array

    @param      nin     number of points along the input axis
    @param      *xi     values on the input axis

    @param      *yi     values for the one dimensional input array

    @param      nout    number of points along the output axis
    @param      xo0     start value for the output axis
    @param      dxo     increment value for the output axis

    @param      mode    mode for boundary estimate (NATURAL, ESTIMATED1 or ESTIMATED2)

    @return     *vo    values for the one dimensional output array

    @warning The memory for the output array **vo is allocated by this function. To free the memory again please use
    a call to free_vector(vo)

*/
double *cubicspline_irreg_reg(
        int nin, double *xi,
        double *yi,
        int nout, double xo0, double dxo,
        enum boundary_mode mode, ...)
{
    va_list ap;
    int ix = 0;
    double y1start = 0.0;
    double y1end = 0.0;
    double *y2 = NULL;
    double *yout = NULL;

    if (mode == EXACT) {
        va_start(ap, mode);
        y1start = va_arg(ap, double);
        y1end = va_arg(ap, double);
        va_end(ap);
    }

    y2 = spline_irreg_init (nin, xi, yi, mode, y1start, y1end);

    yout = vector(nout);
    for (ix=0; ix<nout; ix++) {
        yout[ix] = spline_irreg_interpolate(nin, xi, yi, y2, xo0+ix*dxo);
    }

    free_vector(y2);
    return yout;
}

double *cubicspline_irreg_reg_nonans(
        int nin, double *xi,
        double *yi,
        int nout, double xo0, double dxo,
        enum boundary_mode mode, ...)
{
    va_list ap;
    double *tmpVals = NULL,
           *x = NULL,
           *y = NULL;
    double y1start = 0.0;
    double y1end = 0.0;
    int new_size = 0;

    KMCLIPM_TRY
    {
        remove_2nans(nin, xi, yi, &new_size, &x, &y);
        if (mode == EXACT) {
            va_start(ap, mode);
            y1start = va_arg(ap, double);
            y1end = va_arg(ap, double);
            va_end(ap);
            tmpVals = cubicspline_irreg_reg(new_size, x, y,
                nout, xo0, dxo, mode, y1start, y1end);
        } else {
            tmpVals = cubicspline_irreg_reg(new_size, x, y,
                nout, xo0, dxo, mode);
        }
        cpl_free(x); cpl_free(y);
    }
    KMCLIPM_CATCH
    {
    }
    return tmpVals;
}

/**
    @brief  Cubic splines interpolation with a regular array as input and an irregular output array

    @param      nin     number of points along the input axis
    @param      xi0     start value for the input axis
    @param      dxi     increment value for the input axis
    @param      *yi     values for the one dimensional input array
    @param      nout    number of points along the output axis
    @param      xo      values for the output axis
    @param      mode    mode for boundary estimate (NATURAL, ESTIMATED1 or ESTIMATED2)

    @return     *vo    values for the one dimensional output array

    @warning The memory for the output array **vo is allocated by this function. To free the memory again please use
    a call to free_vector(vo)

*/
double *cubicspline_reg_irreg(
        int nin, double xi0, double dxi,
        double *yi,
        int nout, double *xo,
        enum boundary_mode mode, ...)
{
    va_list ap;
    int ix = 0;
    double y1start = 0.0;
    double y1end = 0.0;
    double *y2 = NULL;
    double *yout = NULL;

    if (mode == EXACT) {
        va_start(ap, mode);
        y1start = va_arg(ap, double);
        y1end = va_arg(ap, double);
        va_end(ap);
    }

    y2 = spline_reg_init (nin, dxi, yi, mode, y1start, y1end);

    yout = vector(nout);
    for (ix=0; ix<nout; ix++) {
        yout[ix] = spline_reg_interpolate(nin, xi0, dxi, yi, y2, xo[ix]);
    }

    free_vector(y2);
    return yout;
}

/**
    @brief  Cubic splines interpolation with a irregular array for input and output

    @param      nin     number of points along the input axis
    @param      *yi     values for the one dimensional input array
    @param      *xi     values on the input axis
    @param      nout    number of points along the output axis
    @param      xo      values for the output axis
    @param      mode    mode for boundary estimate (NATURAL, ESTIMATED1 or ESTIMATED2)

    @return     *vo    values for the one dimensional output array

    @warning The memory for the output array **vo is allocated by this function. To free the memory again please use
    a call to free_vector(vo)

*/
double *cubicspline_irreg_irreg(
        int nin, double *xi,
        double *yi,
        int nout, double *xo,
        enum boundary_mode mode, ...)
{
    va_list ap;
    int ix = 0;
    double y1start = 0.0;
    double y1end = 0.0;
    double *y2 = NULL;
    double *yout = NULL;

    if (mode == EXACT) {
        va_start(ap, mode);
        y1start = va_arg(ap, double);
        y1end = va_arg(ap, double);
        va_end(ap);
    }

    y2 = spline_irreg_init (nin, xi, yi, mode, y1start, y1end);

    yout = vector(nout);
    for (ix=0; ix<nout; ix++) {
        yout[ix] = spline_irreg_interpolate(nin, xi, yi, y2, xo[ix]);
    }

    free_vector(y2);
    return yout;
}

/*erwdoc*/
/**
  @brief xxx
  @param      nin       xxx
  @param      delta_xin xxx
  @param      yin       xxx
  @param      mode      xxx
  @param      y1start   xxx
  @param      y1end     xxx
  @return xxx
*/
double *spline_reg_init (
        int nin, double delta_xin, double* yin,
        enum boundary_mode mode, double y1start, double y1end)
{
    double *y2 = vector(nin);

    int i = 0,k = 0;
    double p = 0., qn,sig = 0., un = 0., *u = NULL;
    double dy1dx1 = 0.;
    double dy1dx2 = 0.;

    u=vector(nin-1);

    switch (mode) {
    case NATURAL:
        y2[0] = 0.0;
        u[0] = 0.0;
        break;
    case EXACT:
        y2[0] = -0.5;
        u[0] = (3.0/delta_xin)*((yin[1]-yin[0])/delta_xin-y1start);
        break;
    case ESTIMATED1:
        y1start = (yin[1]-yin[0]) / delta_xin;
        y1end = (yin[nin-1]-yin[nin-2]) / delta_xin;
        y2[0] = -0.5;
        u[0] = (3.0/delta_xin)*((yin[1]-yin[0])/delta_xin-y1start);
        break;
    case ESTIMATED2:
        dy1dx1 = (yin[1]-yin[0]) / delta_xin;
        dy1dx2 = (yin[2]-yin[1]) / delta_xin;
        y1start = dy1dx1 - (dy1dx2 - dy1dx1)/2.;

        dy1dx1 = (yin[nin-2]-yin[nin-3]) / delta_xin;
        dy1dx2 = (yin[nin-1]-yin[nin-2]) / delta_xin;
        y1end = dy1dx2 + (dy1dx2 - dy1dx1)/2.;

        y2[0] = -0.5;
        u[0] = (3.0/delta_xin)*((yin[1]-yin[0])/delta_xin-y1start);
        break;
    default:
        printf ("Unknown boundary mode for cubic spline, fall back to \"natural\".");
        y2[0] = 0.0;
        u[0] = 0.0;
        mode = NATURAL;
        break;
    }


    for (i=1;i<nin-1;i++) {
        sig=0.5;
        p=sig*y2[i-1]+2.0;
        y2[i]=(sig-1.0)/p;
        u[i]=(yin[i+1]-yin[i])/delta_xin - (yin[i]-yin[i-1])/delta_xin;
        u[i]=(6.0*u[i]/(2. * delta_xin)-sig*u[i-1])/p;
    }

    if (mode == NATURAL) {
        qn = 0.0;
        un = 0.0;
    } else {
        qn = 0.5;
        un = (3.0/delta_xin)*(y1end-(yin[nin-1]-yin[nin-2])/delta_xin);
    }

    y2[nin-1]=(un-qn*u[nin-2])/(qn*y2[nin-2]+1.0);
    for (k=nin-2;k>=0;k--) {
        y2[k]=y2[k]*y2[k+1]+u[k];
    }

    free_vector(u);

    return y2;

}

/*erwdoc*/
/**
  @brief xxx
  @param      nin       xxx
  @param      xin       xxx
  @param      yin       xxx
  @param      mode      xxx
  @param      y1start   xxx
  @param      y1end     xxx
  @return xxx
*/
double *spline_irreg_init (
        int nin, double* xin, double* yin,
        enum boundary_mode mode, double y1start, double y1end)
{
    double *y2 = vector(nin);

    int i = 0,k = 0;
    double p = 0, qn = 0, sig = 0, un = 0, *u = NULL;
    double dy1dx1 = 0;
    double dy1dx2 = 0;

    u=vector(nin-1);

    switch (mode) {
    case NATURAL:
        y2[0] = 0.0;
        u[0] = 0.0;
        break;
    case EXACT:
        y2[0] = -0.5;
        u[0] = (3.0/(xin[1]-xin[0]))*((yin[1]-yin[0])/(xin[1]-xin[0])-y1start);
        break;
    case ESTIMATED1:
        y1start = (yin[1]-yin[0]) / (xin[1]-xin[0]);
        y1end = (yin[nin-1]-yin[nin-2]) / (xin[nin-1]-xin[nin-2]);
        y2[0] = -0.5;
        u[0] = (3.0/(xin[1]-xin[0]))*((yin[1]-yin[0])/(xin[1]-xin[0])-y1start);
        break;
    case ESTIMATED2:
        dy1dx1 = (yin[1]-yin[0]) / (xin[1]-xin[0]);
        dy1dx2 = (yin[2]-yin[1]) / (xin[2]-xin[1]);
        y1start = dy1dx1 - .5 * (dy1dx2 - dy1dx1);
        y1start = dy1dx1 + ((xin[0]-xin[1])/2.) * (dy1dx2 - dy1dx1) / ((xin[2]-xin[0])/2.);

        dy1dx1 = (yin[nin-2]-yin[nin-3]) / (xin[nin-2]-xin[nin-3]);
        dy1dx2 = (yin[nin-1]-yin[nin-2]) / (xin[nin-1]-xin[nin-2]);
        y1end = dy1dx2 +  ((xin[nin-1]-xin[nin-2])/2.) * (dy1dx1 - dy1dx2) / ((xin[nin-3]-xin[nin-1])/2.);
        y2[0] = -0.5;
        u[0] = (3.0/(xin[1]-xin[0]))*((yin[1]-yin[0])/(xin[1]-xin[0])-y1start);
        break;
    default:
        printf ("Unknown boundary mode for cubic spline, fall back to \"natural\".");
        y2[0] = 0.0;
        u[0] = 0.0;
        mode = NATURAL;
        break;
    }

    for (i=1;i<nin-1;i++) {
        sig=(xin[i]-xin[i-1])/(xin[i+1]-xin[i-1]);
        p=sig*y2[i-1]+2.0;
        y2[i]=(sig-1.0)/p;
        u[i]=(yin[i+1]-yin[i])/(xin[i+1]-xin[i]) - (yin[i]-yin[i-1])/(xin[i]-xin[i-1]);
        u[i]=(6.0*u[i]/(xin[i+1]-xin[i-1])-sig*u[i-1])/p;
    }

    if (mode == NATURAL) {
        qn = 0.0;
        un = 0.0;
    } else {
        qn = 0.5;
        un = (3.0/(xin[nin-1]-xin[nin-2]))*(y1end-(yin[nin-1]-yin[nin-2])/(xin[nin-1]-xin[nin-2]));
    }

    y2[nin-1]=(un-qn*u[nin-2])/(qn*y2[nin-2]+1.0);
    for (k=nin-2;k>=0;k--) {
        y2[k]=y2[k]*y2[k+1]+u[k];
    }

    free_vector(u);

    return y2;

}

/*erwdoc*/
/**
  @brief xxx
  @param      nin       xxx
  @param      x0        xxx
  @param      delta_x   xxx
  @param      yin       xxx
  @param      y2in      xxx
  @param      xnew      xxx
  @return xxx
*/
double spline_reg_interpolate(int nin, double x0, double delta_x, double *yin,
                              double* y2in, double xnew)
{
    int ix_low = 0, ix_high = 0;
    double h = 0, b = 0, a = 0;

    ix_low = (xnew - x0) / delta_x;
    if (ix_low < 0) {
        ix_low = 0;
    }
    ix_high = ix_low + 1;
    if (ix_high > (nin - 1)) {
        ix_high = nin - 1;
        ix_low = nin - 2;
    }

    h=delta_x;
    a=((x0+delta_x*ix_high) - xnew)/h;
    b=(xnew - (x0+delta_x*ix_low))/h;

    /*
    a=(xin[ix_high]-xnew)/h;
    b=(xnew-xin[ix_low])/h;
    */
    return a*yin[ix_low]+b*yin[ix_high]+((a*a*a-a)*y2in[ix_low]+(b*b*b-b)*y2in[ix_high])*(h*h)/6.0;
}

/*erwdoc*/
/**
  @brief xxx
  @param      nin       xxx
  @param      xin       xxx
  @param      yin       xxx
  @param      y2in      xxx
  @param      xnew      xxx
  @return xxx
*/
double spline_irreg_interpolate(int nin, double *xin, double *yin, double* y2in, double xnew)
{
    int klo = 0, khi = 0, k = 0;
    double h = 0, b = 0, a = 0;

    klo=0;
    khi=nin-1;
    if (xin[0] < xin[1]) {
        while (khi-klo > 1) {
            k=(khi+klo) >> 1;
            if (xin[k] > xnew) khi=k;
            else klo=k;
        }
    } else {
        while (khi-klo > 1) {
            k=(khi+klo) >> 1;
            if (xin[k] < xnew) khi=k;
            else klo=k;
        }
    }
    h=xin[khi]-xin[klo];
    a=(xin[khi]-xnew)/h;
    b=(xnew-xin[klo])/h;
    return a*yin[klo] +
    b*yin[khi] +
    ((a*a*a-a)*y2in[klo] +
            (b*b*b-b)*y2in[khi])*(h*h)/6.0;
}

/**
  @brief xxx
  @param      i1     xxx
  @param      i2     xxx
  @return xxx
*/
int imin (int i1, int i2) {
  if (i1<i2)
    return i1;
  else
    return i2;
}

/**
  @brief xxx
  @param      i1     xxx
  @param      i2     xxx
  @return xxx
*/
int imax (int i1, int i2) {
  if (i1>i2)
    return i1;
  else
    return i2;
}

/*erwdoc*/
/**
  @brief xxx
  @param      nin       xxx
  @param      xi        xxx
  @param      yi        xxx
  @param      nout      xxx
  @param      xo0       xxx
  @param      dxo       xxx
  @param      order     xxx
  @return xxx
*/
double *polynomial_irreg_reg(
        int nin, double *xi,
        double *yi,
        int nout, double xo0, double dxo,
        int order) {

    unsigned int cx = 0;
    unsigned int polydim = 0;
    double x = 0.;
    double error = 0.;
    double *yout = NULL;
    int ix = 0;
    int old_order = 0;

    if (order >= nin) {
        old_order = order;
        order = nin - 1;

        if (order > 0) {
            cpl_msg_warning(__func__,
                    "too few data points for %dth order polynomial interpolation, order decreased to %d",
                    old_order, order);
        } else {
            cpl_msg_error(__func__,
                    "Only one valid data point! Can't do cubic spline here!");
        }
/*
        report_error(__func__, CPL_ERROR_ILLEGAL_INPUT,
                    "at least \"order + 1\" data points are needed as input");
*/
    }

    yout = vector(nout);
    if (order > 0) {
        polydim = order + 1;
        for (ix=0; ix<nout; ix++) {
              x = xo0 + ix * dxo;
              cx = nin/2;
              if (hunt_for_index(xi, nin, x, &cx)) {
                cx = imin( imax(cx-(polydim-1)/2,0), nin-polydim);
                yout[ix] =  polynomial_interpolation(&xi[cx], &yi[cx], polydim, x, &error);
              } else {
                  yout[ix] = NAN;
              }
        }
    } else {
        for (ix=0; ix<nout; ix++) {
            yout[ix] = NAN;
        }
    }

    return yout;
}

double *polynomial_irreg_reg_nonans(
        int nin, double *xi,
        double *yi,
        int nout, double xo0, double dxo,
        int order) {
    double *tmpVals = NULL,
           *x = NULL,
           *y = NULL;
    int new_size = 0;

    KMCLIPM_TRY
    {
        remove_2nans(nin, xi, yi, &new_size, &x, &y);
        tmpVals = polynomial_irreg_reg(new_size, x, y,
                nout, xo0, dxo, order);
        cpl_free(x); cpl_free(y);
    }
    KMCLIPM_CATCH
    {
    }
    return tmpVals;
}

/*erwdoc*/
/**
  @brief xxx
  @param      nin       xxx
  @param      xi        xxx
  @param      yi        xxx
  @param      nout      xxx
  @param      xo0       xxx
  @param      dxo       xxx
  @param      order     xxx
  @return xxx
*/
double *polynomial_irreg_irreg(
        int nin, double *xi,
        double *yi,
        int nout, const double *xo,
        int order) {

    unsigned int cx = 0;
    unsigned int polydim = 0;
    double x = 0.;
    double error = 0.;
    double *yout = NULL;
    int ix = 0;
    int old_order = 0;

    if (order >= nin) {
        old_order = order;
        order = nin - 1;

        if (order > 0) {
            cpl_msg_warning(__func__,
                    "too few data points for %dth order polynomial interpolation, order decreased to %d",
                    old_order, order);
        } else {
            cpl_msg_error(__func__,
                    "Only one valid data point! Can't do cubic spline here!");
        }
/*
        report_error(__func__, CPL_ERROR_ILLEGAL_INPUT,
                    "at least \"order + 1\" data points are needed as input");
*/
    }

    yout = vector(nout);
    if (order > 0) {
        polydim = order + 1;
        for (ix=0; ix<nout; ix++) {
              x = xo[ix];
              cx = nin/2;
              if (hunt_for_index(xi, nin, x, &cx)) {
                  cx = imin( imax(cx-(polydim-1)/2,0), nin-polydim);
                  yout[ix] =  polynomial_interpolation(&xi[cx], &yi[cx], polydim, x, &error);
              } else {
                  yout[ix] = NAN;
              }
        }
    } else {
        for (ix=0; ix<nout; ix++) {
            yout[ix] = NAN;
        }
    }

    return yout;
}

double *polynomial_irreg_irreg_nonans(
        int nin, double *xi,
        double *yi,
        int nout, const double *xo,
        int order) {
    double *tmpVals = NULL,
           *x = NULL,
           *y = NULL;
    int new_size = 0;

    KMCLIPM_TRY
    {
        remove_2nans(nin, xi, yi, &new_size, &x, &y);
        tmpVals = polynomial_irreg_irreg(new_size, x, y,
                nout, xo, order);
    cpl_free(x); cpl_free(y);
    }
    KMCLIPM_CATCH
    {
    }
    return tmpVals;
}

/*erwdoc*/
/**
  @brief xxx

  @param      xi        xxx
  @param      yi        xxx
  @param      n         xxx
  @param      x         xxx
  @param      dy        xxx
  @return xxx
*/
double polynomial_interpolation (double xi[],
                                 double yi[],
                                 int n,
                                 double x,
                                 double *dy)
{
    int         i       = 0,
                m       = 0,
                ns      = 0;
    double      den     = 0.,
                dif     = 0.,
                dift    = 0.,
                ho      = 0.,
                hp      = 0.,
                w       = 0.,
                *c      = NULL,
                *d      = NULL,
                y       = 0.,
                median  = 0.;
    cpl_vector  *ll     = NULL;

    if (n == 0) {
        return NAN;
    } else {
        /* this is to get rid of wrong polynomial fitting, if there are large steps
         * in xi due to NaN-removal excercised beforehand if the NaNs are left in,
         * the fit will go mad at the end. Even worse: when combining the cubes in a
         * map afterwards, it can happen, that a checkerboard pattern is arising occasionally!
         */
        ll = cpl_vector_new(n-1);
        for (i = 0; i < n-1; i++) {
            cpl_vector_set(ll, i, xi[i]-xi[i+1]);
        }
        median = cpl_vector_get_median(ll);
        cpl_vector_delete(ll); ll = NULL;
        if (fabs(xi[0]-xi[n-1]) > fabs(median*(n-1)*1.5)) {
            return NAN;
        }
    }

    dif=fabs(x-xi[0]);
    c=vector(n);
    d=vector(n);
    for (i=0;i<n;i++) {
            if ( (dift=fabs(x-xi[i])) < dif) {
                    ns=i;
                    dif=dift;
            }
            c[i]=yi[i];
            d[i]=yi[i];
    }

    y=yi[ns--];
    for (m=1;m<n;m++) {
            for (i=0;i<n-m;i++) {
                    ho=xi[i]-x;
                    hp=xi[i+m]-x;
                    w=c[i+1]-d[i];
                    if ( (den=ho-hp) == 0.0) printf("Error in routine polint");
                    den=w/den;
                    d[i]=hp*den;
                    c[i]=ho*den;
            }
            y += (*dy=(2*(ns+1) < (n-m) ? c[ns+1] : d[ns--]));
    }
    free_vector(d);
    free_vector(c);

    return y;
}

/*erwdoc*/
/**
  @brief xxx
  @param      xx       xxx
  @param      n        xxx
  @param      x        xxx
  @param      jlo      xxx
  @return xxx
*/
int hunt_for_index(double xx[], unsigned long n, double x, unsigned int *jlo)
{
    unsigned long jm = 0, jhi = 0, inc = 0;
    int ascnd = 0;

    ascnd=(xx[n-1] > xx[0]);
    if (*jlo <= 0 || *jlo > n-1) {
            *jlo=0;
            jhi=n-1;
    } else {
            inc=1;
            if ((x >= xx[*jlo]) == ascnd) {
                    if (*jlo == (n-1)) return TRUE;
                    jhi=(*jlo)+1;
                    while ((x >= xx[jhi]) == ascnd) {
                            *jlo=jhi;
                            inc += inc;
                            jhi=(*jlo)+inc;
                            if (jhi > n-1) {
                                    jhi=n;
                                    break;
                            }
                    }
            } else {
                    if (*jlo == 0) {
                            *jlo=0;
                            return TRUE;
                    }
                    jhi=(*jlo)--;
                    while ((x < xx[*jlo]) == ascnd) {
                            jhi=(*jlo);
                            inc <<= 1;
                            if (inc >= jhi) {
                                    *jlo=0;
                                    break;
                            }
                            else *jlo=jhi-inc;
                    }
            }
    }
    while (jhi-(*jlo) != 1) {
        if ((jhi == 0) && (jhi == *jlo) && (jhi == jm)) {
            return FALSE;
        }
            jm=(jhi+(*jlo)) >> 1;
            if ((x > xx[jm]) == ascnd)
                *jlo=jm;
            else
                jhi=jm;
    }
    return TRUE;
}

/*erwdoc*/
/**
  @brief xxx
  @param      n       xxx
  @return xxx
*/
double *vector(int n) {
    return (double *) cpl_calloc((size_t)n, (size_t)sizeof(double));
}

/*erwdoc*/
/**
  @brief xxx
  @param      nrow       xxx
  @return xxx
*/
double **blank_matrix(int nrow) {
    double ** matrix = (double **) cpl_calloc((size_t)nrow, (size_t)sizeof(double*));

    return matrix;
}

/*erwdoc*/
/**
  @brief xxx
  @param      nrow       xxx
  @param      ncol       xxx
  @return xxx
*/
double **matrix(int nrow, int ncol) {
    double ** matrix = (double **) cpl_malloc((size_t)((nrow)*sizeof(double*)));

    int i = 0;
    for(i=0;i<nrow;i++) {
        matrix[i] = (double *) cpl_malloc((size_t)((ncol)*sizeof(double)));
    }

    return matrix;
}

/*erwdoc*/
/**
  @brief xxx
  @param      vector       xxx
  @return xxx
*/
void free_vector(double *vector) {
     cpl_free(vector);
}

/**
  @brief xxx
  @param      vector       xxx
  @return xxx
*/
void free_vector2(double **vector) {
    cpl_free(*vector);
    *vector = NULL;
}


/*erwdoc*/
/**
  @brief xxx
  @param      matrix       xxx
  @param      nrow         xxx
  @return xxx
*/
void free_matrix(double ** matrix, int nrow) {
    int i = 0;
    for(i=0;i<nrow;i++) {
        cpl_free(matrix[i]); matrix[i] = NULL;
    }
    cpl_free(matrix); matrix = NULL;
}

void remove_nans(int size_in, double *xin, int *size_out, double **xout) {
    int no_valids = 0,
        ox        = 0,
        i         = 0;

    KMCLIPM_TRY
    {
        for (i = 0; i < size_in; i++) {
            if ((kmclipm_is_nan_or_inf(xin[i]) == 0)) {
                no_valids++;
            }
        }
        *size_out = no_valids;
        KMCLIPM_TRY_EXIT_IFN(
                *xout =  (double *) cpl_calloc(no_valids, sizeof(double)));

        ox = 0;
        for (i = 0; i < size_in; i++) {
            if ((kmclipm_is_nan_or_inf(xin[i]) == 0)) {
                (*xout)[ox] = xin[i];
                ox++;
            }
        }
    }
    KMCLIPM_CATCH
    {
    }

}

void remove_2nans(int size_in, double *xin, double *yin, int *size_out, double **xout, double **yout) {
    int no_valids = 0,
        ox        = 0,
        i         = 0;

    KMCLIPM_TRY
    {
        for (i = 0; i < size_in; i++) {
            if ((kmclipm_is_nan_or_inf(xin[i]) == 0) && (kmclipm_is_nan_or_inf(yin[i]) == 0)) {
                no_valids++;
            }
        }
        *size_out = no_valids;
        KMCLIPM_TRY_EXIT_IFN(
                *xout =  (double *) cpl_calloc(no_valids, sizeof(double)));
        KMCLIPM_TRY_EXIT_IFN(
                *yout =  (double *) cpl_calloc(no_valids, sizeof(double)));

        ox = 0;
        for (i = 0; i < size_in; i++) {
            if ((kmclipm_is_nan_or_inf(xin[i]) == 0) && (kmclipm_is_nan_or_inf(yin[i]) == 0)) {
                (*xout)[ox] = xin[i];
                (*yout)[ox] = yin[i];
                ox++;
            }
        }
    }
    KMCLIPM_CATCH
    {
    }
}

/*erwdoc*/
/**
  @brief xxx
  @param      func       xxx
  @param      code       xxx
  @param      msg        xxx
  @return xxx
*/
void report_error(const char *func, cpl_error_code code, const char *msg) {
    cpl_msg_error (func, "%s", msg);


#if defined CPL_HAVE_VA_ARGS && CPL_HAVE_VA_ARGS == 1
    cpl_error_set_message(func, code, "%s", msg);
#else
    #if defined CPL_VERSION_CODE && CPL_VERSION_CODE >= CPL_VERSION(6, 0, 0) && CPL_VERSION_CODE < CPL_VERSION(6, 3, 1)
    cpl_error_set_message_one(func, code, msg);
    #else
    cpl_error_set_message(func, code, msg);
    #endif
#endif
}

/** @} */
