//-*- Mode: C++ -*-
  // *****************************************************************************
  //                                                                             *
  // @Autors: I.Kulakov; M.Zyzak; I.Kisel                                        *
  // @e-mail: I.Kulakov@gsi.de; M.Zyzak@gsi.de; I.Kisel@compeng.uni-frankfurt.de *
  //                                                                             *
  // *****************************************************************************


#ifndef ALIHLTTPCCATRACKPARAMVECTOR_H
#define ALIHLTTPCCATRACKPARAMVECTOR_H

#include "AliHLTTPCCADef.h"
#include "AliHLTVector.h"
#include "AliHLTTPCCAMath.h"
#include "L1UMeasurementInfo.h"
#include "L1XYMeasurementInfo.h"
#include "L1MaterialInfo.h"
#include "L1Field.h"

class AliHLTTPCCAParam;
class ITSCAHit;
class ITSCAHitV;
class ITSCATarget;

namespace std
{
  template<typename T> struct char_traits;
  template<typename _CharT, typename _Traits> class basic_istream;
  typedef basic_istream<char, char_traits<char> > istream;
  template<typename _CharT, typename _Traits> class basic_ostream;
  typedef basic_ostream<char, char_traits<char> > ostream;
} // namespace std

/**
 * @class AliHLTTPCCATrackParamVector
 *
 * AliHLTTPCCATrackParamVector class describes the track parametrisation
 * which is used by the AliHLTTPCCATracker slice tracker.
 *
 */
class AliHLTTPCCATrackParamVector
{
  friend std::istream &operator>>( std::istream &, AliHLTTPCCATrackParamVector & );
  friend std::ostream &operator<<( std::ostream &, const AliHLTTPCCATrackParamVector & );
 public:
  AliHLTTPCCATrackParamVector():
    fX(0.f),fY(0.f),fTx(0.f),fTy(0.f),fQP(0.f),fZ(0.f),
    fC00(0.f),
    fC10(0.f), fC11(0.f),
    fC20(0.f), fC21(0.f), fC22(0.f),
    fC30(0.f), fC31(0.f), fC32(0.f), fC33(0.f),
    fC40(0.f), fC41(0.f), fC42(0.f), fC43(0.f), fC44(0.f),
    fChi2(0.f), fNDF(0.f)
  { fZB[0] = fZB[1] = 10e10; }

  void InitByTarget( const ITSCATarget& target );
  void InitByHit( const ITSCAHitV& hit, const AliHLTTPCCAParam& param, const sfloat_v& dQP );

  void InitDirection( sfloat_v r0, sfloat_v r1, sfloat_v r2 ) { // initialize direction parameters according to a given tangent vector r = { r0, r1, r2 }
    SetTx( r1/r0 );
    SetTy( r2/r0 );
  }

  
  sfloat_v X()      const { return fX; }
  sfloat_v Y()      const { return fY; }
  sfloat_v Z()      const { return fZ; }
  sfloat_v Tx()     const { return fTx; }
  sfloat_v Ty()     const { return fTy; }
  sfloat_v QP()     const { return fQP;}

  sfloat_v Chi2()  const { return fChi2; }
  short_v   NDF()   const { return fNDF; }

  sfloat_v Angle()       const { return fAlpha; }

  sfloat_v X0()      const { return Z(); }
  sfloat_v X1()      const { return X(); }
  sfloat_v X2()      const { return Y(); }
  sfloat_v Tx1()     const { return Tx(); }
  sfloat_v Tx2()     const { return Ty(); }
  sfloat_v Momentum()    const { return QP(); } // used for triplets comparison

  const sfloat_v& Par( int i ) const {
    switch ( i ) {
      case 0: return fX;
      case 1: return fY;
      case 2: return fTx;
      case 3: return fTy;
      case 4: return fQP;
    };
    assert(0); return fX;
  }
  
  const sfloat_v& Cov( int i ) const {
    switch ( i ) {
      case 0: return fC00;
      case 1: return fC10;
      case 2: return fC11;
      case 3: return fC20;
      case 4: return fC21;
      case 5: return fC22;
      case 6: return fC30;
      case 7: return fC31;
      case 8: return fC32;
      case 9: return fC33;
      case 10: return fC40;
      case 11: return fC41;
      case 12: return fC42;
      case 13: return fC43;
      case 14: return fC44;
    };
    assert(0); return fC00;
  }
  
