/*
 * 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
 */
#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
#include <sinfo_fit.h>
#include <sinfo_msg.h>
#include <sinfo_utils_wrappers.h>
#include <sinfo_error.h>
#include <stdio.h>
#include <math.h>
#include <cpl.h>
/**@{*/
/**
 * @defgroup sinfo_fit Fit functions
 *
 * TBD
 */

/* function prototypes */
/*
  static int
  sinfo_stat_rectangle(cpl_image* img,
  const int kappa,
  const int nclip,
  double *mean,
  double *stdev);
 */
static void
sinfo_fit_amoeba_get_psum(int ndim, int mpts, double** p, double* psum);


static  double
sinfo_fit_amotry(double** p,
                 double y[],
                 double psum[],
                 int ndim,
                 double (*funk)(double[]),
                 int ihi,
                 double fac);



static double
sinfo_spline(double x, double cons[], double ak[], double *sp, double *spp,
             double *sppp, int n);

static double
sinfo_get_chisq(int N, int D,
          int (*f)(const double x[], const double a[], double *result),
          const double *a,
          const double *x,
          const double *y,
          const double *sigma);

static int sinfo_get_candidate(const double *a, const int ia[],
                         int M, int N, int D,
                         double lambda,
                         int    (*f)(const double x[], const double a[],
                                         double *result),
                                         int (*dfda)(const double x[], const double a[],
                                                         double result[]),
                                                         const double *x,
                                                         const double *y,
                                                         const double *sigma,
                                                         double *partials,
                                                         cpl_matrix *alpha,
                                                         cpl_matrix *beta,
                                                         double *a_da);


#define SINFO_FIT_AMOEBA_NMAX 5000

/*-------------------------------------------------------------------------*/

/**
   @name        sinfo_fit_amoeba_get_psum
   @memo        computes sum of p values
   @param        ndim vector's dimension
   @param        mpts No of points
   @param        p matrix of size [1...ndim+1][1...ndim]
   @param        psum result
   @return    void

 */

static void
sinfo_fit_amoeba_get_psum(int ndim, int mpts, double** p, double* psum)
{
    int i=0;
    int j=0;
    double sum=0;
    for (j=0;j<ndim;j++) {
        for (sum=0.0,i=0;i<mpts;i++) {
            sum += p[i][j];
        }
        psum[j]=sum;
    }

}


#define SINFO_FIT_AMOEBA_SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}



/**
   @name        sinfo_fit_amoeba
   @memo        Imp-lements amoeba function for multidimensional minimizzation
   @param        p matrix of size [1...ndim+1][1...ndim]
   @param        y vector[1...ndim+1]
   @param        psum
   @param        ndim vector's dimension
   @param        funk function to be minimized
   @param        ihi
   @param        fac
   @return    int 0, 1, or -1
   @doc
   Multidimensional minimizzation of the function funk(x) where x[1..ndim] is
   a vector in ndim dimensions, by the downhill simplex method of Nedler and
   Mead. The matrix p[1..ndim+1][1..ndim] is input. Its ndim+1 rows are
   ndim-dimensional vectors which are the vertices of the starting simplex.
   Also input is the vector y[1..ndim+1], whose components must be
   pre-initialized to the values of funk avaluated at the ndim+1 vertices (rows)
   of p; and ftol the fractional convergence tolerance to be achieved in the
   function value (N.B.). On output, p and y will have been reset to ndim+1 new
   points all within ftol of a minimum function value, and nfunc gives the
   number of functions taken.

 */


void
sinfo_fit_amoeba(
                double**p,
                double y[],
                int ndim,
                double ftol,
                double (*funk)(double[]),
                int* nfunk)
{


    int i=0;
    int ihi=0;
    int ilo=0;
    int inhi=0;
    int j=0;
    int mpts=ndim+1;
    double rtol=0;
    double swap=0;
    double ysave=0;
    double ytry=0;
    cpl_vector* sum=NULL;
    double* psum=NULL;

    sum=cpl_vector_new(ndim);
    psum=cpl_vector_get_data(sum);
    *nfunk=0;

    sinfo_fit_amoeba_get_psum(ndim,mpts,p,psum);

    for(;;) {
        ilo=0;
        /*
        First we must determine which point is the highest (worst),
        next-highest, and lowest (best), by looping over the points
        in the simplex
         */
        ihi=y[0]>y[1] ? (inhi=1,0) : (inhi=0,1);

        for (i=0;i< mpts;i++) {
            if (y[i] <= y[ilo]) ilo=i;
            if (y[i] > y[ihi]) {
                inhi=ihi;
                ihi=i;
            } else if (y[i] > y[inhi] && i != ihi) inhi=i;
        }
        rtol=2.0*fabs(y[ihi]-y[ilo])/(fabs(y[ihi])+fabs(y[ilo]));

        /*
        compute the fractional range from highest to lowest and return if
        satisfactory
         */
        if(rtol < ftol) { // if returning, but best point and value is in slot 1
            SINFO_FIT_AMOEBA_SWAP(y[0],y[ilo]);
            for (i=0;i<ndim;i++) SINFO_FIT_AMOEBA_SWAP(p[1][i],p[ilo][i]);
            break;
        }
        if (*nfunk >= SINFO_FIT_AMOEBA_NMAX) {
            sinfo_msg_error("NMAX exceeded");
            SINFO_FIT_AMOEBA_SWAP(y[0],y[ilo]);
            for (i=0;i<ndim;i++) SINFO_FIT_AMOEBA_SWAP(p[1][i],p[ilo][i]);
            for (i=0;i<ndim;i++) {
                sinfo_msg("p[1][i]=%g p[ilo][i]=%g ilo=%d",p[1][i],p[ilo][i],ilo);
            }

            assure(*nfunk >= SINFO_FIT_AMOEBA_NMAX,CPL_ERROR_UNSPECIFIED,
                   "NMAX exceeded");
            break;

        }
        *nfunk +=2;
        /*
        Begin a new iteration. First extrapolate by a Factor -1 through the face
        of the simplex across the high point, i.e. reflect the simplex from the
        high point
         */
        ytry=sinfo_fit_amotry(p,y,psum,ndim,funk,ihi,-1.0);
        if(ytry <= y[ilo]) {
            /*
            Gives a result better than the best point, so try an additional
            extrapolation by a factor 2
             */
            ytry=sinfo_fit_amotry(p,y,psum,ndim,funk,ihi,2.0);
        } else if (ytry >= y[inhi]) {

            /*
           The reflected point is worse than the second highest, so look for an
           intermediate lower point, i.e. do a one-dimensional contraction
             */
            ysave=y[ihi];
            ytry=sinfo_fit_amotry(p,y,psum,ndim,funk,ihi,0.5);
            if(ytry >= ysave) {
                /*
               Can't seem to get rid of that high point.
               Better contract around the lowest (best) point
                 */
                for(i=0;i<mpts;i++) {
                    if(i != ilo) {
                        for( j=0;j<ndim;j++) {
                            p[i][j]=psum[j]=0.5*(p[i][j]+p[ilo][j]);
                        }
                        y[i]=(*funk)(psum);
                    }
                }
                *nfunk += ndim; /* Keep track of function evaluations */
                sinfo_fit_amoeba_get_psum(ndim,mpts,p,psum);/* Recomputes psum */
            }
        } else {
            --(*nfunk);
            /* Go back for the test of doneness and the next iteration */
        }
    }
    cleanup:
    cpl_vector_delete(sum);
}


