#ifndef L1Extrapolation_h
#define L1Extrapolation_h

#include "CbmL1Def.h"
#include "L1Field.h"
#include "L1TrackPar.h"


//#define cnst static const fvec
#define cnst const fvec  

// #define ANALYTICEXTRAPOLATION
#ifdef ANALYTICEXTRAPOLATION
inline void L1Extrapolate
( 
 L1TrackPar &T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
 fvec        z_out  , // extrapolate to this z position
 fvec       qp0    , // use Q/p linearisation at this value
 L1FieldRegion &F,
 fvec *w = 0
 )
{
  //cout<<"Extrapolation..."<<endl;
  //
  //  Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
  //  
  
  cnst ZERO = 0.0, ONE = 1.;
  cnst c_light = 0.000299792458, c_light_i = 1./c_light;

  cnst  
    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 fvec qp = T.qp;
  const fvec dz = (z_out - T.z);
  const fvec dz2 = dz*dz;
  const fvec dz3 = dz2*dz;

  // construct coefficients 

  const fvec x   = T.tx;
  const fvec y   = T.ty;
  const fvec xx  = x*x;
  const fvec xy = x*y;
  const fvec yy = y*y;
  const fvec y2 = y*c2;
  const fvec x2 = x*c2;
  const fvec x4 = x*c4;
  const fvec xx31 = xx*c3+c1;
  const fvec xx159 = xx*c15+c9;

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

  const fvec Ayy_x = c3*xx31;
  const fvec Ayyy_x = -x4*xx159;

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

  const fvec Byy_x = c6*xy;
  const fvec Byyy_x = -y*(c45*xx+c9);
  const fvec Byyy_y = -x*xx159;

  // end of coefficients calculation

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

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

  // 

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

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

  fvec syz;
  { 
    cnst 
      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) ;
  }

  fvec Syz;
  {
    cnst 
      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 fvec syy  = sy*sy*c2i;
  const fvec syyy = syy*sy*c3i;

  fvec Syy ;   
  {
    cnst  
    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 ;
  }
  
  fvec Syyy;
  {
    cnst 
      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 fvec 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 fvec sA1   = sx*xy   + sy*Ay   + sz*y ;
  const fvec sA1_x = sx*y - sy*x2 ;
  const fvec sA1_y = sx*x + sz ;

  const fvec sB1   = sx*Bx   - sy*xy   - sz*x ;
  const fvec sB1_x = -sy*y - sz ;
  const fvec sB1_y = sx*y2 - sy*x ;

  const fvec SA1   = Sx*xy   + Sy*Ay   + Sz*y ;
  const fvec SA1_x = Sx*y - Sy*x2 ;
  const fvec SA1_y = Sx*x + Sz;

  const fvec SB1   = Sx*Bx   - Sy*xy   - Sz*x ;
  const fvec SB1_x = -Sy*y - Sz;
  const fvec SB1_y = Sx*y2 - Sy*x;


  const fvec sA2   = syy*Ayy   + syz*Ayz ;
  const fvec sA2_x = syy*Ayy_x - syz*y2 ;
  const fvec sA2_y = -syz*x2 ;
  const fvec sB2   = syy*Byy   + syz*Byz  ;
  const fvec sB2_x = syy*Byy_x + syz*x4 ;
  const fvec sB2_y = syy*xx31 ;
  
  const fvec SA2   = Syy*Ayy   + Syz*Ayz ;
  const fvec SA2_x = Syy*Ayy_x - Syz*y2 ;
  const fvec SA2_y = -Syz*x2 ;
  const fvec SB2   = Syy*Byy   + Syz*Byz ;
  const fvec SB2_x = Syy*Byy_x + Syz*x4 ;
  const fvec SB2_y = Syy*xx31 ;

  const fvec sA3   = syyy*Ayyy  ;
  const fvec sA3_x = syyy*Ayyy_x;
  const fvec sB3   = syyy*Byyy  ;
  const fvec sB3_x = syyy*Byyy_x;
  const fvec sB3_y = syyy*Byyy_y;

 
  const fvec SA3   = Syyy*Ayyy  ;
  const fvec SA3_x = Syyy*Ayyy_x;
  const fvec SB3   = Syyy*Byyy  ;
  const fvec SB3_x = Syyy*Byyy_x;
  const fvec SB3_y = Syyy*Byyy_y;

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

  fvec initialised = ZERO;
  if(w) //TODO use operator {?:}
  {
    const fvec zero = ZERO;
    initialised = fvec( zero < *w );
  }
  else
  {
    const fvec one = ONE;
    const fvec zero = ZERO;
    initialised = fvec( zero < one );
  }

  T.x  += (((x + ht1SA1 + ht2SA2 + ht3SA3)*dz)&initialised) ;
  T.y  += (((y + ht1SB1 + ht2SB2 + ht3SB3)*dz)&initialised) ;
  T.tx += ((ht1sA1 + ht2sA2 + ht3sA3)&initialised);
  T.ty += ((ht1sB1 + ht2sB2 + ht3sB3)&initialised);
  T.z  += ((dz)&initialised);

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

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

  const fvec j02 = dz*(c1 + xt2i*tmp0 + ht1*SA1_x + ht2*SA2_x + ht3*SA3_x);
  const fvec j12 = dz*(     xt2i*tmp1 + ht1*SB1_x + ht2*SB2_x + ht3*SB3_x);
  const fvec j22 =     c1 + xt2i*tmp2 + ht1*sA1_x + ht2*sA2_x + ht3*sA3_x ;
  const fvec j32 =          xt2i*tmp3 + ht1*sB1_x + ht2*sB2_x + ht3*sB3_x ;
    
  const fvec j03 = dz*(     yt2i*tmp0 + ht1*SA1_y + ht2*SA2_y );
  const fvec j13 = dz*(c1 + yt2i*tmp1 + ht1*SB1_y + ht2*SB2_y + ht3*SB3_y );
  const fvec j23 =          yt2i*tmp2 + ht1*sA1_y + ht2*sA2_y  ;
  const fvec j33 =     c1 + yt2i*tmp3 + ht1*sB1_y + ht2*sB2_y + ht3*sB3_y ;
    
  const fvec j04 = ctdz2*( SA1 + c2*ht1*SA2 + c3*ht2*SA3 );
  const fvec j14 = ctdz2*( SB1 + c2*ht1*SB2 + c3*ht2*SB3 );
  const fvec j24 = ctdz *( sA1 + c2*ht1*sA2 + c3*ht2*sA3 );
  const fvec j34 = ctdz *( sB1 + c2*ht1*sB2 + c3*ht2*sB3 );
  

  // extrapolate inverse momentum
  
  T.x +=((j04*dqp)&initialised);
  T.y +=((j14*dqp)&initialised);
  T.tx+=((j24*dqp)&initialised);
  T.ty+=((j34*dqp)&initialised);

 //          covariance matrix transport 
 
  const fvec c42 = T.C42, c43 = T.C43;

  const fvec cj00 = T.C00 + T.C20*j02 + T.C30*j03 + T.C40*j04;
//  const fvec cj10 = T.C10 + T.C21*j02 + T.C31*j03 + T.C41*j04;
  const fvec cj20 = T.C20 + T.C22*j02 + T.C32*j03 + c42*j04;
  const fvec cj30 = T.C30 + T.C32*j02 + T.C33*j03 + c43*j04;
 
  const fvec cj01 = T.C10 + T.C20*j12 + T.C30*j13 + T.C40*j14;
  const fvec cj11 = T.C11 + T.C21*j12 + T.C31*j13 + T.C41*j14;
  const fvec cj21 = T.C21 + T.C22*j12 + T.C32*j13 + c42*j14;
  const fvec cj31 = T.C31 + T.C32*j12 + T.C33*j13 + c43*j14;

//  const fvec cj02 = T.C20*j22 + T.C30*j23 + T.C40*j24;
//  const fvec cj12 = T.C21*j22 + T.C31*j23 + T.C41*j24;
  const fvec cj22 = T.C22*j22 + T.C32*j23 + c42*j24;
  const fvec cj32 = T.C32*j22 + T.C33*j23 + c43*j24;

//  const fvec cj03 = T.C20*j32 + T.C30*j33 + T.C40*j34;
//  const fvec cj13 = T.C21*j32 + T.C31*j33 + T.C41*j34;
  const fvec cj23 = T.C22*j32 + T.C32*j33 + c42*j34;
  const fvec cj33 = T.C32*j32 + T.C33*j33 + c43*j34;

  T.C40+= ((c42*j02 + c43*j03 + T.C44*j04) & initialised); // cj40
  T.C41+= ((c42*j12 + c43*j13 + T.C44*j14) & initialised); // cj41
  T.C42 = ((c42*j22 + c43*j23 + T.C44*j24) & initialised) + (T.C42&(!initialised)); // cj42
  T.C43 = ((c42*j32 + c43*j33 + T.C44*j34) & initialised) + (T.C43&(!initialised)); // cj43

  T.C00 = ((cj00 + j02*cj20 + j03*cj30 + j04*T.C40) & initialised) + (T.C00&(!initialised));
  T.C10 = ((cj01 + j02*cj21 + j03*cj31 + j04*T.C41) & initialised) + (T.C10&(!initialised));
  T.C11 = ((cj11 + j12*cj21 + j13*cj31 + j14*T.C41) & initialised) + (T.C11&(!initialised));

  T.C20 = ((j22*cj20 + j23*cj30 + j24*T.C40) & initialised) + (T.C20&(!initialised)) ;
  T.C30 = ((j32*cj20 + j33*cj30 + j34*T.C40) & initialised) + (T.C30&(!initialised)) ;
  T.C21 = ((j22*cj21 + j23*cj31 + j24*T.C41) & initialised) + (T.C21&(!initialised)) ;
  T.C31 = ((j32*cj21 + j33*cj31 + j34*T.C41) & initialised) + (T.C31&(!initialised)) ;
  T.C22 = ((j22*cj22 + j23*cj32 + j24*T.C42) & initialised) + (T.C22&(!initialised)) ;
  T.C32 = ((j32*cj22 + j33*cj32 + j34*T.C42) & initialised) + (T.C32&(!initialised)) ;
  T.C33 = ((j32*cj23 + j33*cj33 + j34*T.C43) & initialised) + (T.C33&(!initialised)) ;
  //cout<<"Extrapolation ok"<<endl;

}
#else
inline void L1Extrapolate // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
(
 L1TrackPar &T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
 fvec        z_out  , // extrapolate to this z position
 fvec       qp0    , // use Q/p linearisation at this value
 const L1FieldRegion &F,
 fvec *w = 0
 )
{
    //
  // Forth-order Runge-Kutta method for solution of the equation
  // of motion of a particle with parameter qp = Q /P
  //              in the magnetic field B()
  //
  //        | x |          tx
  //        | y |          ty
  // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) )
  //        | ty|   ft * (-tx * ( B(3)+ty+b(2)   - (1+ty**2)*B(1) )  ,
  //
  //   where  ft = c_light*qp*sqrt ( 1 + tx**2 + ty**2 ) .
  //
  //  In the following for RK step  :
  //
  //     x=x[0], y=x[1], tx=x[3], ty=x[4].
  //
  //========================================================================
  //
  //  NIM A395 (1997) 169-184; NIM A426 (1999) 268-282.
  //
  //  the routine is based on LHC(b) utility code
  //
  //========================================================================


  cnst ZERO = 0.0, ONE = 1.;
  cnst c_light = 0.000299792458;
  
  const fvec a[4] = {0.0f, 0.5f, 0.5f, 1.0f};
  const fvec c[4] = {1.0f/6.0f, 1.0f/3.0f, 1.0f/3.0f, 1.0f/6.0f};
  const fvec b[4] = {0.0f, 0.5f, 0.5f, 1.0f};
  
  int step4;
  fvec k[16],x0[4],x[4],k1[16];
  fvec Ax[4],Ay[4],Ax_tx[4],Ay_tx[4],Ax_ty[4],Ay_ty[4];

  //----------------------------------------------------------------

  fvec qp_in = T.qp;
  const fvec z_in  = T.z;
  const fvec h     = (z_out - T.z);
  fvec hC    = h * c_light;
  x0[0] = T.x; x0[1] = T.y;
  x0[2] = T.tx; x0[3] = T.ty;
  //
  //   Runge-Kutta step
  //

  int step;
  int i;

  fvec B[4][3];
  for (step = 0; step < 4; ++step) {
    F.Get( z_in  + a[step] * h, B[step] );
  }
  
  for (step = 0; step < 4; ++step) {
    for(i=0; i < 4; ++i) {
      if(step == 0) {
        x[i] = x0[i];
      }
      else
      {
        x[i] = x0[i] + b[step] * k[step*4-4+i];
      }
    }

    fvec tx = x[2]; 
    fvec ty = x[3]; 
    fvec tx2 = tx * tx; 
    fvec ty2 = ty * ty; 
    fvec txty = tx * ty;
    fvec tx2ty21= 1.f + tx2 + ty2; 
    // if( tx2ty21 > 1.e4 ) return 1;
    fvec I_tx2ty21 = 1.f / tx2ty21 * qp0;
    fvec tx2ty2 = sqrt(tx2ty21 ) ; 
    //   fvec I_tx2ty2 = qp0 * hC / tx2ty2 ; unsused ???
    tx2ty2 *= hC; 
    fvec tx2ty2qp = tx2ty2 * qp0;
    Ax[step] = ( txty*B[step][0] + ty*B[step][2] - ( 1.f + tx2 )*B[step][1] ) * tx2ty2;
    Ay[step] = (-txty*B[step][1] - tx*B[step][2] + ( 1.f + ty2 )*B[step][0] ) * tx2ty2;

    Ax_tx[step] = Ax[step]*tx*I_tx2ty21 + ( ty*B[step][0]-2.f*tx*B[step][1])*tx2ty2qp;
    Ax_ty[step] = Ax[step]*ty*I_tx2ty21 + ( tx*B[step][0]+       B[step][2])*tx2ty2qp;
    Ay_tx[step] = Ay[step]*tx*I_tx2ty21 + (-ty*B[step][1]-       B[step][2])*tx2ty2qp;
    Ay_ty[step] = Ay[step]*ty*I_tx2ty21 + (-tx*B[step][1]+2.f*ty*B[step][0])*tx2ty2qp;

    step4 = step * 4;
    k[step4  ] = tx * h;
    k[step4+1] = ty * h;
    k[step4+2] = Ax[step] * qp0;
    k[step4+3] = Ay[step] * qp0;

  }  // end of Runge-Kutta steps

  fvec initialised = ZERO;
  if(w) //TODO use operator {?:}
  {
    const fvec zero = ZERO;
    initialised = fvec( zero < *w );
  }
  else
  {
    const fvec one = ONE;
    const fvec zero = ZERO;
    initialised = fvec( zero < one );
  }
  
  {
    T.x =   ( (x0[0]+c[0]*k[0]+c[1]*k[4+0]+c[2]*k[8+0]+c[3]*k[12+0]) & initialised) + ( (!initialised) & T.x);
    T.y =   ( (x0[1]+c[0]*k[1]+c[1]*k[4+1]+c[2]*k[8+1]+c[3]*k[12+1]) & initialised) + ( (!initialised) & T.y);
    T.tx =  ( (x0[2]+c[0]*k[2]+c[1]*k[4+2]+c[2]*k[8+2]+c[3]*k[12+2]) & initialised) + ( (!initialised) & T.tx);
    T.ty =  ( (x0[3]+c[0]*k[3]+c[1]*k[4+3]+c[2]*k[8+3]+c[3]*k[12+3]) & initialised) + ( (!initialised) & T.ty);
    T.z =   ( z_out & initialised)  + ( (!initialised) & T.z);
  }

  //
  //     Derivatives    dx/dqp
  //

  x0[0] = 0.f; x0[1] = 0.f; x0[2] = 0.f; x0[3] = 0.f;

  //
  //   Runge-Kutta step for derivatives dx/dqp

  for (step = 0; step < 4; ++step) {
    for(i=0; i < 4; ++i) {
      if(step == 0) {
        x[i] = x0[i];
      } else
      {
        x[i] = x0[i] + b[step] * k1[step*4-4+i];
      }
    }
    step4 = step * 4;
    k1[step4  ] = x[2] * h;
    k1[step4+1] = x[3] * h;
    k1[step4+2] = Ax[step] + Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
    k1[step4+3] = Ay[step] + Ay_tx[step] * x[2] + Ay_ty[step] * x[3];

  }  // end of Runge-Kutta steps for derivatives dx/dqp

  fvec J[25];

  for (i = 0; i < 4; ++i ) {
    J[20+i]=x0[i]+c[0]*k1[i]+c[1]*k1[4+i]+c[2]*k1[8+i]+c[3]*k1[12+i];
  }
  J[24] = 1.;
  //
  //      end of derivatives dx/dqp
  //

  //     Derivatives    dx/tx
  //

  x0[0] = 0.f; x0[1] = 0.f; x0[2] = 1.f; x0[3] = 0.f;

  //
  //   Runge-Kutta step for derivatives dx/dtx
  //

  for (step = 0; step < 4; ++step) {
    for(i=0; i < 4; ++i) {
      if(step == 0) {
        x[i] = x0[i];
      }
      else
        if ( i!=2 )
      {
        x[i] = x0[i] + b[step] * k1[step*4-4+i];
      }
    }
    step4 = step * 4;
    k1[step4  ] = x[2] * h;
    k1[step4+1] = x[3] * h;
        // k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
    k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3];

  }  // end of Runge-Kutta steps for derivatives dx/dtx

  for (i = 0; i < 4; ++i ) {
    if(i != 2)
    {
      J[10+i]=x0[i]+c[0]*k1[i]+c[1]*k1[4+i]+c[2]*k1[8+i]+c[3]*k1[12+i];
    }
  }
  //      end of derivatives dx/dtx
  J[12] = 1.f;
  J[14] = 0.f;

  //     Derivatives    dx/ty
  //

  x0[0] = 0.f; x0[1] = 0.f; x0[2] = 0.f; x0[3] = 1.f;

  //
  //   Runge-Kutta step for derivatives dx/dty
  //

  for (step = 0; step < 4; ++step) {
    for(i=0; i < 4; ++i) {
      if(step == 0) {
        x[i] = x0[i];           // ty fixed
      } else 
        if(i!=3) {
        x[i] = x0[i] + b[step] * k1[step*4-4+i];
      }

    }
    step4 = step * 4;
    k1[step4  ] = x[2] * h;
    k1[step4+1] = x[3] * h;
    k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
    //    k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3];

  }  // end of Runge-Kutta steps for derivatives dx/dty

  for (i = 0; i < 3; ++i ) {
    J[15+i]=x0[i]+c[0]*k1[i]+c[1]*k1[4+i]+c[2]*k1[8+i]+c[3]*k1[12+i];
  }
  //      end of derivatives dx/dty
  J[18] = 1.;
  J[19] = 0.;

  //
  //    derivatives dx/dx and dx/dy

  for(i = 0; i < 10; ++i){ J[i] = 0.;}
  J[0] = 1.; J[6] = 1.;

  fvec dqp = qp_in - qp0;
  
  { // update parameters
    T.x += ((J[5*4+0]*dqp) & initialised);
    T.y += ((J[5*4+1]*dqp) & initialised);
    T.tx +=((J[5*4+2]*dqp) & initialised);
    T.ty +=((J[5*4+3]*dqp) & initialised);
  }
   
  //          covariance matrix transport 
  
//   // if ( C_in&&C_out ) CbmKFMath::multQtSQ( 5, J, C_in, C_out); // TODO
//   j(0,2) = J[5*2 + 0];
//   j(1,2) = J[5*2 + 1];
//   j(2,2) = J[5*2 + 2];
//   j(3,2) = J[5*2 + 3];
//     
//   j(0,3) = J[5*3 + 0];
//   j(1,3) = J[5*3 + 1];
//   j(2,3) = J[5*3 + 2];
//   j(3,3) = J[5*3 + 3];
//     
//   j(0,4) = J[5*4 + 0];
//   j(1,4) = J[5*4 + 1];
//   j(2,4) = J[5*4 + 2];
//   j(3,4) = J[5*4 + 3];
  
  const fvec c42 = T.C42, c43 = T.C43;

  const fvec cj00 = T.C00 + T.C20*J[5*2 + 0] + T.C30*J[5*3 + 0] + T.C40*J[5*4 + 0];
  // const fvec cj10 = T.C10 + T.C21*J[5*2 + 0] + T.C31*J[5*3 + 0] + T.C41*J[5*4 + 0];
  const fvec cj20 = T.C20 + T.C22*J[5*2 + 0] + T.C32*J[5*3 + 0] + c42*J[5*4 + 0];
  const fvec cj30 = T.C30 + T.C32*J[5*2 + 0] + T.C33*J[5*3 + 0] + c43*J[5*4 + 0];
 
  const fvec cj01 = T.C10 + T.C20*J[5*2 + 1] + T.C30*J[5*3 + 1] + T.C40*J[5*4 + 1];
  const fvec cj11 = T.C11 + T.C21*J[5*2 + 1] + T.C31*J[5*3 + 1] + T.C41*J[5*4 + 1];
  const fvec cj21 = T.C21 + T.C22*J[5*2 + 1] + T.C32*J[5*3 + 1] + c42*J[5*4 + 1];
  const fvec cj31 = T.C31 + T.C32*J[5*2 + 1] + T.C33*J[5*3 + 1] + c43*J[5*4 + 1];

  // const fvec cj02 = T.C20*J[5*2 + 2] + T.C30*J[5*3 + 2] + T.C40*J[5*4 + 2];
  // const fvec cj12 = T.C21*J[5*2 + 2] + T.C31*J[5*3 + 2] + T.C41*J[5*4 + 2];
  const fvec cj22 = T.C22*J[5*2 + 2] + T.C32*J[5*3 + 2] + c42*J[5*4 + 2];
  const fvec cj32 = T.C32*J[5*2 + 2] + T.C33*J[5*3 + 2] + c43*J[5*4 + 2];

  // const fvec cj03 = T.C20*J[5*2 + 3] + T.C30*J[5*3 + 3] + T.C40*J[5*4 + 3];
  // const fvec cj13 = T.C21*J[5*2 + 3] + T.C31*J[5*3 + 3] + T.C41*J[5*4 + 3];
  const fvec cj23 = T.C22*J[5*2 + 3] + T.C32*J[5*3 + 3] + c42*J[5*4 + 3];
  const fvec cj33 = T.C32*J[5*2 + 3] + T.C33*J[5*3 + 3] + c43*J[5*4 + 3];

  T.C40 += ((c42*J[5*2 + 0] + c43*J[5*3 + 0] + T.C44*J[5*4 + 0]) & initialised); // cj40
  T.C41 += ((c42*J[5*2 + 1] + c43*J[5*3 + 1] + T.C44*J[5*4 + 1]) & initialised); // cj41
  T.C42 = ((c42*J[5*2 + 2] + c43*J[5*3 + 2] + T.C44*J[5*4 + 2]) & initialised) + ((!initialised) & T.C42);
  T.C43 = ((c42*J[5*2 + 3] + c43*J[5*3 + 3] + T.C44*J[5*4 + 3]) & initialised) + ((!initialised) & T.C43);

  T.C00 = ((cj00 + J[5*2 + 0]*cj20 + J[5*3 + 0]*cj30 + J[5*4 + 0]*T.C40)& initialised) + ((!initialised) & T.C00);
  T.C10 = ((cj01 + J[5*2 + 0]*cj21 + J[5*3 + 0]*cj31 + J[5*4 + 0]*T.C41) & initialised) + ((!initialised) & T.C10);
  T.C11 = ((cj11 + J[5*2 + 1]*cj21 + J[5*3 + 1]*cj31 + J[5*4 + 1]*T.C41) & initialised) + ((!initialised) & T.C11);

  T.C20 = ((J[5*2 + 2]*cj20 + J[5*3 + 2]*cj30 + J[5*4 + 2]*T.C40) & initialised) + ((!initialised) & T.C20);
  T.C30 = ((J[5*2 + 3]*cj20 + J[5*3 + 3]*cj30 + J[5*4 + 3]*T.C40) & initialised) + ((!initialised) & T.C30);
  T.C21 = ((J[5*2 + 2]*cj21 + J[5*3 + 2]*cj31 + J[5*4 + 2]*T.C41) & initialised) + ((!initialised) & T.C21);
  T.C31 = ((J[5*2 + 3]*cj21 + J[5*3 + 3]*cj31 + J[5*4 + 3]*T.C41) & initialised) + ((!initialised) & T.C31);
  T.C22 = ((J[5*2 + 2]*cj22 + J[5*3 + 2]*cj32 + J[5*4 + 2]*T.C42) & initialised) + ((!initialised) & T.C22);
  T.C32 = ((J[5*2 + 3]*cj22 + J[5*3 + 3]*cj32 + J[5*4 + 3]*T.C42) & initialised) + ((!initialised) & T.C32);
  T.C33 = ((J[5*2 + 3]*cj23 + J[5*3 + 3]*cj33 + J[5*4 + 3]*T.C43) & initialised) + ((!initialised) & T.C33);
}
#endif //ANALYTICEXTRAPOLATION

