/*
 * 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        ndim vector's dimension
   @param        ftol fractional goal tolerance
   @param        funk function to be minimized
   @return    void
   @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;
}

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;

}

/*----------------------------------------------------------------------------*/
/**
   @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;
}




/**@}*/