static  double
sinfo_fit_amotry(double** p, double y[], double psum[], int ndim,
                 double (*funk)(double[]),int ihi, double fac)
{
    int j;
    double fac1=0;
    double fac2=0;
    double ytry=0;
    cpl_vector * vtry=NULL;
    double *ptry=NULL;

    vtry=cpl_vector_new(ndim);
    ptry=cpl_vector_get_data(vtry);

    fac1=(1.0-fac)/ndim;
    fac2=fac1-fac;

    for (j=0;j<ndim;j++) {
        ptry[j]=psum[j]*fac1-p[ihi][j]*fac2;
    }
    ytry=(*funk)(ptry);
    if (ytry < y[ihi]) {
        y[ihi]=ytry;
        for (j=0;j<ndim;j++) {
            psum[j] += ptry[j]-p[ihi][j];
            p[ihi][j]=ptry[j];
        }
    }
    sinfo_free_my_vector(&vtry);
    return ytry;
}



/**
 @brief perform a amoeba minimization of the function ftbm(d2,ncon)
 @param    d0 has nvar values, it is the average excluding the highest of d1
 @param    d1 has nvar x nvar+1 values.  it is the simplex array of values
 @param    d2 has ncons values at all times, it is the interface with ftbm
 @param    value is the last nvar+1 values from ftbm
 @param    range range of parameter varaiability
 @param    tol   tolerance
 @param    ivorf is 0 for fixed parameters, 1 for those varied.
 @param    ncon
 @param    nref is the maximum number of reflections
 @param    ftbm function to be fit
 @note     parameters begin varying as d2(i)+-range(i)
 */
double
sinfo_amsub(double d0[], double d1[], double d2[], double value[],
            double range[], double tol, int ivorf[], int ncon, int nref, double
            (*ftbm)(double[], int ncon))

