#ifndef __EUREKA__
#define __EUREKA__

#include <iostream>
#include <vector>
#include <stdexcept>

#include "geometry.hh"
#include "matrix.hh"


namespace EUREKA {

  /**
   * \file
   *
   * Linear fit in fixed direction
   *
   *      t_j  =  t_0  +  (z_j - z_0)/c  +  tan(q)/c x sqrt((x_j - x_0)^2 + (y_j - y_0)^2)
   *
   * <=> (t_j' - t_0')^2 = (x_j - x_0)^2  +  (y_j - y_0)^2         (eq.j)
   *
   * where
   *
   *      t_j'  =  t_j x c/tan(q)  -  (z_j - z_0)/tan(q)
   *      t_0'  =  t_0 x c/tan(q
   *
   *      c     = speed of light (in vacuum)
   *      q     = Cherenkov angle
   *
   * solves set of linear equations { (eq.i - eq.j) } based on (consecutive) pairs of hits
   *
   */
  
  using namespace GEOMETRY;

  namespace $$$ {

    /**
     * Function object for fast linear fit
     */
    class eureka : public track1Z {
    public:
      /**
       * constructor
       */
      eureka() :
	track1Z(),
	A(3)
      {}

      /**
       * \param   __begin   begin of data
       * \param   __end     end of data
       */
      template<class T> track1Z operator()(const T __begin, const T __end)
      {
	if (__begin != __end) {

	  x_ = 0;
	  y_ = 0;
	  z_ = 0;
	  t0 = 0;

	  for (T i = __begin; i != __end; ++i) {
	    x_ += i->x();
	    y_ += i->y();
	    z_ += i->z();
	    t0 += i->t();
	  }
	
	  x_ /= std::distance(__begin,__end);
	  y_ /= std::distance(__begin,__end);
	  z_ /= std::distance(__begin,__end);
	  t0 /= std::distance(__begin,__end);

	  A  = 0;                     // reset matrix

	  y0 = 0;
	  y1 = 0;
	  y2 = 0;

	  for (T i = __begin; i != __end; ++i) {

	    T j = i;

	    ++j; // next

	    if (j == __end) 
	      j = __begin;

	    i__x =  i->x() - x_;
	    i__y =  i->y() - y_;
	    i__t = (i->t() * C  -  t0 * getSpeedOfLight()  -  i->z() + z()) / getTanThetaC();   // [ns] -> [m]

	    j__x =  j->x() - x_;
	    j__y =  j->y() - y_;
	    j__t = (j->t() * C  -  t0 * getSpeedOfLight()  -  j->z() + z()) / getTanThetaC();   // [ns] -> [m]

	    dx = +2 * (j__x - i__x);
	    dy = +2 * (j__y - i__y);
	    dt = -2 * (j__t - i__t);

	    __y = 
	      j__x * j__x  -  i__x * i__x  +
	      j__y * j__y  -  i__y * i__y  +
	      i__t * i__t  -  j__t * j__t;   // opposite sign!

	    A(0,0) += dx * dx;
	    A(0,1) += dx * dy;
	    A(0,2) += dx * dt;
	    A(1,1) += dy * dy;
	    A(1,2) += dy * dt;
	    A(2,2) += dt * dt;

	    y0 += dx * __y;
	    y1 += dy * __y;
	    y2 += dt * __y;
	  }

	  A.invert();                 // may throw exception

	  x_ += A[0][0] * y0  +  A[0][1] * y1  +  A[0][2] * y2;
	  y_ += A[1][0] * y0  +  A[1][1] * y1  +  A[1][2] * y2;
	  t_  = A[2][0] * y0  +  A[2][1] * y1  +  A[2][2] * y2;
	
	  t_ *= getTanThetaC() * getInverseSpeedOfLight();   // [m] -> [ns]
	  t_ += t0;
	}

	return *this;
      }

      symmetric_matrix<double> A;

    protected:
      double t0;
      double i__x, j__x;
      double i__y, j__y;
      double i__t, j__t;
      double dx, dy, dt;
      double __y;
      double y0;
      double y1;
      double y2;
    };
  }


  /**
   * Linear fit using full co-variance matrix
   */
  class eurekaV : public $$$::eureka {
  private:
    /**
     * Auxiliary class for H matrix (corresponds to single row)
     */
    class tuple {
    public:
      tuple(const double& dx_,
	    const double& dy_,
	    const double& dt_) :
	dx(dx_),
	dy(dy_),
	dt(dt_)
      {}
      
      double dx;
      double dy;
      double dt;
    };

