/*
 * 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
 */
/***************************************************************************
* E.S.O. - VLT project
*
*
*
* who       when      what
* --------  --------  ----------------------------------------------
* schreib  05/06/00  created
*/

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

#include "sinfo_vltPort.h"

/*
 * System Headers
 */

/*
 * Local Headers
 */

#include "sinfo_recipes.h"
#include "sinfo_globals.h"

/*----------------------------------------------------------------------------
 *                                    Local variables
 *--------------------------------------------------------------------------*/

static float  sqrarg ;

static double chi1 ;                    /* old reduced chi-squared */
static double sinfo_chi2 ;                    /* new reduced chi-squared */
static double labda ;                   /* mixing parameter */
static double vec[MAXPAR] ;             /* correction sinfo_vector */
static double matrix1[MAXPAR][MAXPAR] ; /* original sinfo_matrix */
static double matrix2[MAXPAR][MAXPAR] ; /* inverse of matrix1 */
static int    nfree ;                   /* number of free parameters */
static int    parptr[MAXPAR] ;          /* parameter pointer */

/*----------------------------------------------------------------------------
 *                                 Defines
 *--------------------------------------------------------------------------*/

#define SQR(a) (sqrarg = (a) , sqrarg*sqrarg)

/*----------------------------------------------------------------------------
 *                    Functions private to this module
 *--------------------------------------------------------------------------*/


static int new_inv_mat (void) ;

static void new_get_mat ( float  * xdat,
                     int    * xdim,
                     float  * ydat,
                     float  * wdat,
                     int    * ndat,
                     float  * fpar,
                     float  * epar/*,
                     int    * npar */) ;

static int new_get_vec ( float  * xdat,
                    int    * xdim,
                    float  * ydat,
                    float  * wdat,
                    int    * ndat,
                    float  * fpar,
                    float  * epar,
                    int    * npar ) ;

static float 
new_gaussian ( float * xdat, float * parlist/*, int * npar*/ );
static void 
new_gaussian_deriv( float * xdat, float * parlist, 
                    float * dervs/*, int * npar*/ );



/**@{*/
/**
 * @addtogroup sinfo_recipes Recipe utilities
 *
 * TBD
 */

/*----------------------------------------------------------------------------
 *                            Function codes
 *--------------------------------------------------------------------------*/


float sinfo_new_f_median(float * array, int n)
{
 pixelvalue p_array[100];
 int i;
 
 for (i=0;i<n;i++)
     p_array[i]= (pixelvalue) array[i];

 return (float) sinfo_new_median(p_array, n);
}


/**
@brief his routine computes the clean mean of a given data
   @name sinfo_new_clean_mean()
   @param array: data array to average
   @param n_elements: number of elements of the data array
   @param throwaway_low: percentage of low value elements to be 
                         thrown away before averaging
   @param throwaway_high: percentage of high value elements to be 
                          thrown away before averaging
   @return the clean mean of a data array
                        FLT_MAX in case of error 
   @doc
   this routine computes the clean mean of a given data array that means the 
   array is first sorted and a given percentage of the lowest and the 
   highest values is not considered for averaging
 */
float sinfo_new_clean_mean( float * array, 
                  int     n_elements,
                  float   throwaway_low,
                  float   throwaway_high )
{
    int i, n ;
    int lo_n, hi_n ;
    float sum ;
    
    if ( array == NULL )
    {
        sinfo_msg_error(" no array given in sinfo_clean_mean!") ;
        return FLT_MAX ;
    }
  
    if ( n_elements <= 0 )
    {
        sinfo_msg_error("wrong number of elements given") ;
        return FLT_MAX ;
    }

    if ( throwaway_low < 0. || throwaway_high < 0. ||
         throwaway_low + throwaway_high >= 100. )
    {
        sinfo_msg_error("wrong throw away percentage given!") ;
        return FLT_MAX ;
    }

    lo_n = (int) (throwaway_low * (float)n_elements / 100.) ;
    hi_n = (int) (throwaway_high * (float)n_elements / 100.) ;

    /* sort the array */
    sinfo_pixel_qsort( array, n_elements ) ;

    n = 0 ;
    sum = 0. ;
    for ( i = lo_n ; i < n_elements - hi_n ; i++ )
    {
        if ( !isnan(array[i]) )
        {
            sum += array[i] ;
            n++ ;
        }
    }
    if ( n == 0 )  
    {
        return FLAG ;
    }
    else
    {
        return sum/(float)n ;
    }
}

/*--------------------------------------------------------------------------*/
/**
@brief sorts an array into ascending numerical order
@name sinfo_new_median()
@param array array of pixelvalues starting at index 0,
@param n number of array elements
@return median of pixelvalues
@doc
       sorts an array into ascending numerical order by 
       using sinfo_pixel_qsort(), and derives the sinfo_median depending
       on an odd or even number of array elements 
 */

pixelvalue sinfo_new_median(pixelvalue * array, int n)
{
    pixelvalue med ;
    
    if ( array == NULL || n <= 0 )
    {
        sinfo_msg_warning("nothing in the pixelvalue array, ZERO returned");
        return ZERO ;
    }
    
    if ( n == 1 )
    {
        return array[0] ;
    }
 
    sinfo_pixel_qsort((float*) array, n) ;
    if ( n % 2 == 1 )
    {
        med = array[n/2] ;
    }
    else
    {
        med = (array[n/2] + array[n/2 - 1])/2. ;
    }
    return med ;
}