{
    double alpha = 1.0, loc_gamma = 1.5;
    double sf, bsave, temp, sum, cval, ccval, beta;
    int idone, nvar, nvec, nrefl, i, j, k, kd1, kval, isign, jvar, imin, imax,
                    i2max, it1, it2, itemp;
    idone = 0;
    isign = 1;
    // we require that nvec=nvar+1, define nvar
    nvar = 0;
    for (i = 0; i < ncon; ++i) {
        if (ivorf[i] == 1) {
            nvar = nvar + 1;
        }
    }
    nvec = nvar + 1;
    // sf is the 'shrink' factor
    sf = 1e5 * nvec;
    nrefl = 0;
    value[0] = (*ftbm)(d2, ncon);
    // sinfo_msg("value[0] = %lg",value[0]);
    // initial and shrink calculation of the d1 array, uses range
    // set d2 in the first position -- using Fortran convention of 1st
    // array element moving first
    cont20: kd1 = -1;
    for (i = 0; i < ncon; ++i) {
        if (ivorf[i] == 1) {
            kd1 = kd1 + 1;
            d1[kd1] = d2[i];
        }
    }
    // now for the next nvar values
    kval = 0;
    for (jvar = 0; jvar < ncon; ++jvar) {
        if (ivorf[jvar] == 1) {
            kval = kval + 1;
            bsave = d2[jvar];
            isign = -isign;
            d2[jvar] = d2[jvar] + isign * range[jvar];
            value[kval] = (*ftbm)(d2, ncon);
            for (i = 0; i < ncon; ++i) {
                if (ivorf[i] == 1) {
                    kd1 = kd1 + 1;
                    d1[kd1] = d2[i];
                }
            }
            d2[jvar] = bsave;
        }
    }
    // sinfo_msg(" d1 ");
    // for (j=0;j<nvec;++j) {
    //   for(i=0;i<nvar;++i) {
    //     sinfo_msg("%12.4lg ",d1[i+j*nvar]);
    //   }
    // }
    /* find highest, second highest, and minimum values
     imax points to the vector with the largest value
     i2max points to the vector with the second largest value
     imin points to the vector with the smallest value  */
    cont40: imin = 1;
    if (value[0] > value[1]) {
        imax = 0;
        i2max = 1;
    }
    else {
        imax = 1;
        i2max = 0;
    }
    for (i = 0; i < nvec; ++i) {
        if (value[i] < value[imin])
            imin = i;
        if (value[i] > value[imax]) {
            i2max = imax;
            imax = i;
        }
        else if ((value[i] > value[i2max]) && (i != imax)) {
            i2max = i;
        }
    }
    // sinfo_msg(" values after sorting ");
    // for(i=0;i<nvec;++i)sinfo_msg("%12.4lg ",value[i]);
    // sinfo_msg("imin %d,i2max %d,imax %d",imin,i2max,imax);
    // scanf("%d",&itest);

    // check if done

    if (nrefl >= nref) {
        sinfo_msg(" maximum number of reflection reached");
        idone = 1;
        goto cont400;
    }
    if (value[imin] != 0.0) {
        temp = (value[imax] - value[imin]) / value[imin];
        if (fabs(temp) <= tol) {
            sinfo_msg(" reached tolerance %lg temp %lg tol", temp, tol);
            idone = 1;
            goto cont400;
        }
    }
    if (value[imax] - value[imin] <= tol) {
        sinfo_msg("value[max]-value[min]<=tol");
        idone = 1;
        goto cont400;
    }

    // *** form d0 the average of all but imax
    for (j = 0; j < nvar; ++j) {
        sum = 0.0;
        for (i = 0; i < nvec; ++i) {
            if (i != imax)
                sum = sum + d1[i * nvar + j];
        }
        d0[j] = sum / (nvec - 1);
    }
    // sinfo_msg(" D0 values ");
    // for(i=0;i<nvar;++i)sinfo_msg("%12.4lg ",d0[i]);
    // scanf("%d",&itest);
    // reflection

    nrefl = nrefl + 1;
    k = -1;
    for (j = 0; j < ncon; ++j) {
        if (ivorf[j] == 1) {
            k = k + 1;
            it1 = imax * nvar + k;
            d2[j] = (1 + alpha) * d0[k] - alpha * d1[it1];
        }
    }

    // sinfo_msg(" refl d2 ");
    // for(i=0;i<nvar;++i) sinfo_msg("%12.4lg ",d2[i]);

    cval = (*ftbm)(d2, ncon);
    // sinfo_msg("refl ftbm %lg",cval);

    // value is higher than i2max so do contraction
    if (cval >= value[i2max])
        goto cont200;

    // value is less than i2max - normal - update d1 and value

    value[imax] = cval;
    k = -1;
    for (j = 0; j < ncon; ++j) {
        if (ivorf[j] == 1) {
            k = k + 1;
            it1 = imax * nvar + k;
            d1[it1] = d2[j];
        }
    }

    // value is less than imin, try expansion
    if (cval < value[imin])
        goto cont300;
    goto cont40;

    // contraction
    cont200:
    // sinfo_msg(" contraction ");
    beta = 0.75;
    for (itemp = 0; itemp < 3; ++itemp) {
        if (cval <= value[imax]) {
            value[imax] = cval;
            k = -1;
            for (j = 0; j < ncon; ++j) {
                if (ivorf[j] == 1) {
                    k = k + 1;
                    it1 = imax * nvar + k;
                    d1[it1] = d2[j];
                }
            }
        }
        k = -1;
        for (j = 0; j < ncon; ++j) {
            if (ivorf[j] == 1) {
                k = k + 1;
                it1 = imax * nvar + k;
                d2[j] = beta * d1[it1] + (1. - beta) * d0[k];
            }
        }
        cval = ftbm(d2, ncon);

        // sinfo_msg(" contraction beta %lg cval %lg ",beta,cval);
        // value is better
        if (cval < value[i2max]) {
            value[imax] = cval;
            k = -1;
            for (j = 0; j < ncon; ++j) {
                if (ivorf[j] == 1) {
                    k = k + 1;
                    it1 = imax * nvar + k;
                    d1[it1] = d2[j];
                }
            }
            if (cval < value[imin])
                sinfo_msg(" contraction minimum %lg", cval);
            goto cont40;
        }
        beta = beta - 0.25;
    }
    sinfo_msg(" contraction failed  ==>shrink");
    // scanf("%d",&itest);
    // value is worse so shrink it

    goto cont400;

    // expansion

    cont300:
    sinfo_msg(" reflection min %lg \n", cval);
    k = -1;
    for (j = 0; j < ncon; ++j) {
        if (ivorf[j] == 1) {
            k = k + 1;
            d2[j] = loc_gamma * d2[j] + (1. - loc_gamma) * d0[k];
        }
    }
    ccval = (*ftbm)(d2, ncon);
    // value is higher than reflected value ==> discard
    if (ccval > cval)
        goto cont40;
    // value is better so use it rather than the reflected point
    sinfo_msg(" expansion minimum %lg \n", ccval);
    value[imax] = ccval;
    k = -1;
    for (j = 0; j < ncon; ++j) {
        if (ivorf[j] == 1) {
            k = k + 1;
            it1 = imax * nvar + k;
            d1[it1] = d2[j];
        }
    }
    goto cont40;

    cont400:
    // sinfo_msg(" following cont400 ");
    // scanf("%d",&itest);
    // recalculate d2 and range
    // the range is the average of dist**2 from d1 with min value
    k = -1;
    for (j = 0; j < ncon; ++j) {
        if (ivorf[j] == 1) {
            k = k + 1;
            it1 = imin * nvar + k;
            d2[j] = d1[it1];
            sum = 0.0;
            for (i = 0; i < nvec; ++i) {
                it1 = i * nvar + k;
                it2 = imin * nvar + k;
                sum = sum + (d1[it1] - d1[it2]) * (d1[it1] - d1[it2]);
            }
            range[j] = sf * sqrt(sum / (nvec - 1));
        }
    }
    value[1] = value[imin];
    sf = .75 * sf;
    if (sf < 0.1)
        idone = 1;
    sinfo_msg(" shrink factor %lg ", sf);
    if (idone != 1)
        goto cont20;
    return value[1];

}

