/home/pascal/depot/filedata/src/interpolation.h

/***************************************************************************
 *   Copyright (C) 2011 by Nicolas PASCAL                                  *
 *                                                                         *
 *   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.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#ifndef INTERPOLATION_H
#define INTERPOLATION_H

#include <cstdlib>
#include <exception>
#include <string>
#include <algorithm>
#include <cmath>
using namespace std;

#include "g_exception.h"
#include <gsl/gsl_spline.h>
#include <gsl/gsl_errno.h>

template < typename T >
const bool is_in_ascending_order ( const T * v, const int v_size, const T * invalid = NULL )
{
    if ( v_size < 2 )
        // only one or 0 value in v -> consider it as sorted in ascending order
        return true;

    int i = 0;
    while ( i < v_size and v[i] == v[0] )
        i+=1;

    if ( i < v_size )
        return ( v[i] > v[0] );
    else
        // all values in v are equal -> consider it as sorted in ascending order
        return true;
}

template < typename T_x_in, typename T_y_in, typename T_x_out, typename T_y_out >
inline T_y_out interpolate_1D (
                         const T_x_out & x,
                         const T_x_in  & x1, const T_x_in  & x2,
                         const T_y_in  & y1, const T_y_in  & y2,
                         const T_y_out * y_out_invalid,
                         const T_x_in  * x_in_invalid  = NULL, const T_y_in  * y_in_invalid  = NULL,
                         const T_x_out * x_out_invalid = NULL,
                         const bool extrapolate = false )
{
    if ( y_out_invalid == NULL )
    {
        string msg ( "A not NULL invalid returned value must have been set in case of interpolation error" ) ;
        g_exception e( __FILE__ , __LINE__ , msg );
        throw e;
    }

    // check limits validity
    if  ( ( x_out_invalid != NULL &&   x  == *x_out_invalid ) ||
          ( x_in_invalid  != NULL && ( x1 == *x_in_invalid || x2 == *x_in_invalid ) ) ||
            y_in_invalid  != NULL && ( y1 == *y_in_invalid  || y2 == *y_in_invalid ) )
    {
        return *y_out_invalid;
    }

    // check if x1 and x2 are in or out the interpolation range
    if ( not extrapolate )
    {
        bool is_increasing = ( x2 >= x1 );
        if (     is_increasing and ( x < x1 or x > x2 ) or
             not is_increasing and ( x > x1 or x < x2 ) )
        {
            // out of interpolation range
            return *y_out_invalid;
        }
    }

    // compute interpolation ( or extrapolation if enabled ) using double precision
    double y_dbl = (double)(y1) + ( (double)(x) - (double)(x1) ) * ( (double)(y2) - (double)(y1) ) / ( (double)(x2) - (double)(x1) );
    return (T_y_out)(y_dbl) ;
};


template < typename T_x_in, typename T_y_in, typename T_x_out, typename T_y_out >
inline T_y_out * v_interpolate_1D (       T_y_out * v_y_out, const T_x_out * v_x_out, const int v_out_size,
                                    const T_y_in  * v_y_in,  const T_x_in  * v_x_in,  const int v_in_size,
                                    const T_y_out * invalid_y_out,
                                    const T_x_in  * invalid_x_in  = NULL, const T_y_in  * invalid_y_in  = NULL,
                                    const T_x_out * invalid_x_out = NULL,
                                    const bool extrapolate = false )
{
    // check the sorting order of v_x_in
    bool in_asc_order = is_in_ascending_order( v_x_in, v_in_size, invalid_x_in );

    // process interpolation element by element
    int i_in, i_out, i_min, i_max;
    T_x_in * low;
    T_y_out y_out;

    for ( i_out = 0 ;  i_out < v_out_size ; ++i_out )
    {
        // find indexes of bounds
        if (in_asc_order)
            low = lower_bound ( const_cast<T_x_in *>(v_x_in), const_cast<T_x_in *>(v_x_in + v_in_size), v_x_out[i_out],
                                less_equal<T_x_in>() );
        else
            low = lower_bound ( const_cast<T_x_in *>(v_x_in), const_cast<T_x_in *>(v_x_in + v_in_size), v_x_out[i_out],
                                greater<T_x_in>() );
        i_in = low - v_x_in;
        // set bounds for an eventual extrapolation
        if ( i_in <= 0 )
        {
            i_min = 0;
            i_max = i_min + 1;
        }
        else if ( i_in >= v_in_size )
        {
            i_max = v_in_size - 1;
            i_min = i_max - 1;
        } else {
            i_min = i_in -1 ;
            i_max = i_in ;
        }

        y_out = interpolate_1D( v_x_out[i_out], v_x_in[i_min], v_x_in[i_max], v_y_in[i_min], v_y_in[i_max],
                                invalid_y_out, invalid_x_in, invalid_y_in, invalid_x_out, extrapolate );
        v_y_out[i_out] = y_out;

// printf("----- v_interpolate_1D -----\n");
// printf("x1=%f x2=%f x=%f\n",v_x_in[i_min],  v_x_in[i_max], v_x_out[i_out]);
// printf("y1=%f y2=%f y=%f\n",v_y_in[i_min],  v_y_in[i_max], v_y_out[i_out]);
    }
    return v_y_out;
};

template < typename T_x_in, typename T_y_in, typename T_x_out, typename T_y_out >
inline void spline_interp (
          T_y_out * y_out, const T_x_out * x_out, const int sz_out,
    const T_y_in  * y_in,  const T_x_in  * x_in,  const int sz_in,
    const T_y_out   invalid_y_out ) {
    // args of interpolation functions are requested as double
    double xi[sz_in];
    copy (x_in, x_in + sz_in, xi);
    double yi[sz_in];
    copy (y_in, y_in + sz_in, yi);
    /* gsl interpolation functions request an ascending order ->
       reverse the vectors if requested */
    bool is_ascending = (x_in[0] < x_in[1]);
    if (! is_ascending ) {
        reverse (xi, xi + sz_in);
        reverse (yi, yi + sz_in);
    }

    // init interpolator
    gsl_interp_accel * acc = gsl_interp_accel_alloc ();
    gsl_spline * spline = gsl_spline_alloc (gsl_interp_akima, sz_in);
    gsl_spline_init (spline, xi, yi, sz_in);
    // compute interpolations
    double left  = xi[0];
    double right = xi [sz_in - 1];
    for (int i = 0 ; i < sz_out ; ++i) {
        // filter out of interpolation range values
        if ((x_out[i] < left) || (x_out[i] > right)) {
            y_out[i] = invalid_y_out;
        } else {
            y_out[i] = gsl_spline_eval (spline, x_out[i], acc);
        }
    }
    // free spline
    gsl_spline_free (spline);
    gsl_interp_accel_free (acc);
}
/*
inline void spline_interp (
          double * y_out, const double * x_out, const int sz_out,
    const double * y_in, const double  * x_in,  const int sz_in,
    const double invalid_y_out ) {
    gsl_interp_accel * acc = gsl_interp_accel_alloc ();
    gsl_spline * spline = gsl_spline_alloc (gsl_interp_cspline, sz_in);
    // compute spline interpolator. Inputs are requested as double
    gsl_spline_init (spline, x_in, y_in, sz_in);
    // compute interpolations
    for (int i = 0 ; i < sz_in ; ++i) {
        // filter out of interpolation range values
        if ((x_out[i] < x_in[0]) || (x_out[i] > x_in [sz_in - 1])) {
            y_out[i] = invalid_y_out;
        } else {
            y_out[i] = gsl_spline_eval (spline, x_out[i], acc);
        }
    }
    // free spline
    gsl_spline_free (spline);
    gsl_interp_accel_free (acc);
}*/

#endif