#ifndef __JFIT__JPOINT3DESTIMATOR__
#define __JFIT__JPOINT3DESTIMATOR__

#include "JMath/JMatrix3S.hh"
#include "JFit/JPoint3D.hh"
#include "JFit/JEstimator.hh"


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

namespace JFIT {

  /**
   * Linear fit of crossing point (position) between axes (objects with position and direction).
   *
   \f{center}\setlength{\unitlength}{0.6cm}\begin{picture}(12,12)

   \put( 5.0, 5.0){\circle*{0.3}}
   \put( 4.0, 5.0){\makebox(0,0)[r]{$(x_{0},y_{0},z_{0})$}}

   \multiput( 5.0, 5.0)(+0.2, 0.0){18}{\makebox(0,0)[c]{.}}
   \put( 9.0, 5.0){\circle*{0.2}}
   \put( 9.0, 5.0){\vector(+1, 0){0.7}}
   \put(10.0, 5.0){\makebox(0,0)[l]{$(\bar{x}_j,\hat{u}_j)$}}

   \multiput( 5.0, 5.0)(-0.2,+0.2){18}{\makebox(0,0)[c]{.}}
   \put( 1.0, 9.0){\circle*{0.2}}
   \put( 1.0, 9.0){\vector(-1,+1){0.6}}
   \put( 0.0,10.0){\makebox(0,0)[r]{$(\bar{x}_k,\hat{u}_k)$}}

   \multiput( 5.0, 5.0)(-0.2,-0.2){18}{\makebox(0,0)[c]{.}}
   \put( 1.0, 1.0){\circle*{0.2}}
   \put( 1.0, 1.0){\vector(-1,-1){0.6}}
   \put( 0.0, 0.0){\makebox(0,0)[r]{$(\bar{x}_i,\hat{u}_i)$}}

   \end{picture}
   \f}
   *
   * where:
   *
   \f{eqnarray*}{
          \bar{x}_i & = & i^{th} \textrm{ position}        \\
          \hat{u}_i & = & i^{th} \textrm{ direction}       \\
   \f}
   *
   * The parameters \f$\{x_0, y_0, z_0\}\f$ are estimated in the constructor of this class.
   */
  template<>
  class JEstimator<JPoint3D> :
    public JPoint3D
  {
  public:
    /**
     * Constructor.
     *
     * 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 %getDX();      // [u]
     *   - double %getDY();      // [u]
     *   - double %getDZ();      // [u]
     *
     * \param  __begin          begin of data
     * \param  __end            end   of data
     */
    template<class T>
    JEstimator(T __begin, T __end)
    {
      using namespace std;

      const int N = distance(__begin, __end);

      if (N > 1) {

	double x = 0;
	double y = 0;
	double z = 0;

	V.reset();
	
	for (T i = __begin; i != __end; ++i) {
	  
	  const double xx = 1.0  -  i->getDX() * i->getDX();
	  const double yy = 1.0  -  i->getDY() * i->getDY();
	  const double zz = 1.0  -  i->getDZ() * i->getDZ();
	  
	  const double xy = -i->getDX() * i->getDY();
	  const double xz = -i->getDX() * i->getDZ();
	  const double yz = -i->getDY() * i->getDZ();
	  
	  V.a00 += xx;
	  V.a01 += xy;
	  V.a02 += xz;
	  
	  V.a11 += yy;
	  V.a12 += yz;
	  
	  V.a22 += zz;
	  
	  x  +=  xx * i->getX()  + xy * i->getY()  +  xz * i->getZ();
	  y  +=  xy * i->getX()  + yy * i->getY()  +  yz * i->getZ();
	  z  +=  xz * i->getX()  + yz * i->getY()  +  zz * i->getZ();
	}
	
	V.a10 = V.a01;
	V.a20 = V.a02;
	V.a21 = V.a12;
	
	V.invert();
	
	__x = V.a00 * x  +  V.a01 * y  +  V.a02 * z;
	__y = V.a10 * x  +  V.a11 * y  +  V.a12 * z;
	__z = V.a20 * x  +  V.a21 * y  +  V.a22 * z;
      
      } else {
	throw JValueOutOfRange("JEstimator<JPoint3D>::JEstimator(): Not enough data points.");
      }
    }


    static const int NUMBER_OF_PARAMETERS  =  3;  //!< number of parameters of fit
    JMATH::JMatrix3S V;                           //!< co-variance matrix of fit parameters
  };
}

#endif