static double
sinfo_spline(double x, double cons[], double ak[], double *sp, double *spp,
             double *sppp, int n)
{
    double retval = 0;
    double xm = 0;
    double xm2 = 0;
    double xm3 = 0;

    int i = 0;

    *sp = 0;
    *spp = 0;
    *sppp = 0;

    for (i = 0; i < n; ++i) {
        if (ak[i] >= x) {
            xm = ak[i] - x;
            xm2 = xm * xm;
            xm3 = xm * xm2;
            sinfo_msg("cons=%g", cons[i]);
            retval += cons[i] * xm3;
            *sp -= 3 * cons[i] * xm2;
            *spp += 6 * cons[i] * xm;
            *sppp -= 6 * cons[i];
        }
    }
    sinfo_msg("1x=%g retval=%g", x, retval);
    return retval;

}

double
sinfo_ftbm(const double x, double cons[])
{
    double retval = 0;
    double ak[4] =
        { -1, -.666666666666666, -.333333333333, 0 };
    double sm1 = 0;
    double spm1 = 0;
    double sppm1 = 0;
    double spppm1 = 0;

    int n = 4;

    sm1 = sinfo_spline(x, cons, ak, &spm1, &sppm1, &spppm1, n) - 1;
    sinfo_msg("x=%g val=%g", x, sm1 + 1);

    retval = sm1 * sm1 + spm1 * spm1 + sppm1 * sppm1 + spppm1 * spppm1;
    sinfo_msg("fitbm: x=%g retval=%g", x, retval);

    return retval;

}

/*----------------------------------------------------------------------------*/
/**
   @brief   Compute chi square
   @param   N       Number of positions
   @param   D       Dimension of x-positions
   @param   f       Function that evaluates the fit function.
   @param   a       The fit parameters.
   @param   x       Where to evaluate the fit function (N x D matrix).
   @param   y       The N values to fit.
   @param   sigma   A vector of size N containing the uncertainties of the
   y-values. If NULL, a constant uncertainty equal to 1 is
   assumed.

   @return  chi square, or a negative number on error.

   This function calculates chi square defined as
   sum_i (y_i - f(x_i, a))^2/sigma_i^2

   Possible #_cpl_error_code_ set in this function:
   - CPL_ERROR_ILLEGAL_INPUT if the fit function could not be evaluated
 */
/*----------------------------------------------------------------------------*/

static double
sinfo_get_chisq(int N, int D,
          int (*f)(const double x[], const double a[], double *result),
          const double *a,
          const double *x,
          const double *y,
          const double *sigma)
{
    double chi_sq;     /* Result */
    int i = 0;

    /* For efficiency, don't check input in this static function */
    chi_sq = 0.0;
    for (i = 0; i < N; i++)
    {
        double fx_i;
        double residual;                 /* Residual in units of uncertainty */
        const double *x_i = &(x[0+i*D]);

        /* Evaluate */
        cpl_ensure( f(x_i,
                        a,
                        &fx_i) == 0, CPL_ERROR_ILLEGAL_INPUT, -1.0);

        /* Accumulate */
        if (sigma == NULL)
        {
            residual = (fx_i - y[i]);
        }
        else
        {
            residual = (fx_i - y[i]) / sigma[i];
        }

        chi_sq += residual*residual;

    }

    return chi_sq;
}


/*----------------------------------------------------------------------------*/
/**
   @brief   Get new position in parameter space (L-M algorithm)
   @param   a       Current fit parameters.
   @param   ia      Non-NULL array defining with non-zero values which
   parameters participate in the fit.
   @param   M       Number of fit parameters
   @param   N       Number of positions
   @param   D       Dimension of x-positions
   @param   lambda  Lambda in L-M algorithm.
   @param   f       Function that evaluates the fit function.
   @param   dfda    Function that evaluates the partial derivaties
   of the fit function w.r.t. fit parameters.
   @param   x       The input positions (pointer to MxD matrix buffer).
   @param   y       The N values to fit.
   @param   sigma   A vector of size N containing the uncertainties of the
   y-values. If NULL, a constant uncertainty equal to 1 is
   assumed.
   @param   partials The partial derivatives (work space).
   @param   alpha   Alpha in L-M algorithm (work space).
   @param   beta    Beta in L-M algorithm (work space).
   @param   a_da    (output) Candidate position in parameter space.

   @return  0 iff okay.

   This function computes a potentially better set of parameters @em a + @em da,
   where @em da solves the equation @em alpha(@em lambda) * @em da = @em beta .

   Possible #_cpl_error_code_ set in this function:
   - CPL_ERROR_ILLEGAL_INPUT if the fit function or its derivative could
   not be evaluated.
   - CPL_ERROR_SINGULAR_MATRIX if @em alpha is singular.

 */