inline void L1Extrapolate0
( 
 L1TrackPar &T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
 fvec       z_out  , // extrapolate to this z position
 L1FieldRegion &F
 )
{
  //
  //  Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
  //  
  
  cnst c_light = 0.000299792458;

  cnst  
    c1 = 1.,
    c2i = 1./2., c3i = 1./3., c6i = 1./6., c12i = 1./12.;

  const fvec qp = T.qp;

  
  const fvec dz = (z_out - T.z);
  const fvec dz2 = dz*dz;

  // construct coefficients 

  const fvec x   = T.tx;
  const fvec y   = T.ty;
  //  const fvec z   = T.z;
  const fvec xx  = x*x;
  const fvec xy = x*y;
  const fvec yy = y*y;
  const fvec Ay = -xx-c1;
  const fvec Bx = yy+c1; 
  const fvec ct    = c_light*sqrt( c1 + xx + yy );

  const fvec dzc2i  = dz*c2i;
  const fvec dz2c3i = dz2*c3i;
  const fvec dzc6i  = dz*c6i;
  const fvec dz2c12i= dz2*c12i;

  const fvec sx = ( F.cx0 + F.cx1*dzc2i + F.cx2*dz2c3i  );
  const fvec sy = ( F.cy0 + F.cy1*dzc2i + F.cy2*dz2c3i );
  const fvec sz = ( F.cz0 + F.cz1*dzc2i + F.cz2*dz2c3i );

  const fvec Sx = ( F.cx0*c2i + F.cx1*dzc6i + F.cx2*dz2c12i );
  const fvec Sy = ( F.cy0*c2i + F.cy1*dzc6i + F.cy2*dz2c12i );
  const fvec Sz = ( F.cz0*c2i + F.cz1*dzc6i + F.cz2*dz2c12i );
  
  
  const fvec ctdz  = ct*dz;
  const fvec ctdz2 = ct*dz2;
    
  const fvec j04 = ctdz2* ( Sx*xy   + Sy*Ay   + Sz*y );
  const fvec j14 = ctdz2* ( Sx*Bx   - Sy*xy   - Sz*x );
  const fvec j24 = ctdz * ( sx*xy   + sy*Ay   + sz*y );
  const fvec j34 = ctdz * ( sx*Bx   - sy*xy   - sz*x );
    
  T.x += x*dz + j04*qp;
  T.y += y*dz + j14*qp;
  T.tx+=j24*qp;
  T.ty+=j34*qp;
  T.z  += dz;
  //T.t += sqrt((x-T.x)*(x-T.x)+(y-T.y)*(y-T.y)+dz*dz)/29.9792458f;

  //          covariance matrix transport 

  const fvec cj00 = T.C00 + T.C20*dz + T.C40*j04;
//  const fvec cj10 = T.C10 + T.C21*dz + T.C41*j04;
  const fvec cj20 = T.C20 + T.C22*dz + T.C42*j04;
  const fvec cj30 = T.C30 + T.C32*dz + T.C43*j04;
 
  const fvec cj01 = T.C10 + T.C30*dz + T.C40*j14;
  const fvec cj11 = T.C11 + T.C31*dz + T.C41*j14;
  const fvec cj21 = T.C21 + T.C32*dz + T.C42*j14;
  const fvec cj31 = T.C31 + T.C33*dz + T.C43*j14;
 
//  const fvec cj02 = T.C20 +  T.C40*j24;
//  const fvec cj12 = T.C21 +  T.C41*j24;
  const fvec cj22 = T.C22 +  T.C42*j24;
  const fvec cj32 = T.C32 +  T.C43*j24;
 
//  const fvec cj03 = T.C30 + T.C40*j34;
//  const fvec cj13 = T.C31 + T.C41*j34;
//  const fvec cj23 = T.C32 + T.C42*j34;
  const fvec cj33 = T.C33 + T.C43*j34;

  T.C40+= T.C42*dz + T.C44*j04; // cj40
  T.C41+= T.C43*dz + T.C44*j14; // cj41
  T.C42+=            T.C44*j24; // cj42
  T.C43+=            T.C44*j34; // cj43

  T.C00 = cj00 + dz*cj20 + j04*T.C40;
  T.C10 = cj01 + dz*cj21 + j04*T.C41;
  T.C11 = cj11 + dz*cj31 + j14*T.C41;

  T.C20 = cj20  + j24*T.C40 ;
  T.C30 = cj30  + j34*T.C40 ;
  T.C21 = cj21  + j24*T.C41 ;
  T.C31 = cj31  + j34*T.C41 ;
  T.C22 = cj22  + j24*T.C42 ;
  T.C32 = cj32  + j34*T.C42 ;
  T.C33 = cj33  + j34*T.C43 ;

}


