#include "fitting.h"
#include "statistics.h" // for Monte-Carlo error estimation

#include <tjutils/tjtest.h>

#ifdef HAVE_LIBGSL

#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_blas.h>


// data struct to be passed through GSL functions
struct ModelData {
  ModelData(unsigned int size) {n=size; y=new float[size]; sigma=new float[size]; x=new float[size];}
  ~ModelData() {delete[] y; delete[] sigma; delete[] x;}

  ModelFunction* modelfunc;  // model function
  unsigned int n; // number of points
  float* y;     // function values
  float* sigma; // uncertainty of function values
  float* x;     // arguments of function
};


struct GslData4Fit {
  gsl_multifit_fdfsolver *solver;
  gsl_matrix *covar;
};


////////////////////////////////////////////////////////////////

Array<float,1> ModelFunction::get_function(const Array<float,1>& xvals) const {
  int n=xvals.extent(0);
  Array<float,1> result(n);
  for(int i=0; i<n; i++) {
    result(i)=evaluate_f(xvals(i));
  }
  return result;
}

const Array<float,1> ModelFunction::defaultArray;

////////////////////////////////////////////////////////////////

const Array<float,1> FunctionFitInterface::defaultArray;

////////////////////////////////////////////////////////////////

// functions to be used by GSL solver

int FunctionFitDerivative_func_f (const gsl_vector * gv, void *params, gsl_vector * f) {

  // get current model function
  ModelData* data=(ModelData*)params;
  ModelFunction* modelfunc=data->modelfunc;
  unsigned int n_fitpars=modelfunc->numof_fitpars();

  // set the fitting parameters in the model function
  for(unsigned int i=0; i<n_fitpars; i++) {
    modelfunc->get_fitpar(i).val=gsl_vector_get(gv, i);
  }

  // calculate vector of values for this set of parameters
  for (unsigned int i = 0; i < data->n; i++) {
    float Yi = modelfunc->evaluate_f(data->x[i]);
    gsl_vector_set (f, i, (data->y[i] - Yi)/data->sigma[i]);
  }

  return GSL_SUCCESS;
}

int FunctionFitDerivative_func_df (const gsl_vector * gv, void *params, gsl_matrix * df) {

  // get current model function
  ModelData* data=(ModelData*)params;
  ModelFunction* modelfunc=data->modelfunc;
  unsigned int n_fitpars=modelfunc->numof_fitpars();

  // set the fitting parameters in the model function
  for(unsigned int i=0; i<n_fitpars; i++) {
    modelfunc->get_fitpar(i).val=gsl_vector_get(gv, i);
  }

  // calculate vector of derivative values for this set of parameters
  fvector dYi(n_fitpars);
  for (unsigned int i = 0; i < data->n; i++) {
    dYi = modelfunc->evaluate_df(data->x[i]);
    float s = data->sigma[i];
    for(unsigned int j=0; j<n_fitpars; j++) {
      gsl_matrix_set (df, i, j, -dYi[j]/s);
    }
  }

  return GSL_SUCCESS;
}

int FunctionFitDerivative_func_fdf (const gsl_vector * gv, void *params, gsl_vector * f, gsl_matrix * df) {
  FunctionFitDerivative_func_f (gv, params, f);
  FunctionFitDerivative_func_df (gv, params, df);
  return GSL_SUCCESS;
}

////////////////////////////////////////////////////////////////


bool FunctionFitDerivative::init(ModelFunction& model_func, unsigned int nvals) {
  Log<OdinData> odinlog("FunctionFitDerivative","init");

  data4fit= new ModelData(nvals);

  data4fit->modelfunc=&model_func;

  gsldata=new GslData4Fit;

  unsigned int npars=model_func.numof_fitpars();
  ODINLOG(odinlog,normalDebug) << "npars=" << npars << STD_endl;

  // allocate and initialize GSL stuff
  gsldata->covar  = gsl_matrix_alloc (npars, npars);
  gsldata->solver = gsl_multifit_fdfsolver_alloc (gsl_multifit_fdfsolver_lmsder, data4fit->n, npars);
  
  return true;
}

FunctionFitDerivative::~FunctionFitDerivative() {
  if(gsldata) {
    gsl_multifit_fdfsolver_free(gsldata->solver);
    gsl_matrix_free(gsldata->covar);
    delete gsldata;
  }
  if(data4fit) delete data4fit;
}


