#ifndef FitFunctional_H
#define FitFunctional_H

  /// Abstract class. Contains discription of basic Fit Functional, which is used by any fit method ( Fit, Smother, DAF )

#include <assert.h>
#include "FitClasses.h"

#define cnst static const Single_t // with Fvec_t 15% slower

cnst INF = .01; cnst INF2 = .0001; cnst ZERO = 0.0; cnst ONE = 1.;
cnst c_light = 0.000299792458; cnst c_light_i = 1./c_light;

cnst PipeRadThick = 0.0009;

class Jacobian_t{ // jacobian elements // j[0][0] - j[3][2] are j02 - j34
 public:
  Fvec_t &operator()(int i, int j) { assert( i>=0 && j>=2 ); return fj[i][j-2]; };

 private:
    //     1 0 ? ? ?
    //     0 1 ? ? ?
    // j = 0 0 ? ? ?
    //     0 0 ? ? ?
    //     0 0 0 0 1
  Fvec_t fj[4][3];
};

class FitFunctional { // base class for all approaches
 public:

    /// extrapolates track parameters
  virtual void ExtrapolateALight( Fvec_t T[], CovV &C,  const Fvec_t &z_out,  Fvec_t& qp0, FieldRegion &F, Fvec_t w = ZERO) const = 0;

 protected:
    /// initial aproximation
  void GuessVec( TrackV &t, Station *vStations, int NStations, bool dir = 0 ) const;
  
  virtual void Filter( TrackV &track, HitInfo &info, Fvec_t &u, Fvec_t w = ONE ) const = 0;
    // filter first mesurement
  virtual void FilterFirst( TrackV &track, HitV &hit, Station &st ) const = 0;
  
   void AddMaterial( TrackV &track, Station &st, Fvec_t &qp0, bool isPipe = ZERO ) const;
   void AddPipeMaterial( TrackV &track, Fvec_t &qp0) const;
   void AddHalfMaterial( TrackV &track, Station &st, Fvec_t &qp0) const;
  
  virtual void ExtrapolateWithMaterial( TrackV &track, const Fvec_t &z_out,  Fvec_t& qp0, FieldRegion &F, Station &st, bool isPipe = 0, Fvec_t w = ZERO) const = 0;

    // ------------ help functions ------------
  virtual void AddMaterial( CovV &C, Fvec_t Q22, Fvec_t Q32, Fvec_t Q33 ) const = 0;
  
    /// extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
  void ExtrapolateJ (
    Fvec_t *T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
    Fvec_t      z_out  , // extrapolate to this z position
    Fvec_t       qp0    , // use Q/p linearisation at this value
    const FieldRegion &F,
    Jacobian_t &j,
    Fvec_t w = 0.f ) const;

    /// calculate covMatrix for Multiple Scattering
  void GetMSMatrix( const Fvec_t &tx, const Fvec_t &ty, const Fvec_t &radThick, const Fvec_t &logRadThick, Fvec_t qp0, Fvec_t &Q22, Fvec_t &Q32, Fvec_t &Q33 ) const;
 private:

    /// extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
#if defined(ANALYTIC_EXTRAPOLATION)
  void ExtrapolateJAnalytic
#elif defined(RK4_EXTRAPOLATION)
  void ExtrapolateJRK4
#endif
  (
    Fvec_t T [], // input track parameters (x,y,tx,ty,Q/p)
    Fvec_t        z_out  , // extrapolate to this z position
    Fvec_t       qp0    , // use Q/p linearisation at this value
    const FieldRegion &F,
    Jacobian_t &j,
    Fvec_t w = 0.f ) const;

};


