///
  /// @primary authors: S.Gorbunov; I.Kisel
  /// @        authors: H.Pabst et al. (Intel)
  ///

#include <iostream>
#include <math.h>
#include "fit.h"
#include <arbb.hpp>

#define rcp(x) (1.f/(x))
#define rsqrt(x) (1.f/sqrt(x))

using namespace std;

void HitInfo::init( int ns ){
  cos_phi = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
  sin_phi = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
  sigma2   = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
  sigma216 = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
}

HitInfo::~HitInfo(){
  if( cos_phi ) arbb::aligned_free(cos_phi);
  if( sin_phi ) arbb::aligned_free(sin_phi);
  if( sigma2 ) arbb::aligned_free(sigma2);
  if( sigma216 ) arbb::aligned_free(sigma216);
}

void HitXYInfo::init( int ns ){
  C00 = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
  C10 = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
  C11 = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
}

HitXYInfo::~HitXYInfo(){
  if( C00 ) arbb::aligned_free(C00);
  if( C10 ) arbb::aligned_free(C10);
  if( C11 ) arbb::aligned_free(C11);
}

Stations::Stations()
    : nStations(0), z(NULL), thick(NULL), zhit(NULL), RL(NULL), RadThick(NULL), logRadThick(NULL),
      SyF(NULL), SyL(NULL), mapX(NULL), mapY(NULL), mapZ(NULL), UInfo(), VInfo(), XYInfo()
{}


Stations::Stations( int ns )
{
    init( ns );
}

void Stations::init( int ns )
{
    nStations = ns;
    
    z = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
    thick = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
    zhit = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
    RL = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
    RadThick = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
    logRadThick = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
    SyF = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));
    SyL = (ftype*)arbb::aligned_malloc(ns*sizeof(ftype));

    mapX = (ftype**)arbb::aligned_malloc(ns*sizeof(ftype*)); 
    mapY = (ftype**)arbb::aligned_malloc(ns*sizeof(ftype*)); 
    mapZ = (ftype**)arbb::aligned_malloc(ns*sizeof(ftype*)); 

    mapXPool = (ftype*) arbb::aligned_malloc( sizeof( ftype ) * ns * MaxNFieldCoeff );
    mapYPool = (ftype*) arbb::aligned_malloc( sizeof( ftype ) * ns * MaxNFieldCoeff );
    mapZPool = (ftype*) arbb::aligned_malloc( sizeof( ftype ) * ns * MaxNFieldCoeff );

    for( int i = 0; i < ns; i ++ ){
        mapX[i] = mapXPool + i * MaxNFieldCoeff;
        mapY[i] = mapYPool + i * MaxNFieldCoeff;
        mapZ[i] = mapZPool + i * MaxNFieldCoeff;
    }

    UInfo.init(ns);
    VInfo.init(ns);
    XYInfo.init(ns);
}


Stations::~Stations()
{
    if( z ) arbb::aligned_free(z);
    if( thick ) arbb::aligned_free(thick);
    if( zhit ) arbb::aligned_free(zhit);
    if( RL ) arbb::aligned_free(RL);
    if( RadThick ) arbb::aligned_free(RadThick);
    if( logRadThick ) arbb::aligned_free(logRadThick);
    if( SyF ) arbb::aligned_free(SyF);
    if( SyL ) arbb::aligned_free(SyL);
      
    if( mapX ) arbb::aligned_free(mapX);
    if( mapY ) arbb::aligned_free(mapY);
    if( mapZ ) arbb::aligned_free(mapZ);

    if( mapXPool ) arbb::aligned_free(mapXPool);
    if( mapYPool ) arbb::aligned_free(mapYPool);
    if( mapZPool ) arbb::aligned_free(mapZPool);
}