bool FunctionFitDerivative::fit(const Array<float,1>& yvals,
         const Array<float,1>& ysigma,
         const Array<float,1>& xvals,
         unsigned int max_iterations, double tolerance) {
  Log<OdinData> odinlog("FunctionFitDerivative","fit");

  if(!gsldata || !data4fit) {
    ODINLOG(odinlog,errorLog) << "not initialized" << STD_endl;
    return false;
  }
  
  bool haveSigma;
  bool haveXvals;

  // check input
  if(data4fit->n!=(unsigned)yvals.size() || data4fit->n<=0) {
    ODINLOG(odinlog,errorLog) << "size mismatch in yvals" << STD_endl;
    return false;
  }
  if((unsigned)ysigma.size()==data4fit->n) haveSigma=true;
  else {
    haveSigma=false;
    ODINLOG(odinlog,normalDebug) << "no error bars provided, taking uniform interval for all x" << STD_endl;
  }
  if((unsigned)xvals.size()==data4fit->n) haveXvals=true;
  else {
    haveXvals=false;
    ODINLOG(odinlog,normalDebug) << "no x values provided, taking index as x value" << STD_endl;
  }

  ModelFunction* func=data4fit->modelfunc;
  unsigned int npars=func->numof_fitpars();
  ODINLOG(odinlog,normalDebug) << "n/npars=" << data4fit->n << "/" <<  npars << STD_endl;

  // copy Blitz arrays to data4fit
  for (unsigned long i=0;i<data4fit->n;i++) {
    data4fit->y[i]=yvals(i);
    if(haveSigma) data4fit->sigma[i]=ysigma(i);
    else data4fit->sigma[i]=0.1;
    if(haveXvals) data4fit->x[i]=xvals(i);
    else data4fit->x[i]=(float)i;
  }
  ODINLOG(odinlog,normalDebug) << "copy arrays done" << STD_endl;


  // initialize GSL fitting function
  gsl_multifit_function_fdf gsl_func;
  gsl_func.f=&FunctionFitDerivative_func_f;
  gsl_func.df=&FunctionFitDerivative_func_df;
  gsl_func.fdf=&FunctionFitDerivative_func_fdf;
  gsl_func.n=data4fit->n;
  gsl_func.p=npars;
  gsl_func.params=(void *)data4fit;
  ODINLOG(odinlog,normalDebug) << "gsl_func done" << STD_endl;

  // starting values
  double x_init[npars];
  for(unsigned int i=0; i<npars; i++) {
    x_init[i]=func->get_fitpar(i).val;
  }
  gsl_vector_view initial_guess = gsl_vector_view_array (x_init, npars);
  ODINLOG(odinlog,normalDebug) << "starting values done" << STD_endl;

  // initialize solver
  gsl_multifit_fdfsolver_set (gsldata->solver, &gsl_func, &initial_guess.vector);
  ODINLOG(odinlog,normalDebug) << "initialize solver done" << STD_endl;


  unsigned int iter = 0;
  int status;
  do {   // main loop
    ODINLOG(odinlog,normalDebug) << "iter= " << iter << STD_endl;
    iter++;
    status = gsl_multifit_fdfsolver_iterate (gsldata->solver);
    ODINLOG(odinlog,normalDebug) << "status(fit)= " << gsl_strerror (status) << STD_endl;
    print_state (iter);
    if (status!=GSL_SUCCESS) break;
    status = gsl_multifit_test_delta (gsldata->solver->dx, gsldata->solver->x, tolerance, tolerance);
    ODINLOG(odinlog,normalDebug) << "status(test)= " << gsl_strerror (status) << STD_endl;
    ODINLOG(odinlog,normalDebug) << "iter/max_iterations= " << iter << "/" << max_iterations << STD_endl;
  } while (status == GSL_CONTINUE && iter < max_iterations);

  if(status!=GSL_SUCCESS && status!=GSL_ENOPROG) { // ignoring error 'iteration is not making progress towards solution' and taking intermediate result as best fit
    ODINLOG(odinlog,errorLog) << gsl_strerror(status) << STD_endl;
    return false;
  }


  gsl_matrix* J=0;
#ifdef HAVE_GSL_MULTIFIT_FDFSOLVER_JAC // for GSL2
  ODINLOG(odinlog,normalDebug) << "solver->f->size=" << gsldata->solver->f->size << STD_endl;
  ODINLOG(odinlog,normalDebug) << "solver->x->size=" << gsldata->solver->x->size << STD_endl;
  J = gsl_matrix_alloc(gsldata->solver->f->size, gsldata->solver->x->size);
  status = gsl_multifit_fdfsolver_jac(gsldata->solver, J);
  if(status!=GSL_SUCCESS) {
    ODINLOG(odinlog,errorLog) << gsl_strerror(status) << STD_endl;
    return false;
  }
#else // pre GSL2
  J=gsldata->solver->J;
#endif

  status = gsl_multifit_covar(J, 0.0, gsldata->covar);
  if(status!=GSL_SUCCESS) {
    ODINLOG(odinlog,errorLog) << gsl_strerror(status) << STD_endl;
    return false;
  }

#ifdef HAVE_GSL_MULTIFIT_FDFSOLVER_JAC // for GSL2
  gsl_matrix_free(J);
#endif

  // copy results
  for(unsigned int i=0;i<npars;i++) {
    func->get_fitpar(i).val=gsl_vector_get(gsldata->solver->x, i);
    func->get_fitpar(i).err=sqrt(gsl_matrix_get(gsldata->covar,i,i));
  }

  return true;
}



