#ifndef FitC_H
#define FitC_H

  /// conventinal KF approach
#include "FitFunctional.h"
#include "FitBase.h"

class FitC: public virtual FitFunctional, public FitBase {
 public:
  void ExtrapolateALight( Fvec_t T[], CovV &C,  const Fvec_t &z_out,  Fvec_t& qp0, FieldRegion &F, Fvec_t w = ZERO) const;
  
 protected:
  void Filter( TrackV &track, HitInfo &info, Fvec_t &u, Fvec_t w = ONE ) const;
  void FilterFirst( TrackV &track, HitV &hit, Station &st ) const;

  void ExtrapolateWithMaterial( TrackV &track, const Fvec_t &z_out,  Fvec_t& qp0, FieldRegion &F, Station &st, bool isPipe = 0, Fvec_t w = 0.f) const;

  void AddMaterial( CovV &C, Fvec_t Q22, Fvec_t Q32, Fvec_t Q33 ) const;
};

//inline // --> causes a runtime overhead and problems for the MS compiler (error C2603)
void FitC::ExtrapolateALight
( 
 Fvec_t T [], // input track parameters (x,y,tx,ty,Q/p)
 CovV &C,     // input covariance matrix
 const Fvec_t &z_out  , // extrapolate to this z position
 Fvec_t       &qp0    , // use Q/p linearisation at this value
 FieldRegion &F,
 Fvec_t w
 ) const
{
  //
  //  Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
  //  
  Jacobian_t j;
  ExtrapolateJ( T, z_out, qp0, F, j, w );
  
 //          covariance matrix transport 
 
  const Fvec_t c42 = C.C42, c43 = C.C43;

  const Fvec_t cj00 = C.C00 + C.C20*j(0,2) + C.C30*j(0,3) + C.C40*j(0,4);
  // const Fvec_t cj10 = C.C10 + C.C21*j(0,2) + C.C31*j(0,3) + C.C41*j(0,4);
  const Fvec_t cj20 = C.C20 + C.C22*j(0,2) + C.C32*j(0,3) + c42*j(0,4);
  const Fvec_t cj30 = C.C30 + C.C32*j(0,2) + C.C33*j(0,3) + c43*j(0,4);
 
  const Fvec_t cj01 = C.C10 + C.C20*j(1,2) + C.C30*j(1,3) + C.C40*j(1,4);
  const Fvec_t cj11 = C.C11 + C.C21*j(1,2) + C.C31*j(1,3) + C.C41*j(1,4);
  const Fvec_t cj21 = C.C21 + C.C22*j(1,2) + C.C32*j(1,3) + c42*j(1,4);
  const Fvec_t cj31 = C.C31 + C.C32*j(1,2) + C.C33*j(1,3) + c43*j(1,4);

  // const Fvec_t cj02 = C.C20*j(2,2) + C.C30*j(2,3) + C.C40*j(2,4);
  // const Fvec_t cj12 = C.C21*j(2,2) + C.C31*j(2,3) + C.C41*j(2,4);
  const Fvec_t cj22 = C.C22*j(2,2) + C.C32*j(2,3) + c42*j(2,4);
  const Fvec_t cj32 = C.C32*j(2,2) + C.C33*j(2,3) + c43*j(2,4);

  // const Fvec_t cj03 = C.C20*j(3,2) + C.C30*j(3,3) + C.C40*j(3,4);
  // const Fvec_t cj13 = C.C21*j(3,2) + C.C31*j(3,3) + C.C41*j(3,4);
  const Fvec_t cj23 = C.C22*j(3,2) + C.C32*j(3,3) + c42*j(3,4);
  const Fvec_t cj33 = C.C32*j(3,2) + C.C33*j(3,3) + c43*j(3,4);

  C.C40+= c42*j(0,2) + c43*j(0,3) + C.C44*j(0,4); // cj40
  C.C41+= c42*j(1,2) + c43*j(1,3) + C.C44*j(1,4); // cj41
  C.C42 = c42*j(2,2) + c43*j(2,3) + C.C44*j(2,4);
  C.C43 = c42*j(3,2) + c43*j(3,3) + C.C44*j(3,4);

  C.C00 = (cj00 + j(0,2)*cj20 + j(0,3)*cj30 + j(0,4)*C.C40);
  C.C10 = (cj01 + j(0,2)*cj21 + j(0,3)*cj31 + j(0,4)*C.C41);
  C.C11 = (cj11 + j(1,2)*cj21 + j(1,3)*cj31 + j(1,4)*C.C41);

  C.C20 = (j(2,2)*cj20 + j(2,3)*cj30 + j(2,4)*C.C40);
  C.C30 = (j(3,2)*cj20 + j(3,3)*cj30 + j(3,4)*C.C40);
  C.C21 = (j(2,2)*cj21 + j(2,3)*cj31 + j(2,4)*C.C41);
  C.C31 = (j(3,2)*cj21 + j(3,3)*cj31 + j(3,4)*C.C41);
  C.C22 = (j(2,2)*cj22 + j(2,3)*cj32 + j(2,4)*C.C42);
  C.C32 = (j(3,2)*cj22 + j(3,3)*cj32 + j(3,4)*C.C42);
  C.C33 = (j(3,2)*cj23 + j(3,3)*cj33 + j(3,4)*C.C43);
}