  sfloat_v Err2X1()      const { return Err2X(); }
  sfloat_v Err2X2()      const { return Err2Y(); }
  
  sfloat_v Err2X()      const { return fC00; }
  sfloat_v Err2Y()      const { return fC11; }
  sfloat_v Err2QP()          const { return fC44; }
  sfloat_v Err2Momentum()    const { return Err2QP(); }



  void SetX( const sfloat_v &v )     {  fX = v; }
  void SetY( const sfloat_v &v )     {  fY = v; }
  void SetZ( const sfloat_v &v )     {  fZ = v; }
  void SetTx( const sfloat_v &v )    { fTx = v; }
  void SetTy( const sfloat_v &v )    { fTy = v; }
  void SetQP( const sfloat_v &v )    { fQP = v; }
  
  void SetChi2( const sfloat_v &v )  {  fChi2 = v; }
  void SetNDF( int v )   { fNDF = v; }
  void SetNDF( const short_v &v )   { fNDF = v; }

  void SetAngle( const sfloat_v& v ) { fAlpha = v; }

  void SetPar( int i, sfloat_v v ) {
    switch ( i ) {
      case 0: fX  = v; break;
      case 1: fY  = v; break;
      case 2: fTx = v; break;
      case 3: fTy = v; break;
      case 4: fQP = v; break;
    }
  }
  void SetCov( int i, sfloat_v v ) {
    switch ( i ) {
      case 0: fC00 = v; break;
      case 1: fC10 = v; break;
      case 2: fC11 = v; break;
      case 3: fC20 = v; break;
      case 4: fC21 = v; break;
      case 5: fC22 = v; break;
      case 6: fC30 = v; break;
      case 7: fC31 = v; break;
      case 8: fC32 = v; break;
      case 9: fC33 = v; break;
      case 10: fC40 = v; break;
      case 11: fC41 = v; break;
      case 12: fC42 = v; break;
      case 13: fC43 = v; break;
      case 14: fC44 = v; break;
    }
  }
  
  void SetX( const sfloat_v &v, const sfloat_m &m )     {  fX( m ) = v; }
  void SetY( const sfloat_v &v, const sfloat_m &m )     {  fY( m ) = v; }
  void SetZ( const sfloat_v &v, const sfloat_m &m )     {  fZ( m ) = v; }
  void SetTx( const sfloat_v &v, const sfloat_m &m )     {  fTx( m ) = v; }
  void SetTy( const sfloat_v &v, const sfloat_m &m )     {  fTy( m ) = v; }
  void SetQP( const sfloat_v &v, const sfloat_m &m )     {  fQP( m ) = v; }
  
  void SetChi2( const sfloat_v &v, const sfloat_m &m  )  {  fChi2(m) = v; }
  void SetNDF( const short_v &v, const short_m &m )   { fNDF(m) = v; }

  void SetField( int i, const L1FieldValue& b, const sfloat_v& zb ) { fB[i] = b; fZB[i] = zb; }
  
  void SetCovX12( sfloat_v v00, sfloat_v v10, sfloat_v v11 ) { fC00 = v00; fC10 = v10; fC11 = v11; }
  
  
  
  sfloat_m Transport( const ITSCAHitV& hit, const AliHLTTPCCAParam& p, const sfloat_m &mask = sfloat_m( true ) );
  sfloat_m Filter( const ITSCAHitV& hit, const AliHLTTPCCAParam& param, const sfloat_m &mask = sfloat_m( true ) );
  
  sfloat_m Transport( const ITSCAHit& hit, const AliHLTTPCCAParam& p, const sfloat_m &mask = sfloat_m( true ) );
  sfloat_m Filter( const ITSCAHit& hit, const AliHLTTPCCAParam& param, const sfloat_m &mask = sfloat_m( true ) );

  sfloat_m AddTarget( const ITSCATarget& t, const sfloat_m &mask = sfloat_m( true ) );
  
 private:
  sfloat_m Transport( const ITSCAHitV& hit, const L1FieldRegion& F, const AliHLTTPCCAParam& p, const sfloat_m &mask = sfloat_m( true ) ); // dbg TODO delme
  
  sfloat_m TransportToX0WithMaterial( const sfloat_v &x0, const L1FieldRegion &F, const L1MaterialInfo &material, const sfloat_v &qp0, const sfloat_m &mask = sfloat_m( true ) );
  sfloat_m TransportToX0( const sfloat_v &x0, const L1FieldRegion &F, const sfloat_v &qp0, const sfloat_m &mask = sfloat_m( true ) );
  sfloat_m PassMaterial( const L1MaterialInfo &info, const sfloat_v &qp0, const sfloat_m &mask = sfloat_m( true ) );
  
