#ifndef __JFIT__JGANDALF__
#define __JFIT__JGANDALF__

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

#include "Jeep/JMessage.hh"
#include "JMath/JVectorND.hh"
#include "JMath/JMatrixNS.hh"
#include "JMath/JZero.hh"
#include "JLang/JManip.hh"
#include "JLang/JException.hh"


/**
 * \author mdejong
 */

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

namespace JFIT {

  using JEEP::JMessage;
  using JLANG::JException;

  
  /**
   * Fit method based on the Levenberg-Marquardt method.
   *
   * The template argument refers to the model that should be fitted to the data.\n
   * This data structure should have arithmetic capabilities.
   *
   * The data member JGandalf::value corresponds to the start c.q.\ final value of
   * the model of the fit procedure and JGandalf::error to the uncertainties.\n
   * The co-variance matrix is stored in data member JGandalf::V.\n
   *
   * The data member JGandalf::parameters constitutes a list of those parameters of the model that should actually be fitted.\n
   * For this, the model should contain the type definition for <tt>parameter_type</tt>.\n
   * Normally, this type definition corresponds to a pointer to a data member of the model.\n
   * Alternatively, the type definition can be <tt>size_t</tt> or <tt>int</tt>.\n
   * In that case, the model class should provide for the element access <tt>operator[]</tt>.\n
   *
   * The first template parameter in the function operator should provide for an implementation of the actual fit function.\n
   * This function should return the data type JGandalf::result_type.\n
   * This data type comprises the values of the chi2 and the gradient for a given data point.\n
   * The function operator returns the minimal chi2 and combined gradient of the data points.
   */
  template<class JModel_t>
  class JGandalf :
    public JMessage< JGandalf<JModel_t> >
  {
  public:

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


    /**
     * Data type of fit parameter.
     */
    typedef typename JModel_t::parameter_type  parameter_type;


    /**
     * Data structure for return value of fit function.
     */
    struct result_type {
      /**
       * Default constructor.
       */
      result_type() :
	chi2    (0.0),
	gradient()
      {}
      

      /**
       * Constructor.
       *
       * \param  chi2         chi2
       * \param  model        gradient
       */
      result_type(const double    chi2,
		  const JModel_t& model) :
	chi2    (chi2),
	gradient(model)
      {}
      

      /**
       * Type conversion operator.
       *
       * \return              chi2
       */
      operator double() const 
      {
	return chi2;
      }


      double   chi2;                  //!< chi2
      JModel_t gradient;              //!< partial derivatives of chi2
    };


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


    /**
     * Multi-dimensional fit of multiple data sets.
     *
     * The fit function should return the chi2 as well as the partial derivatives
     * for the current value of the model and a given data point.
     *
     * \param  fit            fit function
     * \param  __begin        begin of data
     * \param  __end          end   of data
     * \param  args           optional data
     * \return                chi2 and gradient
     */
    template<class JFunction_t, class T, class ...Args>
    result_type operator()(const JFunction_t& fit, T __begin, T __end, Args ...args)
    {
      using namespace std;
      using namespace JPP;

      // note that all model values should be assigned to the start value of the model before use
      // because the actual list of model parameters can vary from fit to fit
      // (e.g. if model consists of a container).

      const size_t N = parameters.size();

      V.resize(N);
      h.resize(N);
      x.resize(N);

      previous.result.chi2 = numeric_limits<double>::max();

      current.result.chi2     = numeric_limits<double>::max();
      current.result.gradient = value;
      current.result.gradient = zero;

      error     = value;
      error     = zero;

      lambda    = LAMBDA_MIN;


      for (numberOfIterations = 0; numberOfIterations != MAXIMUM_ITERATIONS; ++numberOfIterations) {

        DEBUG("step:     " << numberOfIterations << endl);

	reset();

	update(fit, __begin, __end, args...);

	DEBUG("lambda:   " << FIXED(12,5) << lambda              << endl);
        DEBUG("chi2:     " << FIXED(12,5) << current.result.chi2 << endl);

	if (current.result.chi2 < previous.result.chi2) {

	  if (numberOfIterations != 0) {

	    if (fabs(previous.result.chi2 - current.result.chi2) < EPSILON*fabs(previous.result.chi2)) {

	      const result_type result = current.result;

	      reset();

	      update(fit, __begin, __end, args...);

	      try {
		V.invert();
	      }
	      catch (const exception& error) {
		V.reset();
	      }

	      return result;                                        // normal end
	    }

	    if (lambda > LAMBDA_MIN) {
	      lambda /= LAMBDA_DOWN;
	    }

	    // store current values

	    previous.value  = value;
	    previous.error  = error;
	    previous.result = current.result;
	  }

	} else {

	  value   = previous.value;                                 // restore value

	  lambda *= LAMBDA_UP;
	  
	  if (lambda > LAMBDA_MAX) {
	    break;
	  }

	  reset();

	  update(fit, __begin, __end, args...);
	}

	DEBUG("Hesse matrix:" << endl << V << endl);

	// force definite positiveness

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

	  if (V(i,i) < PIVOT) {
	    V(i,i) = PIVOT;
	  }

	  h[i] = 1.0 / sqrt(V(i,i));
	}

	// normalisation
	
	for (size_t row = 0; row != N; ++row) {
	  for (size_t col = 0; col != row; ++col) {
	    V(row,col) *= h[row] * h[col];
	    V(col,row)  = V(row,col);
	  }
	}
	
	for (size_t i = 0; i != N; ++i) {
	  V(i,i) = 1.0 + lambda;
	}

	// solve A x = b

	for (size_t col = 0; col != N; ++col) {
	  x[col] = h[col] * get(current.result.gradient, parameters[col]);
	}

	try {
	  V.solve(x);
	}
	catch (const exception& error) {

	  ERROR("JGandalf: " << error.what() << endl << V << endl);

	  break;
	}

	// update value and error

	for (size_t row = 0; row != N; ++row) {

	  DEBUG("u[" << noshowpos << setw(3) << row << "] = " << showpos << FIXED(15,5) << get(value, parameters[row]));

	  get(value, parameters[row]) -= h[row] * x[row];
	  get(error, parameters[row])  = h[row];

	  DEBUG(" -> " << FIXED(15,5) << get(value, parameters[row]) << noshowpos << endl);
	}
      }

      // abnormal end

      const result_type result = previous.result;

      value   = previous.value;                                     // restore value
      error   = previous.error;                                     // restore error

      reset();

      update(fit, __begin, __end, args...);

      try {
	V.invert();
      }
      catch (const exception& error) {
	V.reset();
      }

      return result;
    }