inline void FitFunctional::GuessVec( TrackV &t, Station *vStations, int nStations, bool dir ) const
{
  Fvec_t *T = t.T;

  Int_t NHits = nStations;

  Fvec_t A0, A1=ZERO, A2=ZERO, A3=ZERO, A4=ZERO, A5=ZERO, a0, a1=ZERO, a2=ZERO,
    b0, b1=ZERO, b2=ZERO;
  Fvec_t z0, x, y, z, S, w, wz, wS;

  Int_t i=NHits-1;
  if(dir) i=0;
  z0 = vStations[i].zhit;
  HitV *hlst = &(t.vHits[i]);
  w = hlst->w;
  A0 = w;
  a0 = w*hlst->x;
  b0 = w*hlst->y;
  HitV *h = t.vHits;
  Station *st = vStations;
  int st_add = 1;
  if(dir) 
  {
    st_add = -1;
    h += NHits-1;
    st += NHits-1;
  }

  for( ; h!=hlst; h+=st_add, st+=st_add ){
    x = h->x;
    y = h->y;
    w = h->w;
    z = st->zhit - z0;
    if(!dir) S = st->SyL;
    else S = st->SyF;
    wz = w*z;
    wS = w*S;
    A0+=w; 
    A1+=wz;  A2+=wz*z;
    A3+=wS;  A4+=wS*z; A5+=wS*S;
    a0+=w*x; a1+=wz*x; a2+=wS*x;
    b0+=w*y; b1+=wz*y; b2+=wS*y;
  }

  Fvec_t A3A3 = A3*A3;
  Fvec_t A3A4 = A3*A4;
  Fvec_t A1A5 = A1*A5;
  Fvec_t A2A5 = A2*A5;
  Fvec_t A4A4 = A4*A4;
  
  Fvec_t det = rcp(-A2*A3A3 + A1*( A3A4+A3A4 - A1A5) + A0*(A2A5-A4A4));
  Fvec_t Ai0 = ( -A4A4 + A2A5 );
  Fvec_t Ai1 = (  A3A4 - A1A5 );
  Fvec_t Ai2 = ( -A3A3 + A0*A5 );
  Fvec_t Ai3 = ( -A2*A3 + A1*A4 );
  Fvec_t Ai4 = (  A1*A3 - A0*A4 );
  Fvec_t Ai5 = ( -A1*A1 + A0*A2 );

  Fvec_t L, L1;
  T[0] = (Ai0*a0 + Ai1*a1 + Ai3*a2)*det;
  T[2] = (Ai1*a0 + Ai2*a1 + Ai4*a2)*det;
  Fvec_t txtx1 = 1.f+T[2]*T[2];
  L    = (Ai3*a0 + Ai4*a1 + Ai5*a2)*det *rcp(txtx1);
  L1 = L*T[2];
  A1 = A1 + A3*L1;
  A2 = A2 + ( A4 + A4 + A5*L1 )*L1;
  b1+= b2 * L1;
  det = rcp(-A1*A1+A0*A2);

  T[1] = (  A2*b0 - A1*b1 )*det;
  T[3] = ( -A1*b0 + A0*b1 )*det;
  T[4] = -L*c_light_i*rsqrt(txtx1 +T[3]*T[3]);
  T[5] = z0;
}   


#if defined(ANALYTIC_EXTRAPOLATION)