Tracks::Tracks()
{
    MC_x = MC_y = MC_z = MC_px = MC_py = MC_pz = MC_q = NULL;
    nHits = NDF = NULL;
    hitsX = NULL;
    hitsY = NULL;
    hitsIsta = NULL;
 //   T = NULL;
 //   C = NULL;
}
void Tracks::init( int ns, int nt )
{
    nTracks = nt;
    MC_x = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nt );
    MC_y = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nt );
    MC_z = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nt );
    MC_px = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nt );
    MC_py = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nt );
    MC_pz = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nt );
    MC_q = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nt );
    nHits = ( int* ) arbb::aligned_malloc( sizeof( int ) * nt );
    NDF = ( int* ) arbb::aligned_malloc( sizeof( int ) * nt );

    nStations = ns;

    memXPool = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nt * nStations );
    memYPool = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nt * nStations );
    memIstaPool = ( int* ) arbb::aligned_malloc( sizeof( int ) * nt * nStations );

    hitsX = ( ftype ** ) arbb::aligned_malloc( sizeof( ftype * ) * nt );
    hitsY = ( ftype ** ) arbb::aligned_malloc( sizeof( ftype * ) * nt );
    hitsIsta = ( int ** ) arbb::aligned_malloc( sizeof( int * ) * nt );

    for( int i = 0; i < nt; i ++ ){
        hitsX[i] = ( ftype* )( memXPool + i * ns );
        hitsY[i] = ( ftype* )( memYPool + i * ns );
        hitsIsta[i] = ( int* )( memIstaPool + i * ns );
    }

    hitsX2 = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nStations * nt  );
    hitsY2 = ( ftype* ) arbb::aligned_malloc( sizeof( ftype ) * nStations * nt  );

 //   T = new ftype[vTracks.nTracks][6];
 //   C = new ftype[vTracks.nTracks][15];
}
Tracks::~Tracks()
{
    //if( MC_x != NULL ) free( MC_x );
    //if( MC_y != NULL ) free( MC_y );
    //if( MC_z != NULL ) free( MC_z );
    //if( MC_px != NULL ) free( MC_px );
    //if( MC_py != NULL ) free( MC_py );
    //if( MC_pz != NULL ) free( MC_pz );
    //if( MC_q != NULL ) free( MC_q );
    //if( nHits != NULL ) free( nHits );
    //if( NDF != NULL ) free( NDF );

    //if( memXPool != NULL ) free( memXPool );
    //if( memYPool != NULL ) free( memYPool );
    //if( memIstaPool != NULL ) free( memIstaPool );
    //
    //if( hitsX != NULL ) free( hitsX );
    //if( hitsY != NULL ) free( hitsY );
    //if( hitsIsta != NULL ) free( hitsIsta );

 //   if( T != NULL ) delete T;
 //   if( C != NULL ) delete C;
}
void Tracks::setMC( int i, ftype *MC_para )
{
    MC_x[i] = MC_para[0];
    MC_y[i] = MC_para[1];
    MC_z[i] = MC_para[2];
    MC_px[i] = MC_para[3];
    MC_py[i] = MC_para[4];
    MC_pz[i] = MC_para[5];
    MC_q[i] = MC_para[6];
}
void Tracks::setHits( int i, int *iSta, ftype *hx, ftype *hy )
{
    for( unsigned int j = 0; j < nStations; j ++ ){
        hitsX[i][j] = hx[j];
        hitsY[i][j] = hy[j];
        hitsIsta[i][j] = iSta[j];
    }
}

#if 0 // TODO implement
void GuessVec( int iTrack, ftype T[6] )
{
    //  Fvec_t *T = t.T;

    int nHits = vStations.nStations;

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

    int i=nHits-1;
    z0 = vStations.zhit[i];
    w = 1;
    A0 = w;
    a0 = w*vTracks.hitsX[iTrack][i];//hlst->x;
    b0 = w*vTracks.hitsY[iTrack][i];//hlst->y;
    for( int j = 0; j < nHits - 1; j ++ ){
        x = vTracks.hitsX[iTrack][j];
        y = vTracks.hitsY[iTrack][j];
        w = 1.0f;
        z = vStations.zhit[j] - z0;
        S = vStations.SyL[j];
        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;
    }

    ftype A3A3 = A3*A3;
    ftype A3A4 = A3*A4;
    ftype A1A5 = A1*A5;
    ftype A2A5 = A2*A5;
    ftype A4A4 = A4*A4;

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

    ftype L, L1;
    T[0] = (Ai0*a0 + Ai1*a1 + Ai3*a2)*det;
    T[2] = (Ai1*a0 + Ai2*a1 + Ai4*a2)*det;
    ftype 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*static_cast<ftype>(c_light_i)*rsqrt(txtx1 +T[3]*T[3]);
    T[5] = z0;
}