/**
@brief Least square fit of a function to a set of data points
@name sinfo_new_lsqfit_c()
@param xdat: position, coordinates of data points.
                              xdat is 2 dimensional: XDAT ( XDIM, NDAT )
@param xdim: dimension of fit
@param ydat: data points
@param wdat: weights for data points
@param ndat: number of data points
@param fpar: on input contains initial estimates of the 
                              parameters for non-linear fits, on output the
                              fitted parameters.
@param epar: contains estimates of the errors in fitted parameters
@param mpar: logical mask telling which parameters are free (non-zero)
                              and which parameters are fixed (0)
@param npar: number of function parameters ( free + fixed )
@param tol:  relative tolerance. sinfo_lsqfit stops when successive iterations
             fail to produce a decrement in reduced chi-squared less
             than tol. If tol is less than the minimum tolerance 
             possible, tol will be set to this value. This means
             that maximum accuracy can be obtained by setting
             tol = 0.0.
@param its:  maximum number of iterations
@param lab:  mixing parameter, lab determines the initial weight
             of steepest descent method relative to the Taylor method
             lab should be a small value (i.e. 0.01). lab can only
             be zero when the partial derivatives are independent
             of the parameters. In fact in this case lab should be
             exactly equal to zero.
 @return returns number of iterations needed to achieve convergence
         according to tol. When this number is negative, the fitting
         was not continued because a fatal error occurred:
#        -1 too many free parameters, maximum is 32
#        -2 no free parameters
#        -3 not enough degrees of freedom
#        -4 maximum number of iterations too small to obtain
#        a solution which satisfies tol.
#        -5 diagonal of sinfo_matrix contains elements which are zero
#        -6 determinant of the coefficient sinfo_matrix is zero
#        -7 square root of a negative number 
@doc     this is a routine for making a least-squares fit of a
         function to a set of data points. The method used is
         described in: Marquardt, J.Soc.Ind.Appl.Math. 11. 431 (1963).
         This method is a mixture of the steepest descent method 
         and the Taylor method.
 */

int sinfo_new_lsqfit_c ( float * xdat,
               int   * xdim,
               float * ydat,
               float * wdat,
               int   * ndat,
               float * fpar,
               float * epar,
               int   * mpar,
               int   * npar,
               float * tol ,
               int   * its ,
               float * lab  )
{
    int i, n, r ;
    int itc ;                      /* fate of fit */
    int found ;                    /* fit converged: 1, not yet converged: 0 */
    int  nuse ;                    /* number of useable data points */
    double tolerance ;             /* accuracy */

    itc   = 0 ;                    /* fate of fit */
    found = 0 ;                    /* reset */
    nfree = 0 ;                    /* number of free parameters */
    nuse  = 0 ;                    /* number of legal data points */

    if ( *tol < (FLT_EPSILON * 10.0 ) )
    {
        tolerance = FLT_EPSILON * 10.0 ;  /* default tolerance */
    }
    else
    {
        tolerance = *tol ;                /* tolerance */
    }
    
    labda = fabs( *lab ) * LABFAC ;   /* start value for mixing parameter */
    for ( i = 0 ; i < (*npar) ; i++ )
    {
        if ( mpar[i] )
        {
            if ( nfree > MAXPAR )         /* too many free parameters */
            {
                return -1 ;
            }
            parptr[nfree++] = i ;         /* a free parameter */
        }
    }
    
    if (nfree == 0)                       /* no free parameters */     
    {
        return -2 ;
    }
    
    for ( n = 0 ; n < (*ndat) ; n++ )
    {
        if ( wdat[n] > 0.0 )              /* legal weight */
        {
            nuse ++ ;
        }
    }
    
    if ( nfree >= nuse )
    {
        return -3 ;                       /* no degrees of freedom */
    }
    if ( labda == 0.0 )                   /* linear fit */
    {
        /* initialize fpar array */
        for ( i = 0 ; i < nfree ; fpar[parptr[i++]] = 0.0 ) ;  
        new_get_mat ( xdat, xdim, ydat, wdat, ndat, fpar, epar/*, npar*/ ) ;
        r =  new_get_vec ( xdat, xdim, ydat, wdat, ndat, fpar, epar, npar ) ;
        if ( r )                         /* error */
        {
            return r ;
        }
        for ( i = 0 ; i < (*npar) ; i++ )
        {
            fpar[i] = epar[i] ;           /* save new parameters */
            epar[i] = 0.0 ;               /* and set errors to zero */
        }
        chi1 = sqrt( chi1 / (double) (nuse - nfree) ) ;
        for ( i = 0 ; i < nfree ; i++ )
        {
            if ( (matrix1[i][i] <= 0.0 ) || (matrix2[i][i] <= 0.0) )
            {
                return -7 ;
            }
            epar[parptr[i]] = chi1 * sqrt( matrix2[i][i] ) / 
                                     sqrt( matrix1[i][i] ) ;
        }
    }
    else                                  /* non-linear fit */
    {
        /*----------------------------------------------------------------
         * the non-linear fit uses the steepest descent method in combination
         * with the Taylor method. The mixing of these methods is controlled
         * by labda. In the outer loop ( called the iteration loop ) we build
         * the matrix and calculate the correction vector. In the inner loop
         * (called the interpolation loop) we check whether we have obtained a
         * better solution than the previous one. If so, we leave the inner 
           loop else we increase labda (give more weight to the steepest 
           descent method) calculate the correction vector and check again. 
           After the inner loop we do a final check on the goodness of the 
           fit and if this satisfies
         * the tolerance we calculate the errors of the fitted parameters.
         */
        while ( !found )                  /* iteration loop */
        {      
            if ( itc++ == (*its) )        /* increase iteration counter */
            {
                return -4 ;               
            }
            new_get_mat( xdat, xdim, ydat, wdat, ndat, fpar, epar/*, npar */) ;
            
            /*-------------------------------------------------------------
             * here we decrease labda since we may assume that each iteration
             * brings us closer to the answer.
             */
            if ( labda > LABMIN )
            {
                labda = labda / LABFAC ;         /* decrease labda */
            }
            r = new_get_vec( xdat, xdim, ydat, wdat, ndat, fpar, epar, npar ) ;
            if ( r )                      /* error */
            {
                return r ;
            }

            while ( chi1 >= sinfo_chi2 )        /* interpolation loop */
            {
                /*-----------------------------------------------------------
                 * The next statement is based on experience, not on the 
                   mathematics of the problem. It is assumed that we have 
                   reached convergence when the pure steepest descent method 
                   does not produce a better solution.
                 */
                if ( labda > LABMAX )    /* assume solution found */
                {
                    break ;
                }
                labda = labda * LABFAC ;     /* increase mixing parameter */
                r = new_get_vec(xdat,xdim,ydat,wdat,ndat,fpar,epar,npar) ;
                if ( r )                  /* error */
                {
                    return r ;
                }
            }

            if ( labda <= LABMAX )        /* save old parameters */
            {
                for ( i = 0 ; i < *npar ; i++ )
                {
                    fpar[i] = epar[i] ;
                }
            }
            if ( (fabs( sinfo_chi2 - chi1 ) <= (tolerance * chi1)) || 
                 (labda > LABMAX) )
            {
                /*-------------------------------------------------------------
                 * we have a satisfying solution, so now we need to calculate 
                   the correct errors of the fitted parameters. This we do by 
                   using the pure Taylor method because we are very close to 
                   the real solution.
                 */
                labda = 0.0 ;              /* for Taylor solution */
                new_get_mat(xdat,xdim,ydat,wdat,ndat,fpar,epar/*, npar */) ;
                r = new_get_vec(xdat,xdim,ydat,wdat,ndat,fpar,epar,npar) ;

                if ( r )                    /* error */
                {
                    return r ;
                }
                for ( i = 0 ; i < (*npar) ; i++ )
                {
                    epar[i] = 0.0 ;          /* set error to zero */
                }
                sinfo_chi2 = sqrt ( sinfo_chi2 / (double) (nuse - nfree) ) ;

                for ( i = 0 ; i < nfree ; i++ )
                {
                    if ( (matrix1[i][i] <= 0.0) || (matrix2[i][i] <= 0.0) )
                    {
                        return -7 ;
                    }
                    epar[parptr[i]] = sinfo_chi2 * sqrt( matrix2[i][i] ) / 
                                                   sqrt( matrix1[i][i] ) ;
                }
                found = 1 ;                  /* we found a solution */
            }
        }
    }
    return itc ;                             /* return number of iterations */
}



