#ifndef __JFIT__JMATRIXNZ__
#define __JFIT__JMATRIXNZ__

#include <cmath>

#include "JMath/JMatrixNS.hh"
#include "JGeometry3D/JVector3D.hh"
#include "JPhysics/JConstants.hh"


/**
 * \author mdejong
 */

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

namespace JFIT {

  using JMATH::JMatrixNS;
  using JGEOMETRY3D::JVector3D;


  /**
   * Determination of the co-variance matrix of hits for a track along z-axis (JFIT::JLine1Z).
   * In this, the given angular and time resolution are taken into account.
   */
  class JMatrixNZ : 
    public JMatrixNS
  {
  public:

    using JMatrixNS::getDot;


    /**
     * Default contructor.
     */
    JMatrixNZ() :
      JMatrixNS()
    {}
	      

    /**
     * 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]
     *
     * \param  pos              reference position [m]
     * \param  __begin          begin of data
     * \param  __end            end   of data
     * \param  alpha            angular resolution [deg]
     * \param  sigma            time    resolution [ns]
     */
    template<class T>
    JMatrixNZ(const JVector3D& pos,
	      T                __begin,
	      T                __end,
	      const double     alpha,
	      const double     sigma) :
      JMatrixNS()
    {
      set(pos, __begin, __end, alpha, sigma);
    }


    /**
     * Set co-variance matrix.
     *
     * 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]
     *
     * \param  pos              reference position [m]
     * \param  __begin          begin of data
     * \param  __end            end   of data
     * \param  alpha            angular resolution [deg]
     * \param  sigma            time    resolution [ns]
     */
    template<class T>
    void set(const JVector3D& pos,
	     T                __begin,
	     T                __end,
	     const double     alpha,
	     const double     sigma)
    {
      using namespace std;
      using namespace JPP;


      const int N = distance(__begin, __end);

      this ->resize(N);
      buffer.resize(N);

      const double ta = alpha * PI / 180.0;
      const double ct = cos(ta);
      const double st = sin(ta);


      // angular resolution

      for (T i = __begin; i != __end; ++i) {
	
	const double dx = i->getX() - pos.getX(); 
	const double dy = i->getY() - pos.getY(); 
	const double dz = i->getZ() - pos.getZ(); 

	const double R  = sqrt(dx*dx + dy*dy);
	
	double x  = ta * getKappaC() * getInverseSpeedOfLight();
	double y  = ta * getKappaC() * getInverseSpeedOfLight();
	double v  = ta * getInverseSpeedOfLight();
	double w  = ta * getInverseSpeedOfLight();

	if (R != 0.0) {
	  x *= dx / R;
	  y *= dy / R;
	}

	x *=  (dz*ct - dx*st);
	y *=  (dz*ct - dy*st);
	v *= -(dx*ct + dz*st);
	w *= -(dy*ct + dz*st);
	
	buffer[distance(__begin,i)] = variance(x, y, v, w);
      }

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

	for (int j = 0; j != i; ++j) {
	  (*this)(i, j) = getDot(buffer[i], buffer[j]);
	  (*this)(j, i) = (*this)(i, j);
	}

	(*this)(i, i) = getDot(buffer[i], buffer[i]) +  sigma * sigma;
      }
    }

  private:
    /**
     * Auxiliary data structure for co-variance calculation.
     */
    struct variance {
      /**
       * Default constructor.
       */
      variance() :
	x(0.0),
	y(0.0),
	v(0.0),
	w(0.0)
      {}


      /**
       * Constructor.
       *
       * \param  __x       x
       * \param  __y       y
       * \param  __v       v
       * \param  __w       w
       */
      variance(const double __x,
	       const double __y,
	       const double __v,
	       const double __w) :
	x(__x),
	y(__y),
	v(__v),
	w(__w)
      {}


      double x;
      double y;
      double v;
      double w;
    };


    std::vector<variance> buffer;


    /**
     * Get dot product.
     *
     * \param  first     first  variance
     * \param  second    second variance
     * \return           dot product
     */
    static inline double getDot(const variance& first, const variance& second)
    {
      return (first.x * second.x  +
	      first.y * second.y  +
	      first.v * second.v  +
	      first.w * second.w);
	      
    }
  };
}

#endif