inline void filterFirst( int iTrack, int iStation, ftype T[6], ftype C[15] )
{
    //  CovV &C = track.C;
    ftype hit_w = 1.0f; //! It seems that hit.w always be 1.0.
    ftype w1 = static_cast<ftype>(ONE-hit_w); 
    ftype sigma2 = hit_w*vStations.Sigma2[iStation] + static_cast<ftype>(w1*INF);
    // initialize covariance matrix 
    C[0] = sigma2;                                    //! C00
    C[1] = static_cast<ftype>(ZERO); C[2] = sigma2;                  //! C10, C11
    C[3] = static_cast<ftype>(ZERO); C[4] = static_cast<ftype>(ZERO); C[5] = static_cast<ftype>(INF);   //! C20, C21, C22
    C[6] = static_cast<ftype>(ZERO); C[7] = static_cast<ftype>(ZERO); C[8] = static_cast<ftype>(ZERO);
    C[9] = static_cast<ftype>(INF);  //! C30, C31, std::complex<f32> , C33
    C[10]= static_cast<ftype>(ZERO); C[11]= static_cast<ftype>(ZERO); C[12]= static_cast<ftype>(ZERO);
    C[13]= static_cast<ftype>(ZERO); C[14]= static_cast<ftype>(INF); //! C40, C41, C42, C43, C44

    T[0] = hit_w*vTracks.hitsX[iTrack][iStation] + w1*T[0];
    T[1] = hit_w*vTracks.hitsY[iTrack][iStation] + w1*T[1];
}

inline void addMaterial( int iTrack, int iStation, ftype &qp0, ftype T[6], ftype C[15] )
{
    ftype mass2 = static_cast<ftype>(0.1396*0.1396);

    ftype tx = T[2];
    ftype ty = T[3];
    ftype txtx = tx*tx;
    ftype tyty = ty*ty;
    ftype txtx1 = txtx + static_cast<ftype>(ONE);
    ftype h = txtx + tyty;
    ftype t = sqrt(txtx1 + tyty);
    ftype h2 = h*h;
    ftype qp0t = qp0*t;

    ftype c1=static_cast<ftype>(0.0136), c2=static_cast<ftype>(c1*0.038), c3=static_cast<ftype>(c2*0.5),
          c4=static_cast<ftype>(-c3/2.0), c5=static_cast<ftype>(c3/3.0), c6=static_cast<ftype>(-c3/4.0);

    ftype s0 = (c1+c2*vStations.logRadThick[iStation] + c3*h + h2*(c4 + c5*h +c6*h2) )*qp0t;

    ftype a = static_cast<ftype>(ONE+mass2*qp0*qp0t)*vStations.RadThick[iStation]*s0*s0;

    C[5] += txtx1*a;        //! C22
    C[8] += tx*ty*a;        //! std::complex<f32> 
    C[9] += static_cast<ftype>(ONE+tyty)*a;   //! C33
}

void combine( const ftype H[3], const ftype w, ftype des[3] ){
    des[0] += w*(H[0]-des[0]);
    des[1] += w*(H[1]-des[1]);
    des[2] += w*(H[2]-des[2]);
}