/**
@brief this function converts any ZEROs in an image to 0
  @name sinfo_new_convert_ZEROs_to_0_for_images()
  @param im input image
  @return nothing
 */
void sinfo_new_convert_ZEROs_to_0_for_images(cpl_image * im)
{
    int i ;
    int ilx=0;
    int ily=0;
    float* pidata=NULL;

    if ( im == NULL )
    {
        sinfo_msg_error("no input image given!\n") ; 
        return ;
    }
    ilx=cpl_image_get_size_x(im);
    ily=cpl_image_get_size_y(im);
    pidata=cpl_image_get_data(im);
    for ( i = 0 ; i < (int) ilx*ily ; i++ )
    {
        if( isnan(pidata[i]) )
        {
            pidata[i] = 0. ;
        }
    }
    return ;
}

/**
@brief this function converts any ZEROs in a cube to 0
 @name sinfo_new_convert_ZEROs_to_0_for_cubes()
 @param cube input cube
 @param z_min
 @param z_max
 @param nothing 
*/
void sinfo_new_convert_ZEROs_to_0_for_cubes(cpl_imagelist * cube)
{
    int i ;
    int inp=0;
    cpl_image* i_img=NULL;  

    if ( cube == NULL )
    {
        sinfo_msg_error("no input cube given!") ; 
        return ;
    }
    inp=cpl_imagelist_get_size(cube);

    for ( i = 0 ; i < inp ; i++ )
    {
      i_img=cpl_imagelist_get(cube,i);
      sinfo_new_convert_ZEROs_to_0_for_images(i_img) ;
      cpl_imagelist_set(cube,i_img,i);
    }
    return ;
}


/**
@brief this function converts any ZEROs in a cube to 0
  @name sinfo_new_convert_ZEROs_to_0_for_cubes_range()
  @param cube input cube
  @param z_min 
  @param z_max
  @return nothing 
 */
void 
sinfo_new_convert_ZEROs_to_0_for_cubes_range(cpl_imagelist * cube,
                                             const int z_min,const int z_max)
{
    int i ;
    cpl_image* i_img=NULL;  

    if ( cube == NULL )
    {
        sinfo_msg_error("no input cube given!") ; 
        return ;
    }
    for ( i = z_min ; i < z_max ; i++ )
    {
      i_img=cpl_imagelist_get(cube,i);
      sinfo_new_convert_ZEROs_to_0_for_images(i_img) ;
      cpl_imagelist_set(cube,i_img,i);
    }
    return ;
}
/**
@brief this function converts any 0 in an image to ZEROs
 @name sinfo_new_convert_0_to_ZEROs_for_images()
 @param im input image
 @return nothing 
 */