inline void FitFunctional::ExtrapolateJAnalytic // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
(
  Fvec_t T [], // input track parameters (x,y,tx,ty,Q/p)
  Fvec_t        z_out  , // extrapolate to this z position
  Fvec_t       qp0    , // use Q/p linearisation at this value
  const FieldRegion &F,
  Jacobian_t &j,
  Fvec_t w
 ) const
{
  //cout<<"Extrapolation..."<<endl;
  //
  //  Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
  //  
  
  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_t qp = T[4];
  const Fvec_t dz = (z_out - T[5]);
  const Fvec_t dz2 = dz*dz;
  const Fvec_t dz3 = dz2*dz;

  // construct coefficients 

  const Fvec_t x   = T[2];
  const Fvec_t y   = T[3];
  const Fvec_t xx  = x*x;
  const Fvec_t xy = x*y;
  const Fvec_t yy = y*y;
  const Fvec_t y2 = y*c2;
  const Fvec_t x2 = x*c2;
  const Fvec_t x4 = x*c4;
  const Fvec_t xx31 = xx*c3+c1;
  const Fvec_t xx159 = xx*c15+c9;

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

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

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

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

  // end of coefficients calculation

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

  // get field integrals
  const Fvec_t ddz = T[5]-F.z;
  const Fvec_t Fx0 = F.x0 + F.x1*ddz + F.x2*ddz*ddz;
  const Fvec_t Fx1 = (F.x1 + c2*F.x2*ddz)*dz;
  const Fvec_t Fx2 = F.x2*dz2;
  const Fvec_t Fy0 = F.y0 + F.y1*ddz + F.y2*ddz*ddz;
  const Fvec_t Fy1 = (F.y1 + c2*F.y2*ddz)*dz;
  const Fvec_t Fy2 = F.y2*dz2;
  const Fvec_t Fz0 = F.z0 + F.z1*ddz + F.z2*ddz*ddz;
  const Fvec_t Fz1 = (F.z1 + c2*F.z2*ddz)*dz;
  const Fvec_t Fz2 = F.z2*dz2;

  // 

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

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

  Fvec_t 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_t 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_t syy  = sy*sy*c2i;
  const Fvec_t syyy = syy*sy*c3i;

  Fvec_t 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_t 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_t 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_t sA1   = sx*xy   + sy*Ay   + sz*y ;
  const Fvec_t sA1_x = sx*y - sy*x2 ;
  const Fvec_t sA1_y = sx*x + sz ;

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

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

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


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

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

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

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

  T[0]  += ((x + ht1SA1 + ht2SA2 + ht3SA3)*dz);
  T[1]  += ((y + ht1SB1 + ht2SB2 + ht3SB3)*dz);
  T[2]  += (ht1sA1 + ht2sA2 + ht3sA3);
  T[3]  += (ht1sB1 + ht2sB2 + ht3sB3);
  T[5]  += (dz);

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

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

    //     1 0 ? ? ?
    //     0 1 ? ? ?
    // j = 0 0 ? ? ?
    //     0 0 ? ? ?
    //     0 0 0 0 1
  
  j(0,2) = dz*(c1 + xt2i*tmp0 + ht1*SA1_x + ht2*SA2_x + ht3*SA3_x);
  j(1,2) = dz*(     xt2i*tmp1 + ht1*SB1_x + ht2*SB2_x + ht3*SB3_x);
  j(2,2) =     c1 + xt2i*tmp2 + ht1*sA1_x + ht2*sA2_x + ht3*sA3_x ;
  j(3,2) =          xt2i*tmp3 + ht1*sB1_x + ht2*sB2_x + ht3*sB3_x ;
    
  j(0,3) = dz*(     yt2i*tmp0 + ht1*SA1_y + ht2*SA2_y );
  j(1,3) = dz*(c1 + yt2i*tmp1 + ht1*SB1_y + ht2*SB2_y + ht3*SB3_y );
  j(2,3) =          yt2i*tmp2 + ht1*sA1_y + ht2*sA2_y  ;
  j(3,3) =     c1 + yt2i*tmp3 + ht1*sB1_y + ht2*sB2_y + ht3*sB3_y ;
    
  j(0,4) = ctdz2*( SA1 + c2*ht1*SA2 + c3*ht2*SA3 );
  j(1,4) = ctdz2*( SB1 + c2*ht1*SB2 + c3*ht2*SB3 );
  j(2,4) = ctdz *( sA1 + c2*ht1*sA2 + c3*ht2*sA3 );
  j(3,4) = ctdz *( sB1 + c2*ht1*sB2 + c3*ht2*sB3 );
    
  // extrapolate inverse momentum
  
  T[0] += (j(0,4)*dqp);
  T[1] += (j(1,4)*dqp);
  T[2] += (j(2,4)*dqp);
  T[3] += (j(3,4)*dqp);
}
  
#elif defined(RK4_EXTRAPOLATION)

inline void FitFunctional::ExtrapolateJRK4 // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
(
 Fvec_t *T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
 Fvec_t        z_out  , // extrapolate to this z position
 Fvec_t       qp0    , // use Q/p linearisation at this value
 const FieldRegion &F,
 Jacobian_t &j,
 Fvec_t w
 ) const
{
    //
  // 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
  //
  //========================================================================



  
  const Fvec_t a[4] = {0.0f, 0.5f, 0.5f, 1.0f};
  const Fvec_t c[4] = {1.0f/6.0f, 1.0f/3.0f, 1.0f/3.0f, 1.0f/6.0f};
  const Fvec_t b[4] = {0.0f, 0.5f, 0.5f, 1.0f};
  
  int step4;
  Fvec_t k[16],x0[4],x[4],k1[16];
  Fvec_t Ax[4],Ay[4],Ax_tx[4],Ay_tx[4],Ax_ty[4],Ay_ty[4];

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

  Fvec_t qp_in = T[4];
  const Fvec_t z_in  = T[5];
  const Fvec_t h     = (z_out - T[5]);
  Fvec_t hC    = h * c_light;
  x0[0] = T[0]; x0[1] = T[1];
  x0[2] = T[2]; x0[3] = T[3];
  //
  //   Runge-Kutta step
  //

  int step;
  int i;

  Fvec_t 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_t tx = x[2]; 
    Fvec_t ty = x[3]; 
    Fvec_t tx2 = tx * tx; 
    Fvec_t ty2 = ty * ty; 
    Fvec_t txty = tx * ty;
    Fvec_t tx2ty21= 1.f + tx2 + ty2; 
    // if( tx2ty21 > 1.e4 ) return 1;
    Fvec_t I_tx2ty21 = 1.f / tx2ty21 * qp0;
    Fvec_t tx2ty2 = sqrt(tx2ty21 ) ; 
    //   Fvec_t I_tx2ty2 = qp0 * hC / tx2ty2 ; unsused ???
    tx2ty2 *= hC; 
    Fvec_t 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

  {
    T[0] = (x0[0]+c[0]*k[0]+c[1]*k[4+0]+c[2]*k[8+0]+c[3]*k[12+0]);
    T[1] = (x0[1]+c[0]*k[1]+c[1]*k[4+1]+c[2]*k[8+1]+c[3]*k[12+1]);
    T[2] = (x0[2]+c[0]*k[2]+c[1]*k[4+2]+c[2]*k[8+2]+c[3]*k[12+2]);
    T[3] = (x0[3]+c[0]*k[3]+c[1]*k[4+3]+c[2]*k[8+3]+c[3]*k[12+3]);
    T[5] = z_out;
  }

  //
  //     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_t 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_t dqp = qp_in - qp0;
  
  { // update parameters
    T[0] +=(J[5*4+0]*dqp);
    T[1] +=(J[5*4+1]*dqp);
    T[2] +=(J[5*4+2]*dqp);
    T[3] +=(J[5*4+3]*dqp);
  }
   
  //          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];
}
#endif // _EXTRAPOLATION