void FunctionFitDerivative::print_state (size_t iter) {
#ifdef ODIN_DEBUG
  Log<OdinData> odinlog("FunctionFitDerivative","fit");
  ODINLOG(odinlog,normalDebug) << "iter=" << iter << STD_endl;
  for(unsigned int i=0;i<data4fit->modelfunc->numof_fitpars();i++) {
    ODINLOG(odinlog,normalDebug) << "  x" << i << "=" <<  gsl_vector_get (gsldata->solver->x,i) << STD_endl;
  }
  ODINLOG(odinlog,normalDebug) << " |f(x)|=" << fabs(gsl_blas_dnrm2 (gsldata->solver->f)) << STD_endl;
#endif
}




///////////////////////////////////////////////////////////////////

float ExponentialFunction::evaluate_f(float x) const {
  return A.val * exp (lambda.val * x);
}

fvector ExponentialFunction::evaluate_df(float x) const {
  fvector result(numof_fitpars());
  result[0]= exp (lambda.val * x);
  result[1]= x * A.val * exp (lambda.val * x);
  return result;
}

unsigned int ExponentialFunction::numof_fitpars() const {return 2;}

fitpar& ExponentialFunction::get_fitpar(unsigned int i) {
  if(i==0) return A;
  if(i==1) return lambda;
  return dummy_fitpar;
}

///////////////////////////////////////////////////////////////////


float ExponentialFunctionWithOffset::evaluate_f(float x) const {
  return A.val * exp (lambda.val * x) + C.val;
}

fvector ExponentialFunctionWithOffset::evaluate_df(float x) const {
  fvector result(numof_fitpars());
  result[0]=exp (lambda.val * x);
  result[1]=x * A.val * exp (lambda.val * x);
  result[2]=1.0;
  return result;
}

unsigned int ExponentialFunctionWithOffset::numof_fitpars() const {return 3;}

fitpar& ExponentialFunctionWithOffset::get_fitpar(unsigned int i) {
  if(i==0) return A;
  if(i==1) return lambda;
  if(i==2) return C;
  return dummy_fitpar;
}


///////////////////////////////////////////////////////////////////

float GaussianFunction::evaluate_f(float x) const {
  float arg= (x-x0.val) / fwhm.val;
  return A.val * exp( -2.0 * arg * arg );
}

fvector GaussianFunction::evaluate_df(float x) const {
  fvector result(numof_fitpars());

  float arg= (x-x0.val) / fwhm.val;
  float expterm=exp( -2.0 * arg * arg );

  result[0]= expterm;
  result[1]= 4.0*A.val/(fwhm.val*fwhm.val)*(x-x0.val)*expterm;
  result[2]= 4.0*A.val/(fwhm.val*fwhm.val*fwhm.val)*(x-x0.val)*(x-x0.val)*expterm;
  return result;
}

unsigned int GaussianFunction::numof_fitpars() const {return 3;}

fitpar& GaussianFunction::get_fitpar(unsigned int i) {
  if(i==0) return A;
  if(i==1) return x0;
  if(i==2) return fwhm;
  return dummy_fitpar;
}

///////////////////////////////////////////////////////////////////

float SinusFunction::evaluate_f(float x) const {
  return A.val * sin (m.val * x + c.val);
}

fvector SinusFunction::evaluate_df(float x) const {
  fvector result(numof_fitpars());
  result[0]= sin (m.val * x + c.val);
  result[1]= A.val * x * cos (m.val * x + c.val);
  result[2]= A.val * cos (m.val * x + c.val);
  return result;
}

unsigned int SinusFunction::numof_fitpars() const {return 3;}

fitpar& SinusFunction::get_fitpar(unsigned int i) {
  if(i==0) return A;
  if(i==1) return m;
  if(i==2) return c;
  return dummy_fitpar;
}

///////////////////////////////////////////////////////////////////

void GammaVariateFunction::set_pars(float alphaval, float xmax, float ymax) {
  A.val=ymax*pow(xmax,-alphaval)*exp(alphaval);
  alpha.val=alphaval;
  beta.val=xmax/alphaval;
}

float GammaVariateFunction::evaluate_f(float x) const {
  Log<OdinData> odinlog("GammaVariateFunction","evaluate_f");
  if(x<=0.0) {
    ODINLOG(odinlog,errorLog) << "function not defined for x=" << x << STD_endl;
    return 0.0;
  }
  return A.val*pow(x,alpha.val)*exp(-(x)/beta.val);
}

fvector GammaVariateFunction::evaluate_df(float x) const {
  Log<OdinData> odinlog("GammaVariateFunction","evaluate_df");
  fvector result(numof_fitpars());
  if(x<=0.0) {
    ODINLOG(odinlog,errorLog) << "function not defined for x=" << x << STD_endl;
    return result;
  }
  result[0] = pow(x,alpha.val)*exp(-x/beta.val);
  result[1] = A.val*pow(x,alpha.val)*exp(-x/beta.val)*log(x);
  result[2] = A.val*pow(x,alpha.val+float(1.0))*exp(-x/beta.val)/pow(beta.val,2);
  return result;
}

unsigned int GammaVariateFunction::numof_fitpars() const {return 3;}

fitpar& GammaVariateFunction::get_fitpar(unsigned int i) {
  if(i==0) return A;
  if(i==1) return alpha;
  if(i==2) return beta;
  return dummy_fitpar;
}

///////////////////////////////////////////////////////////////////