void sinfo_new_convert_0_to_ZEROs_for_images(cpl_image * im)
{
    int i ;
    int ilx=0;
    int ily=0;
    float* pidata=NULL;

    if ( im == NULL )
    {
        sinfo_msg_error("no input image given!") ;
        return ;
    }
    ilx=cpl_image_get_size_x(im);
    ily=cpl_image_get_size_y(im);
    pidata=cpl_image_get_data(im);
    for ( i = 0 ; i < (int) ilx*ily ; i++ )
    {
        if( pidata[i] == 0. )
        {
            pidata[i] = ZERO ;
        }
    }
    return ;
}

/**
@brief this function converts any 0 in a cube to ZERO
 @name new_convert_0_to_ZEROs_for_cubes()
 @param cube input cube
 @return nothing 
 */
void sinfo_new_convert_0_to_ZERO_for_cubes(cpl_imagelist * cube)
{
    int i ;
    int inp=0;
    cpl_image* i_img=NULL;

    if ( cube == NULL )
    {
        sinfo_msg_error("no input cube given!") ;
        return ;
    }
    inp=cpl_imagelist_get_size(cube);
    for ( i = 0 ; i < inp ; i++ )
    {
      i_img=cpl_imagelist_get(cube,i);
      sinfo_new_convert_0_to_ZEROs_for_images(i_img) ;
      cpl_imagelist_set(cube,i_img,i);
    }
    return ;
}


/**
@brief his function converts any 0 in a cube to ZERO
   @name new_convert_0_to_ZEROs_for_cubes()
   @param cube input cube
   @param z_min  min z to be considered
   @param z_max  max z to be considered
   @return nothing
 */
void 
sinfo_new_convert_0_to_ZERO_for_cubes_range(cpl_imagelist * cube,
                                            const int z_min,const int z_max)
{
    int i ;
    int inp=0;
    cpl_image* i_img=NULL;

    if ( cube == NULL )
    {
        sinfo_msg_error("no input cube given!") ;
        return ;
    }
    inp=cpl_imagelist_get_size(cube);
    for ( i = z_min ; i < z_max ; i++ )
    {
      i_img=cpl_imagelist_get(cube,i);
      sinfo_new_convert_0_to_ZEROs_for_images(i_img) ;
      cpl_imagelist_set(cube,i_img,i);
    }
    return ;
}
/**
@brief this function multiplies each image value by -1
@name new_invert_image()
 @param im input image
 @return inverted input image (overwritten)
 */
void sinfo_new_invert(cpl_image * im)
{
    int i ;
    int ilx=0;
    int ily=0;
    float* pidata=NULL;

    ilx=cpl_image_get_size_x(im);
    ily=cpl_image_get_size_y(im);
    pidata=cpl_image_get_data(im);

    for ( i = 0 ; i < (int) ilx*ily ; i++ )
    {
        pidata[i] = -pidata[i] ;
    }
    return ;
}

/**
@brief  this routine determines the nearest integer to a specified real value
   @name sinfo_new_nint()
   @param x : specified real value
   @return nearest integer to specified real value 
 */

int sinfo_new_nint ( double x ) 
{
    int k ;

    k = x ;
    if ( x >= 0. )
    {
        if ( (x - (double) k) <= 0.5 )
        {
            return k ;
        }
        else
        {
            return k + 1 ;
        }
    }
    else
    {
        if ( (x - (double) k) <= -0.5 )
        {
            return k - 1;
        }
        else
        {
            return k ;
        }
    }
}


/**
   @brief simple cross-correlation of two 1d-signals without FFT
   @name sinfo_new_xcorrel ()
   @param line_i:      the reference signal
   @param width_i:     number of samples in reference signal
   @param line_t:      candidate signal to compare
   @param width_t:     number of samples in candidate signal
   @param half_search: half-size of the search domain 
   @param delta:       output sinfo_correlation offset.
   @param maxpos:      position index of the maximum of the output array
   @param xcorr_max:   maximum value of the output array
   @return correlation function, must be freed.
 */
#define STEP_MIN        (-half_search)
#define STEP_MAX        (half_search)

double * sinfo_new_xcorrel(
    float      *    line_i,
    int             width_i,
    float      *    line_t,
    int             width_t,
    int             half_search,
    int     *       delta,
    int       *     maxpos,
    double     *    xcorr_max 
    
)
{
    double  * xcorr ;
    double   mean_i, mean_t ;
    double   rms_i, rms_t ;
    double   sum, sqsum ;
    double   norm ;
    int      nsteps ;
    int      i ;
    int      step ;
    int      nval ;
    /*double   r;*/


    /* Compute normalization factors */
    sum = sqsum = 0.00 ;
    for (i=0 ; i<width_i ; i++) {
        sum += (double)line_i[i] ;
        sqsum += (double)line_i[i] * (double)line_i[i];
    }
    mean_i = sum / (double)width_i ;
    sqsum /= (double)width_i ;
    rms_i = sqsum - mean_i*mean_i ;

    sum = sqsum = 0.00 ;
    for (i=0 ; i<width_t ; i++) {
        sum += (double)line_t[i] ;
        sqsum += (double)line_t[i] * (double)line_t[i];
    }
    mean_t = sum / (double)width_t ;
    sqsum /= (double)width_t ;
    rms_t = sqsum - mean_t*mean_t ;

    norm = 1.00 / sqrt(rms_i * rms_t);

    nsteps = (STEP_MAX - STEP_MIN)  ;
    xcorr = cpl_malloc(nsteps * sizeof(double));
    for (step=STEP_MIN ; step<STEP_MAX ; step++) {
        xcorr[step-STEP_MIN] = 0.00 ;
        nval = 0 ;
        for (i=0 ; i<width_t ; i++) {
            if ((i+step >= 0) &&
                (i+step < width_i)) {
            xcorr[step-STEP_MIN] += ((double)line_t[i] - mean_t) *
                                    ((double)line_i[i+step] - mean_i) *
                                    norm ;
                nval++ ;
            }
        }
        xcorr[step-STEP_MIN] /= (double)nval ;
    }
    *xcorr_max = xcorr[0] ;
    *maxpos    = 0 ;
    for (i=0 ; i<nsteps ; i++) {
        if (xcorr[i]>*xcorr_max) {
            *maxpos = i ;
            *xcorr_max = xcorr[i];
        }
    }
    (*delta) = + (STEP_MIN + *maxpos);
    return xcorr ;
}