inline void L1ExtrapolateTime
( 
 L1TrackPar &T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
 fvec       dz   // extrapolate to this z position
 )
{

  cnst c_light = 29.9792458;

  T.t += sqrt((T.tx*T.tx)+(T.ty*T.ty)+1)*dz/c_light;
  
  const fvec k1 = T.tx*dz/(c_light*sqrt((T.tx*T.tx)+(T.ty*T.ty)+1));
  const fvec k2 = T.ty*dz/(c_light*sqrt((T.tx*T.tx)+(T.ty*T.ty)+1));

  fvec c52 = T.C52;
  fvec c53 = T.C53; 
  
//   cout<<k1<<" k1 "<<k2<<" k2 "<<endl;
  
//  fvec ha = T.C53;
//  fvec ha2 = T.C54;
  
  T.C50 += k1*T.C20 + k2*T.C30 ;
  T.C51 += k1*T.C21 + k2*T.C31 ;
  T.C52 += k1*T.C22 + k2*T.C32 ;
  T.C53 += k1*T.C32 + k2*T.C33 ;
  T.C54 += k1*T.C42 + k2*T.C43 ;
  T.C55 += k1*(T.C52 + c52) + k2*(T.C53 + c53);
  
//   cout<<ha<<" c53 "<<T.C53<<endl;
//     cout<<ha2<<" c54 "<<T.C54<<endl;
}