/*----------------------------------------------------------------------------*/
static int
sinfo_get_candidate(const double *a, const int ia[],
              int M, int N, int D,
              double lambda,
              int    (*f)(const double x[], const double a[], double *result),
              int (*dfda)(const double x[], const double a[], double result[]),
              const double *x,
              const double *y,
              const double *sigma,
              double *partials,
              cpl_matrix *alpha,
              cpl_matrix *beta,
              double *a_da)
{
    int Mfit = 0;         /* Number of non-constant fit parameters */
    cpl_matrix *da;       /* Solution of   alpha * da = beta */
    double *alpha_data;
    double *beta_data;
    double *da_data;
    int i, imfit = 0;
    int j, jmfit = 0;
    int k = 0;

    /* For efficiency, don't check input in this static function */

    Mfit = cpl_matrix_get_nrow(alpha);

    alpha_data    = cpl_matrix_get_data(alpha);
    beta_data     = cpl_matrix_get_data(beta);

    /* Build alpha, beta:
     *
     *  alpha[i,j] = sum_{k=1,N} (sigma_k)^-2 * df/da_i * df/da_j  *
     *                           (1 + delta_ij lambda) ,
     *
     *   beta[i]   = sum_{k=1,N} (sigma_k)^-2 * ( y_k - f(x_k) ) * df/da_i
     *
     * where (i,j) loop over the non-constant parameters (0 to Mfit-1),
     * delta is Kronecker's delta, and all df/da are evaluated in x_k
     */

    cpl_matrix_fill(alpha, 0.0);
    cpl_matrix_fill(beta , 0.0);

    for (k = 0; k < N; k++)
    {
        double sm2 = 0.0;                /* (sigma_k)^-2 */
        double fx_k = 0.0;               /* f(x_k)       */
        const double *x_k = &(x[0+k*D]); /* x_k          */

        if (sigma == NULL)
        {
            sm2 = 1.0;
        }
        else
        {
            sm2 = 1.0 / (sigma[k] * sigma[k]);
        }

        /* Evaluate f(x_k) */
        cpl_ensure( f(x_k, a, &fx_k) == 0, CPL_ERROR_ILLEGAL_INPUT, -1);

        /* Evaluate (all) df/da (x_k) */
        cpl_ensure( dfda(x_k, a, partials) == 0,
                    CPL_ERROR_ILLEGAL_INPUT, -1);

        for (i = 0, imfit = 0; i < M; i++)
        {
            if (ia[i] != 0)
            {
                /* Beta */
                beta_data[imfit] +=
                                sm2 * (y[k] - fx_k) * partials[i];

                /* Alpha is symmetrical, so compute
               only lower-left part */
                for (j = 0, jmfit = 0; j < i; j++)
                {
                    if (ia[j] != 0)
                    {
                        alpha_data[jmfit + imfit*Mfit] +=
                                        sm2 * partials[i] *
                                        partials[j];

                        jmfit += 1;
                    }
                }

                /* Alpha, diagonal terms */
                j = i;
                jmfit = imfit;

                alpha_data[jmfit + imfit*Mfit] +=
                                sm2 * partials[i] *
                                partials[j] * (1 + lambda);

                imfit += 1;
            }
        }

        cpl_ensure( imfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1);
    }

    /* Create upper-right part of alpha */
    for (i = 0, imfit = 0; i < M; i++) {
        if (ia[i] != 0) {
            for (j = i+1, jmfit = imfit+1; j < M; j++) {
                if (ia[j] != 0) {
                    alpha_data[jmfit+imfit*Mfit] = alpha_data[imfit+jmfit*Mfit];
                    jmfit += 1;
                }
            }
            cpl_ensure( jmfit == Mfit,CPL_ERROR_ILLEGAL_INPUT,-1 );
            imfit += 1;
        }
    }
    cpl_ensure( imfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1);

    da = cpl_matrix_solve(alpha, beta);

    cpl_ensure(da != NULL, cpl_error_get_code(), -1);

    /* Create a+da vector by adding a and da */
    da_data   = cpl_matrix_get_data(da);

    for (i = 0, imfit = 0; i < M; i++)
    {
        if (ia[i] != 0)
        {
            a_da[i] = a[i] + da_data[0 + imfit*1];

            imfit += 1;
        }
        else
        {
            a_da[i] = a[i];
        }
    }

    cpl_ensure( imfit == Mfit ,CPL_ERROR_ILLEGAL_INPUT,-1);

    cpl_matrix_delete(da);

    return 0;
}