/* FILE ELEMENT: sinfo_nev_ille.c                                           */
/*                                                                    */
/**********************************************************************/
/*                                                                    */
/*                      double sinfo_nev_ille()                             */
/*                                                                    */
/**********************************************************************/
/*                                                                    */
/*  DESCRIPTION:                                                      */
/*  For a given table (x , f(x )), i = 0(1)n  and a given argument z  */
/*  the function computes the interpolated value for the argument z   */
/*  using Neville's interpolation/ extrapolation algorithm.           */
/*                                                                    */
/*  FUNCTIONS CALLED:                                                 */
/*  System library: <stdio.h> printf(), fabs();                       */
/*  Numlib library: None                                              */
/*  Local functions: nevtable();                                      */
/*  User supplied: None                                               */
/*                                                                    */
/*  PROGRAMMED BY: T.Haavie                                           */
/*  DATE/VERSION: 88-07-06/1.0                                        */
/*                                                                    */
/**********************************************************************/
double sinfo_nev_ille(double x[], double f[], int n, double z, int* flag)
               /* PARAMETERS(input):                                  */
/* double x[];     Abscissae values in the table.                      */
/* double f[];     Function values in the table.                       */
/* int n;         The number of elements in the table is n+1.         */
/* double z;       Argument to be used in interpolation/extrapolation. */


/*                PARAMETERS(input/output):                           */
/* int *flag;    Flag parameter(output):                             */
               /* = 0, n < 0 and/or eps < 0, should be positive+.     */
               /* = 1, required rel.err. is not obtained.             */
               /* = 2, required rel. err. is obtained.                */

/* the computed estimate for the interpolated/extrapolated  value re- */
/* turned through function name sinfo_nev_ille.                             */

{
        double p[11]; /* Array used for storing the new row elements */
                       /* in the interpolation/extrapolation table.   */
        double q[11]; /* Array used for storing the old row elements */
                       /* in the interpolation/extrapolation table    */

        double factor;
        int m, k;

       

        if (n < 0 )
        {
                *flag = 0;
                return(0.);
        }


        q[0] = f[0];               /* Set initial value in the table. */

        for (k = 1; k <= n; k++)   /* k counts rows in the table.     */
        {
                p[0] = f[k];
                for (m = 1; m <= k; m++) /* m counts element in row.  */
                {
                        factor = (z - x[k]) / (x[k] - x[k-m]);
                        p[m] = p[m-1] + factor * (p[m-1] - q[m-1]);
                }


                for (m = 0; m <= k; m++) /* Shift old row to new row.  */
                        q[m] = p[m];

        } /* End of k-loop. */

        *flag = 1;              /* Required rel.error is not obtained. */
        return(p[n]);

} /* End of sinfo_nev_ille(). */



float sinfo_new_nev_ille(float x[], float f[], int n, float z, int* flag)
               /* PARAMETERS(input):                                  */
/* float x[];     Abscissae values in the table.                      */
/* float f[];     Function values in the table.                       */
/* int n;         The number of elements in the table is n+1.         */
/* float z;       Argument to be used in interpolation/extrapolation. */


/*                PARAMETERS(input/output):                           */
/* int *flag;    Flag parameter(output):                             */
               /* = 0, n < 0 and/or eps < 0, should be positive+.     */
               /* = 1, required rel.err. is not obtained.             */
               /* = 2, required rel. err. is obtained.                */

/* the computed estimate for the interpolated/extrapolated  value re- */
/* turned through function name sinfo_nev_ille.                             */

{
        float p[11]; /* Array used for storing the new row elements */
                       /* in the interpolation/extrapolation table.   */
        float q[11]; /* Array used for storing the old row elements */
                       /* in the interpolation/extrapolation table    */

        float factor;
        int m, k;

       

        if (n < 0 )
        {
                *flag = 0;
                return(0.);
        }


        q[0] = f[0];               /* Set initial value in the table. */

        for (k = 1; k <= n; k++)   /* k counts rows in the table.     */
        {
                p[0] = f[k];
                for (m = 1; m <= k; m++) /* m counts element in row.  */
                {
                        factor = (z - x[k]) / (x[k] - x[k-m]);
                        p[m] = p[m-1] + factor * (p[m-1] - q[m-1]);
                }


                for (m = 0; m <= k; m++) /* Shift old row to new row.  */
                        q[m] = p[m];

        } /* End of k-loop. */

        *flag = 1;              /* Required rel.error is not obtained. */
        return(p[n]);

} /* End of sinfo_nev_ille(). */


