#ifndef FitSR_H
#define FitSR_H

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

class FitSR: 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 = ZERO) const;

 private:
  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 FitSR::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 );
      
  { // update covariance matrix
      // solve [ S_new' ] = [ W ] = T * [ S'*J']

      // A = S'*J'
#ifdef SQUARE_ROOT1S  // Potter with square matrix
    Fvec_t  A00 = C.S00 + C.S20*j(0,2) + C.S30*j(0,3) + C.S40*j(0,4);
    Fvec_t  A01 = C.S10 + C.S20*j(1,2) + C.S30*j(1,3) + C.S40*j(1,4);
    Fvec_t  A02 =         C.S20*j(2,2) + C.S30*j(2,3) + C.S40*j(2,4);
    Fvec_t  A03 =         C.S20*j(3,2) + C.S30*j(3,3) + C.S40*j(3,4);
    Fvec_t  A04 =                                       C.S40;
    Fvec_t  A10 = C.S01 + C.S21*j(0,2) + C.S31*j(0,3) + C.S41*j(0,4);
    Fvec_t  A11 = C.S11 + C.S21*j(1,2) + C.S31*j(1,3) + C.S41*j(1,4);
    Fvec_t  A12 =         C.S21*j(2,2) + C.S31*j(2,3) + C.S41*j(2,4);
    Fvec_t  A13 =         C.S21*j(3,2) + C.S31*j(3,3) + C.S41*j(3,4);
    Fvec_t  A14 =                                       C.S41;
    Fvec_t  A20 = C.S02 + C.S22*j(0,2) + C.S32*j(0,3) + C.S42*j(0,4);
    Fvec_t  A21 = C.S12 + C.S22*j(1,2) + C.S32*j(1,3) + C.S42*j(1,4);
    Fvec_t  A22 =         C.S22*j(2,2) + C.S32*j(2,3) + C.S42*j(2,4);
    Fvec_t  A23 =         C.S22*j(3,2) + C.S32*j(3,3) + C.S42*j(3,4);
    Fvec_t  A24 =                                       C.S42;
    Fvec_t  A30 = C.S03 + C.S23*j(0,2) + C.S33*j(0,3) + C.S43*j(0,4);
    Fvec_t  A31 = C.S13 + C.S23*j(1,2) + C.S33*j(1,3) + C.S43*j(1,4);
    Fvec_t  A32 =         C.S23*j(2,2) + C.S33*j(2,3) + C.S43*j(2,4);
    Fvec_t  A33 =         C.S23*j(3,2) + C.S33*j(3,3) + C.S43*j(3,4);
    Fvec_t  A34 =                                       C.S43;
    Fvec_t  A40 = C.S04 + C.S24*j(0,2) + C.S34*j(0,3) + C.S44*j(0,4);
    Fvec_t  A41 = C.S14 + C.S24*j(1,2) + C.S34*j(1,3) + C.S44*j(1,4);
    Fvec_t  A42 =         C.S24*j(2,2) + C.S34*j(2,3) + C.S44*j(2,4);
    Fvec_t  A43 =         C.S24*j(3,2) + C.S34*j(3,3) + C.S44*j(3,4);
    Fvec_t  A44 =                                       C.S44;
#else // triangular
    Fvec_t  A00 = C.S00 + C.S20*j(0,2) + C.S30*j(0,3) + C.S40*j(0,4);
    Fvec_t  A01 = C.S10 + C.S20*j(1,2) + C.S30*j(1,3) + C.S40*j(1,4);
    Fvec_t  A02 =         C.S20*j(2,2) + C.S30*j(2,3) + C.S40*j(2,4);
    Fvec_t  A03 =         C.S20*j(3,2) + C.S30*j(3,3) + C.S40*j(3,4);
    Fvec_t  A04 =                                       C.S40;
    Fvec_t  A10 =         C.S21*j(0,2) + C.S31*j(0,3) + C.S41*j(0,4);
    Fvec_t  A11 = C.S11 + C.S21*j(1,2) + C.S31*j(1,3) + C.S41*j(1,4);
    Fvec_t  A12 =         C.S21*j(2,2) + C.S31*j(2,3) + C.S41*j(2,4);
    Fvec_t  A13 =         C.S21*j(3,2) + C.S31*j(3,3) + C.S41*j(3,4);
    Fvec_t  A14 =                                       C.S41;
    Fvec_t  A20 =         C.S22*j(0,2) + C.S32*j(0,3) + C.S42*j(0,4);
    Fvec_t  A21 =         C.S22*j(1,2) + C.S32*j(1,3) + C.S42*j(1,4);
    Fvec_t  A22 =         C.S22*j(2,2) + C.S32*j(2,3) + C.S42*j(2,4);
    Fvec_t  A23 =         C.S22*j(3,2) + C.S32*j(3,3) + C.S42*j(3,4);
    Fvec_t  A24 =                                       C.S42;
    Fvec_t  A30 =                        C.S33*j(0,3) + C.S43*j(0,4);
    Fvec_t  A31 =                        C.S33*j(1,3) + C.S43*j(1,4);
    Fvec_t  A32 =                        C.S33*j(2,3) + C.S43*j(2,4);
    Fvec_t  A33 =                        C.S33*j(3,3) + C.S43*j(3,4);
    Fvec_t  A34 =                                       C.S43;
    Fvec_t  A40 =                                       C.S44*j(0,4);
    Fvec_t  A41 =                                       C.S44*j(1,4);
    Fvec_t  A42 =                                       C.S44*j(2,4);
    Fvec_t  A43 =                                       C.S44*j(3,4);
    Fvec_t  A44 =                                       C.S44;
#endif // square matrix

#define S_DEFINE( k )                                                   \
    const Fvec_t s##k = sqrt( A0##k*A0##k + A1##k*A1##k + A2##k*A2##k + A3##k*A3##k + A4##k*A4##k ); \
    const Fvec_t is##k = 1.f/s##k;

#define W_DEFINE( k, j ) const Fvec_t W##k##j = is##k*( A0##k*A0##j + A1##k*A1##j + A2##k*A2##j + A3##k*A3##j + A4##k*A4##j )
    
#define A_UPDATE( k, j, l ) A##l##j -= is##k * W##k##j * A##l##k
    
#define A_UPDATE_COLUMN( k, j )                 \
    A_UPDATE( k, j, 0 );                        \
    A_UPDATE( k, j, 1 );                        \
    A_UPDATE( k, j, 2 );                        \
    A_UPDATE( k, j, 3 );                        \
    A_UPDATE( k, j, 4 );        


    
    S_DEFINE( 0 );
    const Fvec_t W00 = s0;
    W_DEFINE( 0, 1 );
    W_DEFINE( 0, 2 );
    W_DEFINE( 0, 3 );
    W_DEFINE( 0, 4 );
    A_UPDATE_COLUMN( 0, 1 );
    A_UPDATE_COLUMN( 0, 2 );
    A_UPDATE_COLUMN( 0, 3 );
    A_UPDATE_COLUMN( 0, 4 );

    S_DEFINE( 1 );
    const Fvec_t W11 = s1;
    W_DEFINE( 1, 2 );
    W_DEFINE( 1, 3 );
    W_DEFINE( 1, 4 );
    A_UPDATE_COLUMN( 1, 2 );
    A_UPDATE_COLUMN( 1, 3 );
    A_UPDATE_COLUMN( 1, 4 );

    S_DEFINE( 2 );
    const Fvec_t W22 = s2;
    W_DEFINE( 2, 3 );
    W_DEFINE( 2, 4 );
    A_UPDATE_COLUMN( 2, 3 );
    A_UPDATE_COLUMN( 2, 4 );

    S_DEFINE( 3 );
    const Fvec_t W33 = s3;
    W_DEFINE( 3, 4 );
    A_UPDATE_COLUMN( 3, 4 );

    S_DEFINE( 4 );
    const Fvec_t W44 = s4;
    
    C.S00 = W00;
    C.S10 = W01;
    C.S11 = W11;
    C.S20 = W02;
    C.S21 = W12;
    C.S22 = W22;
    C.S30 = W03;
    C.S31 = W13;
    C.S32 = W23;
    C.S33 = W33;
    C.S40 = W04;
    C.S41 = W14;
    C.S42 = W24;
    C.S43 = W34;
    C.S44 = W44;
#ifdef SQUARE_ROOT1S  // Potter with square matrix
    C.S01 =
      C.S02 = C.S12 =
      C.S03 = C.S13 = C.S23 =
      C.S04 = C.S14 = C.S24 = C.S34 = ZERO;
#endif // SQUARE_ROOT1S
    
#undef A_UPDATE_COLUMN
#undef A_UPDATE
#undef W_DEFINE
#undef S_DEFINE
  }
}