// inline // --> causes problems for the MS compiler (error C2603)
void extrapolateALight( 
                       ftype T[],         // input track parameters (x,y,tx,ty,Q/p)
                       ftype C[],         // input covariance matrix
                       const ftype zOut,  // extrapolate to this z position
                       ftype       qp0,   // use Q/p linearisation at this value
                       FieldRegion &F
                       )
{
    //
    //  Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
    //  

    ftype  
        c1 = static_cast<ftype>(1.), c2 = static_cast<ftype>(2.), c3 = static_cast<ftype>(3.),
        c4 = static_cast<ftype>(4.), c6 = static_cast<ftype>(6.), c9 = static_cast<ftype>(9.),
        c15 = static_cast<ftype>(15.), c18 = static_cast<ftype>(18.), c45 = static_cast<ftype>(45.),
        c2i = static_cast<ftype>(1./2.), c3i = static_cast<ftype>(1./3.), c6i = static_cast<ftype>(1./6.),
        c12i = static_cast<ftype>(1./12.);

    ftype qp = T[4];
    ftype dz = (zOut - T[5]);
    ftype dz2 = dz*dz;
    ftype dz3 = dz2*dz;

    ftype T0 = T[0];
    ftype T1 = T[1];
    ftype T2 = T[2];
    ftype T3 = T[3];
    ftype T5 = T[5];

    // construct coefficients 

    ftype x   = T[2]; // tx !!
    ftype y   = T[3]; // ty !!
    ftype xx  = x*x;
    ftype xy = x*y;
    ftype yy = y*y;
    ftype y2 = y*c2;
    ftype x2 = x*c2;
    ftype x4 = x*c4;
    ftype xx31 = xx*c3+c1;
    ftype xx159 = xx*c15+c9;

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

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

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

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

    // end of coefficients calculation

    ftype t2   = c1 + xx + yy;
    ftype t    = sqrt( t2 );
    ftype h    = static_cast<ftype>(qp0*c_light);
    ftype ht   = h*t;

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

    // 

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

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

    ftype syz;
    { 
        ftype 
            d = static_cast<ftype>(1./360.), 
            c00 = static_cast<ftype>(30.*6.*d), c01 = static_cast<ftype>(30.*2.*d),   c02 = static_cast<ftype>(30.*d),
            c10 = static_cast<ftype>(3.*40.*d), c11 = static_cast<ftype>(3.*15.*d),   c12 = static_cast<ftype>(3.*8.*d), 
            c20 = static_cast<ftype>(2.*45.*d), c21 = static_cast<ftype>(2.*2.*9.*d), c22 = static_cast<ftype>(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) ;
    }

    ftype Syz;
    {
        ftype 
            d = static_cast<ftype>(1./2520.), 
            c00 = static_cast<ftype>(21.*20.*d), c01 = static_cast<ftype>(21.*5.*d), c02 = static_cast<ftype>(21.*2.*d),
            c10 =  static_cast<ftype>(7.*30.*d), c11 =  static_cast<ftype>(7.*9.*d), c12 =  static_cast<ftype>(7.*4.*d), 
            c20 =  static_cast<ftype>(2.*63.*d), c21 = static_cast<ftype>(2.*21.*d), c22 = static_cast<ftype>(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 ) ;
    }

    ftype syy  = sy*sy*c2i;
    ftype syyy = syy*sy*c3i;

    ftype Syy ;   
    {
        ftype  
            d= static_cast<ftype>(1./2520.), c00= static_cast<ftype>(420.*d), c01= static_cast<ftype>(21.*15.*d),
          c02= static_cast<ftype>(21.*8.*d), c03= static_cast<ftype>(63.*d), c04= static_cast<ftype>(70.*d),
          c05= static_cast<ftype>(20.*d);
        Syy =  Fy0*(c00*Fy0+c01*Fy1+c02*Fy2) + Fy1*(c03*Fy1+c04*Fy2) + c05*Fy2*Fy2 ;
    }

    ftype Syyy;
    {
        ftype 
            d = static_cast<ftype>(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;
        ftype 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                  ;
    }


    ftype sA1   = sx*xy   + sy*Ay   + sz*y ;
    ftype sA1_x = sx*y - sy*x2 ;
    ftype sA1_y = sx*x + sz ;

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

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

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


    ftype sA2   = syy*Ayy   + syz*Ayz ;
    ftype sA2_x = syy*Ayy_x - syz*y2 ;
    ftype sA2_y = -syz*x2 ;
    ftype sB2   = syy*Byy   + syz*Byz  ;
    ftype sB2_x = syy*Byy_x + syz*x4 ;
    ftype sB2_y = syy*xx31 ;

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

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


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

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

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

    ftype ctdz  = static_cast<ftype>(c_light*t*dz);
    ftype ctdz2 = static_cast<ftype>(c_light*t*dz2);

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

    ftype j02 = dz*(c1 + xt2i*tmp0 + ht1*SA1_x + ht2*SA2_x + ht3*SA3_x);
    ftype j12 = dz*(     xt2i*tmp1 + ht1*SB1_x + ht2*SB2_x + ht3*SB3_x);
    ftype j22 =     c1 + xt2i*tmp2 + ht1*sA1_x + ht2*sA2_x + ht3*sA3_x ;
    ftype j32 =          xt2i*tmp3 + ht1*sB1_x + ht2*sB2_x + ht3*sB3_x ;

    ftype j03 = dz*(     yt2i*tmp0 + ht1*SA1_y + ht2*SA2_y );
    ftype j13 = dz*(c1 + yt2i*tmp1 + ht1*SB1_y + ht2*SB2_y + ht3*SB3_y );
    ftype j23 =          yt2i*tmp2 + ht1*sA1_y + ht2*sA2_y  ;
    ftype j33 =     c1 + yt2i*tmp3 + ht1*sB1_y + ht2*sB2_y + ht3*sB3_y ;

    ftype j04 = ctdz2*( SA1 + c2*ht1*SA2 + c3*ht2*SA3 );
    ftype j14 = ctdz2*( SB1 + c2*ht1*SB2 + c3*ht2*SB3 );
    ftype j24 = ctdz *( sA1 + c2*ht1*sA2 + c3*ht2*sA3 );
    ftype j34 = ctdz *( sB1 + c2*ht1*sB2 + c3*ht2*sB3 );

    // extrapolate inverse momentum

    T[0]+=j04*dqp;
    T[1]+=j14*dqp;
    T[2]+=j24*dqp;
    T[3]+=j34*dqp;

    //          covariance matrix transport 

    ftype c42 = C[12], c43 = C[13];

    ftype cj00 = C[0] + C[3]*j02 + C[6]*j03 + C[10]*j04;
    ftype cj20 = C[3] + C[5]*j02 + C[8]*j03 + C[12]*j04;
    ftype cj30 = C[6] + C[8]*j02 + C[9]*j03 + C[13]*j04;

    ftype cj01 = C[1] + C[3]*j12 + C[6]*j13 + C[10]*j14;
    ftype cj11 = C[2] + C[4]*j12 + C[7]*j13 + C[11]*j14;
    ftype cj21 = C[4] + C[5]*j12 + C[8]*j13 + c42*j14;
    ftype cj31 = C[7] + C[8]*j12 + C[9]*j13 + c43*j14;

    ftype cj22 = C[5]*j22 + C[8]*j23 + c42*j24;
    ftype cj32 = C[8]*j22 + C[9]*j23 + c43*j24;

    ftype cj23 = C[5]*j32 + C[8]*j33 + c42*j34;
    ftype cj33 = C[8]*j32 + C[9]*j33 + c43*j34;

    C[10]+= c42*j02 + c43*j03 + C[14]*j04; // cj40
    C[11]+= c42*j12 + c43*j13 + C[14]*j14; // cj41
    C[12] = c42*j22 + c43*j23 + C[14]*j24; // cj42
    C[13] = c42*j32 + c43*j33 + C[14]*j34; // cj43

    C[0] = cj00 + j02*cj20 + j03*cj30 + j04*C[10];
    C[1] = cj01 + j02*cj21 + j03*cj31 + j04*C[11];
    C[2] = cj11 + j12*cj21 + j13*cj31 + j14*C[11];

    C[3] = j22*cj20 + j23*cj30 + j24*C[10] ;
    C[6] = j32*cj20 + j33*cj30 + j34*C[10] ;
    C[4] = j22*cj21 + j23*cj31 + j24*C[11] ;
    C[7] = j32*cj21 + j33*cj31 + j34*C[11] ;
    C[5] = j22*cj22 + j23*cj32 + j24*C[12] ;
    C[8] = j32*cj22 + j33*cj32 + j34*C[12] ;
    C[9] = j32*cj23 + j33*cj33 + j34*C[13] ;

}


inline void filter( int iTrack, ftype info[4], ftype u, ftype w, ftype T[6], ftype C[15] )
{

    // convert input

    ftype wi, zeta, zetawi, HCH;

    ftype F0, F1, F2, F3, F4;
    ftype  K1, K2, K3, K4;

    zeta = info[0]*T[0] + info[1]*T[1] - u;
    // F = CH'
    F0 = info[0]*C[0] + info[1]*C[1];
    F1 = info[0]*C[1] + info[1]*C[2];

    HCH = ( F0*info[0] +F1*info[1] );
    int initialised = ( HCH < info[3] );

    F2 = info[0]*C[3] + info[1]*C[4];
    F3 = info[0]*C[6] + info[1]*C[7];
    F4 = info[0]*C[10] + info[1]*C[11];

    wi = w*rcp(info[2] +HCH);
    zetawi = w* zeta *rcp( (initialised & (int)info[2]) + HCH);
    ftype chi2 = 0;
    chi2 += initialised & (int)(zeta * zetawi);    

    vTracks.NDF[iTrack]  += (int)w;

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

    T[0]-= F0*zetawi;
    T[1]-= F1*zetawi;
    T[2]-= F2*zetawi;
    T[3]-= F3*zetawi;
    T[4]-= F4*zetawi;

    C[0]-= F0*F0*wi;
    C[1]-= K1*F0;
    C[2]-= K1*F1;
    C[3]-= K2*F0;
    C[4]-= K2*F1;
    C[5]-= K2*F2;
    C[6]-= K3*F0;
    C[7]-= K3*F1;
    C[8]-= K3*F2;
    C[9]-= K3*F3;
    C[10]-= K4*F0;
    C[11]-= K4*F1;
    C[12]-= K4*F2;
    C[13]-= K4*F3;
    C[14]-= K4*F4;
}

void fit( int iTrack, ftype T[6], ftype C[15] )
{
    ftype c16 = 16.;
    ftype xInfo[4], yInfo[4];
    xInfo[0] = ONE;                     //! cos_phi
    xInfo[1] = ZERO;                    //! sin_phi
    xInfo[2] = vStations.Sigma2[0];     //! sigma2
    xInfo[3] = xInfo[2]*c16;            //! sigma216

    yInfo[0] = ZERO;                    //! cos_phi
    yInfo[1] = ONE;                     //! sin_phi
    yInfo[2] = xInfo[2];                //! sigma2
    yInfo[3] = xInfo[3];                //! sigma216

    // upstream 

    GuessVec( iTrack, T );

    // downstream

    FieldRegion f;
    ftype z0,z1,z2, dz;
    ftype H0[3], H1[3], H2[3];

    ftype qp0 = T[4];
    int i= vStations.nStations-1;
    //  HitV *h = &t.vHits[i];

    filterFirst( iTrack, i, T, C );
    addMaterial( iTrack, i, qp0, T, C );

    z1 = vStations.z[ i ];
    vStations.field( i, T[0], T[1], H1 );
    ftype HH[3];
    ftype h_w = 1.0f;   //h.w, since it's always 1.0.
    vStations.field(i, vTracks.hitsX[iTrack][i], vTracks.hitsY[iTrack][i], HH);
    combine( HH, h_w, H1 );

    z2 = vStations.z[ i-2 ];
    dz = z2-z1;
    vStations.field(i-2, T[0]+T[2]*dz,T[1]+T[3]*dz,H2);
    vStations.field(i-2, vTracks.hitsX[iTrack][i-2], vTracks.hitsY[iTrack][i-2], HH);
    combine( HH, h_w, H2 );

    for( --i; i>=0; i-- ){
        //   h = &t.vHits[i];
        z0 = vStations.z[i];    
        dz = (z1-z0);
        vStations.field(i, T[0]-T[2]*dz,T[1]-T[3]*dz,H0);
        vStations.field(i, vTracks.hitsX[iTrack][i], vTracks.hitsY[iTrack][i], HH);
        combine( HH, h_w, H0 );
        f.set( H0, z0, H1, z1, H2, z2);

        //CalcRegion(f,t.T,vStations[i+1].Map,vStations[i+1].MapLeft, st.Map ); 

        extrapolateALight( T, C, vStations.zhit[i], qp0, f );
        //ExtrapolateALight( t.T, t.C, st.z+st.thick/4, qp0);//, f );
        addMaterial( iTrack, i, qp0, T, C );
        //ExtrapolateALight( t.T, t.C, st.zhit, qp0);//, f );
        filter( iTrack, xInfo, vTracks.hitsX[iTrack][i], h_w, T, C );
        filter( iTrack, yInfo, vTracks.hitsY[iTrack][i], h_w, T, C );
        //ExtrapolateALight( t.T, t.C, st.z-st.thick/4, qp0);//, f );
        //AddMaterial( t, st, qp0 );
        //ExtrapolateALight( t.T, t.C, st.zhit, qp0);//, f );
        memcpy( H2, H1, sizeof(ftype) * 3 );
        memcpy( H1, H0, sizeof(ftype) * 3 );
        z2 = z1;
        z1 = z0;
    }
}

void fitTracks( ftype (*T)[6], ftype (*C)[15] )
{
    for( int times=0; times<Ntimes; times++){
        for( unsigned int i=0; i<vTracks.nTracks; i++ ){
            fit( i, T[i], C[i] );
        }
    }

    for( unsigned int i=0; i<vTracks.nTracks; i++ ){ // loop on set of 4 tracks
        extrapolateALight( T[i], C[i], vTracks.MC_z[i], T[i][4], magField );
    }
}

#endif // 0