/**
@brief calculates the correction vector.
   @name new_get_vec()
   @param xdat: position
   @param xdim: factor of the indices of the position array
   @param ydat: real data
   @param wdat: weights
   @param ndat: number of data points
   @param fpar: function parameters
   @param epar: partial derivatives of the function
   @param npar: number of function parameters
   @return  integer (
            #  0         if it had worked, 
            # -5 or -7   if diagonal element is wrong, or 
            # -6,        if determinant is zero )
   @doc
        The matrix has been
        built by getmat(), we only have to rescale it for the 
        current value of labda. The sinfo_matrix is rescaled so that
        the diagonal gets the value 1 + labda.
        Next we calculate the inverse of the sinfo_matrix and then
        the correction vector.
 */
            
static int new_get_vec ( float * xdat,
                    int   * xdim,
                    float * ydat,
                    float * wdat,
                    int   * ndat,
                    float * fpar,
                    float * epar,
                    int   * npar )
{
    double dj ;
    double dy ;
    double mii ;
    double mji ;
    double mjj ;
    double wn ;
    int i, j, n, r ;

    /* loop to modify and scale the sinfo_matrix */
    for ( j = 0 ; j < nfree ; j++ )
    {
        mjj = matrix1[j][j] ;
        if ( mjj <= 0.0 )             /* diagonal element wrong */
        {
            return -5 ;
        }
        mjj = sqrt( mjj ) ;
        for ( i = 0 ; i < j ; i++ )
        {
            mji = matrix1[j][i] / mjj / sqrt( matrix1[i][i] ) ;
            matrix2[i][j] = matrix2[j][i] = mji ;
        }
        matrix2[j][j] = 1.0 + labda ;  /* scaled value on diagonal */
    }    
    
    if ( (r = new_inv_mat()) )       /* sinfo_invert sinfo_matrix inlace */
    {
        return r ;
    }
    
    for ( i = 0 ; i < (*npar) ; i ++ )
    {
        epar[i] = fpar[i] ;
    }
    
    /* loop to calculate correction sinfo_vector */
    for ( j = 0 ; j < nfree ; j++ )
    {
        dj = 0.0 ;
        mjj = matrix1[j][j] ;
        if ( mjj <= 0.0)               /* not allowed */
        {
            return -7 ;
        }
        mjj = sqrt ( mjj ) ;
        for ( i = 0 ; i < nfree ; i++ )
        {
            mii = matrix1[i][i] ;
            if ( mii <= 0.0 )
            {
                return -7 ;
            }
            mii = sqrt( mii ) ;
            dj += vec[i] * matrix2[j][i] / mjj / mii ;
        }
        epar[parptr[j]] += dj ;       /* new parameters */
    }    
    chi1 = 0.0 ;                      /* reset reduced chi-squared */
 
    /* loop through the data points */
    for ( n = 0 ; n < (*ndat) ; n++ )
    {
        wn = wdat[n] ;               /* get weight */
        if ( wn > 0.0 )              /* legal weight */
        {
            dy = ydat[n] - new_gaussian( &xdat[(*xdim) * n], epar/*, npar*/ ) ;
            chi1 += wdat[n] * dy * dy ;
        }
    }
    return 0 ;
}   
    

/**
@brief builds the matrix 
   @name  new_get_mat()
   @param xdat: position
   @param xdim: factor of the indices of the position array
   @param ydat: real data
   @param wdat: weights
   @param ndat: number of data points
   @param fpar: function parameters
   @param epar: partial derivatives of the function
   @param npar: number of function parameters
   @return void
 
 */

static void new_get_mat ( float * xdat,
                     int   * xdim,
                     float * ydat,
                     float * wdat,
                     int   * ndat,
                     float * fpar,
                     float * epar/*,
                     int   * npar */)
{
    double wd ;
    double wn ;
    double yd ;
    int i, j, n ;

    for ( j = 0 ; j < nfree ; j++ )
    {
        vec[j] = 0.0 ; /* zero sinfo_vector */
        for ( i = 0 ; i<= j ; i++ )   
        /* zero sinfo_matrix only on and below diagonal */
        {
            matrix1[j][i] = 0.0 ;
        }
    }
    sinfo_chi2 = 0.0 ;  /* reset reduced chi-squared */
    
    /* loop through data points */
    for ( n = 0 ; n < (*ndat) ; n++ )
    {
        wn = wdat[n] ;
        if ( wn > 0.0 )  /* legal weight ? */
        {
            yd = ydat[n] - new_gaussian( &xdat[(*xdim) * n], fpar/*, npar*/ ) ;
            new_gaussian_deriv( &xdat[(*xdim) * n], fpar, epar/*, npar*/ ) ;
            sinfo_chi2 += yd * yd * wn ; /* add to chi-squared */
            for ( j = 0 ; j < nfree ; j++ )
            {
                wd = epar[parptr[j]] * wn ;  /* weighted derivative */
                vec[j] += yd * wd ;       /* fill sinfo_vector */
                for ( i = 0 ; i <= j ; i++ ) /* fill sinfo_matrix */
                {
                    matrix1[j][i] += epar[parptr[i]] * wd ;
                }
            }
        }
    }                   
}  
   




 
/**
@brief calculates the inverse of matrix2.
   @name new_inv_mat()
   @return integer (0 if it worked, -6 if determinant is zero) 
   @see The algorithm used is the Gauss-Jordan algorithm described in Stoer,
        Numerische Mathematik, 1. Teil.
 */