inline void FitC::Filter( TrackV &track, HitInfo &info, Fvec_t &u, Fvec_t w ) const
{
  const Fvec_t p = ONE/w;
  cnst w_th = 0.001f; // max w to filter measurement
  const Fvec_m_t mask = w > w_th;

  const Fvec_t sigma2 = info.sigma2*p;
  
  // convert input
  Fvec_t *T = track.T;
  CovV &C = track.C;

  register Fvec_t wi, zeta, zetawi, HCH;
  
  register Fvec_t F0, F1, F2, F3, F4;
  register Fvec_t  K1, K2, K3, K4;

//   Fvec_t wi, zeta, zetawi, HCH;
//   
//   Fvec_t F0, F1, F2, F3, F4;
//   Fvec_t  K1, K2, K3, K4;
  
  zeta = info.cos_phi*T[0] + info.sin_phi*T[1] - u;
  // F = CH'

  F0 = info.cos_phi*C.C00 + info.sin_phi*C.C10;
  F1 = info.cos_phi*C.C10 + info.sin_phi*C.C11;
  
  HCH = ( F0*info.cos_phi +F1*info.sin_phi );
  
  
  F2 = info.cos_phi*C.C20 + info.sin_phi*C.C21;
  F3 = info.cos_phi*C.C30 + info.sin_phi*C.C31;
  F4 = info.cos_phi*C.C40 + info.sin_phi*C.C41;
  
  const Fvec_m_t initialised = HCH<info.sigma216*p;
  
#if (defined(X87) || defined(SIMPLSIMD) )
#if defined(X87)
  wi = mask ? rcp(sigma2 + HCH) : 0;
  zetawi = w* zeta *rcp( (double)((long long)initialised & (long long)sigma2) + HCH); // FIXME
  track.Chi2 += (double)((long long)initialised & (long long)(zeta * zetawi));
#else // header
  wi = mask & rcp(sigma2 + HCH);
  zetawi = mask & ( zeta *rcp( (initialised&sigma2) + HCH) );
  track.Chi2 += initialised & (zeta * zetawi);
#endif
#elif defined (VC)
  wi = ZERO;
  wi(mask) = rcp(sigma2 + HCH);
  Fvec_t sigma2m = ZERO;
  sigma2m(initialised) = sigma2;
  zetawi = zeta*rcp(sigma2m + HCH);
  track.Chi2(initialised) += (zeta * zetawi);    
#endif
  
  track.NDF += w;

  K1 = F1*wi;
  K2 = F2*wi;
  K3 = F3*wi;
  K4 = F4*wi;

  T[0]-= F0*zetawi;
  T[1]-= F1*zetawi;
  T[2]-= F2*zetawi;
  T[3]-= F3*zetawi;
  T[4]-= F4*zetawi;

  C.C00-= F0*F0*wi;
  C.C10-= K1*F0;
  C.C11-= K1*F1;
  C.C20-= K2*F0;
  C.C21-= K2*F1;
  C.C22-= K2*F2;
  C.C30-= K3*F0;
  C.C31-= K3*F1;
  C.C32-= K3*F2;
  C.C33-= K3*F3;
  C.C40-= K4*F0;
  C.C41-= K4*F1;
  C.C42-= K4*F2;
  C.C43-= K4*F3;
  C.C44-= K4*F4;
}

inline void FitC::FilterFirst( TrackV &track, HitV &hit, Station &st ) const
{

  CovV &C = track.C;
  Fvec_t w1 = ONE-hit.w;
  Fvec_t c00 = hit.w*st.XYInfo.C00 + w1*INF;
  Fvec_t c10 = hit.w*st.XYInfo.C10 + w1*INF;
  Fvec_t c11 = hit.w*st.XYInfo.C11 + w1*INF;

  // // initialize covariance matrix 
  C.C00= c00; 
  C.C10= c10;       C.C11= c11; 
  C.C20= ZERO;      C.C21= ZERO;      C.C22= INF2; // needed for stability of smoother. improve FilterTracks and CHECKME
  C.C30= ZERO;      C.C31= ZERO;      C.C32= ZERO; C.C33= INF2;
  C.C40= ZERO;      C.C41= ZERO;      C.C42= ZERO; C.C43= ZERO; C.C44= INF;

  track.T[0] = hit.w*hit.x + w1*track.T[0];
  track.T[1] = hit.w*hit.y + w1*track.T[1];
  track.NDF = -3.0;
  track.Chi2 = ZERO;
}

inline void FitC::AddMaterial( CovV &C, Fvec_t Q22, Fvec_t Q32, Fvec_t Q33 ) const
{
  C.C22 += Q22;
  C.C32 += Q32; C.C33 += Q33; 
}

inline void FitC::ExtrapolateWithMaterial( TrackV &track,  const Fvec_t &z_out,  Fvec_t& qp0, FieldRegion &F, Station &st, bool isPipe, Fvec_t w) const
{
  ExtrapolateALight( track.T, track.C, z_out, qp0, F, w);
  FitFunctional::AddMaterial( track, st, qp0, isPipe ); // FIXME
}

#endif