#ifndef CPL_VECTOR_FIT_MAXITER
#define CPL_VECTOR_FIT_MAXITER 1000
#endif
/*----------------------------------------------------------------------------*/
/**
   @brief   Fit a function to a set of data
   @param   x        N x D matrix of the positions to fit.
   Each matrix row is a D-dimensional position.
   @param   sigma_x  Uncertainty (one sigma, gaussian errors assumed)
   assosiated with @em x. Taking into account the
   uncertainty of the independent variable is currently
   unsupported, and this parameter must therefore be set
   to NULL.
   @param   y        The N values to fit.
   @param   sigma_y  Vector of size N containing the uncertainties of
   the y-values. If this parameter is NULL, constant
   uncertainties are assumed.
   @param   a        Vector containing M fit parameters. Must contain
   a guess solution on input and contains the best
   fit parameters on output.
   @param   ia       Array of size M defining which fit parameters participate
   in the fit (non-zero) and which fit parameters are held
   constant (zero). At least one element must be non-zero.
   Alternatively, pass NULL to fit all parameters.
   @param   f        Function that evaluates the fit function
   at the position specified by the first argument (an array of
   size D) using the fit parameters specified by the second
   argument (an array of size M). The result must be output
   using the third parameter, and the function must return zero
   iff the evaluation succeded.
   @param   dfda     Function that evaluates the first order partial
   derivatives of the fit function with respect to the fit
   parameters at the position specified by the first argument
   (an array of size D) using the parameters specified by the
   second argument (an array of size M). The result must
   be output using the third parameter (array of size M), and
   the function must return zero iff the evaluation succeded.
   @param mse        If non-NULL, the mean squared error of the best fit is
   computed.
   @param red_chisq  If non-NULL, the reduced chi square of the best fit is
   computed. This requires @em sigma_y to be specified.
   @param covariance If non-NULL, the formal covariance matrix of the best
   fit parameters is computed (or NULL on error). On success
   the diagonal terms of the covariance matrix are guaranteed
   to be positive. However, terms that involve a constant
   parameter (as defined by the input array @em ia) are
   always set to zero. Computation of the covariacne matrix
   requires @em sigma_y to be specified.


   @return  CPL_ERROR_NONE iff OK.

   This function makes a minimum chi squared fit of the specified function
   to the specified data set using the Levenberg-Marquardt algorithm.

   Possible #_cpl_error_code_ set in this function:
   - CPL_ERROR_NULL_INPUT if an input pointer other than @em sigma_x, @em
   sigma_y, @em mse, @em red_chisq or @em covariance is NULL.
   - CPL_ERROR_ILLEGAL_INPUT if an input matrix/vector is empty, if @em ia
   contains only zero values, if N <= M and @em red_chisq is non-NULL,
   if any element of @em sigma_x or @em sigma_y is non-positive, or if
   evaluation of the fit function or its derivative failed.
   - CPL_ERROR_INCOMPATIBLE_INPUT if the dimensions of the input
   vectors/matrices do not match, or if chi square or covariance computation
   is requested and @em sigma_y is NULL.
   - CPL_ERROR_ILLEGAL_OUTPUT if memory allocation failed.
   - CPL_ERROR_CONTINUE if the Levenberg-Marquardt algorithm failed to converge.
   - CPL_ERROR_SINGULAR_MATRIX if the covariance matrix could not be computed.

 */