  sfloat_m Filter( const sfloat_v &u, const L1UMeasurementInfo &info, const sfloat_m &active );

  sfloat_m FilterVtx( const sfloat_v &xV, const sfloat_v& yV, const L1XYMeasurementInfo &info, sfloat_v& extrDx, sfloat_v& extrDy, sfloat_v J[], const sfloat_m &active = sfloat_m( true ) );

  sfloat_m TransportJXY0ToX0( const sfloat_v &x0, const L1FieldRegion &F, sfloat_v& extrDx, sfloat_v& extrDy, sfloat_v &J04, sfloat_v &J14, const sfloat_m &active = sfloat_m( true )  );

  
  sfloat_v fX,fY,fTx,fTy,fQP,fZ,
                                 fC00,
                                 fC10, fC11,
                                 fC20, fC21, fC22,
                                 fC30, fC31, fC32, fC33,
                                 fC40, fC41, fC42, fC43, fC44;

  sfloat_v fChi2;   // the chi^2 value
  short_v   fNDF;    // the Number of Degrees of Freedom

  L1FieldValue fB[2]; // field at two previous points, which track passed
  sfloat_v fZB[2]; // fZ position of the filld point
  
  sfloat_v fAlpha; // coor system
};

#include "debug.h"


inline sfloat_m AliHLTTPCCATrackParamVector::Filter( const sfloat_v &u, const L1UMeasurementInfo &info, const sfloat_m &active )
{
  sfloat_v wi, zeta, zetawi, HCH;
  sfloat_v F0, F1, F2, F3, F4;
  sfloat_v K1, K2, K3, K4;

  zeta = info.cos_phi*fX + info.sin_phi*fY - u;

    // F = CH'
  F0 = info.cos_phi*fC00 + info.sin_phi*fC10;
  F1 = info.cos_phi*fC10 + info.sin_phi*fC11;

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

  F2 = info.cos_phi*fC20 + info.sin_phi*fC21;
  F3 = info.cos_phi*fC30 + info.sin_phi*fC31;
  F4 = info.cos_phi*fC40 + info.sin_phi*fC41;

    // #if 0 // use mask
    //   const sfloat_m mask = (HCH < info.sigma2 * 16.);
    //   wi = w/( (mask & info.sigma2) +HCH );
    //   zetawi = zeta *wi;
    //   fChi2 +=  mask & (zeta * zetawi);
    // #else
  wi = 1/( info.sigma2 + HCH );
  zetawi = zeta *wi;
  fChi2(active) +=  zeta * zetawi;
    // #endif // 0
  fNDF( static_cast<short_m>(active) ) += 1;

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

  fX(active)   -= F0*zetawi;
  fY(active)   -= F1*zetawi;
  fTx(active)  -= F2*zetawi;
  fTy(active)  -= F3*zetawi;
  fQP(active)  -= F4*zetawi;

  fC00(active) -= F0*F0*wi;
  fC10(active) -= K1*F0;
  fC11(active) -= K1*F1;
  fC20(active) -= K2*F0;
  fC21(active) -= K2*F1;
  fC22(active) -= K2*F2;
  fC30(active) -= K3*F0;
  fC31(active) -= K3*F1;
  fC32(active) -= K3*F2;
  fC33(active) -= K3*F3;
  fC40(active) -= K4*F0;
  fC41(active) -= K4*F1;
  fC42(active) -= K4*F2;
  fC43(active) -= K4*F3;
  fC44(active) -= K4*F4;
  
  return active;
}

inline sfloat_m AliHLTTPCCATrackParamVector::TransportToX0WithMaterial( const sfloat_v &x0, const L1FieldRegion &F, const L1MaterialInfo &material, const sfloat_v &qp0, const sfloat_m &mask )
{ // TODO material
  sfloat_m active = mask;
  active &= TransportToX0( x0, F, qp0, active );
  active &= PassMaterial( material, qp0, active );
  return active;
}