inline void FitSR::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
  Fvec_m_t mask = w > w_th;
  
    // convert input
  Fvec_t *T = track.T;
  CovV   &C = track.C;
  
  const Fvec_t V = info.sigma2*p;

  const Fvec_t &H0 = info.cos_phi;
  const Fvec_t &H1 = info.sin_phi;


    // F = S'*H'
  const Fvec_t F0 = C.S00*H0 + C.S10*H1;
#if !defined(SQUARE_ROOT1S)
  const Fvec_t F1 =            C.S11*H1;
#else
  const Fvec_t F1 = C.S01*H0 + C.S11*H1;
  const Fvec_t F2 = C.S02*H0 + C.S12*H1;
  const Fvec_t F3 = C.S03*H0 + C.S13*H1;
  const Fvec_t F4 = C.S04*H0 + C.S14*H1;
#endif
  
  const Fvec_t zeta = (H0*T[0] + H1*T[1] - u);

#ifdef SQUARE_ROOT1S  // Potter
      // a = 1/(F'*F + V)
  const Fvec_t FF = F0*F0 + F1*F1;
  
#ifndef VC
  const Fvec_t a = mask & 1/(FF + V);
#else
  Fvec_t a = ZERO;
  a(mask) = ONE*rcp(FF + V);
#endif // VC

    // K = a*S*F
  const Fvec_t aF0 = a*F0;
  const Fvec_t aF1 = a*F1;
  const Fvec_t aF2 = a*F2;
  const Fvec_t aF3 = a*F3;
  const Fvec_t aF4 = a*F4;
  const Fvec_t K0 = C.S00*aF0 + C.S01*aF1 + C.S02*aF2 + C.S03*aF3 + C.S04*aF4;
  const Fvec_t K1 = C.S10*aF0 + C.S11*aF1 + C.S12*aF2 + C.S13*aF3 + C.S14*aF4;
  const Fvec_t K2 = C.S20*aF0 + C.S21*aF1 + C.S22*aF2 + C.S23*aF3 + C.S24*aF4;
  const Fvec_t K3 = C.S30*aF0 + C.S31*aF1 + C.S32*aF2 + C.S33*aF3 + C.S34*aF4;
  const Fvec_t K4 = C.S40*aF0 + C.S41*aF1 + C.S42*aF2 + C.S43*aF3 + C.S44*aF4;

  T[0] -= K0*zeta;
  T[1] -= K1*zeta;
  T[2] -= K2*zeta;
  T[3] -= K3*zeta;
  T[4] -= K4*zeta;
  track.Chi2 +=  zeta * a * zeta ;
  track.NDF  += 1;
  
  const Fvec_t gamma = 1/(1 + sqrt(a*V));
  
    // S = S - S*a*gamma*F*F'
