#ifndef L1Filtration_h
#define L1Filtration_h

#include "L1Types.h"
#include "L1UMeasurementInfo.h"
#include "L1XYMeasurementInfo.h"
#include "L1TrackPar.h"

#define cnst static const fvec

inline void L1Filter( L1TrackPar &T, L1UMeasurementInfo &info, fvec u )
{

  cnst ZERO = 0.0, ONE = 1.;

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

  zeta = info.cos_phi*T.x + info.sin_phi*T.y - u;

  // F = CH'
  F0 = info.cos_phi*T.C00 + info.sin_phi*T.C10;
  F1 = info.cos_phi*T.C10 + info.sin_phi*T.C11;

  HCH = ( F0*info.cos_phi + F1*info.sin_phi );

  F2 = info.cos_phi*T.C20 + info.sin_phi*T.C21;
  F3 = info.cos_phi*T.C30 + info.sin_phi*T.C31;
  F4 = info.cos_phi*T.C40 + info.sin_phi*T.C41;

  wi = 1./(info.sigma2 +HCH);
  zetawi = zeta *wi;
  T.chi2 +=  zeta * zetawi ;
  T.NDF  += ONE;

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

  T.x  -= F0*zetawi;
  T.y  -= F1*zetawi;
  T.tx -= F2*zetawi;
  T.ty -= F3*zetawi;
  T.qp -= F4*zetawi;

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

inline void L1FilterVtx( L1TrackPar &T, fvec x, fvec y, L1XYMeasurementInfo &info, 
			 fvec extrDx, fvec extrDy, fvec J[] )
{
  cnst TWO = 2.;

  register fvec zeta0, zeta1, S00, S10, S11, si;
  register fvec F00, F10, F20, F30, F40,  F01, F11, F21, F31, F41 ;
  register fvec K00, K10, K20, K30, K40, K01, K11, K21, K31, K41;

  //zeta0 = T.x + J[0]*T.tx + J[1]*T.ty + J[2]*T.qp - x;
  //zeta1 = T.y + J[3]*T.tx + J[4]*T.ty + J[5]*T.qp - y;

  zeta0 = T.x + extrDx - x;
  zeta1 = T.y + extrDy - y;

  // F = CH'
  F00 = T.C00;       F01 = T.C10;    
  F10 = T.C10;       F11 = T.C11;
  F20 = J[0]*T.C22;  F21 = J[3]*T.C22;
  F30 = J[1]*T.C33;  F31 = J[4]*T.C33;
  F40 = J[2]*T.C44;  F41 = J[5]*T.C44;

  S00 = info.C00 + F00 + J[0]*F20 + J[1]*F30 + J[2]*F40;
  S10 = info.C10 + F10 + J[3]*F20 + J[4]*F30 + J[5]*F40;
  S11 = info.C11 + F11 + J[3]*F21 + J[4]*F31 + J[5]*F41;

  si = 1./(S00*S11 - S10*S10);
  //si = fvec(rcp(fvec((S00*S11 - S10*S10)[0])));
  fvec S00tmp = S00;
  S00 =  si*S11;
  S10 = -si*S10;
  S11 =  si*S00tmp;

  T.chi2+=  zeta0*zeta0*S00 + 2.*zeta0*zeta1*S10 + zeta1*zeta1*S11;
  T.NDF += TWO;

  K00 = F00*S00 + F01*S10;  K01 = F00*S10 + F01*S11;
  K10 = F10*S00 + F11*S10;  K11 = F10*S10 + F11*S11;
  K20 = F20*S00 + F21*S10;  K21 = F20*S10 + F21*S11;
  K30 = F30*S00 + F31*S10;  K31 = F30*S10 + F31*S11;
  K40 = F40*S00 + F41*S10;  K41 = F40*S10 + F41*S11;

  T.x  -= K00*zeta0 + K01*zeta1;
  T.y  -= K10*zeta0 + K11*zeta1;
  T.tx -= K20*zeta0 + K21*zeta1;
  T.ty -= K30*zeta0 + K31*zeta1;
  T.qp -= K40*zeta0 + K41*zeta1;

  T.C00-=  ( K00*F00 + K01*F01 );
  T.C10-=  ( K10*F00 + K11*F01 );
  T.C11-=  ( K10*F10 + K11*F11 );
  T.C20 = -( K20*F00 + K21*F01 );
  T.C21 = -( K20*F10 + K21*F11 );
  T.C22-=  ( K20*F20 + K21*F21 );
  T.C30 = -( K30*F00 + K31*F01 );
  T.C31 = -( K30*F10 + K31*F11 );
  T.C32 = -( K30*F20 + K31*F21 );
  T.C33-=  ( K30*F30 + K31*F31 );
  T.C40 = -( K40*F00 + K41*F01 );
  T.C41 = -( K40*F10 + K41*F11 );
  T.C42 = -( K40*F20 + K41*F21 );
  T.C43 = -( K40*F30 + K41*F31 );
  T.C44-=  ( K40*F40 + K41*F41 );
}


inline void L1FilterXY( L1TrackPar &T, fvec x, fvec y, L1XYMeasurementInfo &info )
{
  cnst TWO = 2.;

  register fvec zeta0, zeta1, S00, S10, S11, si;
  register fvec F00, F10, F20, F30, F40,  F01, F11, F21, F31, F41 ;
  register fvec K00, K10, K20, K30, K40, K01, K11, K21, K31, K41;

  zeta0 = T.x - x;
  zeta1 = T.y - y;

  // F = CH'
  F00 = T.C00;
  F10 = T.C10;
  F20 = T.C20;
  F30 = T.C30;
  F40 = T.C40;
  F01 = T.C10;
  F11 = T.C11;
  F21 = T.C21;
  F31 = T.C31;
  F41 = T.C41;

  S00 = F00 + info.C00;
  S10 = F10 + info.C10;
  S11 = F11 + info.C11;

  si = 1./(S00*S11 - S10*S10);
  fvec S00tmp = S00;
  S00 =  si*S11;
  S10 = -si*S10;
  S11 =  si*S00tmp;

  T.chi2+=  zeta0*zeta0*S00 + 2.*zeta0*zeta1*S10 + zeta1*zeta1*S11;
  T.NDF  += TWO;

  K00 = F00*S00 + F01*S10;  K01 = F00*S10 + F01*S11;
  K10 = F10*S00 + F11*S10;  K11 = F10*S10 + F11*S11;
  K20 = F20*S00 + F21*S10;  K21 = F20*S10 + F21*S11;
  K30 = F30*S00 + F31*S10;  K31 = F30*S10 + F31*S11;
  K40 = F40*S00 + F41*S10;  K41 = F40*S10 + F41*S11;

  T.x  -= K00*zeta0 + K01*zeta1;
  T.y  -= K10*zeta0 + K11*zeta1;
  T.tx -= K20*zeta0 + K21*zeta1;
  T.ty -= K30*zeta0 + K31*zeta1;
  T.qp -= K40*zeta0 + K41*zeta1;

  T.C00-= K00*F00 + K01*F01;
  T.C10-= K10*F00 + K11*F01;
  T.C11-= K10*F10 + K11*F11;
  T.C20-= K20*F00 + K21*F01;
  T.C21-= K20*F10 + K21*F11;
  T.C22-= K20*F20 + K21*F21;
  T.C30-= K30*F00 + K31*F01;
  T.C31-= K30*F10 + K31*F11;
  T.C32-= K30*F20 + K31*F21;
  T.C33-= K30*F30 + K31*F31;
  T.C40-= K40*F00 + K41*F01;
  T.C41-= K40*F10 + K41*F11;
  T.C42-= K40*F20 + K41*F21;
  T.C43-= K40*F30 + K41*F31;
  T.C44-= K40*F40 + K41*F41;
}



/*
inline void L1Filter1D( L1TrackPar &T, fvec &u, fvec &sigma2, fvec H[] )
{

  cnst ZERO = 0.0, ONE = 1.;

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

  zeta = H[0]*T.x +H[1]*T.y +H[2]*T.tx +H[3]*T.ty +H[4]*T.qp - u;

  // F = CH'
  F0 = H[0]*T.C00 + H[1]*T.C10 + H[2]*T.C20+ H[3]*T.C30+ H[4]*T.C40;
  F1 = H[0]*T.C10 + H[1]*T.C11 + H[2]*T.C21+ H[3]*T.C31+ H[4]*T.C41;
  F2 = H[0]*T.C20 + H[1]*T.C21 + H[2]*T.C22+ H[3]*T.C32+ H[4]*T.C42;
  F3 = H[0]*T.C30 + H[1]*T.C31 + H[2]*T.C32+ H[3]*T.C33+ H[4]*T.C43;
  F4 = H[0]*T.C40 + H[1]*T.C41 + H[2]*T.C42+ H[3]*T.C43+ H[4]*T.C44;

  HCH = H[0]*F0 +H[1]*F1 +H[2]*F2 +H[3]*F3 +H[4]*F4 ;

  wi = rcp(fvec(sigma2 +HCH));
  zetawi =  zeta * wi;
  T.chi2 +=  zeta * zetawi;
  T.NDF  += ONE;

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

  T.x  -= F0*zetawi;
  T.y  -= F1*zetawi;
  T.tx -= F2*zetawi;
  T.ty -= F3*zetawi;
  T.qp -= F4*zetawi;

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


inline void L1Filter2D( L1TrackPar &T, fvec &x, fvec &y, L1XYMeasurementInfo &info, fvec H[] )
{
  cnst TWO = 2.;

  register fvec zeta0, zeta1, S00, S10, S11, si;
  register fvec F00, F10, F20, F30, F40,  F01, F11, F21, F31, F41 ;
  register fvec K00, K10, K20, K30, K40, K01, K11, K21, K31, K41;

  zeta0 = H[0]*T.x +H[1]*T.y +H[2]*T.tx +H[3]*T.ty +H[4]*T.qp - x;
  zeta1 = H[5]*T.x +H[6]*T.y +H[7]*T.tx +H[8]*T.ty +H[9]*T.qp - y;

  // F = CH'
  F00 = H[0]*T.C00 + H[1]*T.C10 + H[2]*T.C20+ H[3]*T.C30+ H[4]*T.C40;
  F10 = H[0]*T.C10 + H[1]*T.C11 + H[2]*T.C21+ H[3]*T.C31+ H[4]*T.C41;
  F20 = H[0]*T.C20 + H[1]*T.C21 + H[2]*T.C22+ H[3]*T.C32+ H[4]*T.C42;
  F30 = H[0]*T.C30 + H[1]*T.C31 + H[2]*T.C32+ H[3]*T.C33+ H[4]*T.C43;
  F40 = H[0]*T.C40 + H[1]*T.C41 + H[2]*T.C42+ H[3]*T.C43+ H[4]*T.C44;
  F01 = H[5]*T.C00 + H[6]*T.C10 + H[7]*T.C20+ H[8]*T.C30+ H[9]*T.C40;
  F11 = H[5]*T.C10 + H[6]*T.C11 + H[7]*T.C21+ H[8]*T.C31+ H[9]*T.C41;
  F21 = H[5]*T.C20 + H[6]*T.C21 + H[7]*T.C22+ H[8]*T.C32+ H[9]*T.C42;
  F31 = H[5]*T.C30 + H[6]*T.C31 + H[7]*T.C32+ H[8]*T.C33+ H[9]*T.C43;
  F41 = H[5]*T.C40 + H[6]*T.C41 + H[7]*T.C42+ H[8]*T.C43+ H[9]*T.C44;

  S00 = H[0]*F00 +H[1]*F10 +H[2]*F20 +H[3]*F30 +H[4]*F40 + info.C00;
  S10 = H[5]*F00 +H[6]*F10 +H[7]*F20 +H[8]*F30 +H[9]*F40 + info.C10;
  S11 = H[5]*F01 +H[6]*F11 +H[7]*F21 +H[8]*F31 +H[9]*F41 + info.C11;

  si = rcp(fvec(S00*S11 - S10*S10));
  fvec S00tmp = S00;
  S00 =  si*S11;
  S10 = -si*S10;
  S11 =  si*S00tmp;

  T.chi2+=  zeta0*zeta0*S00 + 2.*zeta0*zeta1*S10 + zeta1*zeta1*S11;
  T.NDF  += TWO;

  K00 = F00*S00 + F01*S10;  K01 = F00*S10 + F01*S11;
  K10 = F10*S00 + F11*S10;  K11 = F10*S10 + F11*S11;
  K20 = F20*S00 + F21*S10;  K21 = F20*S10 + F21*S11;
  K30 = F30*S00 + F31*S10;  K31 = F30*S10 + F31*S11;
  K40 = F40*S00 + F41*S10;  K41 = F40*S10 + F41*S11;

  T.x  -= K00*zeta0 + K01*zeta1;
  T.y  -= K10*zeta0 + K11*zeta1;
  T.tx -= K20*zeta0 + K21*zeta1;
  T.ty -= K30*zeta0 + K31*zeta1;
  T.qp -= K40*zeta0 + K41*zeta1;

  T.C00-= K00*F00 + K01*F01;
  T.C10-= K10*F00 + K11*F01;
  T.C11-= K10*F10 + K11*F11;
  T.C20-= K20*F00 + K21*F01;
  T.C21-= K20*F10 + K21*F11;
  T.C22-= K20*F20 + K21*F21;
  T.C30-= K30*F00 + K31*F01;
  T.C31-= K30*F10 + K31*F11;
  T.C32-= K30*F20 + K31*F21;
  T.C33-= K30*F30 + K31*F31;
  T.C40-= K40*F00 + K41*F01;
  T.C41-= K40*F10 + K41*F11;
  T.C42-= K40*F20 + K41*F21;
  T.C43-= K40*F30 + K41*F31;
  T.C44-= K40*F40 + K41*F41;
}




inline void L1FilterFirst( fvec *T, L1Cov &C, fvec &Chi2, fvec &NDF, L1XYMeasurementInfo &info, fvec &x, fvec &y, fvec &w )
{
  cnst ZERO = 0.0, ONE = 1.;

  fvec w1 = !w;
  C.C00= w&info.C00 + w1&INF;
  C.C10= w&info.C10 + w1&INF;  C.C11= w&info.C11 + w1&INF;
  C.C20= ZERO;      C.C21= ZERO;      C.C22= INF;
  C.C30= ZERO;      C.C31= ZERO;      C.C32= ZERO; C.C33= INF;
  C.C40= ZERO;      C.C41= ZERO;      C.C42= ZERO; C.C43= ZERO; C.C44= INF;

  T[0] = w&x + w1&T[0];
  T[1] = w&y + w1&T[1];
  NDF = -3.0;
  Chi2 = ZERO;
}
*/

#undef cnst

#endif