inline sfloat_m AliHLTTPCCATrackParamVector::TransportToX0( const sfloat_v &x0_out, const L1FieldRegion &F, const sfloat_v &qp0, const sfloat_m &mask )
{
    //cout<<"Extrapolation..."<<endl;
  //
  //  Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
  //  

  const sfloat_v c_light = 0.000299792458;

  const sfloat_v   
    c1 = 1., c2 = 2., c3 = 3., c4 = 4., c6 = 6., c9 = 9., c15 = 15., c18 = 18., c45 = 45.,
    c2i = 1./2., c3i = 1./3., c6i = 1./6., c12i = 1./12.;

  const sfloat_v dz = (x0_out - fZ);
  const sfloat_v dz2 = dz*dz;
  const sfloat_v dz3 = dz2*dz;

  // construct coefficients
  
  const sfloat_v x = fTx;
  const sfloat_v y = fTy;
  const sfloat_v xx = x*x;
  const sfloat_v xy = x*y;
  const sfloat_v yy = y*y;
  const sfloat_v y2 = y*c2;
  const sfloat_v x2 = x*c2;
  const sfloat_v x4 = x*c4;
  const sfloat_v xx31 = xx*c3+c1;
  const sfloat_v xx159 = xx*c15+c9;

  const sfloat_v Ay = -xx-c1;
  const sfloat_v Ayy = x*(xx*c3+c3);
  const sfloat_v Ayz = -c2*xy; 
  const sfloat_v Ayyy = -(c15*xx*xx+c18*xx+c3);

  const sfloat_v Ayy_fX = c3*xx31;
  const sfloat_v Ayyy_fX = -x4*xx159;

  const sfloat_v Bx = yy+c1; 
  const sfloat_v Byy = y*xx31; 
  const sfloat_v Byz = c2*xx+c1;
  const sfloat_v Byyy = -xy*xx159;

  const sfloat_v Byy_fX = c6*xy;
  const sfloat_v Byyy_fX = -y*(c45*xx+c9);
  const sfloat_v Byyy_fY = -x*xx159;

  // end of coefficients calculation

  const sfloat_v t2   = c1 + xx + yy;
  const sfloat_v t    = sqrt( t2 );
  const sfloat_v h    = qp0*c_light;
  const sfloat_v ht   = h*t;

  // get field integrals
  const sfloat_v ddz = fZ-F.z0;
  const sfloat_v Fx0 = F.cx0 + F.cx1*ddz + F.cx2*ddz*ddz;
  const sfloat_v Fx1 = (F.cx1 + c2*F.cx2*ddz)*dz;
  const sfloat_v Fx2 = F.cx2*dz2;
  const sfloat_v Fy0 = F.cy0 + F.cy1*ddz + F.cy2*ddz*ddz;
  const sfloat_v Fy1 = (F.cy1 + c2*F.cy2*ddz)*dz;
  const sfloat_v Fy2 = F.cy2*dz2;
  const sfloat_v Fz0 = F.cz0 + F.cz1*ddz + F.cz2*ddz*ddz;
  const sfloat_v Fz1 = (F.cz1 + c2*F.cz2*ddz)*dz;
  const sfloat_v Fz2 = F.cz2*dz2;

  // 

  const sfloat_v sx = ( Fx0 + Fx1*c2i + Fx2*c3i  );
  const sfloat_v sy = ( Fy0 + Fy1*c2i + Fy2*c3i );
  const sfloat_v sz = ( Fz0 + Fz1*c2i + Fz2*c3i );

  const sfloat_v Sx = ( Fx0*c2i + Fx1*c6i + Fx2*c12i );
  const sfloat_v Sy = ( Fy0*c2i + Fy1*c6i + Fy2*c12i );
  const sfloat_v Sz = ( Fz0*c2i + Fz1*c6i + Fz2*c12i );

  sfloat_v syz;
  { 
    const sfloat_v 
      d = 1./360., 
      c00 = 30.*6.*d, c01 = 30.*2.*d,   c02 = 30.*d,
      c10 = 3.*40.*d, c11 = 3.*15.*d,   c12 = 3.*8.*d, 
      c20 = 2.*45.*d, c21 = 2.*2.*9.*d, c22 = 2.*2.*5.*d;
    syz = Fy0*( c00*Fz0 + c01*Fz1 + c02*Fz2) 
      +   Fy1*( c10*Fz0 + c11*Fz1 + c12*Fz2) 
      +   Fy2*( c20*Fz0 + c21*Fz1 + c22*Fz2) ;
  }

  sfloat_v Syz;
  {
    const sfloat_v 
      d = 1./2520., 
      c00 = 21.*20.*d, c01 = 21.*5.*d, c02 = 21.*2.*d,
      c10 =  7.*30.*d, c11 =  7.*9.*d, c12 =  7.*4.*d, 
      c20 =  2.*63.*d, c21 = 2.*21.*d, c22 = 2.*10.*d;
    Syz = Fy0*( c00*Fz0 + c01*Fz1 + c02*Fz2 ) 
      +   Fy1*( c10*Fz0 + c11*Fz1 + c12*Fz2 ) 
      +   Fy2*( c20*Fz0 + c21*Fz1 + c22*Fz2 ) ;
  }

  const sfloat_v syy  = sy*sy*c2i;
  const sfloat_v syyy = syy*sy*c3i;

  sfloat_v Syy ;   
  {
    const sfloat_v  
    d= 1./2520., c00= 420.*d, c01= 21.*15.*d, c02= 21.*8.*d,
    c03= 63.*d, c04= 70.*d, c05= 20.*d;
    Syy =  Fy0*(c00*Fy0+c01*Fy1+c02*Fy2) + Fy1*(c03*Fy1+c04*Fy2) + c05*Fy2*Fy2 ;
  }
  
  sfloat_v Syyy;
  {
    const sfloat_v 
      d = 1./181440., 
      c000 =   7560*d, c001 = 9*1008*d, c002 = 5*1008*d, 
      c011 = 21*180*d, c012 = 24*180*d, c022 =  7*180*d, 
      c111 =    540*d, c112 =    945*d, c122 =    560*d, c222 = 112*d;
    const sfloat_v Fy22 = Fy2*Fy2;
    Syyy = Fy0*( Fy0*(c000*Fy0+c001*Fy1+c002*Fy2)+ Fy1*(c011*Fy1+c012*Fy2)+c022*Fy22 )
      +    Fy1*( Fy1*(c111*Fy1+c112*Fy2)+c122*Fy22) + c222*Fy22*Fy2                  ;
  }
  
  
  const sfloat_v sA1   = sx*xy   + sy*Ay   + sz*y ;
  const sfloat_v sA1_fX = sx*y - sy*x2 ;
  const sfloat_v sA1_fY = sx*x + sz ;

  const sfloat_v sB1   = sx*Bx   - sy*xy   - sz*x ;
  const sfloat_v sB1_fX = -sy*y - sz ;
  const sfloat_v sB1_fY = sx*y2 - sy*x ;

  const sfloat_v SA1   = Sx*xy   + Sy*Ay   + Sz*y ;
  const sfloat_v SA1_fX = Sx*y - Sy*x2 ;
  const sfloat_v SA1_fY = Sx*x + Sz;

  const sfloat_v SB1   = Sx*Bx   - Sy*xy   - Sz*x ;
  const sfloat_v SB1_fX = -Sy*y - Sz;
  const sfloat_v SB1_fY = Sx*y2 - Sy*x;


  const sfloat_v sA2   = syy*Ayy   + syz*Ayz ;
  const sfloat_v sA2_fX = syy*Ayy_fX - syz*y2 ;
  const sfloat_v sA2_fY = -syz*x2 ;
  const sfloat_v sB2   = syy*Byy   + syz*Byz  ;
  const sfloat_v sB2_fX = syy*Byy_fX + syz*x4 ;
  const sfloat_v sB2_fY = syy*xx31 ;
  
  const sfloat_v SA2   = Syy*Ayy   + Syz*Ayz ;
  const sfloat_v SA2_fX = Syy*Ayy_fX - Syz*y2 ;
  const sfloat_v SA2_fY = -Syz*x2 ;
  const sfloat_v SB2   = Syy*Byy   + Syz*Byz ;
  const sfloat_v SB2_fX = Syy*Byy_fX + Syz*x4 ;
  const sfloat_v SB2_fY = Syy*xx31 ;

  const sfloat_v sA3   = syyy*Ayyy  ;
  const sfloat_v sA3_fX = syyy*Ayyy_fX;
  const sfloat_v sB3   = syyy*Byyy  ;
  const sfloat_v sB3_fX = syyy*Byyy_fX;
  const sfloat_v sB3_fY = syyy*Byyy_fY;

 
  const sfloat_v SA3   = Syyy*Ayyy  ;
  const sfloat_v SA3_fX = Syyy*Ayyy_fX;
  const sfloat_v SB3   = Syyy*Byyy  ;
  const sfloat_v SB3_fX = Syyy*Byyy_fX;
  const sfloat_v SB3_fY = Syyy*Byyy_fY;

  const sfloat_v ht1 = ht*dz;
  const sfloat_v ht2 = ht*ht*dz2;
  const sfloat_v ht3 = ht*ht*ht*dz3;
  const sfloat_v ht1sA1 = ht1*sA1;
  const sfloat_v ht1sB1 = ht1*sB1;
  const sfloat_v ht1SA1 = ht1*SA1;
  const sfloat_v ht1SB1 = ht1*SB1;
  const sfloat_v ht2sA2 = ht2*sA2;
  const sfloat_v ht2SA2 = ht2*SA2;
  const sfloat_v ht2sB2 = ht2*sB2;
  const sfloat_v ht2SB2 = ht2*SB2;
  const sfloat_v ht3sA3 = ht3*sA3;
  const sfloat_v ht3sB3 = ht3*sB3; 
  const sfloat_v ht3SA3 = ht3*SA3;
  const sfloat_v ht3SB3 = ht3*SB3;

  fX (mask)  += ((x + ht1SA1 + ht2SA2 + ht3SA3)*dz);
  fY (mask)  += ((y + ht1SB1 + ht2SB2 + ht3SB3)*dz);
  fTx(mask)  += (ht1sA1 + ht2sA2 + ht3sA3);
  fTy(mask)  += (ht1sB1 + ht2sB2 + ht3sB3);
  fZ (mask)  += (dz);

  const sfloat_v ctdz  = c_light*t*dz;
  const sfloat_v ctdz2 = c_light*t*dz2;

  const sfloat_v dqp = fQP - qp0;
  const sfloat_v t2i = c1*rcp(t2);// /t2;
  const sfloat_v xt2i = x*t2i;
  const sfloat_v yt2i = y*t2i;
  const sfloat_v tmp0 = ht1SA1 + c2*ht2SA2 + c3*ht3SA3;
  const sfloat_v tmp1 = ht1SB1 + c2*ht2SB2 + c3*ht3SB3;
  const sfloat_v tmp2 = ht1sA1 + c2*ht2sA2 + c3*ht3sA3;
  const sfloat_v tmp3 = ht1sB1 + c2*ht2sB2 + c3*ht3sB3;

  const sfloat_v j02 = dz*(c1 + xt2i*tmp0 + ht1*SA1_fX + ht2*SA2_fX + ht3*SA3_fX);
  const sfloat_v j12 = dz*(     xt2i*tmp1 + ht1*SB1_fX + ht2*SB2_fX + ht3*SB3_fX);
  const sfloat_v j22 =     c1 + xt2i*tmp2 + ht1*sA1_fX + ht2*sA2_fX + ht3*sA3_fX ;
  const sfloat_v j32 =          xt2i*tmp3 + ht1*sB1_fX + ht2*sB2_fX + ht3*sB3_fX ;
    
  const sfloat_v j03 = dz*(     yt2i*tmp0 + ht1*SA1_fY + ht2*SA2_fY );
  const sfloat_v j13 = dz*(c1 + yt2i*tmp1 + ht1*SB1_fY + ht2*SB2_fY + ht3*SB3_fY );
  const sfloat_v j23 =          yt2i*tmp2 + ht1*sA1_fY + ht2*sA2_fY  ;
  const sfloat_v j33 =     c1 + yt2i*tmp3 + ht1*sB1_fY + ht2*sB2_fY + ht3*sB3_fY ;
    
  const sfloat_v j04 = ctdz2*( SA1 + c2*ht1*SA2 + c3*ht2*SA3 );
  const sfloat_v j14 = ctdz2*( SB1 + c2*ht1*SB2 + c3*ht2*SB3 );
  const sfloat_v j24 = ctdz *( sA1 + c2*ht1*sA2 + c3*ht2*sA3 );
  const sfloat_v j34 = ctdz *( sB1 + c2*ht1*sB2 + c3*ht2*sB3 );
  

  // extrapolate inverse momentum
  
  fX (mask)+=j04*dqp;
  fY (mask)+=j14*dqp;
  fTx(mask)+=j24*dqp;
  fTy(mask)+=j34*dqp;

 //          covariance matrix transport 
 
  const sfloat_v c42 = fC42, c43 = fC43;

  const sfloat_v cj00 = fC00 + fC20*j02 + fC30*j03 + fC40*j04;
//  const sfloat_v cj10 = fC10 + fC21*j02 + fC31*j03 + fC41*j04;
  const sfloat_v cj20 = fC20 + fC22*j02 + fC32*j03 + c42*j04;
  const sfloat_v cj30 = fC30 + fC32*j02 + fC33*j03 + c43*j04;
 
  const sfloat_v cj01 = fC10 + fC20*j12 + fC30*j13 + fC40*j14;
  const sfloat_v cj11 = fC11 + fC21*j12 + fC31*j13 + fC41*j14;
  const sfloat_v cj21 = fC21 + fC22*j12 + fC32*j13 + c42*j14;
  const sfloat_v cj31 = fC31 + fC32*j12 + fC33*j13 + c43*j14;

//  const sfloat_v cj02 = fC20*j22 + fC30*j23 + fC40*j24;
//  const sfloat_v cj12 = fC21*j22 + fC31*j23 + fC41*j24;
  const sfloat_v cj22 = fC22*j22 + fC32*j23 + c42*j24;
  const sfloat_v cj32 = fC32*j22 + fC33*j23 + c43*j24;

//  const sfloat_v cj03 = fC20*j32 + fC30*j33 + fC40*j34;
//  const sfloat_v cj13 = fC21*j32 + fC31*j33 + fC41*j34;
  const sfloat_v cj23 = fC22*j32 + fC32*j33 + c42*j34;
  const sfloat_v cj33 = fC32*j32 + fC33*j33 + c43*j34;

  fC40(mask)+= (c42*j02 + c43*j03 + fC44*j04); // cj40
  fC41(mask)+= (c42*j12 + c43*j13 + fC44*j14); // cj41
  fC42(mask) = (c42*j22 + c43*j23 + fC44*j24); // cj42
  fC43(mask) = (c42*j32 + c43*j33 + fC44*j34); // cj43

  fC00(mask) = (cj00 + j02*cj20 + j03*cj30 + j04*fC40);
  fC10(mask) = (cj01 + j02*cj21 + j03*cj31 + j04*fC41);
  fC11(mask) = (cj11 + j12*cj21 + j13*cj31 + j14*fC41);

  fC20(mask) = (j22*cj20 + j23*cj30 + j24*fC40);
  fC30(mask) = (j32*cj20 + j33*cj30 + j34*fC40);
  fC21(mask) = (j22*cj21 + j23*cj31 + j24*fC41);
  fC31(mask) = (j32*cj21 + j33*cj31 + j34*fC41);
  fC22(mask) = (j22*cj22 + j23*cj32 + j24*fC42);
  fC32(mask) = (j32*cj22 + j33*cj32 + j34*fC42);
  fC33(mask) = (j32*cj23 + j33*cj33 + j34*fC43);

  return mask;
}

