#ifndef __JCOMPASS_JMODEL__
#define __JCOMPASS_JMODEL__

#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JEigen3D.hh"
#include "JLang/JException.hh"
#include "JMath/JMath.hh"
#include "JMath/JMathToolkit.hh"
#include "JLang/JSharedPointer.hh"
#include "JFit/JMEstimator.hh"

#include "JCompass/JHit.hh"


/**
 * \author mdejong
 */
namespace JCOMPASS {}
namespace JPP { using namespace JCOMPASS; }

namespace JCOMPASS {

  using JMATH::JMath;
  using JGEOMETRY3D::JQuaternion3D;
  using JLANG::JValueOutOfRange;
  using JFIT::JMEstimator;
  using JFIT::getMEstimator;

  /**
   * Model.
   */
  struct JModel :
    public JMath<JModel>
  {
    /**
     * Default constructor.
     */
    JModel() :
      Q0(JQuaternion3D::getIdentity()),
      Q1(JQuaternion3D::getIdentity())
    {}


    /**
     * Constructor.
     *
     * \param Q0              tilt
     * \param Q1              twist
     */
    JModel(const JQuaternion3D& Q0,
	   const JQuaternion3D& Q1) :
      Q0(Q0),
      Q1(Q1)
    {}


    /**
     * Constructor.
     *
     * The data type corresponding to the hits should provide for the following policy methods.
     * <pre>
     *    double        getZ();             // get z-position
     *    JQuaternion3D getQuaternion();    // get quaternion
     * </pre>
     * Note that the input data should be ordered with increasing z-positions.
     *
     * \param __begin         begin of data
     * \param __end           end   of data
     */
    template<class T>
    JModel(T __begin,
	   T __end) :
      Q0(JQuaternion3D::getIdentity()),
      Q1(JQuaternion3D::getIdentity())
    {
      using namespace std;
      using namespace JPP;

      const int N = distance(__begin, __end);

      if (N >= NUMBER_OF_PARAMETERS) {

	vector<JQuaternion3D> buffer;

	for (T q = __begin, p = q++; q != __end; ++p, ++q) {

	  const double dz = q->getZ() - p->getZ();

	  if (dz != 0.0) {

	    JQuaternion3D Q(p->getQuaternion());

	    Q.conjugate();
	    Q.mul(q->getQuaternion());
	    Q.pow(1.0 / dz);

	    buffer.push_back(Q);
	  }
	}

	Q1  = getAverage(buffer.begin(), buffer.end());
	Q1  = JQuaternion3D::decomposition(Q1, JVector3Z_t).twist;

	const double z1 = getAverage(make_array(__begin, __end, &JHit::getZ));

	Q0  = getAverage(make_array(__begin, __end, &JHit::getQuaternion));
	Q0  = pow(Q1, -z1) * Q0;

      } else {
        throw JValueOutOfRange("JModel: Not enough data points.");
      }
    }


    /**
     * Add model.
     *
     * \param  model          model
     * \return                this model
     */
    JModel& add(const JModel& model)
    {
      Q0 *= model.Q0;
      Q1 *= model.Q1;

      Q0.normalise();
      Q1.normalise();

      return *this;
    }


    /**
     * Subtract model.
     *
     * \param  model          model
     * \return                this model
     */
    JModel& sub(const JModel& model)
    {
      Q0 *= model.Q0.getConjugate();
      Q1 *= model.Q1.getConjugate();

      Q0.normalise();
      Q1.normalise();

      return *this;
    }


    /**
     * Scale model.
     *
     * \param  factor         multiplication factor
     * \return                this model
     */
    JModel& mul(const double factor)
    {
      Q0.pow(factor);
      Q1.pow(factor);

      return *this;
    }


    /**
     * Get quaternion at given z-position.
     *
     * \param z               z-position.
     * \return                quaternion
     */
    JQuaternion3D operator()(const double z) const
    {
      using namespace JPP;

      return pow(Q1, z) * Q0;
    }

    static const int NUMBER_OF_PARAMETERS  =  4;  //!< number of parameters of fit per quaternion

    JQuaternion3D Q0;   //!< tilt
    JQuaternion3D Q1;   //!< twist
  };


  /**
   * Auxiliary data structure for chi2 evaluation.
   */
  struct JChi2 {
    /**
     * Constructor.
     *
     * \param  type        M-Estimator type
     */
    JChi2(const int type) :
      estimator(getMEstimator(type))
    {}
 

    /**
     * Fit function.
     *
     * \param  model          model
     * \param  hit            hit
     * \return                chi2
     */
    inline double operator()(const JModel& model, const JHit& hit) const
    {
      return estimator->getRho(getAngle(model(hit.getZ()), hit.getQuaternion()) / hit.getSigma());
    }

    JLANG::JSharedPointer<JMEstimator> estimator;  //!< M-Estimator function
  };
}

#endif