static int new_inv_mat (void)
{
    double even ;
    double hv[MAXPAR] ;
    double mjk ;
    double rowmax ;
    int evin ;
    int i, j, k, row ;
    int per[MAXPAR] ;
   
    /* set permutation array */
    for ( i = 0 ; i < nfree ; i++ )
    {
        per[i] = i ;
    }
    
    for ( j = 0 ; j < nfree ; j++ ) /* in j-th column */
    {
        /* determine largest element of a row */                               
        rowmax = fabs ( matrix2[j][j] ) ;
        row = j ;                         

        for ( i = j + 1 ; i < nfree ; i++ )
        {
            if ( fabs ( matrix2[i][j] ) > rowmax )
            {
                rowmax = fabs( matrix2[i][j] ) ;
                row = i ;
            }
        }

        /* determinant is zero! */
        if ( matrix2[row][j] == 0.0 )
        {
            return -6 ;
        }
        
        /*if the largest element is not on the diagonal, then permutate rows */
        if ( row > j )
        {
            for ( k = 0 ; k < nfree ; k++ )
            {
                even = matrix2[j][k] ;
                matrix2[j][k] = matrix2[row][k] ;
                matrix2[row][k] = even ;
            }
            /* keep track of permutation */
            evin = per[j] ;
            per[j] = per[row] ;
            per[row] = evin ;
        }
        
        /* modify column */
        even = 1.0 / matrix2[j][j] ;
        for ( i = 0 ; i < nfree ; i++ )
        {
            matrix2[i][j] *= even ;
        }
        matrix2[j][j] = even ;
        
        for ( k = 0 ; k < j ; k++ )
        {
            mjk = matrix2[j][k] ;
            for ( i = 0 ; i < j ; i++ )
            {
                matrix2[i][k] -= matrix2[i][j] * mjk ;
            }
            for ( i = j + 1 ; i < nfree ; i++ )
            {
                matrix2[i][k] -= matrix2[i][j] * mjk ;
            }
            matrix2[j][k] = -even * mjk ;
        }
    
        for ( k = j + 1 ; k < nfree ; k++ )
        {
            mjk = matrix2[j][k] ;
            for ( i = 0 ; i < j ; i++ )
            {
                matrix2[i][k]  -= matrix2[i][j] * mjk ;
            }
            for ( i = j + 1 ; i < nfree ; i++ )
            {
                matrix2[i][k]  -= matrix2[i][j] * mjk ;
            }
            matrix2[j][k] = -even * mjk ;
        }
    }
    
    /* finally, repermute the columns */
    for ( i = 0 ; i < nfree ; i++ )
    {
        for ( k = 0 ; k < nfree ; k++ )
        {
            hv[per[k]] = matrix2[i][k] ;
        }
        for ( k = 0 ; k < nfree ; k++ )
        {
            matrix2[i][k] = hv[k] ;
        }
    }
        
    /* all is well */
    return 0 ;
}       




 /**
@brief  calculates the value of a Gaussian with parameters 
        parlist at the position xdat 
   @name new_gaussian()
   @param xdat position array, 
   @param parlist parameter list,

@doc
   The parameters are:
   #                     parlist(0): Amplitude
   #                     parlist(1): FWHM
   #                     parlist(2): x0, center of gauss versus 
                                         center of column
   #                     parlist(3): Zero level
 @return function value of a 1-d sinfo_gaussian:
                        F(x) = par(0) * EXP ( -4.0 * LN(2.0) *
                               [((x - parlist(2))/parlist(1))^2]) + parlist(3)
                        FWHM^2 = 8 * ln (2) * sigma^2 
 */

float new_gaussian ( float * xdat, float * parlist/*, int * npar*/ )
{
    double  xd ;  /* FWHM's of gauss function */
    double   x ;  /* position */

    xd = fabs((double) parlist[1]) ;
    x  = (double) xdat[0] - (double) parlist[2] ;
    return (float) (
           (double) parlist[0] * exp( -4.0 * log(2.0) * (x/xd) * (x/xd) )
           + (double) parlist[3] ) ;
}
      
       
/**
@brief calculates the partial derivatives for a Gaussian with 
       parameters parlist at position xdat 
   @name new_gaussian_deriv()
   @param xdat position array, 
   @param parlist parameter list, 
   @param dervs derivatives of params

@doc 
                        The parameters are:
   #                     parlist(0): Amplitude
   #                     parlist(1): Axis(FWHM)
   #                     parlist(2): x0, center of gauss versus center of column
   #                     parlist(3): Zero level
                          derivative value of Gaussian at position xdat: dervs
   #                     dervs[0]: partial derivative by the amplitude
   #                     dervs[1]: partial derivative by FWHM 
   #                     dervs[2]: partial derivative by the x position 
                                   (parlist[2])
   #                     dervs[3]: partial derivative by the zero level
   @return nothing, void
 
 */

void 
new_gaussian_deriv(float * xdat,float * parlist,float * dervs/*, int * npar*/ )
{
    double xd ; /* FWHM of sinfo_gaussian */
    double x, expon ; /* position and exponent */

    xd = fabs( (double) parlist[1] ) ;
    
    /* offset from peak position */
    x = (double) xdat[0] - (double) parlist[2] ;

    /* determine the derivatives: */
    expon = -4.0 * log(2.0) * (x/xd) * (x/xd) ;
    expon = exp( expon ) ;

    /* partial derivative by the amplitude */
    dervs[0] = (float) expon ;

    /* calculate a * exp(-arg) */
    expon = (double) parlist[0] * expon ;

    /* partial derivative by FWHM */
    dervs[1] = (float) ( expon * 8.0 * log(2.0) * x*x / (xd*xd*xd) ) ;

    /* partial derivative by the x position (parlist[2]) */
    dervs[2] = (float) (expon * 8.0 * log(2.0) * x/(xd*xd) ) ;

    /* partial derivative by the zero level */
    dervs[3] = 1.0 ;
}


