/*
 * This file is part of the ESO SINFONI Pipeline
 * Copyright (C) 2004,2005 European Southern Observatory
 *
 * 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, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
 */
/*----------------------------------------------------------------------------

   File name     :    function_1d.c
   Author         :    Nicolas Devillard
   Created on    :    Tue, Sept 23 1997    
   Description    :    1d signal processing related routines    

 ---------------------------------------------------------------------------*/
/*
 $Id: sinfo_function_1d.c,v 1.7 2012-03-02 08:42:20 amodigli Exp $
 $Author: amodigli $
 $Date: 2012-03-02 08:42:20 $
 $Revision: 1.7 $
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
/*----------------------------------------------------------------------------
                                   Includes
 ---------------------------------------------------------------------------*/
#include <math.h>
#include <string.h>
#include "sinfo_function_1d.h"
#include "sinfo_fit_curve.h"
#include "sinfo_median.h"

/**@{*/
/**
 * @defgroup sinfo_function_1d 1d functions
 *
 * TBD
 */


/*----------------------------------------------------------------------------
                                   Defines
 ---------------------------------------------------------------------------*/
/*
 * This parameter sets up the half size of the domain around which a
 * centroid position will be computed.
 */
#define HALF_CENTROID_DOMAIN    5
/*----------------------------------------------------------------------------
                        Private function prototypes
 ---------------------------------------------------------------------------*/
static double * function1d_generate_smooth_kernel(int filt_type, int hw);

static int
function1d_search_value(
                pixelvalue  *   x,
                int             len,
                pixelvalue      key,
                int         *   foundPtr
) ;

/*----------------------------------------------------------------------------
                              Function codes
 ---------------------------------------------------------------------------*/
/**
  @name        sinfo_function1d_new
  @memo        Allocates a new array of pixelvalues.
  @param    nsamples    Number of values to store in the array.
  @return    Pointer to newly allocated array of pixelvalues.
  @doc

  The returned array must be freed using sinfo_function1d_del(), 
  not cpl_free().
  This is in case some further housekeeping attributes are allocated
  together with the object in the future.

  Returns NULL in case of error.

 */

pixelvalue *
sinfo_function1d_new(int nsamples)
{
    if (nsamples<1) return NULL ;
    return cpl_calloc(nsamples, sizeof(pixelvalue)) ;
}


/**
  @name        sinfo_function1d_del
  @memo        Deallocate an array of pixelvalues.
  @param    s    Array to deallocate.
  @return    void
  @doc

  Deallocates an array allocated by sinfo_function1d_new().
 */
void 
sinfo_function1d_del(pixelvalue * s)
{
    if (s)
        cpl_free(s);
    return ;
}

/**
  @name        sinfo_function1d_dup
  @memo        Copy an array of pixelvalues to a new array.
  @param    arr        Array to copy.
  @param    ns        Number of samples in the array.
  @return    Pointer to newly allocated array of pixelvalues.
  @doc

  Creates a new array using sinfo_function1d_new(), with the same number of
  samples as the input signal, then copies over all values from source
  to destination array using memcpy().

  The returned array must be freed using sinfo_function1d_del(), 
  not cpl_free().

 */
pixelvalue * 
sinfo_function1d_dup(pixelvalue * arr, int ns)
{
    pixelvalue    *    n_arr ;

    n_arr = sinfo_function1d_new(ns);
    memcpy(n_arr, arr, ns * sizeof(pixelvalue));
    return n_arr ;
}

/**
  @name        sinfo_function1d_filter_lowpass
  @memo        Apply a low-pass filter to a 1d signal.
  @param    input_sig    Input signal
  @param    samples        Number of samples in the signal
  @param    filter_type    Type of filter to use.
  @param    hw            Filter half-width.
  @return    Pointer to newly allocated array of pixels.
  @doc

  This kind of low-pass filtering consists in a convolution with a
  given kernel. The chosen filter type determines the kind of kernel
  to apply for convolution. Possible kernels and associated symbols
  can be found in function_1d.h.

  Smoothing the signal is done by applying this kind of low-pass
  filter several times.

  The returned smooth signal has been allocated using
  sinfo_function1d_new(), it must be freed using sinfo_function1d_del(). The
  returned signal has exactly as many samples as the input signal.

 */