    static int     MAXIMUM_ITERATIONS;                         //!< maximal number of iterations
    static double  EPSILON;                                    //!< maximal distance to minimum
    static double  LAMBDA_MIN;                                 //!< minimal value control parameter
    static double  LAMBDA_MAX;                                 //!< maximal value control parameter
    static double  LAMBDA_UP;                                  //!< multiplication factor control parameter
    static double  LAMBDA_DOWN;                                //!< multiplication factor control parameter
    static double  PIVOT;                                      //!< minimal value diagonal element of Hesse matrix

    std::vector<parameter_type> parameters;                    //!< fit parameters
    int                         numberOfIterations;            //!< number of iterations
    double                      lambda;                        //!< control parameter
    JModel_t                    value;                         //!< value
    JModel_t                    error;                         //!< error
    JMATH::JMatrixNS            V;                             //!< Hesse matrix

  private:
    /**
     * Reset current parameters.
     */
    void reset()
    {
      using namespace JPP;

      current.result.chi2     = 0.0;
      current.result.gradient = zero;

      V.reset();	  
    }


    /**
     * Recursive method to update current parameters.
     *
     * \param  fit            fit function
     * \param  __begin        begin of data
     * \param  __end          end   of data
     * \param  args           optional data
     */
    template<class JFunction_t, class T, class ...Args>
    inline void update(const JFunction_t& fit, T __begin, T __end, Args ...args)
    {
      for (T i = __begin; i != __end; ++i) {

	const result_type& result = fit(value, *i);

	current.result.chi2     += result.chi2;
	current.result.gradient += result.gradient;

	for (size_t row = 0; row != parameters.size(); ++row) {
	  for (size_t col = row; col != parameters.size(); ++col) {
	    V(row,col) += get(result.gradient, parameters[row]) * get(result.gradient, parameters[col]);
	  }
	}
      }

      update(fit, args...);
    }


    /**
     * Termination method to update current parameters.
     *
     * \param  fit            fit function
     */
    template<class JFunction_t>
    inline void update(const JFunction_t& fit)
    {
      for (size_t row = 0; row != parameters.size(); ++row) {
	for (size_t col = 0; col != row; ++col) {
	  V(row,col) = V(col,row);
	}
      }
    }


    /**
     * Read/write access to parameter value by data member.
     *
     * \param  model                    model
     * \param  parameter                parameter
     * \return                          value
     */
    static inline double get(const JModel_t& model, double JModel_t::*parameter)
    {
      return model.*parameter;
    }


    /**
     * Read/write access to parameter value by data member.
     *
     * \param  model                    model
     * \param  parameter                parameter
     * \return                          value
     */
    static inline double& get(JModel_t& model, double JModel_t::*parameter)
    {
      return model.*parameter;
    }


    /**
     * Read/write access to parameter value by index.
     *
     * \param  model                    model
     * \param  index                    index
     * \return                          value
     */
    static inline double get(const JModel_t& model, const size_t index)
    {
      return model[index];
    }


    /**
     * Read/write access to parameter value by index.
     *
     * \param  model                    model
     * \param  index                    index
     * \return                          value
     */
    static inline double& get(JModel_t& model, const size_t index)
    {
      return model[index];
    }


    /**
     * Read/write access to parameter value by index.
     *
     * \param  model                    model
     * \param  index                    index
     * \return                          value
     */
    static inline double get(const JModel_t& model, const int index)
    {
      return model[index];
    }


    /**
     * Read/write access to parameter value by index.
     *
     * \param  model                    model
     * \param  index                    index
     * \return                          value
     */
    static inline double& get(JModel_t& model, const int index)
    {
      return model[index];
    }

    std::vector<double>         h;                             // normalisation vector
    JMATH::JVectorND            x;

    struct {
      result_type               result;                        // result
    } current;

    struct {
      JModel_t                  value;                         // value
      JModel_t                  error;                         // error
      result_type               result;                        // result
    } previous;
  };


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


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


  /**
   * minimal value control parameter
   */
  template<class JModel_t>
  double JGandalf<JModel_t>::LAMBDA_MIN      =   0.01;


  /**
   * maximal value control parameter
   */
  template<class JModel_t>
  double JGandalf<JModel_t>::LAMBDA_MAX      = 100.0;


  /**
   * multiplication factor control parameter
   */
  template<class JModel_t>
  double JGandalf<JModel_t>::LAMBDA_UP       =  10.0;


  /**
   * multiplication factor control parameter
   */
  template<class JModel_t>
  double JGandalf<JModel_t>::LAMBDA_DOWN     =  10.0;


  /**
   * minimal value diagonal element of matrix
   */
  template<class JModel_t>
  double JGandalf<JModel_t>::PIVOT           =   std::numeric_limits<double>::epsilon();
}

#endif