inline sfloat_m AliHLTTPCCATrackParamVector::PassMaterial( const L1MaterialInfo &info, const sfloat_v &qp0, const sfloat_m &mask )
{
  const sfloat_v mass2 = 0.1395679f*0.1395679f;

  sfloat_v txtx = fTx*fTx;
  sfloat_v tyty = fTy*fTy;
  sfloat_v txtx1 = txtx + 1.f;
  sfloat_v h = txtx + tyty;
  sfloat_v t = sqrt(txtx1 + tyty);
  sfloat_v h2 = h*h;
  sfloat_v qp0t = qp0*t;
  
  const sfloat_v c1=0.0136f, c2=c1*0.038f, c3=c2*0.5f, c4=-c3/2.0f, c5=c3/3.0f, c6=-c3/4.0f;
    
  sfloat_v s0 = (c1+c2*info.logRadThick + c3*h + h2*(c4 + c5*h +c6*h2) )*qp0t;    
  sfloat_v a = ( (1.f +mass2*qp0*qp0t)*info.RadThick*s0*s0 );
// std::cout <<" a " << a << std::endl;
//  a=0.000005;
  fC22(mask) += txtx1*a;
  fC32(mask) += fTx*fTy*a; fC33(mask) += (1.f + tyty)*a;
  
  return mask;
}