/*==================================================================*/


/**
@brief fits a straight line to a set of x, y data points by 
       minimizing chi-square.
  @name sinfo_myfit()
  @param x 
  @param y set of data points, 
  @param ndata number of data points 
  @param sig individual standard deviations,
  @param mwt
  @param a
  @param b
  @param siga
  @param sigb
  @param sinfo_chi2  chi square
  @param q     goodness of fit probability
  @return void
  @see Numeric. Rec. page 665 
 */

void 
sinfo_my_fit(float x[], float y[], int ndata, float sig[], int mwt, float *a, 
           float *b, float *siga, float *sigb, float *chi2, float *q)
{
    int i ;
    float wt, t, sxoss, sx=0., sy=0., st2=0., ss, sigdat ;

    *b = 0. ;             /*accumulate sums ...*/
    if ( mwt )
    {
        ss = 0. ;
        for ( i = 0 ; i < ndata ; i++ )  /*... with weights*/
        {
            wt = 1./SQR(sig[i]) ;
            ss += wt ;
            sx += x[i]*wt ;
            sy += y[i]*wt ;
        }
    }
    else
    {
        for ( i = 0 ; i < ndata ; i++ ) /*... or without weights*/
        {
             sx += x[i] ;
             sy += y[i] ;
        }
        ss = ndata ;
    }
    sxoss = sx/ss ;
             
    if ( mwt )
    {
        for ( i = 0 ; i < ndata ; i ++ )
        {
            t = (x[i] - sxoss)/sig[i] ;
            st2 += t*t ;
            *b += t*y[i]/sig[i] ;
        }
    }
    else
    {
        for ( i = 0 ; i < ndata ; i++ )
        {
            t = x[i] - sxoss ;
            st2 += t*t ;
            *b += t*y[i] ;           
        }
    }

    *b /= st2 ;
    *a = (sy - sx*(*b))/ss ;
    *siga = sqrt ((1.0 + sx*sx/(ss*st2))/ss) ;
    *sigb = sqrt (1.0/st2) ;
    *chi2 = 0.0 ;  /*calculate chi-square*/
    if ( mwt == 0 )
    {
        for ( i = 0 ; i < ndata ; i++ )
        {
            *chi2 += SQR (y[i] - (*a) - (*b)*x[i]) ;
        }
        *q = 1. ;
        
        /*------------------------------------------------------------------
         * for unweighted data evaluate typical sig using chi2, and adjust
         * the standard deviation
         */
        sigdat = sqrt ((*chi2)/(ndata - 2)) ;
        *siga *= sigdat ;
        *sigb *= sigdat ;
    }
    else
    {
        for (i = 0 ; i < ndata ; i++)
        {
            *chi2 += SQR ((y[i] - (*a) - (*b) * x[i])/sig[i]) ;
        }    
        *q = 1. ; /* delete rest of lines. q is not a good value */
    }
}

/**
@brief this routine computes the cross correlation between two data
       arrays of the same size and returns the integer shift between 
       both arrays
@name sinfo_new_correlation()
@param data1: first data array
@param data2: second data array 
@param ndata: number of data elements in the arrays,
@note both arrays must have the same number of elements.
@return integer shift of the second array with respect to the first array. 
                        2^32/2 if something went wrong.

 */

int sinfo_new_correlation ( float * data1, float * data2, int ndata )
{
    /*float help[3*ndata] ; 
    float corsum[3*ndata] ;*/
    float* help=NULL ; 
    float* corsum=NULL ;
    float maxval ;
    int i, j, k, position, shift ;
    int /*start,end,size,*/ndata3,limit;
    
    
    /*ndata3=3*ndata;*/
    ndata3=ndata+300;

    if ( NULL == data1 || NULL == data2 || ndata <= 1 )
    {
        sinfo_msg_error(" wrong input for sinfo_correlation\n") ;
        return INT32_MAX ;
    }

    /* initialize the help arrays with zeros */
    help=cpl_calloc(ndata+300,sizeof(float));
    for ( i = 0 ; i < ndata3 ; i++ )
    {
        help[i] = 0. ;
    }

    /* shift the second data array by ndata in the help array */
    for ( i = 0 ; i < ndata ; i++ )
    {
        help[(300/2) + i] = data2[i] ;
    }

    /* compute the cross sinfo_correlation sum array */
    corsum=cpl_calloc(ndata+300,sizeof(float));
    for ( j = 0 ; j < ndata3 ; j++ )
    {
        if ( ndata3-j <= ndata) 
        limit = ndata3-j;
    else
        limit = ndata;
        corsum[j] = 0. ;
        for ( k = 0 ; k < limit ; k++ )
        {
            /*if ( k + j >= ndata3 )
            {
                break ;
            }*/
            corsum[j] += data1[k] * help[k + j] ;
        }
    }

    /* search for the maximal corsum value and determine its position */
    maxval = -FLT_MAX ;
    position = -1 ;
    for ( i = 0 ; i < ndata3 ; i++ )
    {
        if ( maxval < corsum[i] )
        {
            maxval = corsum[i] ;
            position = i ;
        }
    }
    
    /* determine shift of data2 relative to the data1 array */
    shift = position - 300/2 ;
    cpl_free(help);
    cpl_free(corsum);
 
    return shift ;
}

/*--------------------------------------------------------------------------*/
/**@}*/