#define GAFF_DEFINE(i,j) const Fvec_t gaFF##i##j = gamma*aF##i*F##j;
  GAFF_DEFINE(0,0);
  GAFF_DEFINE(1,0);
  GAFF_DEFINE(1,1);
  GAFF_DEFINE(2,0);
  GAFF_DEFINE(2,1);
  GAFF_DEFINE(2,2);
  GAFF_DEFINE(3,0);
  GAFF_DEFINE(3,1);
  GAFF_DEFINE(3,2);
  GAFF_DEFINE(3,3);
  GAFF_DEFINE(4,0);
  GAFF_DEFINE(4,1);
  GAFF_DEFINE(4,2);
  GAFF_DEFINE(4,3);
  GAFF_DEFINE(4,4);
#undef GAFF_DEFINE

  CovV Cin = C;
  
  C.S00 -= Cin.S00*gaFF00 + Cin.S01*gaFF10 + Cin.S02*gaFF20 + Cin.S03*gaFF30 + Cin.S04*gaFF40;
  C.S01 -= Cin.S00*gaFF10 + Cin.S01*gaFF11 + Cin.S02*gaFF21 + Cin.S03*gaFF31 + Cin.S04*gaFF41;
  C.S02 -= Cin.S00*gaFF20 + Cin.S01*gaFF21 + Cin.S02*gaFF22 + Cin.S03*gaFF32 + Cin.S04*gaFF42;
  C.S03 -= Cin.S00*gaFF30 + Cin.S01*gaFF31 + Cin.S02*gaFF32 + Cin.S03*gaFF33 + Cin.S04*gaFF43;
  C.S04 -= Cin.S00*gaFF40 + Cin.S01*gaFF41 + Cin.S02*gaFF42 + Cin.S03*gaFF43 + Cin.S04*gaFF44;
  C.S10 -= Cin.S10*gaFF00 + Cin.S11*gaFF10 + Cin.S12*gaFF20 + Cin.S13*gaFF30 + Cin.S14*gaFF40;
  C.S11 -= Cin.S10*gaFF10 + Cin.S11*gaFF11 + Cin.S12*gaFF21 + Cin.S13*gaFF31 + Cin.S14*gaFF41;
  C.S12 -= Cin.S10*gaFF20 + Cin.S11*gaFF21 + Cin.S12*gaFF22 + Cin.S13*gaFF32 + Cin.S14*gaFF42;
  C.S13 -= Cin.S10*gaFF30 + Cin.S11*gaFF31 + Cin.S12*gaFF32 + Cin.S13*gaFF33 + Cin.S14*gaFF43;
  C.S14 -= Cin.S10*gaFF40 + Cin.S11*gaFF41 + Cin.S12*gaFF42 + Cin.S13*gaFF43 + Cin.S14*gaFF44;
  C.S20 -= Cin.S20*gaFF00 + Cin.S21*gaFF10 + Cin.S22*gaFF20 + Cin.S23*gaFF30 + Cin.S24*gaFF40;
  C.S21 -= Cin.S20*gaFF10 + Cin.S21*gaFF11 + Cin.S22*gaFF21 + Cin.S23*gaFF31 + Cin.S24*gaFF41;
  C.S22 -= Cin.S20*gaFF20 + Cin.S21*gaFF21 + Cin.S22*gaFF22 + Cin.S23*gaFF32 + Cin.S24*gaFF42;
  C.S23 -= Cin.S20*gaFF30 + Cin.S21*gaFF31 + Cin.S22*gaFF32 + Cin.S23*gaFF33 + Cin.S24*gaFF43;
  C.S24 -= Cin.S20*gaFF40 + Cin.S21*gaFF41 + Cin.S22*gaFF42 + Cin.S23*gaFF43 + Cin.S24*gaFF44;
  C.S30 -= Cin.S30*gaFF00 + Cin.S31*gaFF10 + Cin.S32*gaFF20 + Cin.S33*gaFF30 + Cin.S34*gaFF40;
  C.S31 -= Cin.S30*gaFF10 + Cin.S31*gaFF11 + Cin.S32*gaFF21 + Cin.S33*gaFF31 + Cin.S34*gaFF41;
  C.S32 -= Cin.S30*gaFF20 + Cin.S31*gaFF21 + Cin.S32*gaFF22 + Cin.S33*gaFF32 + Cin.S34*gaFF42;
  C.S33 -= Cin.S30*gaFF30 + Cin.S31*gaFF31 + Cin.S32*gaFF32 + Cin.S33*gaFF33 + Cin.S34*gaFF43;
  C.S34 -= Cin.S30*gaFF40 + Cin.S31*gaFF41 + Cin.S32*gaFF42 + Cin.S33*gaFF43 + Cin.S34*gaFF44;
  C.S40 -= Cin.S40*gaFF00 + Cin.S41*gaFF10 + Cin.S42*gaFF20 + Cin.S43*gaFF30 + Cin.S44*gaFF40;
  C.S41 -= Cin.S40*gaFF10 + Cin.S41*gaFF11 + Cin.S42*gaFF21 + Cin.S43*gaFF31 + Cin.S44*gaFF41;
  C.S42 -= Cin.S40*gaFF20 + Cin.S41*gaFF21 + Cin.S42*gaFF22 + Cin.S43*gaFF32 + Cin.S44*gaFF42;
  C.S43 -= Cin.S40*gaFF30 + Cin.S41*gaFF31 + Cin.S42*gaFF32 + Cin.S43*gaFF33 + Cin.S44*gaFF43;
  C.S44 -= Cin.S40*gaFF40 + Cin.S41*gaFF41 + Cin.S42*gaFF42 + Cin.S43*gaFF43 + Cin.S44*gaFF44;