inline void FitFunctional::ExtrapolateJ // extrapolates track parameters and returns jacobian for extrapolation of CovMatrix
(
 Fvec_t *T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
 Fvec_t      z_out  , // extrapolate to this z position
 Fvec_t       qp0    , // use Q/p linearisation at this value
 const FieldRegion &F,
 Jacobian_t &j,
 Fvec_t w
 ) const
{
#if defined(ANALYTIC_EXTRAPOLATION)
  
  ExtrapolateJAnalytic( T, z_out, qp0, F, j, w );
  
#elif defined(RK4_EXTRAPOLATION)
  
  ExtrapolateJRK4( T, z_out, qp0, F, j, w );
  
#endif // _EXTRAPOLATION
}

  /// calculate covMatrix for Multiple Scattering
inline void FitFunctional::GetMSMatrix( const Fvec_t &tx, const Fvec_t &ty, const Fvec_t &radThick, const Fvec_t &logRadThick, Fvec_t qp0, Fvec_t &Q22, Fvec_t &Q32, Fvec_t &Q33 ) const
{
  cnst mass2 = 0.1396f*0.1396f;

  Fvec_t txtx = tx*tx;
  Fvec_t tyty = ty*ty;
  Fvec_t txtx1 = txtx + ONE;
  Fvec_t h = txtx + tyty;
  Fvec_t t = sqrt(txtx1 + tyty);
  Fvec_t h2 = h*h;
  Fvec_t qp0t = qp0*t;
  
  cnst c1=0.0136f, c2=c1*0.038f, c3=c2*0.5f, c4=-c3/2.0f, c5=c3/3.0f, c6=-c3/4.0f;
    
  Fvec_t s0 = (c1+c2*logRadThick + c3*h + h2*(c4 + c5*h +c6*h2) )*qp0t;
    
  Fvec_t a = (ONE+mass2*qp0*qp0t)*radThick*s0*s0;

  
  Q22 = txtx1*a;
  Q32 = tx*ty*a;
  Q33 = (ONE + tyty)*a;
}


inline void FitFunctional::AddMaterial( TrackV &track, Station &st, Fvec_t &qp0, bool isPipe ) const
{
  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 );
  
  AddMaterial( track.C, Q22, Q32, Q33 );
}

inline void FitFunctional::AddPipeMaterial( TrackV &track, Fvec_t &qp0 ) const
{
  Fvec_t Q22, Q32, Q33;
  GetMSMatrix( track.T[2], track.T[3], PipeRadThick, log(PipeRadThick), qp0, Q22, Q32, Q33 );

  AddMaterial( track.C, Q22, Q32, Q33 );
}

inline void FitFunctional::AddHalfMaterial( TrackV &track, Station &st, Fvec_t &qp0 ) const
{
  Fvec_t Q22, Q32, Q33;
  GetMSMatrix( track.T[2], track.T[3], st.RadThick*0.5f, st.logRadThick+log(0.5f), qp0, Q22, Q32, Q33 );

  AddMaterial( track.C, Q22, Q32, Q33 );
}

#endif