bool LinearFunction::fit(const Array<float,1>& yvals,
           const Array<float,1>& ysigma,
           const Array<float,1>& xvals) {
  Log<OdinData> odinlog("LinearFunction","fit");

  m=c=fitpar(); // reset

  unsigned int n=yvals.size();
  ODINLOG(odinlog,normalDebug) << "n=" << n << STD_endl;


  if(n<2) {
    ODINLOG(odinlog,errorLog) << "n=" << n << " too small" << STD_endl;
    return false;
  }

  bool haveSigma;
  bool haveXvals;

  if(ysigma.size()==n) haveSigma=true;
  else {
    haveSigma=false;
    ODINLOG(odinlog,normalDebug) << "no error bars provided, taking uniform interval for all x" << STD_endl;
  }
  if(xvals.size()==n) haveXvals=true;
  else {
    haveXvals=false;
    ODINLOG(odinlog,normalDebug) << "no x values provided, taking index as x value" << STD_endl;
  }

  Array<float,1> s(n);
  Array<float,1> x(n);

  if(haveSigma) s=ysigma;
  else s=1.0;

  if(haveXvals) x=xvals;
  else for(unsigned int i=0; i<n; i++) x(i)=float(i);

  if(n==2) {
    m.val=secureDivision(yvals(1)-yvals(0), x(1)-x(0));
    c.val=yvals(0)-x(0)*m.val;
    return true;
  }

  Array<float,1> s2(n);
  s2=s*s;

  float S=  sum(1.0/(s2));
  float Sx= sum(x  /(s2));
  float Sxx=sum(x*x/(s2));
  ODINLOG(odinlog,normalDebug) << "S/Sx/Sxx=" << S << "/" << Sx << "/" << Sxx << STD_endl;

  float Sy= sum(yvals/(s2));
  float Sxy=sum(x*yvals/(s2));
  ODINLOG(odinlog,normalDebug) << "Sy/Sxy=" << Sy << "/" << Sxy << STD_endl;

  float Delta=S*Sxx - Sx*Sx;
  ODINLOG(odinlog,normalDebug) << "Delta=" << Delta << STD_endl;

  float beta=secureDivision(S*Sxy-Sx*Sy, Delta);
  float alpha=secureDivision(Sxx*Sy-Sx*Sxy, Delta);
  ODINLOG(odinlog,normalDebug) << "alpha/beta=" << beta << "/" << beta << STD_endl;

  Array<float,1> summand(n);
  summand=(yvals-beta*x-alpha);
  summand*=summand;

  float ystdev=sqrt(secureInv(double(n)-2.0)*sum( summand ));
  ODINLOG(odinlog,normalDebug) << "ystdev=" << ystdev << STD_endl;

  float betastdev=ystdev*sqrt(1.0/( sum(x*x) - secureInv(n)*sum(x)*sum(x) ));
  ODINLOG(odinlog,normalDebug) << "betastdev=" << betastdev << STD_endl;

  float alphastdev=betastdev*sqrt( secureInv(n)*sum(x*x) );
  ODINLOG(odinlog,normalDebug) << "alphastdev=" << alphastdev << STD_endl;

  m.val=beta;
  m.err=betastdev;

  c.val=alpha;
  c.err=alphastdev;

  return true;
}

Array<float,1> LinearFunction::get_function(const Array<float,1>& xvals) const {
  return Array<float,1>(m.val * xvals + c.val);
}

const Array<float,1> LinearFunction::defaultArray;


//////////////////////////////////////////////////////////////

struct GslData4DownhillSimplex {
  gsl_vector* x;
  gsl_vector* ss;
  gsl_multimin_function minex_func;
  gsl_multimin_fminimizer* s;
};


double DownhillSimplex_func_f(const gsl_vector * gv, void *params) {

  // get current model function
  const  MinimizationFunction* minfunc=(const MinimizationFunction*)params;
  unsigned int n_fitpars=minfunc->numof_fitpars();

  fvector x(n_fitpars);
  for(unsigned int i=0; i<n_fitpars; i++) x[i]=gsl_vector_get(gv, i);
  return minfunc->evaluate(x);
}


DownhillSimplex::DownhillSimplex(MinimizationFunction& function) {
  ndim=function.numof_fitpars();

  gsldata=new GslData4DownhillSimplex;
  gsldata->x = gsl_vector_alloc(ndim);
  gsldata->ss = gsl_vector_alloc(ndim);

  gsldata->minex_func.n = ndim;
  gsldata->minex_func.f = &DownhillSimplex_func_f;
  gsldata->minex_func.params = &function;

#ifdef HAVE_GSL_MULTIMIN_FMINIMIZER_NMSIMPLEX2
  gsldata->s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex2, ndim); // newer algorithm, only available in recent GSL
#else
  gsldata->s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex, ndim);
#endif

}

DownhillSimplex::~DownhillSimplex() {
  // free GSL stuff
  gsl_vector_free(gsldata->x);
  gsl_vector_free(gsldata->ss);
  gsl_multimin_fminimizer_free(gsldata->s);
  delete gsldata;
}