#elif defined(SQUARE_ROOT1) // SR1 // Potter with triangular

      // a = 1/(F'*F + V)
  const Fvec_t FF = F0*F0 + F1*F1;
  
#ifndef VC
  const Fvec_t a = mask & 1/(FF + V);
#else
  Fvec_t a = ZERO;
  a(mask) = ONE*rcp(FF + V);
#endif // VC

    // K = a*S*F
  const Fvec_t aF0 = a*F0;
  const Fvec_t aF1 = a*F1;
  const Fvec_t K0 = C.S00*aF0;
  const Fvec_t K1 = C.S10*aF0 + C.S11*aF1;
  const Fvec_t K2 = C.S20*aF0 + C.S21*aF1;
  const Fvec_t K3 = C.S30*aF0 + C.S31*aF1;
  const Fvec_t K4 = C.S40*aF0 + C.S41*aF1;

  T[0] -= K0*zeta;
  T[1] -= K1*zeta;
  T[2] -= K2*zeta;
  T[3] -= K3*zeta;
  T[4] -= K4*zeta;
  track.Chi2 +=  zeta * a * zeta ;
  track.NDF  += 1;

  
#if 0 // only for square matrix S, not triangular
  const Fvec_t gamma = 1/(1 + sqrt(a*V));
  
    // S = S - S*a*gamma*F*F' // CHECKME SFF' is not triangular!. why this is correct for triangular matrix? 
  const Fvec_t gaFF00 = gamma*aF0*F0;
  const Fvec_t gaFF10 = gamma*aF1*F0;
  const Fvec_t gaFF11 = gamma*aF1*F1;
  
  C.S00 -= C.S00*gaFF00;
  const Fvec_t S10 = C.S10;
  C.S10 -= C.S10*gaFF00 + C.S11*gaFF10;
  C.S11 -=   S10*gaFF10 + C.S11*gaFF11;
  const Fvec_t S20 = C.S20;
  C.S20 -= C.S20*gaFF00 + C.S21*gaFF10;
  C.S21 -=   S20*gaFF10 + C.S21*gaFF11;
  const Fvec_t S30 = C.S30;
  C.S30 -= C.S30*gaFF00 + C.S31*gaFF10;
  C.S31 -=   S30*gaFF10 + C.S31*gaFF11;
  const Fvec_t S40 = C.S40;
  C.S40 -= C.S40*gaFF00 + C.S41*gaFF10;
  C.S41 -=   S40*gaFF10 + C.S41*gaFF11;
#else
    // G = I - a*F*F'
  CovVConventional G;
  G.C00 = 1 - aF0*F0;
  G.C10 =   - aF1*F0;
  G.C20 = 0;
  G.C30 = 0;
  G.C40 = 0;
  G.C11 = 1 - aF1*F1;
  G.C21 = 0;
  G.C31 = 0;
  G.C41 = 0;
  G.C22 = 1;
  G.C32 = 0;
  G.C42 = 0;
  G.C33 = 1;
  G.C43 = 0;
  G.C44 = 1;
  
    // S -> S*sqrt(G)
  C *= CovV(G);
#endif
  
#else // SR2
    // solve [ (V+F'F)^1/2'  (V+F'F)^1/2*K'  ] = [ W ] = T * [ V'^1/2 0 ] 
    //       [      0             S_new'     ]               [ F      S']

      // A = [ V'^1/2 0 ]
      //     [ F      S']
      //      ? 0 0 0 0 0
      //      ? ? ? ? ? ?
      // A  = ? 0 ? ? ? ?
      //      0 0 0 ? ? ?
      //      0 0 0 0 ? ?
      //      0 0 0 0 0 ?


  Fvec_t  A00 = sqrt(V);
  Fvec_t  A01 = ZERO;
  Fvec_t  A02 = ZERO;
  Fvec_t  A03 = ZERO;
  Fvec_t  A04 = ZERO;
  Fvec_t  A05 = ZERO;

#ifndef VC
  Fvec_t  A10 = mask & F0;
  Fvec_t  A20 = mask & F1;
#else
  Fvec_t  A10 = ZERO;
  Fvec_t  A20 = ZERO;
  A10(mask) = F0;
  A20(mask) = F1;
#endif // VC
  
  Fvec_t  A30 = ZERO;
  Fvec_t  A40 = ZERO;
  Fvec_t  A50 = ZERO;
  Fvec_t  A11 = C.S00;
  Fvec_t  A12 = C.S10;
  Fvec_t  A13 = C.S20;
  Fvec_t  A14 = C.S30;
  Fvec_t  A15 = C.S40;
  Fvec_t  A21 = ZERO;
  Fvec_t  A22 = C.S11;
  Fvec_t  A23 = C.S21;
  Fvec_t  A24 = C.S31;
  Fvec_t  A25 = C.S41;
  Fvec_t  A31 = ZERO;
  Fvec_t  A32 = ZERO;
  Fvec_t  A33 = C.S22;
  Fvec_t  A34 = C.S32;
  Fvec_t  A35 = C.S42;
  Fvec_t  A41 = ZERO;
  Fvec_t  A42 = ZERO;
  Fvec_t  A43 = ZERO;
  Fvec_t  A44 = C.S33;
  Fvec_t  A45 = C.S43;
  Fvec_t  A51 = ZERO;
  Fvec_t  A52 = ZERO;
  Fvec_t  A53 = ZERO;
  Fvec_t  A54 = ZERO;
  Fvec_t  A55 = C.S44;

  Fvec_t W01;
  Fvec_t W02;
  Fvec_t W03;
  Fvec_t W04;
  Fvec_t W05;
  Fvec_t& W11 = C.S00;
  Fvec_t& W12 = C.S10;
  Fvec_t& W22 = C.S11;
  Fvec_t& W13 = C.S20;
  Fvec_t& W23 = C.S21;
  Fvec_t& W33 = C.S22;
  Fvec_t& W14 = C.S30;
  Fvec_t& W24 = C.S31;
  Fvec_t& W34 = C.S32;
  Fvec_t& W44 = C.S33;
  Fvec_t& W15 = C.S40;
  Fvec_t& W25 = C.S41;
  Fvec_t& W35 = C.S42;
  Fvec_t& W45 = C.S43;
  Fvec_t& W55 = C.S44;