pixelvalue *
sinfo_function1d_filter_lowpass(
                pixelvalue    *    input_sig,
                int                samples,
                int                filter_type,
                int                hw
)
{
    pixelvalue    *    out_sig ;
    int                i, j ;
    double            replace ;
    double        *    kernel ;

    /* allocate output signal */
    out_sig = sinfo_function1d_new(samples);

    /* generate low-pass filter kernel */
    kernel = function1d_generate_smooth_kernel(filter_type, hw) ;

    /* compute sinfo_edge effects for the first hw elements */
    for (i=0 ; i<hw ; i++) {
        replace = 0.0 ;
        for (j=-hw ; j<=hw ; j++) {
            if (i+j<0) {
                replace += kernel[hw+j] * (double)input_sig[0] ;
            } else {
                replace += kernel[hw+j] * (double)input_sig[i+j] ;
            }
        }
        out_sig[i] = (pixelvalue)replace ;
    }

    /* compute sinfo_edge effects for the last hw elements */
    for (i=samples-hw ; i<samples ; i++) {
        replace = 0.0 ;
        for (j=-hw ; j<=hw ; j++) {
            if (i+j>samples-1) {
                replace += kernel[hw+j] * (double)input_sig[samples-1] ;
            } else {
                replace += kernel[hw+j] * (double)input_sig[i+j] ;
            }
        }
        out_sig[i] = (pixelvalue)replace ;
    }

    /* compute all other elements */
    for (i=hw ; i<samples-hw ; i++) {
        replace = 0.0 ;
        for (j=-hw ; j<=hw ; j++) {
            replace += kernel[hw+j] * (double)input_sig[i+j] ;
        }
        out_sig[i] = (pixelvalue)replace ;
    }

    cpl_free(kernel) ;
    return out_sig ;

}

/**
  @name        function1d_generate_smooth_kernel
  @memo        Generate a kernel for smoothing filters (low-pass).
  @param    filt_type    Type of kernel to generate.
  @param    hw            Kernel half-width.
  @return    Pointer to newly allocated kernel.
  @doc

  Supported kernels can be found in function_1d.h

  The returned array has been allocated with cpl_calloc(), it must be
  freed using cpl_free(). The returned array contains 2h+1 doubles, where
  h is the kernel half-width.

 */

static double * 
function1d_generate_smooth_kernel(int filt_type, int hw)
{
    double  *   kernel ;
    double      norm ;
    int         i ;

    kernel = (double*)cpl_calloc(2*hw+1, sizeof(double)) ;

    switch(filt_type) {

    case LOW_PASS_LINEAR:
        for (i=-hw ; i<=hw ; i++) {
            /* flat kernel */
            kernel[hw+i] = 1.0 / (double)(2*hw+1) ;
        }
        break ;

    case LOW_PASS_GAUSSIAN:
        norm = 0.00 ;
        for (i=-hw ; i<=hw ; i++) {
            /* sinfo_gaussian kernel */
            kernel[hw+i] = exp(-(double)(i*i)) ;
            norm += kernel[hw+i] ;
        }
        for (i=0 ; i<2*hw+1 ; i++) {
            kernel[i] /= norm ;
        }
        break ;

    default:
        sinfo_msg_error("unrecognized low pass filter: "
                        "cannot generate kernel") ;
        cpl_free( kernel );
        return (double*)NULL ;
        break ;
    }

    return kernel ;
}


#undef LOWFREQ_PASSES


/**
  @name        sinfo_function1d_interpolate_linear
  @memo        Linear signal interpolation.
  @param    x        Input list of x positions.
  @param    y        Input list of y positions.
  @param    len        Number of samples in x and y.
  @param    spl_x    List of abscissas where the signal must be computed.
  @param    spl_y    Output list of computed signal values.
  @param    spl_len    Number of samples in spl_x and spl_y.
  @return    void
  @doc

  To apply this interpolation, you need to provide a list of x and y
  positions, and a list of x positions where you want y to be computed
  (with linear interpolation).

  The returned signal has spl_len samples. It has been allocated using
  sinfo_function1d_new() and must be deallocated using sinfo_function1d_del().

 */

void 
sinfo_function1d_interpolate_linear(
                pixelvalue    *    x,
                pixelvalue    *    y,
                int                len,
                pixelvalue    *    spl_x,
                pixelvalue    *    spl_y,
                int                spl_len
)
{
    double        a, b ;
    int            i, j ;


    for (i=0 ; i<spl_len ; i++) {
        /* Find (x1,y1) on the left of the current point */
        int found = 0 ;
        for (j=0 ; j<(len-1) ; j++) {
            if ((spl_x[i]>=x[j]) && (spl_x[i]<=x[j+1])) {
                found++ ;
                break ;
            }
        }
        if (!found) {
            spl_y[i] = 0.0;
        } else {
            a = ((double)y[j+1]-(double)y[j]) /
                            ((double)x[j+1]-(double)x[j]);
            b = (double)y[j] - a * (double)x[j] ;
            spl_y[i] = (pixelvalue)(a * (double)spl_x[i] + b) ;
        }
    }
    return ;
}