bool DownhillSimplex::get_minimum_parameters(fvector& result, const fvector& starting_point, const fvector& step_size, unsigned int max_iterations, double tolerance) {
  Log<OdinData> odinlog("DownhillSimplex","get_minimum_parameters");

  result.resize(ndim);

  if(starting_point.size()!=ndim) {
    ODINLOG(odinlog,errorLog) << "size mismatch: starting_point.size()=" << starting_point.size() << ", ndim=" << ndim << STD_endl;
    return false;
  }
  if(step_size.size()!=ndim) {
    ODINLOG(odinlog,errorLog) << "size mismatch: starting_point.size()=" << starting_point.size() << ", ndim=" << ndim << STD_endl;
    return false;
  }
 
  for(unsigned int idim=0; idim<ndim; idim++) {
    gsl_vector_set (gsldata->x, idim, starting_point[idim]);
    gsl_vector_set (gsldata->ss, idim, step_size[idim]);
  }

  gsl_multimin_fminimizer_set (gsldata->s, &(gsldata->minex_func), gsldata->x, gsldata->ss);

  unsigned int niterations=0;
  int status;
  double size;

  do {
    niterations++;
    status = gsl_multimin_fminimizer_iterate(gsldata->s);
           
    if (status) break;
     
    size = gsl_multimin_fminimizer_size(gsldata->s);
    status = gsl_multimin_test_size(size, tolerance);
     
/*
          if (status == GSL_SUCCESS)
             {
               printf ("converged to minimum at\n");
             }
     
           printf ("%5d %10.3e %10.3e f() = %7.3f size = %.3f\n", 
                   iter,
                   gsl_vector_get (s->x, 0), 
                   gsl_vector_get (s->x, 1), 
                   s->fval, size);
*/

  } while (status == GSL_CONTINUE && niterations < max_iterations);

  for(unsigned int idim=0; idim<ndim; idim++) result[idim]=gsl_vector_get(gsldata->s->x, idim);

  ODINLOG(odinlog,normalDebug) << "result" << result << STD_endl;
  ODINLOG(odinlog,normalDebug) << "min value=" << gsldata->s->fval << STD_endl;
    
  return status==GSL_SUCCESS;
}

//////////////////////////////////////////////////////////////

FunctionFitDownhillSimplex::FunctionFitDownhillSimplex() : func(0), ds(0) {
}

FunctionFitDownhillSimplex::~FunctionFitDownhillSimplex() {
  delete ds;
}


bool FunctionFitDownhillSimplex::init(ModelFunction& model_func, unsigned int nvals) {
  Log<OdinData> odinlog("FunctionFitDownhillSimplex","init");
  func=&model_func;
  ODINLOG(odinlog,normalDebug) << "initialized, numof_fitpars=" << func->numof_fitpars() << STD_endl;

  if(!ds) ds=new DownhillSimplex(*this); // has to be initialized after func was assigned

  yvals_cache.resize(nvals);
  ysigma_cache.resize(nvals);
  xvals_cache.resize(nvals);

  return true;
}


bool FunctionFitDownhillSimplex::fit(const Array<float,1>& yvals,
           const Array<float,1>& ysigma,
           const Array<float,1>& xvals,
           unsigned int max_iterations, double tolerance) {
  Log<OdinData> odinlog("FunctionFitDownhillSimplex","fit");

  if(!ds) {
    ODINLOG(odinlog,errorLog) << "not initialized" << STD_endl;
    return false;
  }

  if(yvals.size() != yvals_cache.size()) {
    ODINLOG(odinlog,errorLog) << "size mismatch in yvals" << STD_endl;
    return false;
  }
  yvals_cache=yvals;

  bool has_ysigma=false;
  if(ysigma.size()) {
    if(ysigma.size()!= ysigma_cache.size()) {
      ODINLOG(odinlog,errorLog) << "size mismatch in ysigma" << STD_endl;
      return false;
    }
    ysigma_cache=ysigma;
    has_ysigma=true;
  } else {
    ysigma_cache=0.0;
  }


  if(xvals.size()) {
    if(xvals.size()!= xvals_cache.size()) {
      ODINLOG(odinlog,errorLog) << "size mismatch in xvals" << STD_endl;
      return false;
    }
    xvals_cache=xvals;
  } else {
    for(unsigned int i=0; i<xvals_cache.size(); i++) xvals_cache(i)=i;
  }

  unsigned int npars=numof_fitpars();

  fvector starting_point(npars);
  fvector step_size(npars);
  for(unsigned int ipar=0; ipar<npars; ipar++) {
    float fval=func->get_fitpar(ipar).val;
    starting_point[ipar]=fval;
    step_size[ipar]=0.1*fabs(fval); // 10% in positive direction
  }


  if(has_ysigma) {
    int nruns=1000;

    STD_vector<Array<float,1> > fitpar_ensemble(npars);
    for(unsigned int ipar=0; ipar<npars; ipar++) fitpar_ensemble[ipar].resize(nruns);

    Array<float,1> yvals_cache_orig(yvals_cache.copy());
    RandomDist rd;

    for(int irun=0; irun<nruns; irun++) {
      for(unsigned int i=0; i<yvals_cache.size(); i++) yvals_cache(i)=yvals_cache_orig(i)+rd.gaussian(ysigma_cache(i));

      fvector mcminvec;
      if(!ds->get_minimum_parameters(mcminvec, starting_point, step_size, max_iterations, tolerance)) {
        return false;
      }
      for(unsigned int ipar=0; ipar<npars; ipar++) fitpar_ensemble[ipar](irun)=mcminvec[ipar];

    }

    for(unsigned int ipar=0; ipar<npars; ipar++) {
      ODINLOG(odinlog,normalDebug) << "fitpar_ensemble[" << ipar << "]" << fitpar_ensemble[ipar] << STD_endl;
      func->get_fitpar(ipar).err=statistics(fitpar_ensemble[ipar]).stdev;
    }

    yvals_cache=yvals_cache_orig;
  }


  // final run without noise
  fvector minvec;
  if(!ds->get_minimum_parameters(minvec, starting_point, step_size, max_iterations, tolerance)) {
    return false;
  }
  for(unsigned int ipar=0; ipar<npars; ipar++) func->get_fitpar(ipar).val=minvec[ipar];

  return true;
}