inline sfloat_m AliHLTTPCCATrackParamVector::FilterVtx( const sfloat_v &xV, const sfloat_v& yV, const L1XYMeasurementInfo &info, sfloat_v& extrDx, sfloat_v& extrDy, sfloat_v J[], const sfloat_m &active )
{
  sfloat_v zeta0, zeta1, S00, S10, S11, si;
  sfloat_v F00, F10, F20, F30, F40,  F01, F11, F21, F31, F41 ;
  sfloat_v K00, K10, K20, K30, K40, K01, K11, K21, K31, K41;

  zeta0 = fX + extrDx - xV;
  zeta1 = fY + extrDy - yV;

    // H = 1 0 J[0] J[1] J[2]
    //     0 1 J[3] J[4] J[5]
  
  // F = CH'
  F00 = fC00;       F01 = fC10;    
  F10 = fC10;       F11 = fC11;
  F20 = J[0]*fC22;  F21 = J[3]*fC22;
  F30 = J[1]*fC33;  F31 = J[4]*fC33;
  F40 = J[2]*fC44;  F41 = J[5]*fC44;

  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);
  sfloat_v S00tmp = S00;
  S00 =  si*S11;
  S10 = -si*S10;
  S11 =  si*S00tmp;

  fChi2(active) +=  zeta0*zeta0*S00 + 2.*zeta0*zeta1*S10 + zeta1*zeta1*S11;

  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;

  fX (active)  -= K00*zeta0 + K01*zeta1;
  fY (active)  -= K10*zeta0 + K11*zeta1;
  fTx(active)  -= K20*zeta0 + K21*zeta1;
  fTy(active)  -= K30*zeta0 + K31*zeta1;
  fQP(active)  -= K40*zeta0 + K41*zeta1;

  fC00(active) -=  ( K00*F00 + K01*F01 );
  fC10(active) -=  ( K10*F00 + K11*F01 );
  fC11(active) -=  ( K10*F10 + K11*F11 );
  fC20(active)  = -( K20*F00 + K21*F01 );
  fC21(active)  = -( K20*F10 + K21*F11 );
  fC22(active) -=  ( K20*F20 + K21*F21 );
  fC30(active)  = -( K30*F00 + K31*F01 );
  fC31(active)  = -( K30*F10 + K31*F11 );
  fC32(active)  = -( K30*F20 + K31*F21 );
  fC33(active) -=  ( K30*F30 + K31*F31 );
  fC40(active)  = -( K40*F00 + K41*F01 );
  fC41(active)  = -( K40*F10 + K41*F11 );
  fC42(active)  = -( K40*F20 + K41*F21 );
  fC43(active)  = -( K40*F30 + K41*F31 );
  fC44(active) -=  ( K40*F40 + K41*F41 );

  return active;
}

