#ifndef __JFIT__JLINE1ZESTIMATOR__
#define __JFIT__JLINE1ZESTIMATOR__

#include "JPhysics/JConstants.hh"	  
#include "JMath/JMatrix3S.hh"
#include "JMath/JSVD3D.hh"
#include "JFit/JMatrixNZ.hh"
#include "JFit/JLine1Z.hh"
#include "JFit/JEstimator.hh"



/**
 * \file
 * Linear fit of JFIT::JLine1Z.
 * \author mdejong
 */
namespace JFIT {}
namespace JPP { using namespace JFIT; }

namespace JFIT {

  /**
   * Linear fit of straight line parallel to z-axis to set of hits (objects with position and time).
   *
   \f{center}\setlength{\unitlength}{0.7cm}\begin{picture}(8,12)

   \put( 1.0, 0.5){\vector(0,1){9}}
   \put( 1.0,10.0){\makebox(0,0){$z$}}
   \put( 1.0, 0.0){\makebox(0,0){muon}}

   \put( 1.0, 8.0){\line(1,0){6}}
   \put( 4.0, 8.5){\makebox(0,0)[c]{$R$}}

   \multiput( 1.0, 2.0)(0.5, 0.5){12}{\qbezier(0.0,0.0)(-0.1,0.35)(0.25,0.25)\qbezier(0.25,0.25)(0.6,0.15)(0.5,0.5)}
   \put( 4.5, 4.5){\makebox(0,0)[l]{photon}}

   \put( 1.0, 2.0){\circle*{0.2}}
   \put( 0.5, 2.0){\makebox(0,0)[r]{$(x_{0},y_{0},z_{0},t_{0})$}}

   \put( 1.0, 8.0){\circle*{0.2}}
   \put( 0.5, 8.0){\makebox(0,0)[r]{$(x_{0},y_{0},z_{j})$}}

   \put( 7.0, 8.0){\circle*{0.2}}
   \put( 7.0, 9.0){\makebox(0,0)[c]{PMT}}
   \put( 7.5, 8.0){\makebox(0,0)[l]{$(x_{j},y_{j},z_{j},t_{j})$}}

   \put( 1.1, 2.1){
   \put(0.0,1.00){\vector(-1,0){0.1}}
   \qbezier(0.0,1.0)(0.25,1.0)(0.5,0.75)
   \put(0.5,0.75){\vector(1,-1){0.1}}
   \put(0.4,1.5){\makebox(0,0){$\theta_{c}$}}
   }

   \end{picture}
   \f}

   \f[
         ct_j  =  ct_0  +  (z_j - z_0)  +  \tan(\theta_{c}) \sqrt((x_j - x_0)^2 + (y_j - y_0)^2)
   \f]
   *
   * where:
   *
   \f{eqnarray*}{
          x_0 & = & \textrm{x position of track       (fit parameter)}            \\
          y_0 & = & \textrm{y position of track       (fit parameter)}            \\
          z_0 & = & \textrm{z position of track}                                  \\
          t_0 & = & \textrm{time of track at } z = z_0 \textrm{ (fit parameter)}  \\
                                                                                  \\
          c            & = & \textrm{speed of light in vacuum}                    \\
          \theta_{c}   & = & \textrm{Cherenkov angle}                             \\
   \f}
   *
   * Defining:
   *
   \f{eqnarray*}{
         t_j'  & \equiv &  ct_j / \tan(\theta_{c})  -  (z_j - z_0)/\tan(\theta_{c})   \\
         t_0'  & \equiv &  ct_0 / \tan(\theta_{c})                                    \\
   \f}
   *
   \f[
   \Rightarrow (t_j' - t_0')^2 = (x_j - x_0)^2  +  (y_j - y_0)^2
   \f]
   *
   * The parameters \f$ \{x_0, y_0, t_0\} \f$ are estimated in the constructor of this class based on 
   * consecutive pairs of equations by which the quadratic terms in \f$ x_0 \f$, \f$ y_0 \f$ and \f$ t_0 \f$ are eliminated.
   */
  template<>
  class JEstimator<JLine1Z> : 
    public JLine1Z 
  {
  public:
    /**
     * Default constructor.
     */
    JEstimator() :
      JLine1Z()
    {}


    /**
     * Constructor.
     *
     * \param  __begin    begin of data
     * \param  __end      end   of data
     */
    template<class T>
    JEstimator(T __begin, T __end) :
      JLine1Z()
    {
      (*this)(__begin, __end);
    }


    /**
     * Fit.
     *
     * The template argument <tt>T</tt> refers to an iterator of a data structure which should have the following member methods:
     *   - double %getX();       // [m]
     *   - double %getY();       // [m]
     *   - double %getZ();       // [m]
     *   - double %getT();       // [ns]
     *
     * \param  __begin    begin of data
     * \param  __end      end   of data
     * \return            this fit
     */
    template<class T>
    const JEstimator<JLine1Z>& operator()(T __begin, T __end)
    {
      using namespace std;
      using namespace JPP;

      const int N = distance(__begin, __end);

      if (N >= NUMBER_OF_PARAMETERS) {

	__x = 0.0;
	__y = 0.0;
	__z = 0.0;
	__t = 0.0;

	double t0 = 0.0;

	for (T i = __begin; i != __end; ++i) {
	  __x += i->getX();
	  __y += i->getY();
	  __z += i->getZ();
	  t0  += i->getT();
	}
	
	const double W = 1.0/N;

	__x *= W;
	__y *= W;
	__z *= W;
	t0  *= W;

	V.reset();

	t0 *= getSpeedOfLight();

	double y0 = 0.0;
	double y1 = 0.0;
	double y2 = 0.0;

	T j = __begin;

	double xi =  j->getX() - getX();
	double yi =  j->getY() - getY();
	double ti = (j->getT() * getSpeedOfLight()  -  t0  -  j->getZ() + getZ()) / getKappaC();

	for (bool done = false; !done; ) {

	  if ((done = (++j == __end))) {
	    j = __begin;
	  }

	  double xj =  j->getX() - getX();
	  double yj =  j->getY() - getY();
	  double tj = (j->getT() * getSpeedOfLight()  -  t0  -  j->getZ() + getZ()) / getKappaC();
	    
	  double dx = xj - xi;
	  double dy = yj - yi;
	  double dt = ti - tj;    // opposite sign!
	    
	  const double y  = ((xj + xi) * dx +
			     (yj + yi) * dy +
			     (tj + ti) * dt);

	  dx *= 2;
	  dy *= 2;
	  dt *= 2;
	    
	  V.a00 += dx * dx;
	  V.a01 += dx * dy;
	  V.a02 += dx * dt;
	  V.a11 += dy * dy;
	  V.a12 += dy * dt;
	  V.a22 += dt * dt;

	  y0 += dx * y;
	  y1 += dy * y;
	  y2 += dt * y;

	  xi = xj;
	  yi = yj;
	  ti = tj;
	}

	t0 *= getInverseSpeedOfLight();

	V.a10 = V.a01;
	V.a20 = V.a02;
	V.a21 = V.a12;

	svd.decompose(V);

	if (fabs(svd.S.a11) < MINIMAL_SVD_WEIGHT * fabs(svd.S.a00)) {
	  THROW(JValueOutOfRange, "JEstimator<JLine1Z>::JEstimator(): singular value " << svd.S.a11 << ' ' << svd.S.a00);
	}

	V = svd.invert(MINIMAL_SVD_WEIGHT);
	
	__x += V.a00 * y0  +  V.a01 * y1  +  V.a02 * y2;
	__y += V.a10 * y0  +  V.a11 * y1  +  V.a12 * y2;
	__t  = V.a20 * y0  +  V.a21 * y1  +  V.a22 * y2;
	
	__t *= getKappaC() * getInverseSpeedOfLight();
	__t += t0;

      } else {
	throw JValueOutOfRange("JEstimator<JLine1Z>::JEstimator(): Not enough data points.");
      }

      return *this;
    }


    /**
     * Update track parameters using updated co-variance matrix (e.g.\ matrix with one hit switched off).
     *
     * In this, it is assumed that the changes in x and y positions are small
     * compared to the actual distances between the track and the hits.
     *
     * The template argument <tt>T</tt> refers to an iterator of a data structure which should have the following member methods:
     *   - double %getX();       // [m]
     *   - double %getY();       // [m]
     *   - double %getZ();       // [m]
     *   - double %getT();       // [ns]
     *
     * \param  __begin    begin of data
     * \param  __end      end   of data
     * \param  A          co-variance matrix of hits
     */
    template<class T>
    void update(T __begin, T __end, const JMatrixNZ& A)
    {
      using namespace std;
      using namespace JPP;

      const int N = distance(__begin, __end);

      if (N != (int) A.size()) {
	THROW(JValueOutOfRange, "JEstimator<JLine1Z>::update(): Wrong number of points " << N << " != " << A.size());
      }

      if (N >= NUMBER_OF_PARAMETERS) {

	double x1 = 0.0;
	double y1 = 0.0;
	double t1 = 0.0;

	V.reset();
	
	T i = __begin;
	
	for (size_t row = 0; row != A.size(); ++row, ++i) {
	  
	  const double dx = i->getX() - getX();
	  const double dy = i->getY() - getY();
	  
	  const double rt = sqrt(dx*dx + dy*dy);
	  
	  double xr = getKappaC() * getInverseSpeedOfLight();
	  double yr = getKappaC() * getInverseSpeedOfLight();
	  double tr = 1.0;
	  
	  if (rt != 0.0) {
	    xr *= -dx / rt;
	    yr *= -dy / rt;
	  }

	  T j = __begin;

	  for (size_t col = 0; col != A.size(); ++col, ++j) {
	    
	    const double dx = j->getX() - getX();
	    const double dy = j->getY() - getY();
	    const double dz = j->getZ() - getZ();
	    
	    const double rt = sqrt(dx*dx + dy*dy);
	    
	    double xc = getKappaC() * getInverseSpeedOfLight();
	    double yc = getKappaC() * getInverseSpeedOfLight();
	    double tc = 1.0;

	    if (rt != 0.0) {
	      xc *= -dx / rt;
	      yc *= -dy / rt;
	    }
	    
	    const double ts = j->getT() - (dz + rt * getKappaC()) * getInverseSpeedOfLight();
	    
	    const double vs = A(row,col);

	    x1 += xr * vs * ts;
	    y1 += yr * vs * ts;
	    t1 += tr * vs * ts;
	    	    
	    V.a00 += xr * vs * xc;
	    V.a01 += xr * vs * yc;
	    V.a02 += xr * vs * tc;
	    V.a11 += yr * vs * yc;
	    V.a12 += yr * vs * tc;
	    V.a22 += tr * vs * tc;
	  }
	}
	
	V.a10 = V.a01;
	V.a20 = V.a02;
	V.a21 = V.a12;
	
	svd.decompose(V);

	if (fabs(svd.S.a11) < MINIMAL_SVD_WEIGHT * fabs(svd.S.a00)) {
	  THROW(JValueOutOfRange, "JEstimator<JLine1Z>::update(): singular value " << svd.S.a11 << ' ' << svd.S.a00);
	}

	V = svd.invert(MINIMAL_SVD_WEIGHT);
	
	__x += V.a00 * x1  +  V.a01 * y1  +  V.a02 * t1;
	__y += V.a10 * x1  +  V.a11 * y1  +  V.a12 * t1;
	__t  = V.a20 * x1  +  V.a21 * y1  +  V.a22 * t1;

      } else {
	THROW(JValueOutOfRange, "JEstimator<JLine1Z>::update(): Not enough data points " << N);
      }
    }


    static const int NUMBER_OF_PARAMETERS  =  3;  //!< number of parameters of fit
    JMATH::JMatrix3S V;                           //!< co-variance matrix of fit parameters
    JMATH::JSVD3D    svd;
    static double    MINIMAL_SVD_WEIGHT;          //!< minimal SVD weight.
  };

  /**
   * Set default minimal SVD weight.
   */
  double JEstimator<JLine1Z>::MINIMAL_SVD_WEIGHT = 1.0e-4;
}

#endif