unsigned int FunctionFitDownhillSimplex::numof_fitpars() const {
  Log<OdinData> odinlog("FunctionFitDownhillSimplex","numof_fitpars");
  if(func) return func->numof_fitpars();
  else ODINLOG(odinlog,errorLog) << "not initialized" << STD_endl;
  return 0;
}


float FunctionFitDownhillSimplex::evaluate(const fvector& pars) const {
  Log<OdinData> odinlog("FunctionFitDownhillSimplex","evaluate");
  double result=0.0; // double for higher accuracy when summing up small numbers

  unsigned int npars=numof_fitpars();
  if(pars.size() != npars) {
    ODINLOG(odinlog,errorLog) << "size mismatch in npars" << STD_endl;
    return result;
  }

  ODINLOG(odinlog,normalDebug) << "pars" << pars << STD_endl;

  for(unsigned int ipar=0; ipar<npars; ipar++) func->get_fitpar(ipar).val=pars[ipar];

  for(unsigned int i=0; i<xvals_cache.size(); i++) {
    float f=func->evaluate_f(xvals_cache(i));
    result+=pow(f-yvals_cache(i),2);
  }

  return result;
}

//////////////////////////////////////////////////////////////
// Unit test

#ifndef NO_UNIT_TEST


class DownhillSimplexTestFunction : public MinimizationFunction {

  unsigned int numof_fitpars() const {return 2;}
  float evaluate(const fvector& xvec) const {return pow(xvec[0]-2.0,2)+pow(xvec[1]-3.0,2);}

};


typedef STD_map<STD_string,FunctionFitInterface*> FitterMap;

class FunctionFitTest : public UnitTest {

 public:
  FunctionFitTest() : UnitTest("FunctionFit") {}