/**
  @name        function1d_search_value
  @memo        Conducts a binary search for a value.
  @param    x     Contains the abscissas of interpolation.
  @param    len     Length of the x array.
  @param    key     The value to locate in x.
  @param    foundPtr Output flag, 1 if value was found, else 0.
  @return    The index of the largest value in x for which x[i]<key.
  @doc

  This function does a binary search for a value in an array. This
  routine is to be called only if key is in the interval between x[0]
  and x[len-1]. The input x array is supposed sorted.

 */

static int
function1d_search_value(
                pixelvalue    *    x,
                int             len,
                pixelvalue         key,
                int         *    foundPtr
)
{


    int low  = 0;
    int high = len - 1;

    while (high >= low) {
        int middle = (high + low) / 2;
        if (key > x[middle]) {
            low = middle + 1;
        } else if (key < x[middle]) {
            high = middle - 1;
        } else {
            *foundPtr = 1;
            return (middle);
        }
    }
    *foundPtr = 0;
    return (low);
}

/**
  @name        sinfo_function1d_natural_spline
  @memo        Interpolate a sinfo_vector along new abscissas.
  @param    x        List of x positions.
  @param    y        List of y positions.
  @param    len        Number of samples in x and y.
  @param    splX    Input new list of x positions.
  @param    splY    Output list of interpolated y positions.
  @param    splLen    Number of samples in splX and splY.
  @return    Int 0 if Ok, -1 if error.
  @doc

  Reference:

  \begin{verbatim}
      Numerical Analysis, R. Burden, J. Faires and A. Reynolds.
      Prindle, Weber & Schmidt 1981 pp 112
  \end{verbatim}

  Provide in input a known list of x and y values, and a list where
  you want the signal to be interpolated. The returned signal is
  written into splY.

 */

int
sinfo_function1d_natural_spline(
                pixelvalue    *     x,
                pixelvalue    *     y,
                int             len,
                pixelvalue    *     splX,
                pixelvalue    *     splY,
                int             splLen
)
{
    int             end;
    int             loc,
    found;
    register int     i,
    j,
    n;
    double         *    h; /* sinfo_vector of deltas in x */
    double         *    alpha;
    double         *    l,
    *    mu,
    *    z,
    *    a,
    *    b,
    *    c,
    *    d;

    end = len - 1;

    a = cpl_malloc(sizeof(double) * splLen * 9) ;
    b = a + len;
    c = b + len;
    d = c + len;
    h = d + len;
    l = h + len;
    z = l + len;
    mu = z + len;
    alpha = mu + len;

    for (i = 0; i < len; i++) {
        a[i] = (double)y[i];
    }

    /* Calculate sinfo_vector of differences */
    for (i = 0; i < end; i++) {
        h[i] = (double)x[i + 1] - (double)x[i];
        if (h[i] < 0.0) {
            cpl_free(a) ;
            return -1;
        }
    }

    /* Calculate alpha sinfo_vector */
    for (n = 0, i = 1; i < end; i++, n++) {
        /* n = i - 1 */
        alpha[i] = 3.0 * ((a[i+1] / h[i]) - (a[i] / h[n]) - (a[i] / h[i]) +
                        (a[n] / h[n]));
    }

    /* Vectors to solve the tridiagonal sinfo_matrix */
    l[0] = l[end] = 1.0;
    mu[0] = mu[end] = 0.0;
    z[0] = z[end] = 0.0;
    c[0] = c[end] = 0.0;

    /* Calculate the intermediate results */
    for (n = 0, i = 1; i < end; i++, n++) {
        /* n = i-1 */
        l[i] = 2 * (h[i] + h[n]) - h[n] * mu[n];
        mu[i] = h[i] / l[i];
        z[i] = (alpha[i] - h[n] * z[n]) / l[i];
    }
    for (n = end, j = end - 1; j >= 0; j--, n--) {
        /* n = j + 1 */
        c[j] = z[j] - mu[j] * c[n];
        b[j] = (a[n] - a[j]) / h[j] - h[j] * (c[n] + 2.0 * c[j]) / 3.0;
        d[j] = (c[n] - c[j]) / (3.0 * h[j]);
    }

    /* Now calculate the new values */
    for (j = 0; j < splLen; j++) {
        double v = (double)splX[j];
        splY[j] = (pixelvalue)0;

        /* Is it outside the interval? */
        if ((v < (double)x[0]) || (v > (double)x[end])) {
            continue;
        }
        /* Search for the interval containing v in the x sinfo_vector */
        loc = function1d_search_value(x, len, (pixelvalue)v, &found);
        if (found) {
            splY[j] = y[loc];
        } else {
            loc--;
            v -= (double)x[loc];
            splY[j] = (pixelvalue)(    a[loc] +
                            v * (b[loc] +
                                            v * (c[loc] +
                                                            v * d[loc])));
        }
    }
    cpl_free(a) ;
    return 0;
}
/**@}*/