inline void L1ExtrapolateLine( L1TrackPar &T, fvec z_out)
{
  fvec dz = (z_out - T.z);

  T.x += T.tx*dz;
  T.y += T.ty*dz;
  T.z +=      dz;

  const fvec dzC32_in = dz * T.C32;  

  T.C21 += dzC32_in;
  T.C10 += dz * (  T.C21 + T.C30 );

  const fvec C20_in = T.C20;

  T.C20 += dz * T.C22;
  T.C00 += dz * ( T.C20 + C20_in );

  const fvec C31_in = T.C31;

  T.C31 += dz * T.C33;
  T.C11 += dz * ( T.C31 + C31_in );
  T.C30 += dzC32_in;

  T.C40 += dz * T.C42;
  T.C41 += dz * T.C43;
}

inline void L1ExtrapolateXC00Line( const L1TrackPar &T, fvec z_out, fvec& x, fvec& C00 )
{
  const fvec dz = (z_out - T.z);
  x = T.x + T.tx*dz;
  C00 = T.C00 + dz * ( 2*T.C20 + dz * T.C22 );
}

inline void L1ExtrapolateYC11Line( const L1TrackPar &T, fvec z_out, fvec& y, fvec& C11 )
{
  const fvec dz = (z_out - T.z);
  y = T.y + T.ty*dz;
  C11 = T.C11 + dz * ( 2*T.C31 + dz * T.C33 );
}