 private:
  bool check() const {
    Log<UnitTest> odinlog(this,"check");

    /////////////////////////////////////////////////////

    DownhillSimplexTestFunction dstf;
    DownhillSimplex ds(dstf);

    fvector starting_point(2);
    starting_point=0.0;

    fvector step_size(2);
    step_size=1.0;

    fvector dsresult;
    if(!ds.get_minimum_parameters(dsresult,starting_point, step_size)) {
      ODINLOG(odinlog,errorLog) << "DownhillSimplex fit failed" << STD_endl;
    }

    fvector dsexpected(2);
    dsexpected[0]=2.0;
    dsexpected[1]=3.0;
    float maxdiff=(dsresult-dsexpected).maxabs();
    ODINLOG(odinlog,normalDebug) << "maxdiff=" << maxdiff << STD_endl;
    if((dsresult-dsexpected).maxabs()>1.0e-3) {
      ODINLOG(odinlog,errorLog) << "DownhillSimplex failed, result" << dsresult << ", but expected" << dsexpected << STD_endl;
      return false;
    }

    /////////////////////////////////////////////////////

    FitterMap fittermap;
    fittermap["FunctionFitDerivative"]=new FunctionFitDerivative;
    fittermap["FunctionFitDownhillSimplex"]=new FunctionFitDownhillSimplex;

    int numoftestvals=5;
    Array<float,1> yvals;
    Array<float,1> ysigma;
    Array<float,1> xvals;

    for(FitterMap::iterator it=fittermap.begin(); it!=fittermap.end(); ++it) {

      STD_string fitterlabel=it->first;
      FunctionFitInterface* fitter=it->second;

/*

      ExponentialFunctionWithOffset expf_offset;
      if(!fitter->init(expf_offset,numoftestvals)) return false;

      yvals.resize(numoftestvals);
      ysigma.resize(numoftestvals);
      xvals.resize(numoftestvals);

      // some arbitrary values for testing
      xvals(0)=0.1; yvals(0)=12.4; ysigma(0)=0.7;
      xvals(1)=1.2; yvals(1)=8.4;  ysigma(1)=1.2;
      xvals(2)=1.9; yvals(2)=6.1;  ysigma(2)=0.9;
      xvals(3)=3.0; yvals(3)=5.0;  ysigma(3)=0.8;
      xvals(4)=4.1; yvals(4)=5.0;  ysigma(4)=1.0;


      expf_offset.A.val=1.0;  // starting value
      expf_offset.lambda.val=-1.0; // starting value
      expf_offset.C.val=1.0;  // starting value

      if(!fitter->fit(yvals,ysigma,xvals)) {
        ODINLOG(odinlog,errorLog) << fitterlabel << "(ExponentialFunctionWithOffset) fit failed" << STD_endl;
        return false;
      } 

      float A_expected=8.77292;
      float A_err_expected=1.34177;
      float lambda_expected=-0.76177;
      float lambda_err_expected=0.37545;
      float C_expected=4.30505;
      float C_err_expected=1.33666;

      STD_string A_str              =ftos(expf_offset.A.val) +"+-"+ftos(expf_offset.A.err);
      STD_string A_expected_str     =ftos(A_expected    ) +"+-"+ftos(A_err_expected);
      STD_string lambda_str         =ftos(expf_offset.lambda.val) +"+-"+ftos(expf_offset.lambda.err);
      STD_string lambda_expected_str=ftos(lambda_expected)+"+-"+ftos(lambda_err_expected);
      STD_string C_str              =ftos(expf_offset.C.val) +"+-"+ftos(expf_offset.C.err);
      STD_string C_expected_str     =ftos(C_expected)     +"+-"+ftos(C_err_expected);


      // gives different results depending on the GSL version
      if(A_str     !=A_expected_str)      {ODINLOG(odinlog,errorLog) << fitterlabel << "(ExponentialFunctionWithOffset) failed: A="      << A_str      << ", but expected A="      << A_expected_str      << STD_endl; return false;}
      if(lambda_str!=lambda_expected_str) {ODINLOG(odinlog,errorLog) << fitterlabel << "(ExponentialFunctionWithOffset) failed: lambda=" << lambda_str << ", but expected lambda=" << lambda_expected_str << STD_endl; return false;}
      if(C_str     !=C_expected_str)      {ODINLOG(odinlog,errorLog) << fitterlabel << "(ExponentialFunctionWithOffset) failed: C="      << C_str      << ", but expected C="      << C_expected_str      << STD_endl; return false;}
*/

      /////////////////////////////////////////////////////

      int numoftestvals2=100;

      ExponentialFunction expf;
      if(!fitter->init(expf,numoftestvals2)) return false;

      // arbitrary values for testing
      float A_expected=44.5;
      float A_err_expected=0.0124275;
      float lambda_expected=0.78;
      float lambda_err_expected=0.000184347;

      expf.A.val=A_expected;
      expf.lambda.val=lambda_expected;

      yvals.resize(numoftestvals2);
      ysigma.resize(0); // no error bars
      xvals.resize(numoftestvals2);

      for(int i=0; i<numoftestvals2; i++) xvals(i)=2.0 * double(i)/double(numoftestvals2);

      // check self integrity of fit
      yvals=expf.get_function(xvals);

      // starting values
      expf.A.val=1.0;
      expf.lambda.val=-1.0;


      if(!fitter->fit(yvals,ysigma,xvals)) {
        ODINLOG(odinlog,errorLog) << fitterlabel << "(ExponentialFunction) fit failed" << STD_endl;
        return false;
      }


      float tolerance=1e-4;
      if( (fabs(expf.A.val-     A_expected)     /A_expected)     >tolerance ) {ODINLOG(odinlog,errorLog) << fitterlabel << "(ExponentialFunction) failed: A="      << expf.A.val      << ", but expected A="      << A_expected      << STD_endl; return false;}
      if( (fabs(expf.lambda.val-lambda_expected)/lambda_expected)>tolerance ) {ODINLOG(odinlog,errorLog) << fitterlabel << "(ExponentialFunction) failed: lambda=" << expf.lambda.val << ", but expected lambda=" << lambda_expected << STD_endl; return false;}

      // test error only if provided, i.e. if err is non-zero
      if( expf.A.err!=0.0      && (fabs(expf.A.err-     A_err_expected)     /A_err_expected)     >tolerance ) {ODINLOG(odinlog,errorLog) << fitterlabel << "(ExponentialFunction) failed: A_err="      << expf.A.err      << ", but expected A_err="      << A_err_expected      << STD_endl; return false;}
      if( expf.lambda.err!=0.0 && (fabs(expf.lambda.err-lambda_err_expected)/lambda_err_expected)>tolerance ) {ODINLOG(odinlog,errorLog) << fitterlabel << "(ExponentialFunction) failed: lambda_err=" << expf.lambda.err << ", but expected lambda_err=" << lambda_err_expected << STD_endl; return false;}

//      ODINLOG(odinlog,normalDebug) << fitterlabel << ": A.err/lambda.err=" << expf.A.err << "/" << expf.lambda.err << STD_endl;

    } // end iteration over fittermap

    /////////////////////////////////////////////////////

    LinearFunction linf;

    yvals.resize(numoftestvals);
    ysigma.resize(numoftestvals);
    xvals.resize(numoftestvals);

    // some arbitrary values for testing
    xvals(0)=0.1; yvals(0)=2.4;  ysigma(0)=0.7;
    xvals(1)=1.2; yvals(1)=3.4;  ysigma(1)=1.2;
    xvals(2)=1.9; yvals(2)=4.1;  ysigma(2)=0.9;
    xvals(3)=3.0; yvals(3)=5.0;  ysigma(3)=0.8;
    xvals(4)=4.1; yvals(4)=6.0;  ysigma(4)=1.0;

    if(!linf.fit(yvals,ysigma,xvals)) {
      ODINLOG(odinlog,errorLog) << "LinearFunction fit failed" << STD_endl;
      return false;
    }

    float m_expected=0.89863;
    float m_err_expected=0.01351;
    float c_expected=2.32634;
    float c_err_expected=0.03357;

    STD_string m_str              =ftos(linf.m.val) +"+-"+ftos(linf.m.err);
    STD_string m_expected_str     =ftos(m_expected) +"+-"+ftos(m_err_expected);
    STD_string c_str              =ftos(linf.c.val) +"+-"+ftos(linf.c.err);
    STD_string c_expected_str     =ftos(c_expected) +"+-"+ftos(c_err_expected);

    if(m_str     !=m_expected_str)      {ODINLOG(odinlog,errorLog) << "LinearFunction failed: m="      << m_str      << ", but expected m="      << m_expected_str      << STD_endl; return false;}
    if(c_str     !=c_expected_str)      {ODINLOG(odinlog,errorLog) << "LinearFunction failed: c="      << c_str      << ", but expected c="      << c_expected_str      << STD_endl; return false;}

    /////////////////////////////////////////////////////

    const int polydegree=4;
    PolynomialFunction<polydegree> polyf;

    float a_expected[polydegree];
    polyf.a[0].val=a_expected[0]=-4.0;
    polyf.a[1].val=a_expected[1]=0.5;
    polyf.a[2].val=a_expected[2]=0.0;
    polyf.a[3].val=a_expected[3]=-1.0;

    int numoftestvals3=100;
    Array<float,1> xvals3(numoftestvals3);
    Array<float,1> ysigma3(numoftestvals3);
    for(int i=0; i<numoftestvals3; i++) {
      xvals3(i)=float(i)/float(numoftestvals3)-0.5;
      ysigma3(i)=fabs(sin(i+0.5)); // jittered error
    }

    Array<float,1> yvals3(polyf.get_function(xvals3));

//    Data<float,1>(yvals3).autowrite("yvals3.asc");
//    Data<float,1>(ysigma3).autowrite("ysigma3.asc");

    if(!polyf.fit(yvals3,ysigma3,xvals3)) {
      ODINLOG(odinlog,errorLog) << "PolynomialFunction fit failed" << STD_endl;
      return false;
    }

    for(int i=0; i<polydegree; i++) {
      float fitval=polyf.a[i].val;
      float expval=a_expected[i];
      if((fitval-expval)>1.0e-3) {
        ODINLOG(odinlog,errorLog) << "PolynomialFunction failed: a[" << i << "]=" << fitval << ", but expected " << expval << STD_endl;
        return false;
      }
    }


    /////////////////////////////////////////////////////

    Array<float,2> values(3,3);
    values=10.0;
    values(1,1)=0.0;

    Array<float,2> reliability(3,3);
    reliability=1.0;
    reliability(1,1)=0.0;

    Data<float,2> pfresult(3,3);

    pfresult=polyniomial_fit(values,reliability,1,2.0);
    if(fabs(pfresult(1,1)-10.0)>1.0e-3) {
      ODINLOG(odinlog,errorLog) << "values=" << values << STD_endl;
      ODINLOG(odinlog,errorLog) << "reliability=" << reliability << STD_endl;
      ODINLOG(odinlog,errorLog) << "pfresult=" << pfresult << STD_endl;
      ODINLOG(odinlog,errorLog) << "polyniomial_fit failed" << STD_endl;
      return false;
    }


    int testsize=20;
    Data<float,2> testarray(testsize,testsize);
    Data<float,2> testrelia(testsize,testsize);
    TinyVector<int,2> index;
    for(unsigned int i=0; i<testarray.numElements(); i++) {
      index=testarray.create_index(i);
      float radius=norm((index(0)-testsize/2),(index(1)-testsize/2));
      radius/=(0.5*float(testsize));
      if(radius<(2.0/3.0)) {
        testarray(index)=radius*radius;
        testrelia(index)=1.0;
      } else {
        testarray(index)=0.0;
        testrelia(index)=0.0;
      }
    }

    pfresult.resize(testsize,testsize);
    pfresult=polyniomial_fit(testarray,testrelia,2,5.0);
    pfresult=pfresult*testrelia;

    float diff=sum(fabs(testarray-pfresult));
    if(diff>1.0e-3) {
      ODINLOG(odinlog,errorLog) << "polyniomial_fit failed, diff=" << diff << STD_endl;
      return false;
    }

    return true;

  }

};

void alloc_FunctionFitTest() {new FunctionFitTest();} // create test instance
#endif



//////////////////////////////////////////////////////////////


#else
#error "GNU Scientific library is missing!"
#endif