inline sfloat_m AliHLTTPCCATrackParamVector::TransportJXY0ToX0( const sfloat_v &x0, const L1FieldRegion &F, sfloat_v& extrDx, sfloat_v& extrDy, sfloat_v &J04, sfloat_v &J14, const sfloat_m &active  )
{
  const sfloat_v c_light = 0.000299792458, c1 = 1., c2i = 0.5, c6i = 1./6., c12i = 1./12.;

  const sfloat_v dz = x0 - fZ;
  sfloat_v dz2 = dz*dz;
  sfloat_v dzc6i = dz*c6i;
  sfloat_v dz2c12i = dz2*c12i;

  sfloat_v xx  = fTx*fTx;
  sfloat_v yy = fTy*fTy;
  sfloat_v xy  = fTx*fTy;

  sfloat_v Ay = -xx-c1;
  sfloat_v Bx = yy+c1; 

  sfloat_v ctdz2 = c_light*sqrt( c1 + xx + yy )*dz2;

  sfloat_v Sx = F.cx0*c2i + F.cx1*dzc6i + F.cx2*dz2c12i ;
  sfloat_v Sy = F.cy0*c2i + F.cy1*dzc6i + F.cy2*dz2c12i ;
  sfloat_v Sz = F.cz0*c2i + F.cz1*dzc6i + F.cz2*dz2c12i ;
    
  extrDx(active) = ( fTx )*dz ;
  extrDy(active) = ( fTy )*dz ;
  J04(active) = ctdz2 * (Sx*xy   + Sy*Ay   + Sz*fTy);
  J14(active) = ctdz2 * (Sx*Bx   - Sy*xy   - Sz*fTx);

  return active;
}

typedef AliHLTTPCCATrackParamVector TrackParamVector;

// std::istream &operator>>( std::istream &in, AliHLTTPCCATrackParamVector &ot );
// std::ostream &operator<<( std::ostream &out, const AliHLTTPCCATrackParamVector &ot );

#endif