/*----------------------------------------------------------------------------*/
cpl_error_code
sinfo_fit_lm(const cpl_matrix *x,
             const cpl_matrix *sigma_x,
             const cpl_vector *y,
             const cpl_vector *sigma_y,
             cpl_vector *a,
             const int ia[],
             int    (*f)(const double x[],
                             const double a[],
                             double *result),
                             int (*dfda)(const double x[],
                                             const double a[],
                                             double result[]),
                                             double *mse,
                                             double *red_chisq,
                                             cpl_matrix **covariance)
{
    const double *x_data     = NULL; /* Pointer to input data                  */
    const double *y_data     = NULL; /* Pointer to input data                  */
    const double *sigma_data = NULL; /* Pointer to input data                  */
    int N    = 0;                    /* Number of data points                  */
    int D    = 0;                    /* Dimension of x-points                  */
    int M    = 0;                    /* Number of fit parameters               */
    int Mfit = 0;                    /* Number of non-constant fit
                                       parameters                             */

    double lambda    = 0.0;          /* Lambda in L-M algorithm                */
    double MAXLAMBDA = 10e40;        /* Parameter to control the graceful exit
                                       if steepest descent unexpectedly fails */
    double chi_sq    = 0.0;          /* Current  chi^2                         */
    int count        = 0;            /* Number of successive small improvements
                                       in chi^2 */
    int iterations   = 0;

    cpl_matrix *alpha  = NULL;       /* The MxM ~curvature matrix used in L-M  */
    cpl_matrix *beta   = NULL;       /* Mx1 matrix = -.5 grad(chi^2)           */
    double *a_data     = NULL;       /* Parameters, a                          */
    double *a_da       = NULL;       /* Candidate position a+da                */
    double *part       = NULL;       /* The partial derivatives df/da          */
    int *ia_local      = NULL;       /* non-NULL version of ia                 */

    /* If covariance computation is requested, then either
     * return the covariance matrix or return NULL.
     */
    if (covariance != NULL) *covariance = NULL;

    /* Validate input */
    cpl_ensure_code(x       != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(sigma_x == NULL, CPL_ERROR_UNSUPPORTED_MODE);
    cpl_ensure_code(y       != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(a       != NULL, CPL_ERROR_NULL_INPUT);
    /* ia may be NULL */
    cpl_ensure_code(f       != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(dfda    != NULL, CPL_ERROR_NULL_INPUT);

    /* Chi^2 and covariance computations require sigmas to be known */
    cpl_ensure_code( sigma_y != NULL ||
                     (red_chisq == NULL && covariance == NULL),
                     CPL_ERROR_INCOMPATIBLE_INPUT);

    D = cpl_matrix_get_ncol(x);
    N = cpl_matrix_get_nrow(x);
    M = cpl_vector_get_size(a);
    cpl_ensure_code(N > 0 && D > 0 && M > 0, CPL_ERROR_ILLEGAL_INPUT);

    cpl_ensure_code( cpl_vector_get_size(y) == N,
                     CPL_ERROR_INCOMPATIBLE_INPUT);

    x_data = cpl_matrix_get_data_const(x);
    y_data = cpl_vector_get_data_const(y);
    a_data = cpl_vector_get_data(a);

    if (sigma_y != NULL)
    {
        cpl_ensure_code( cpl_vector_get_size(sigma_y) == N,
                        CPL_ERROR_INCOMPATIBLE_INPUT);
        /* Sigmas must be positive */
        cpl_ensure_code( cpl_vector_get_min (sigma_y) > 0,
                         CPL_ERROR_ILLEGAL_INPUT);
        sigma_data = cpl_vector_get_data_const(sigma_y);
    }

    ia_local = cpl_malloc(M * sizeof(int));
    cpl_ensure_code(ia_local != NULL, CPL_ERROR_ILLEGAL_OUTPUT);

    /* Count non-constant fit parameters, copy ia */
    if (ia != NULL)
    {
        int i;

        Mfit = 0;
        for (i = 0; i < M; i++)
        {
            ia_local[i] = ia[i];

            if (ia[i] != 0)
            {
                Mfit += 1;
            }
        }

        if (! (Mfit > 0))
        {
            cpl_free(ia_local);
            cpl_ensure_code( CPL_FALSE,
                             CPL_ERROR_ILLEGAL_INPUT);
        }
    }
    else
    {
        /* All parameters participate */
        int i;

        Mfit = M;

        for (i = 0; i < M; i++)
        {
            ia_local[i] = 1;
        }
    }

    /* To compute reduced chi^2, we need N > Mfit */
    if (! ( red_chisq == NULL || N > Mfit ) )
    {
        cpl_free(ia_local);
        cpl_ensure_code(
                        CPL_FALSE,
                        CPL_ERROR_ILLEGAL_INPUT);
    }

    /* Create alpha, beta, a_da, part  work space */
    alpha = cpl_matrix_new(Mfit, Mfit);
    if (alpha == NULL)
    {
        cpl_free(ia_local);
        cpl_ensure_code(
                        CPL_FALSE,
                        CPL_ERROR_ILLEGAL_OUTPUT);
    }

    beta = cpl_matrix_new(Mfit, 1);
    if (beta == NULL)
    {
        cpl_free(ia_local);
        cpl_matrix_delete(alpha);
        cpl_ensure_code(
                        CPL_FALSE,
                        CPL_ERROR_ILLEGAL_OUTPUT);
    }

    a_da = cpl_malloc(M * sizeof(double));
    if (a_da == NULL)
    {
        cpl_free(ia_local);
        cpl_matrix_delete(alpha);
        cpl_matrix_delete(beta);
        cpl_ensure_code(
                        CPL_FALSE,
                        CPL_ERROR_ILLEGAL_OUTPUT);
    }

    part = cpl_malloc(M * sizeof(double));
    if (part == NULL)
    {
        cpl_free(ia_local);
        cpl_matrix_delete(alpha);
        cpl_matrix_delete(beta);
        cpl_free(a_da);
        cpl_ensure_code(
                        CPL_FALSE,
                        CPL_ERROR_ILLEGAL_OUTPUT);
    }

    /* Initialize loop variables */
    lambda = 0.001;
    count = 0;
    iterations = 0;
    if( (chi_sq = sinfo_get_chisq(N, D, f, a_data, x_data, y_data, sigma_data)) < 0)
    {
        cpl_free(ia_local);
        cpl_matrix_delete(alpha);
        cpl_matrix_delete(beta);
        cpl_free(a_da);
        cpl_free(part);
        cpl_ensure_code(
                        CPL_FALSE,
                        cpl_error_get_code());
    }

    /* uves_msg_debug("Initial chi^2 = %f", chi_sq); */

    /* Iterate until chi^2 didn't improve substantially many (say, 5)
      times in a row */
    while (count < 5 &&
                    lambda < MAXLAMBDA &&
                    iterations < CPL_VECTOR_FIT_MAXITER)
    {
        /* In each iteration lambda increases, or chi^2 decreases or
         count increases. Because chi^2 is bounded from below
         (and lambda and count from above), the loop will terminate */

        double chi_sq_candidate = 0.0;
        int returncode = 0;

        /* Get candidate position in parameter space = a+da,
         * where  alpha * da = beta .
         * Increase lambda until alpha is non-singular
         */

        while( (returncode = sinfo_get_candidate(a_data, ia_local,
                        M, N, D,
                        lambda, f, dfda,
                        x_data, y_data, sigma_data,
                        part, alpha, beta, a_da)
        ) != 0
                        && cpl_error_get_code() == CPL_ERROR_SINGULAR_MATRIX
                        && lambda < MAXLAMBDA)
        {
            /* uves_msg_debug("Singular matrix. lambda = %e", lambda); */
            cpl_error_reset();
            lambda *= 9.0;
        }

        /* Set error if lambda diverged */
        if ( !( lambda < MAXLAMBDA ) )
        {
            cpl_free(ia_local);
            cpl_matrix_delete(alpha);
            cpl_matrix_delete(beta);
            cpl_free(a_da);
            cpl_free(part);
            cpl_ensure_code(
                            CPL_FALSE,
                            CPL_ERROR_CONTINUE);
        }

        if (returncode != 0)
        {
            cpl_free(ia_local);
            cpl_matrix_delete(alpha);
            cpl_matrix_delete(beta);
            cpl_free(a_da);
            cpl_free(part);
            cpl_ensure_code(
                            CPL_FALSE,
                            cpl_error_get_code());
        }

        /* Get chi^2(a+da) */
        if ((chi_sq_candidate = sinfo_get_chisq(N, D, f, a_da,
                        x_data, y_data, sigma_data)) < 0)
        {
            cpl_free(ia_local);
            cpl_matrix_delete(alpha);
            cpl_matrix_delete(beta);
            cpl_free(a_da);
            cpl_free(part);
            cpl_ensure_code(
                            CPL_FALSE,
                            cpl_error_get_code());
        }

        /* uves_msg_debug("Chi^2 = %f  Candidate = %f  Lambda = %e",
         chi_sq, chi_sq_candidate, lambda);  */

        if (chi_sq_candidate > chi_sq)
        {
            /* Move towards steepest descent */
            lambda *= 9.0;
        }
        else
        {
            /* Move towards Newton's algorithm */
            lambda /= 10.0;

            /* Count the number of successive improvements in chi^2 of
            less than 0.01 relative */
            if ( chi_sq == 0 ||
                            (chi_sq - chi_sq_candidate)/chi_sq < .01)
            {
                count += 1;
            }
            else
            {
                /* Chi^2 improved by a significant amount,
               reset counter */
                count = 0;
            }

            /* chi^2 improved, update a and chi^2 */
            {
                int i;
                for (i = 0; i < M; i++) a_data[i] = a_da[i];
            }
            chi_sq = chi_sq_candidate;
        }
        iterations++;
    }

    /* Set error if we didn't converge */
    if ( !( lambda < MAXLAMBDA && iterations < CPL_VECTOR_FIT_MAXITER ) )
    {
        cpl_free(ia_local);
        cpl_matrix_delete(alpha);
        cpl_matrix_delete(beta);
        cpl_free(a_da);
        cpl_free(part);
        cpl_ensure_code(
                        CPL_FALSE,
                        CPL_ERROR_CONTINUE);
    }

    /* Compute mse if requested */
    if (mse != NULL)
    {
        int i;

        *mse = 0.0;

        for(i = 0; i < N; i++)
        {
            double fx_i = 0.0;
            double residual = 0.0;

            /* Evaluate f(x_i) at the best fit parameters */
            if( f(&(x_data[i*D]),
                            a_data,
                            &fx_i) != 0)
            {
                cpl_free(ia_local);
                cpl_matrix_delete(alpha);
                cpl_matrix_delete(beta);
                cpl_free(a_da);
                cpl_free(part);
                cpl_ensure_code(
                                CPL_FALSE,
                                CPL_ERROR_ILLEGAL_INPUT);
            }

            residual = y_data[i] - fx_i;
            *mse += residual * residual;
        }
        *mse /= N;
    }

    /* Compute reduced chi^2 if requested */
    if (red_chisq != NULL)
    {
        /* We already know the optimal chi^2 (and that N > Mfit)*/
        *red_chisq = chi_sq / (N-Mfit);
    }

    /* Compute covariance matrix if requested
     * cov = alpha(lambda=0)^-1
     */
    if (covariance != NULL)
    {
        cpl_matrix *cov;

        if( sinfo_get_candidate(a_data, ia_local,
                        M, N, D, 0.0, f, dfda,
                        x_data, y_data, sigma_data,
                        part, alpha, beta, a_da)
                        != 0)
        {
            cpl_free(ia_local);
            cpl_matrix_delete(alpha);
            cpl_matrix_delete(beta);
            cpl_free(a_da);
            cpl_free(part);
            cpl_ensure_code(
                            CPL_FALSE,
                            cpl_error_get_code());
        }

        cov = cpl_matrix_invert_create(alpha);
        if (cov == NULL)
        {
            cpl_free(ia_local);
            cpl_matrix_delete(alpha);
            cpl_matrix_delete(beta);
            cpl_free(a_da);
            cpl_free(part);
            cpl_ensure_code(
                            CPL_FALSE,
                            cpl_error_get_code());
        }

        /* Make sure that variances are positive */
        {
            int i;
            for (i = 0; i < Mfit; i++)
            {
                if ( !(cpl_matrix_get(cov, i, i) > 0) )
                {
                    cpl_free(ia_local);
                    cpl_matrix_delete(alpha);
                    cpl_matrix_delete(beta);
                    cpl_free(a_da);
                    cpl_free(part);
                    cpl_matrix_delete(cov);
                    *covariance = NULL;
                    cpl_ensure_code(
                                    CPL_FALSE,
                                    CPL_ERROR_SINGULAR_MATRIX);
                }
            }
        }

        /* Expand covariance matrix from Mfit x Mfit
         to M x M. Set rows/columns corresponding to fixed
         parameters to zero */

        *covariance = cpl_matrix_new(M, M);
        if (*covariance == NULL)
        {
            cpl_free(ia_local);
            cpl_matrix_delete(alpha);
            cpl_matrix_delete(beta);
            cpl_free(a_da);
            cpl_free(part);
            cpl_matrix_delete(cov);
            cpl_ensure_code(
                            CPL_FALSE,
                            CPL_ERROR_ILLEGAL_OUTPUT);
        }

        {
            int j, jmfit;

            for (j = 0, jmfit = 0; j < M; j++)
                if (ia_local[j] != 0)
                {
                    int i, imfit;

                    for (i = 0, imfit = 0; i < M; i++)
                        if (ia_local[i] != 0)
                        {
                            cpl_matrix_set(*covariance, i, j,
                                            cpl_matrix_get(
                                                            cov, imfit, jmfit));
                            imfit += 1;
                        }

                    cpl_ensure( imfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1);

                    jmfit += 1;
                }

            cpl_ensure( jmfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1 );
        }

        cpl_matrix_delete(cov);
    }

    cpl_free(ia_local);
    cpl_matrix_delete(alpha);
    cpl_matrix_delete(beta);
    cpl_free(a_da);
    cpl_free(part);

    return CPL_ERROR_NONE;
}




/**@}*/