inline void L1ExtrapolateC10Line( const L1TrackPar &T, fvec z_out, fvec& C10 )
{
  const fvec dz = (z_out - T.z);
  C10 = T.C10 + dz * ( T.C21 + T.C30 + dz * T.C32 );
}

inline void L1ExtrapolateJXY // is not used currently
( 
 fvec &tx, fvec &ty, fvec &qp, // input track parameters
 fvec  dz  , // extrapolate to this dz 
 L1FieldRegion &F,
 fvec &extrDx, fvec &extrDy, fvec extrJ[]
 )
{
  //
  //  Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
  //  
  
//   cnst ZERO = 0.0, ONE = 1.;
  cnst c_light = 0.000299792458;

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

  fvec dz2 = dz*dz;
  fvec dz3 = dz2*dz;

  // construct coefficients 

  fvec x   = tx;
  fvec y   = ty;
  fvec xx  = x*x;
  fvec xy = x*y;
  fvec yy = y*y;
  fvec y2 = y*c2;
  fvec x2 = x*c2;
  fvec x4 = x*c4;
  fvec xx31 = xx*c3+c1;
  fvec xx159 = xx*c15+c9;

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

  fvec Ayy_x = c3*xx31;
  fvec Ayyy_x = -x4*xx159;

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

  fvec Byy_x = c6*xy;
  fvec Byyy_x = -y*(c45*xx+c9);
  fvec Byyy_y = -x*xx159;

  // end of coefficients calculation

  fvec t2   = c1 + xx + yy;
  fvec t    = sqrt( t2 );
  fvec h    = qp*c_light;
  fvec ht   = h*t;

  // get field integrals
  fvec Fx0 = F.cx0;
  fvec Fx1 = F.cx1*dz;
  fvec Fx2 = F.cx2*dz2;
  fvec Fy0 = F.cy0;
  fvec Fy1 = F.cy1*dz;
  fvec Fy2 = F.cy2*dz2;
  fvec Fz0 = F.cz0;
  fvec Fz1 = F.cz1*dz;
  fvec Fz2 = F.cz2*dz2;

  // 

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

  fvec Syz;
  {
    cnst 
      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 ) ;
  }

  fvec Syy ;   
  {
    cnst  
    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 ;
  }
  
  fvec Syyy;
  {
    cnst 
      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;
    fvec 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                  ;
  }
    
  fvec SA1   = Sx*xy   + Sy*Ay   + Sz*y ;
  fvec SA1_x = Sx*y - Sy*x2 ;
  fvec SA1_y = Sx*x + Sz;

  fvec SB1   = Sx*Bx   - Sy*xy   - Sz*x ;
  fvec SB1_x = -Sy*y - Sz;
  fvec SB1_y = Sx*y2 - Sy*x;

  fvec SA2   = Syy*Ayy   + Syz*Ayz ;
  fvec SA2_x = Syy*Ayy_x - Syz*y2 ;
  fvec SA2_y = -Syz*x2 ;
  fvec SB2   = Syy*Byy   + Syz*Byz ;
  fvec SB2_x = Syy*Byy_x + Syz*x4 ;
  fvec SB2_y = Syy*xx31 ;

  fvec SA3   = Syyy*Ayyy  ;
  fvec SA3_x = Syyy*Ayyy_x;
  fvec SB3   = Syyy*Byyy  ;
  fvec SB3_x = Syyy*Byyy_x;
  fvec SB3_y = Syyy*Byyy_y;

  fvec ht1 = ht*dz;
  fvec ht2 = ht*ht*dz2;
  fvec ht3 = ht*ht*ht*dz3;
  fvec ht1SA1 = ht1*SA1;
  fvec ht1SB1 = ht1*SB1;
  fvec ht2SA2 = ht2*SA2;
  fvec ht2SB2 = ht2*SB2;
  fvec ht3SA3 = ht3*SA3;
  fvec ht3SB3 = ht3*SB3;

  extrDx = ( x + ht1SA1 + ht2SA2 + ht3SA3 )*dz ;
  extrDy = ( y + ht1SB1 + ht2SB2 + ht3SB3 )*dz ;

  fvec ctdz2 = c_light*t*dz2;

  fvec t2i = c1*rcp(t2);// /t2;
  fvec xt2i = x*t2i;
  fvec yt2i = y*t2i;
  fvec tmp0 = ht1SA1 + c2*ht2SA2 + c3*ht3SA3;
  fvec tmp1 = ht1SB1 + c2*ht2SB2 + c3*ht3SB3;

  extrJ[0] = dz*(c1 + xt2i*tmp0 + ht1*SA1_x + ht2*SA2_x + ht3*SA3_x);  // j02
  extrJ[1] = dz*(     yt2i*tmp0 + ht1*SA1_y + ht2*SA2_y );             // j03
  extrJ[2] = ctdz2*( SA1 + c2*ht1*SA2 + c3*ht2*SA3 );                  // j04
  extrJ[3] = dz*(     xt2i*tmp1 + ht1*SB1_x + ht2*SB2_x + ht3*SB3_x);  // j12    
  extrJ[4] = dz*(c1 + yt2i*tmp1 + ht1*SB1_y + ht2*SB2_y + ht3*SB3_y ); // j13
  extrJ[5] = ctdz2*( SB1 + c2*ht1*SB2 + c3*ht2*SB3 );                  // j14

}

inline void L1ExtrapolateJXY0
( 
 fvec &tx, fvec &ty, // input track slopes
 fvec  dz  , // extrapolate to this dz position
 L1FieldRegion &F,
  fvec &extrDx, fvec &extrDy, fvec &J04, fvec &J14
 )
{
  cnst c_light = 0.000299792458, c1 = 1., c2i = 0.5, c6i = 1./6., c12i = 1./12.;

  fvec dz2 = dz*dz;
  fvec dzc6i = dz*c6i;
  fvec dz2c12i = dz2*c12i;

  fvec xx  = tx*tx;
  fvec yy = ty*ty;
  fvec xy  = tx*ty;

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

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

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

#undef cnst

#endif