  public:
    /**
     * constructor
     * \param   __begin   begin of data
     * \param   __end     end of data
     */
    template<class T> eurekaV(const T __begin, const T __end) :
      $$$::eureka()
    {
      if (__begin != __end) {
	  
	z_ = 0;
	t0 = 0;
	
	for (T i = __begin; i != __end; ++i) {
	  z_ += i->z();
	  t0 += i->t();
	}
	
	z_ /= std::distance(__begin,__end);
	t0 /= std::distance(__begin,__end);
	
	const int N = std::distance(__begin,__end);
	
	symmetric_matrix<double> V(N);
	
	std::vector<double> Y;
	std::vector<tuple>  H;
	
	
	for (T i = __begin; i != __end; ++i) {
	  
	  T j = i;
	  
	  ++j; // next
	  
	  if (j == __end) 
	    j = __begin;
	  
	  i__t = (i->t() * C  -  t0 * C  -  i->z() + z()) / getTanThetaC();   // [ns] -> [m]
	  j__t = (j->t() * C  -  t0 * C  -  j->z() + z()) / getTanThetaC();   // [ns] -> [m]
	  
	  dx = +2 * (j->x() - i->x());
	  dy = +2 * (j->y() - i->y());
	  dt = -2 * (j__t   - i__t);
	  
	  H.push_back(tuple(dx,dy,dt));
	  
	  __y = 
	    j->x() * j->x()  -  i->x() * i->x()  +
	    j->y() * j->y()  -  i->y() * i->y()  +
	    i__t   * i__t    -  j__t   * j__t;   // opposite sign!
	  
	  Y.push_back(__y);
	  
	  double i__s = i->sigma() * C / getTanThetaC();   // [ns] -> [m]
	  double j__s = j->sigma() * C / getTanThetaC();   // [ns] -> [m]
	  
	  int k = std::distance(__begin,i);
	  int l = k + 1;
	  
	  if (l == N) 
	    l = 0;
	  
	  V(k,k) = ( 2 * i__t * i__s  *  2 * i__t * i__s  +
		     2 * j__t * j__s  *  2 * j__t * j__s);
	  V(k,l) = (-2 * j__t * j__s  *  2 * j__t * j__s);
	}
	  
	  
	V.invert();                 // may throw exception
	

	A  = 0;                     // reset matrix
	
	y0 = 0;
	y1 = 0;
	y2 = 0;

	for (int i = 0; i != N; ++i) {
	  for (int j = 0; j != N; ++j) {
	    
	    y0     += H[i].dx * V[i][j] * Y[j];
	    y1     += H[i].dy * V[i][j] * Y[j];
	    y2     += H[i].dt * V[i][j] * Y[j];
	    
	    A(0,0) += H[i].dx * V[i][j] * H[j].dx;
	    A(0,1) += H[i].dx * V[i][j] * H[j].dy;
	    A(0,2) += H[i].dx * V[i][j] * H[j].dt;
	    
	    A(1,1) += H[i].dy * V[i][j] * H[j].dy;
	    A(1,2) += H[i].dy * V[i][j] * H[j].dt;
	    
	    A(2,2) += H[i].dt * V[i][j] * H[j].dt;
	  }
	}
	
	
	A.invert();                 // may throw exception
	
	x_  = A[0][0] * y0  +  A[0][1] * y1  +  A[0][2] * y2;
	y_  = A[1][0] * y0  +  A[1][1] * y1  +  A[1][2] * y2;
	t_  = A[2][0] * y0  +  A[2][1] * y1  +  A[2][2] * y2;
	
	t_ *= getTanThetaC() / C;   // [m] -> [ns]
	t_ += t0;


	for (T i = __begin; i != __end; ++i) {
	  
	  T j = i;
	  
	  ++j; // next
	  
	  if (j == __end) 
	    j = __begin;

	  dx   = i->x() - x();
	  dy   = i->y() - y();
	  
	  i__t = (t() * C  -  t0 * C) / getTanThetaC()  +  sqrt(dx*dx + dy*dy);   // [ns] -> [m]

	  dx   = j->x() - x();
	  dy   = j->y() - y();

	  j__t = (t() * C  -  t0 * C) / getTanThetaC()  +  sqrt(dx*dx + dy*dy);   // [ns] -> [m]
	  
	  __y = 
	    j->x() * j->x()  -  i->x() * i->x()  +
	    j->y() * j->y()  -  i->y() * i->y()  +
	    i__t   * i__t    -  j__t   * j__t;   // opposite sign!

	  int k = std::distance(__begin,i);
	  
	  Y[k] -= __y;
	}


	chi2_ = 0;	

	for (int i = 0; i != N; ++i) {
	  for (int j = 0; j != N; ++j)
	    chi2_ += Y[i] * V[i][j] * Y[j];
	}
      }
    }
    
    const double& chi2() const { return chi2_; }
    
  private:
    double chi2_;
  };


  /**
   * Function object for fast linear fit
   */
  static $$$::eureka eureka;
}

#endif