#define S_DEFINE( k ) \
    const Fvec_t s##k = sqrt( A0##k*A0##k + A1##k*A1##k + A2##k*A2##k + A3##k*A3##k + A4##k*A4##k + A5##k*A5##k ); \
    const Fvec_t is##k = ONE/s##k;

#define W_DEFINE( k, j ) W##k##j = is##k*( A0##k*A0##j + A1##k*A1##j + A2##k*A2##j + A3##k*A3##j + A4##k*A4##j + A5##k*A5##j )
    
#define A_UPDATE( k, j, l ) A##l##j -= is##k * W##k##j * A##l##k
    
#define A_UPDATE_COLUMN( k, j ) \
    A_UPDATE( k, j, 0 );        \
    A_UPDATE( k, j, 1 );        \
    A_UPDATE( k, j, 2 );        \
    A_UPDATE( k, j, 3 );        \
    A_UPDATE( k, j, 4 );        \
    A_UPDATE( k, j, 5 );        


    
    S_DEFINE( 0 );
    const Fvec_t W00 = s0;
    W_DEFINE( 0, 1 );
    W_DEFINE( 0, 2 );
    W_DEFINE( 0, 3 );
    W_DEFINE( 0, 4 );
    W_DEFINE( 0, 5 );
    A_UPDATE_COLUMN( 0, 1 );
    A_UPDATE_COLUMN( 0, 2 );
    A_UPDATE_COLUMN( 0, 3 );
    A_UPDATE_COLUMN( 0, 4 );
    A_UPDATE_COLUMN( 0, 5 );
    
    S_DEFINE( 1 );
    W11 = s1;
    W_DEFINE( 1, 2 );
    W_DEFINE( 1, 3 );
    W_DEFINE( 1, 4 );
    W_DEFINE( 1, 5 );
    A_UPDATE_COLUMN( 1, 2 );
    A_UPDATE_COLUMN( 1, 3 );
    A_UPDATE_COLUMN( 1, 4 );
    A_UPDATE_COLUMN( 1, 5 );
    
    S_DEFINE( 2 );
    W22 = s2;
    W_DEFINE( 2, 3 );
    W_DEFINE( 2, 4 );
    W_DEFINE( 2, 5 );
    A_UPDATE_COLUMN( 2, 3 );
    A_UPDATE_COLUMN( 2, 4 );
    A_UPDATE_COLUMN( 2, 5 );

    S_DEFINE( 3 );
    W33 = s3;
    W_DEFINE( 3, 4 );
    W_DEFINE( 3, 5 );
    A_UPDATE_COLUMN( 3, 4 );
    A_UPDATE_COLUMN( 3, 5 );

    S_DEFINE( 4 );
    W44 = s4;
    W_DEFINE( 4, 5 );
    A_UPDATE_COLUMN( 4, 5 );
    
    S_DEFINE( 5 );
    W55 = s5;
    
#undef A_UPDATE_COLUMN
#undef A_UPDATE
#undef W_DEFINE
#undef S_DEFINE

      // Get state vector update

    const Fvec_t zetaA = zeta/W00;

    T[0] -= W01*zetaA;
    T[1] -= W02*zetaA;
    T[2] -= W03*zetaA;
    T[3] -= W04*zetaA;
    T[4] -= W05*zetaA;
    track.Chi2 +=  zetaA * zetaA;
    track.NDF  += 1;
  
#endif // IMPL1
}

