#ifndef __JFIT__JSIMPLEX__
#define __JFIT__JSIMPLEX__

#include <cmath>
#include <vector>
#include <ostream>
#include <iomanip>

#include "JLang/JManip.hh"
#include "Jeep/JMessage.hh"


/**
 * \author mdejong
 */

namespace JFIT {}
namespace JPP { using namespace JFIT; }

namespace JFIT {

  using JEEP::JMessage;


  /**
   * Simple fit method based on Powell's algorithm, see reference:
   * Numerical Recipes in C++, W.H. Press, S.A. Teukolsky, W.T. Vetterling and B.P. Flannery,
   * Cambridge University Press.
   *
   * The template argument refers to the model that should be fitted to the data.
   * This data structure should have arithmetic capabalities.
   *
   * The data member JSimplex::value corresponds to the start, current or final value
   * of the model of the fit procedure.
   * The data member JSimplex::step corresponds to the step directions.
   * Note that the step directions may change during the fit.
   * The template fit function should return the data type JGandalf::result_type
   * which is composed of the values of the chi2 and gradient of a hit.
   * The function operator returns the chi2 of the fit.
   */
  template<class JModel_t>
  class JSimplex :
    public JMessage< JSimplex<JModel_t> >
  {
  public:

    typedef double                                      result_type;

    using JMessage< JSimplex<JModel_t> >::debug;


    /**
     * Default constructor.
     */
    JSimplex()
    {}


    /**
     * Multi-dimensional fit.
     *
     * The given fit function should return the equivalent of chi2 for 
     * the current value of the given model and a given data point.
     *
     * \param  fit            fit function
     * \param  __begin        begin of data
     * \param  __end          end   of data
     * \return                chi2
     */
    template<class JFunction_t, class T>
    double operator()(const JFunction_t& fit, T __begin, T __end)
    {
      using namespace std;
      using namespace JPP;

      double chi2_old = evaluate(fit, __begin, __end);

      const int N = step.size();

      if (N != 0) {

	p0   = value;
	p1   = value;
	wall = value;

	double chi2[N];

	numberOfIterations = 0;

	while (numberOfIterations != MAXIMUM_ITERATIONS) {

	  DEBUG("old: " << FIXED(12,5) << chi2_old << endl);

	  p0 = value;

	  for (int i = 0; i != N; ++i) {

	    DEBUG("step: " << i << ' ' << setw(5) << numberOfIterations << endl);

	    chi2[i] = (*this)(fit, __begin, __end, step[i]);
	  }

	  // overall step direction of last iteration

	  wall  = value;
	  wall -= p0;

	  const double chi2_new = (*this)(fit, __begin, __end, wall);

	  DEBUG("new: " << FIXED(12,5) << chi2_new << endl);


	  if (fabs(chi2_old - chi2_new) < EPSILON*fabs(chi2_old)) {
	    return chi2_new;
	  }

	  // double overall step

	  wall   = value;
	  wall  -= p0;
	  value += wall;

	  const double fe = evaluate(fit, __begin, __end);

	  value -= wall;


	  for (int i = N-1; i != 0; --i) {
	    chi2[i] = chi2[i-1] - chi2[i];
	  }

	  chi2[0] = chi2_old - chi2[0];


	  double df = 0.0;

	  for (int i = 0; i != N; ++i) {
	    if (chi2[i] > df) {
	      df = chi2[i];
	    }
	  }

	  const double fn = chi2_new;
	  const double f0 = chi2_old;
	  const double ff = f0 - fn - df;

	  // shift step directions

	  if (fe < f0 && 2.0*(f0 - 2.0*fn + fe)*ff*ff < (f0-fe)*(f0-fe)*df) {

	    for (int i = 0; i != N - 1; ++i) {
	      step[i] = step[i+1];
	    }

	    step[N-1] = wall;
	  }

	  chi2_old = chi2_new;
	}
      }

      return chi2_old;
    }


    /**
     * 1D fit.
     *
     * The given fit function should return the equivalent of chi2 for 
     * the current value of the given model and a given data point.
     *
     * \param  fit            fit function
     * \param  __begin        begin of data
     * \param  __end          end   of data
     * \param  step           step direction
     */
    template<class JFunction_t, class T>
    double operator()(const JFunction_t& fit, T __begin, T __end, const JModel_t& step)
    {
      using namespace std;
      using namespace JPP;

      double lambda = 0.5;                                     // control parameter
      double factor = 1.0;                                     // multiplication factor

      double chi2_old = evaluate(fit, __begin, __end);

      for (int i = 0; numberOfIterations != MAXIMUM_ITERATIONS; ++numberOfIterations, ++i) {

	p1      = step;
	p1     *= lambda;
	value  += p1;

        const double chi2_new = evaluate(fit, __begin, __end);

	DEBUG("step: " << setw(3) << i << ' ' << FIXED(12,5) << chi2_old << ' ' << FIXED(12,5) << chi2_new << ' ' << FIXED(5,2) << lambda << endl);

	if (fabs(chi2_old - chi2_new) < EPSILON*fabs(chi2_old)) {

	  if (chi2_new > chi2_old) {

	    p1      = step;
	    p1     *= lambda;
	    value  -= p1;                                      // undo last step

	    return chi2_old;

	  } else {

	    return chi2_new;
	  }
	}

	if (chi2_new < chi2_old) {

	  chi2_old = chi2_new;

	} else {
	    
	  p1      = step;
	  p1     *= lambda;
          value  -=  p1;                                       // step back
	  lambda  = -lambda;                                   // change direction
	  
	  if (i != 0) {
	    factor = 0.5;                                      // reduce step size
	  }
	}
	
	lambda = factor * lambda;
      }

      return chi2_old;
    }


    static int     MAXIMUM_ITERATIONS;                         //!< maximal number of iterations
    static double  EPSILON;                                    //!< maximal distance to minimum
 
    JModel_t              value;
    std::vector<JModel_t> step;
    int                   numberOfIterations;

  private:
    /**
     * Evaluate chi2 for given data set.
     *
     * \param  fit            fit function
     * \param  __begin        begin of data
     * \param  __end          end   of data
     */
    template<class JFunction_t, class T>
    inline double evaluate(const JFunction_t& fit, T __begin, T __end) const
    {
      double chi2 = 0.0;

      for (T hit = __begin; hit != __end; ++hit) {
        chi2 += fit(value, *hit);
      }

      return chi2;
    }

    mutable JModel_t p0;
    mutable JModel_t p1;
    mutable JModel_t wall;
  };


  /**
   * maximal number of iterations.
   */
  template<class JModel_t>
  int JSimplex<JModel_t>::MAXIMUM_ITERATIONS = 1000;


  /**
   * maximal distance to minimum.
   */
  template<class JModel_t>
  double JSimplex<JModel_t>::EPSILON = 1.0e-4;
}

#endif