inline void FitSR::FilterFirst( TrackV &track, HitV &hit, Station &st ) const
{
  CovV &C = track.C;
  Fvec_t INFs = sqrt(INF);
  Fvec_t INFs2 = sqrt(INF2);
  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.S00= sqrt(c00); 
  C.S10= c10/C.S00;   C.S11= c11 - C.S10*C.S10;
  C.S20= ZERO;      C.S21= ZERO;      C.S22= INFs2;
  C.S30= ZERO;      C.S31= ZERO;      C.S32= ZERO; C.S33= INFs2;
  C.S40= ZERO;      C.S41= ZERO;      C.S42= ZERO; C.S43= ZERO; C.S44= INFs;
#ifdef SQUARE_ROOT1S  // Potter with square matrix
    C.S01 =
      C.S02 = C.S12 =
      C.S03 = C.S13 = C.S23 =
      C.S04 = C.S14 = C.S24 = C.S34 = ZERO;
#endif // SQUARE_ROOT1S

  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 FitSR::AddMaterial( CovV &C, Fvec_t Q22, Fvec_t Q32, Fvec_t Q33 ) const
{
    // sQ = sqrt(Q)'
  const Fvec_t sQ22 = sqrt( Q22 );
  const Fvec_t sQ32 = Q32 / sQ22;
  const Fvec_t sQ33 = sqrt( Q33 - sQ32*sQ32 );

  { // update covariance matrix

      // solve [ S_new' ] = T * [   S'   ]
      //       [   0    ]       [sqrt(Q)']

      // A = [   S'   ]
      //     [sqrt(Q)']
      //      ? ? ? ? ?
      //      0 ? ? ? ?
      //      0 0 ? ? ?
      //      0 0 0 ? ?
      // A  = 0 0 0 0 ?
      //      0 0 0 0 0
      //      0 0 0 0 0
      //      0 0 ? ? 0
      //      0 0 0 ? 0
      //      0 0 0 0 0

    const Fvec_t ZERO = 0.f;
    Fvec_t  A00 = C.S00;
    Fvec_t  A01 = C.S10;
    Fvec_t  A02 = C.S20;
    Fvec_t  A03 = C.S30;
    Fvec_t  A04 = C.S40;
#ifdef SQUARE_ROOT1S  // Potter with square matrix
    Fvec_t  A10 = C.S01;
    Fvec_t  A11 = C.S11;
    Fvec_t  A12 = C.S21;
    Fvec_t  A13 = C.S31;
    Fvec_t  A14 = C.S41;
    Fvec_t  A20 = C.S02;
    Fvec_t  A21 = C.S12;
    Fvec_t  A22 = C.S22;
    Fvec_t  A23 = C.S32;
    Fvec_t  A24 = C.S42;
    Fvec_t  A30 = C.S03;
    Fvec_t  A31 = C.S13;
    Fvec_t  A32 = C.S23;
    Fvec_t  A33 = C.S33;
    Fvec_t  A34 = C.S43;
    Fvec_t  A40 = C.S04;
    Fvec_t  A41 = C.S14;
    Fvec_t  A42 = C.S24;
    Fvec_t  A43 = C.S34;
    Fvec_t  A44 = C.S44;
#else // triangle
    Fvec_t  A10 = ZERO;
    Fvec_t  A11 = C.S11;
    Fvec_t  A12 = C.S21;
    Fvec_t  A13 = C.S31;
    Fvec_t  A14 = C.S41;
    Fvec_t  A20 = ZERO;
    Fvec_t  A21 = ZERO;
    Fvec_t  A22 = C.S22;
    Fvec_t  A23 = C.S32;
    Fvec_t  A24 = C.S42;
    Fvec_t  A30 = ZERO;
    Fvec_t  A31 = ZERO;
    Fvec_t  A32 = ZERO;
    Fvec_t  A33 = C.S33;
    Fvec_t  A34 = C.S43;
    Fvec_t  A40 = ZERO;
    Fvec_t  A41 = ZERO;
    Fvec_t  A42 = ZERO;
    Fvec_t  A43 = ZERO;
    Fvec_t  A44 = C.S44;
#endif // SQUARE_ROOT1S
    Fvec_t A70 = ZERO;
    Fvec_t A71 = ZERO;
    Fvec_t A72 = sQ22;
    Fvec_t A73 = sQ32;
    Fvec_t A74 = ZERO;
    Fvec_t A80 = ZERO;
    Fvec_t A81 = ZERO;
    Fvec_t A82 = ZERO;
    Fvec_t A83 = sQ33;
    Fvec_t A84 = ZERO;

    
#define S_DEFINE( k ) \
    const Fvec_t s##k = sqrt( A0##k*A0##k + A1##k*A1##k + A2##k*A2##k + A3##k*A3##k + A4##k*A4##k + A7##k*A7##k + A8##k*A8##k ); \
    const Fvec_t is##k = 1.f/s##k;

    
#define W_DEFINE( k, j ) \
    const Fvec_t W##k##j = is##k*( A0##k*A0##j + A1##k*A1##j + A2##k*A2##j + A3##k*A3##j + A4##k*A4##j + A7##k*A7##j + A8##k*A8##j )
    
#define A_UPDATE( k, j, l ) A##l##j -= is##k * W##k##j * A##l##k
    
#define A_UPDATE_COLUMN( k, j ) \
      A_UPDATE( k, j, 0 );      \
    A_UPDATE( k, j, 1 );        \
    A_UPDATE( k, j, 2 );        \
    A_UPDATE( k, j, 3 );        \
    A_UPDATE( k, j, 4 );        \
    A_UPDATE( k, j, 7 );        \
    A_UPDATE( k, j, 8 );       

    S_DEFINE( 0 );
    const Fvec_t W00 = s0;
    W_DEFINE( 0, 1 );
    W_DEFINE( 0, 2 );
    W_DEFINE( 0, 3 );
    W_DEFINE( 0, 4 );
    A_UPDATE_COLUMN( 0, 1 );
    A_UPDATE_COLUMN( 0, 2 );
    A_UPDATE_COLUMN( 0, 3 );
    A_UPDATE_COLUMN( 0, 4 );

    S_DEFINE( 1 );
    const Fvec_t W11 = s1;
    W_DEFINE( 1, 2 );
    W_DEFINE( 1, 3 );
    W_DEFINE( 1, 4 );
    A_UPDATE_COLUMN( 1, 2 );
    A_UPDATE_COLUMN( 1, 3 );
    A_UPDATE_COLUMN( 1, 4 );

    S_DEFINE( 2 );
    const Fvec_t W22 = s2;
    W_DEFINE( 2, 3 );
    W_DEFINE( 2, 4 );
    A_UPDATE_COLUMN( 2, 3 );
    A_UPDATE_COLUMN( 2, 4 );

    S_DEFINE( 3 );
    const Fvec_t W33 = s3;
    W_DEFINE( 3, 4 );
    A_UPDATE_COLUMN( 3, 4 );

    S_DEFINE( 4 );
    const Fvec_t W44 = s4;
    
    C.S00 = W00;
    C.S10 = W01;
    C.S11 = W11;
    C.S20 = W02;
    C.S21 = W12;
    C.S22 = W22;
    C.S30 = W03;
    C.S31 = W13;
    C.S32 = W23;
    C.S33 = W33;
    C.S40 = W04;
    C.S41 = W14;
    C.S42 = W24;
    C.S43 = W34;
    C.S44 = W44;
#ifdef SQUARE_ROOT1S  // Potter with square matrix
    C.S01 =
      C.S02 = C.S12 =
      C.S03 = C.S13 = C.S23 =
      C.S04 = C.S14 = C.S24 = C.S34 = ZERO;
#endif // SQUARE_ROOT1S
    
#undef A_UPDATE_COLUMN
#undef A_UPDATE
#undef W_DEFINE
#undef S_DEFINE
  }
}



inline void FitSR::ExtrapolateWithMaterial( TrackV &track,  const Fvec_t &z_out,  Fvec_t& qp0, FieldRegion &F, Station &st, bool isPipe, Fvec_t w) const
{
  Fvec_t *T = track.T;
  CovV   &C = track.C;

  Jacobian_t j;
  ExtrapolateJ( T, z_out, qp0, F, j, w );
  
  
  Fvec_t Q22, Q32, Q33;
  if (isPipe)
    GetMSMatrix( track.T[2], track.T[3], st.RadThick + PipeRadThick, log(st.RadThick + PipeRadThick), qp0, Q22, Q32, Q33 );
  else
    GetMSMatrix( track.T[2], track.T[3], st.RadThick, st.logRadThick, qp0, Q22, Q32, Q33 );

  { // update covariance matrix

      // solve [ S_new' ] = T * [   S'*J' ]
      //       [   0    ]       [sqrt(Q)' ]

      // A = [   S'   ]
      //     [sqrt(Q)']
      //      ? ? ? ? ?
      //      0 ? ? ? ?
      //      0 0 ? ? ?
      //      0 0 0 ? ?
      // A  = 0 0 0 0 ?
      //      0 0 0 0 0
      //      0 0 0 0 0
      //      0 0 ? ? 0
      //      0 0 0 ? 0
      //      0 0 0 0 0

    const Fvec_t ZERO = 0.f;
#ifdef SQUARE_ROOT1S  // Potter with square matrix
    Fvec_t  A00 = C.S00 + C.S20*j(0,2) + C.S30*j(0,3) + C.S40*j(0,4);
    Fvec_t  A01 = C.S10 + C.S20*j(1,2) + C.S30*j(1,3) + C.S40*j(1,4);
    Fvec_t  A02 =         C.S20*j(2,2) + C.S30*j(2,3) + C.S40*j(2,4);
    Fvec_t  A03 =         C.S20*j(3,2) + C.S30*j(3,3) + C.S40*j(3,4);
    Fvec_t  A04 =                                       C.S40;
    Fvec_t  A10 = C.S01 + C.S21*j(0,2) + C.S31*j(0,3) + C.S41*j(0,4);
    Fvec_t  A11 = C.S11 + C.S21*j(1,2) + C.S31*j(1,3) + C.S41*j(1,4);
    Fvec_t  A12 =         C.S21*j(2,2) + C.S31*j(2,3) + C.S41*j(2,4);
    Fvec_t  A13 =         C.S21*j(3,2) + C.S31*j(3,3) + C.S41*j(3,4);
    Fvec_t  A14 =                                       C.S41;
    Fvec_t  A20 = C.S02 + C.S22*j(0,2) + C.S32*j(0,3) + C.S42*j(0,4);
    Fvec_t  A21 = C.S12 + C.S22*j(1,2) + C.S32*j(1,3) + C.S42*j(1,4);
    Fvec_t  A22 =         C.S22*j(2,2) + C.S32*j(2,3) + C.S42*j(2,4);
    Fvec_t  A23 =         C.S22*j(3,2) + C.S32*j(3,3) + C.S42*j(3,4);
    Fvec_t  A24 =                                       C.S42;
    Fvec_t  A30 = C.S03 + C.S23*j(0,2) + C.S33*j(0,3) + C.S43*j(0,4);
    Fvec_t  A31 = C.S13 + C.S23*j(1,2) + C.S33*j(1,3) + C.S43*j(1,4);
    Fvec_t  A32 =         C.S23*j(2,2) + C.S33*j(2,3) + C.S43*j(2,4);
    Fvec_t  A33 =         C.S23*j(3,2) + C.S33*j(3,3) + C.S43*j(3,4);
    Fvec_t  A34 =                                       C.S43;
    Fvec_t  A40 = C.S04 + C.S24*j(0,2) + C.S34*j(0,3) + C.S44*j(0,4);
    Fvec_t  A41 = C.S14 + C.S24*j(1,2) + C.S34*j(1,3) + C.S44*j(1,4);
    Fvec_t  A42 =         C.S24*j(2,2) + C.S34*j(2,3) + C.S44*j(2,4);
    Fvec_t  A43 =         C.S24*j(3,2) + C.S34*j(3,3) + C.S44*j(3,4);
    Fvec_t  A44 =                                       C.S44;
#else // triangular
    Fvec_t  A00 = C.S00 + C.S20*j(0,2) + C.S30*j(0,3) + C.S40*j(0,4);
    Fvec_t  A01 = C.S10 + C.S20*j(1,2) + C.S30*j(1,3) + C.S40*j(1,4);
    Fvec_t  A02 =         C.S20*j(2,2) + C.S30*j(2,3) + C.S40*j(2,4);
    Fvec_t  A03 =         C.S20*j(3,2) + C.S30*j(3,3) + C.S40*j(3,4);
    Fvec_t  A04 =                                 C.S40;
    Fvec_t  A10 =         C.S21*j(0,2) + C.S31*j(0,3) + C.S41*j(0,4);
    Fvec_t  A11 = C.S11 + C.S21*j(1,2) + C.S31*j(1,3) + C.S41*j(1,4);
    Fvec_t  A12 =         C.S21*j(2,2) + C.S31*j(2,3) + C.S41*j(2,4);
    Fvec_t  A13 =         C.S21*j(3,2) + C.S31*j(3,3) + C.S41*j(3,4);
    Fvec_t  A14 =                                 C.S41;
    Fvec_t  A20 =         C.S22*j(0,2) + C.S32*j(0,3) + C.S42*j(0,4);
    Fvec_t  A21 =         C.S22*j(1,2) + C.S32*j(1,3) + C.S42*j(1,4);
    Fvec_t  A22 =         C.S22*j(2,2) + C.S32*j(2,3) + C.S42*j(2,4);
    Fvec_t  A23 =         C.S22*j(3,2) + C.S32*j(3,3) + C.S42*j(3,4);
    Fvec_t  A24 =                                 C.S42;
    Fvec_t  A30 =                     C.S33*j(0,3) + C.S43*j(0,4);
    Fvec_t  A31 =                     C.S33*j(1,3) + C.S43*j(1,4);
    Fvec_t  A32 =                     C.S33*j(2,3) + C.S43*j(2,4);
    Fvec_t  A33 =                     C.S33*j(3,3) + C.S43*j(3,4);
    Fvec_t  A34 =                                 C.S43;
    Fvec_t  A40 =                                 C.S44*j(0,4);
    Fvec_t  A41 =                                 C.S44*j(1,4);
    Fvec_t  A42 =                                 C.S44*j(2,4);
    Fvec_t  A43 =                                 C.S44*j(3,4);
    Fvec_t  A44 =                                 C.S44;
#endif // SQUARE_ROOT1S
    Fvec_t A70 = ZERO;
    Fvec_t A71 = ZERO;
    Fvec_t A72 = sqrt( Q22 ); // sQ22; // sQ = sqrt(Q)'
    Fvec_t A73 = Q32 / A72; // sQ32;
    Fvec_t A74 = ZERO;
    Fvec_t A80 = ZERO;
    Fvec_t A81 = ZERO;
    Fvec_t A82 = ZERO;
    Fvec_t A83 = sqrt( Q33 - A73*A73 ); // sQ33;
    Fvec_t A84 = ZERO;
    
#define S_DEFINE( k ) \
    const Fvec_t s##k = sqrt( A0##k*A0##k + A1##k*A1##k + A2##k*A2##k + A3##k*A3##k + A4##k*A4##k + A7##k*A7##k + A8##k*A8##k ); \
    const Fvec_t is##k = 1.f/s##k;

    
#define W_DEFINE( k, j ) \
    C.S##j##k = is##k*( A0##k*A0##j + A1##k*A1##j + A2##k*A2##j + A3##k*A3##j + A4##k*A4##j + A7##k*A7##j + A8##k*A8##j ) // W##k##j = C.S##j##k
    
#define A_UPDATE( k, j, l ) A##l##j -= is##k * C.S##j##k * A##l##k
    
#define A_UPDATE_COLUMN( k, j ) \
    A_UPDATE( k, j, 0 );        \
    A_UPDATE( k, j, 1 );        \
    A_UPDATE( k, j, 2 );        \
    A_UPDATE( k, j, 3 );        \
    A_UPDATE( k, j, 4 );        \
    A_UPDATE( k, j, 7 );        \
    A_UPDATE( k, j, 8 );
    

    S_DEFINE( 0 );
    C.S00 = s0;
    W_DEFINE( 0, 1 );
    W_DEFINE( 0, 2 );
    W_DEFINE( 0, 3 );
    W_DEFINE( 0, 4 );
    A_UPDATE_COLUMN( 0, 1 );
    A_UPDATE_COLUMN( 0, 2 );
    A_UPDATE_COLUMN( 0, 3 );
    A_UPDATE_COLUMN( 0, 4 );

    S_DEFINE( 1 );
    C.S11 = s1;
    W_DEFINE( 1, 2 );
    W_DEFINE( 1, 3 );
    W_DEFINE( 1, 4 );
    A_UPDATE_COLUMN( 1, 2 );
    A_UPDATE_COLUMN( 1, 3 );
    A_UPDATE_COLUMN( 1, 4 );

    S_DEFINE( 2 );
    C.S22 = s2;
    W_DEFINE( 2, 3 );
    W_DEFINE( 2, 4 );
    A_UPDATE_COLUMN( 2, 3 );
    A_UPDATE_COLUMN( 2, 4 );

    S_DEFINE( 3 );
    C.S33 = s3;
    W_DEFINE( 3, 4 );
    A_UPDATE_COLUMN( 3, 4 );

    S_DEFINE( 4 );
    C.S44 = s4;

#ifdef SQUARE_ROOT1S  // Potter with square matrix
    C.S01 =
      C.S02 = C.S12 =
      C.S03 = C.S13 = C.S23 =
      C.S04 = C.S14 = C.S24 = C.S34 = ZERO;
#endif // SQUARE_ROOT1S
    
#undef A_UPDATE_COLUMN
#undef A_UPDATE
#undef W_DEFINE
#undef S_DEFINE
  }
  
}


#endif