/*
 *====================================================================
 *
 *  SIMD Kalman filter(KF) track fitter
 *  
 *  Authors: M.Zyzak, I.Kulakov
 *  Primary authors: I.Kisel, S.Gorbunov
 *
 *  e-mail : I.Kisel@gsi.de, M.Zyzak@gsi.de
 *
 *====================================================================
 */


#include "L1Algo/L1Algo.h"
#include "L1Algo/L1TrackPar.h"
#include "L1Algo/L1Extrapolation.h"
#include "L1Algo/L1AddMaterial.h"

#include "TStopwatch.h"

#include <iostream>
#include <vector>

using std::cout;
using std::endl;
using std::vector;

const fvec c_light = 0.000299792458, c_light_i = 1./c_light;
const fvec ZERO = 0.;
const fvec ONE = 1.;
const fvec vINF = 0.1;

void L1Algo::KFTrackFitter()
{
//  cout << " Start L1 Track Fitter " << endl;
  int start_hit = 0; // for interation in vRecoHits[]

  static L1FieldValue fB0, fB1, fB2 _fvecalignment;
  static L1FieldRegion fld _fvecalignment;

  static int nHits = NStations;
  int iVec=0, i=0;
  int nTracks_SIMD = fvecLen;
  L1TrackPar T; // fitting parametr coresponding to current track

  L1Track *t[fvecLen];

  int ista;
  L1Station *sta = vStations;
  fvec x[nHits], v[nHits], y[nHits], z[nHits];
  fvec w[nHits];
  fvec y_temp;
  fvec fz0, fz1, fz2, dz, z_start, z_end;
  L1FieldValue fB[nHits], fB_temp _fvecalignment;

  fvec ZSta[nHits];
  for(int iHit = 0; iHit<nHits; iHit++)
  {
    ZSta[iHit] = sta[iHit].z;
  }

  unsigned N_vTracks = vTracks.size();

  for(unsigned itrack = 0; itrack < N_vTracks; itrack+=fvecLen)
  {
    if(N_vTracks - itrack < fvecLen)
      nTracks_SIMD = N_vTracks - itrack;

    for(i=0; i<nTracks_SIMD; i++)
      t[i] = & vTracks[itrack+i]; // current track

    // get hits of current track
    for(i=0; i<nHits; i++)
    {
      w[i] = ZERO;
      z[i] = ZSta[i];
    }

    for(iVec=0; iVec<nTracks_SIMD; iVec++)
    {
      int nHitsTrack = t[iVec]->NHits;
      for(i = 0; i < nHitsTrack; i++ )
      {
        L1StsHit &hit = vStsHits[vRecoHits[start_hit++]];
        ista = vSFlag[hit.f]/4;
        w[ista][iVec] = 1.;

        x[ista][iVec]  = vStsStrips[hit.f] ;
        v[ista][iVec]  = vStsStripsB[hit.b];
        y_temp         = sta[ista].yInfo.cos_phi*x[ista][iVec] + sta[ista].yInfo.sin_phi*v[ista][iVec];
        y[ista][iVec]  = y_temp[iVec];
//         z[ista][iVec]  = hit.z;
        z[ista][iVec]  = sta[ista].z[0];
        sta[ista].fieldSlice.GetFieldValue( x[ista], y[ista], fB_temp );
        fB[ista].x[iVec] = fB_temp.x[iVec];
        fB[ista].y[iVec] = fB_temp.y[iVec];
        fB[ista].z[iVec] = fB_temp.z[iVec];
//         if(i == 0) z_start[iVec] = hit.z;
//         if(i == nHitsTrack) z_end[iVec] = hit.z;
        if(i == 0) z_start[iVec] = sta[ista].z[0];
        if(i == nHitsTrack) z_end[iVec] = sta[ista].z[0];
      }
    }

//fit backward

    GuessVec( T, x, y, z, w, nHits);

    i= nHits-1;

    FilterFirst( T, x[i],y[i],w[i], sta[i] );
    L1AddMaterial( T, sta[i].materialInfo, T.qp );

    fz1 = z[i];
    sta[i].fieldSlice.GetFieldValue( T.x, T.y, fB1 );
    fB1.Combine( fB[i], w[i] );

    fz2 = z[i-2];
    dz = fz2-fz1;
    sta[i].fieldSlice.GetFieldValue( T.x + T.tx*dz, T.y + T.ty*dz, fB2 );
    fB2.Combine( fB[i-2], w[i-2] );
    fld.Set( fB2, fz2, fB1, fz1, fB0, fz0 );
    for( --i; i>=0; i-- )
    {
      fz0 = z[i];
      dz = (fz1-fz0);
      sta[i].fieldSlice.GetFieldValue( T.x - T.tx*dz, T.y - T.ty*dz, fB0 );
      fB0.Combine( fB[i], w[i] );
      fld.Set( fB0, fz0, fB1, fz1, fB2, fz2 );

      L1Extrapolate( T, z[i], T.qp, fld );
      L1AddMaterial( T, sta[i].materialInfo, T.qp );
      Filter( T, sta[i].frontInfo, x[i], w[i] );
      Filter( T, sta[i].backInfo,  v[i], w[i] );
      fB2 = fB1; 
      fz2 = fz1;
      fB1 = fB0; 
      fz1 = fz0;
    }

    for(iVec=0; iVec<nTracks_SIMD; iVec++)
    {
      t[iVec]->TFirst[0] = T.x[iVec];
      t[iVec]->TFirst[1] = T.y[iVec];
      t[iVec]->TFirst[2] = T.tx[iVec];
      t[iVec]->TFirst[3] = T.ty[iVec];
      t[iVec]->TFirst[4] = T.qp[iVec];
      t[iVec]->TFirst[5] = z_start[iVec];
//      t[iVec]->TFirst[5] = T.z[iVec];
      
      t[iVec]->CFirst[0] = T.C00[iVec];
      t[iVec]->CFirst[1] = T.C10[iVec];
      t[iVec]->CFirst[2] = T.C11[iVec];
      t[iVec]->CFirst[3] = T.C20[iVec];
      t[iVec]->CFirst[4] = T.C21[iVec];
      t[iVec]->CFirst[5] = T.C22[iVec];
      t[iVec]->CFirst[6] = T.C30[iVec];
      t[iVec]->CFirst[7] = T.C31[iVec];
      t[iVec]->CFirst[8] = T.C32[iVec];
      t[iVec]->CFirst[9] = T.C33[iVec];
      t[iVec]->CFirst[10] = T.C40[iVec];
      t[iVec]->CFirst[11] = T.C41[iVec];
      t[iVec]->CFirst[12] = T.C42[iVec];
      t[iVec]->CFirst[13] = T.C43[iVec];
      t[iVec]->CFirst[14] = T.C44[iVec];

      t[iVec]->chi2 = T.chi2[iVec];
      t[iVec]->NDF = (int)T.NDF[iVec];
    }

    // fit forward

    i= 0;
    FilterLast( T, x[i],y[i],w[i], sta[i] );
    L1AddMaterial( T, sta[i].materialInfo, T.qp );

    fz1 = z[i];
    sta[i].fieldSlice.GetFieldValue( T.x, T.y, fB1 );
    fB1.Combine( fB[i], w[i] );

    fz2 = z[i+2];
    dz = fz2-fz1;
    sta[i].fieldSlice.GetFieldValue( T.x + T.tx*dz, T.y + T.ty*dz, fB2 );
    fB2.Combine( fB[i+2], w[i+2] );
    fld.Set( fB2, fz2, fB1, fz1, fB0, fz0 );

    for( ++i; i<nHits; i++ )
    {
      fz0 = z[i];
      dz = (fz1-fz0);
      sta[i].fieldSlice.GetFieldValue( T.x - T.tx*dz, T.y - T.ty*dz, fB0 );
      fB0.Combine( fB[i], w[i] );
      fld.Set( fB0, fz0, fB1, fz1, fB2, fz2 );

      L1Extrapolate( T, z[i], T.qp, fld );
      L1AddMaterial( T, sta[i].materialInfo, T.qp );
      Filter( T, sta[i].frontInfo, x[i], w[i] );
      Filter( T, sta[i].backInfo,  v[i], w[i] );

      fB2 = fB1; 
      fz2 = fz1;
      fB1 = fB0; 
      fz1 = fz0;
    }

    for(iVec=0; iVec<nTracks_SIMD; iVec++)
    {
      t[iVec]->TLast[0] = T.x[iVec];
      t[iVec]->TLast[1] = T.y[iVec];
      t[iVec]->TLast[2] = T.tx[iVec];
      t[iVec]->TLast[3] = T.ty[iVec];
      t[iVec]->TLast[4] = T.qp[iVec];
      t[iVec]->TLast[5] = z_end[iVec];
      
      t[iVec]->CLast[0] = T.C00[iVec];
      t[iVec]->CLast[1] = T.C10[iVec];
      t[iVec]->CLast[2] = T.C11[iVec];
      t[iVec]->CLast[3] = T.C20[iVec];
      t[iVec]->CLast[4] = T.C21[iVec];
      t[iVec]->CLast[5] = T.C22[iVec];
      t[iVec]->CLast[6] = T.C30[iVec];
      t[iVec]->CLast[7] = T.C31[iVec];
      t[iVec]->CLast[8] = T.C32[iVec];
      t[iVec]->CLast[9] = T.C33[iVec];
      t[iVec]->CLast[10] = T.C40[iVec];
      t[iVec]->CLast[11] = T.C41[iVec];
      t[iVec]->CLast[12] = T.C42[iVec];
      t[iVec]->CLast[13] = T.C43[iVec];
      t[iVec]->CLast[14] = T.C44[iVec];

      t[iVec]->chi2 = T.chi2[iVec];
      t[iVec]->NDF = (int)T.NDF[iVec];
    }
  }
}

void L1Algo::GuessVec( L1TrackPar &t, fvec *xV, fvec *yV, fvec *zV, fvec *wV, int NHits )
{
  fvec A0, A1=ZERO, A2=ZERO, A3=ZERO, A4=ZERO, A5=ZERO, a0, a1=ZERO, a2=ZERO,
    b0, b1=ZERO, b2=ZERO;
  fvec z0, x, y, z, S, w, wz, wS;

  Int_t i=NHits-1;
  z0 = zV[i];
  w = wV[i];
  A0 = w;
  a0 = w*xV[i];
  b0 = w*yV[i];
  L1Station *st = vStations;
  for( i=0 ; i<NHits; i++, st++ ){
    x = xV[i];
    y = yV[i];
    w = wV[i];
    z = zV[i] - z0;
    S = st->Sy;
    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 A3A3 = A3*A3;
  fvec A3A4 = A3*A4;
  fvec A1A5 = A1*A5;
  fvec A2A5 = A2*A5;
  fvec A4A4 = A4*A4;
  
  fvec det = rcp(-A2*A3A3 + A1*( A3A4+A3A4 - A1A5) + A0*(A2A5-A4A4));
  fvec Ai0 = ( -A4A4 + A2A5 );
  fvec Ai1 = (  A3A4 - A1A5 );
  fvec Ai2 = ( -A3A3 + A0*A5 );
  fvec Ai3 = ( -A2*A3 + A1*A4 );
  fvec Ai4 = (  A1*A3 - A0*A4 );
  fvec Ai5 = ( -A1*A1 + A0*A2 );

  fvec L, L1;
  t.x = (Ai0*a0 + Ai1*a1 + Ai3*a2)*det;
  t.tx = (Ai1*a0 + Ai2*a1 + Ai4*a2)*det;
  fvec txtx1 = 1.+t.tx*t.tx;
  L    = (Ai3*a0 + Ai4*a1 + Ai5*a2)*det *rcp(txtx1);
  L1 = L*t.tx;
  A1 = A1 + A3*L1;
  A2 = A2 + ( A4 + A4 + A5*L1 )*L1;
  b1+= b2 * L1;
  det = rcp(-A1*A1+A0*A2);

  t.y = (  A2*b0 - A1*b1 )*det;
  t.ty = ( -A1*b0 + A0*b1 )*det;
  t.qp = -L*c_light_i*rsqrt(txtx1 +t.ty*t.ty);
  t.z = z0;
}

void L1Algo::FilterFirst( L1TrackPar &track,fvec &x, fvec &y, fvec &w, L1Station &st )
{
  fvec w1 = ONE - w;
  fvec sigma21 = w*st.XYInfo.C00 + w1*vINF;
  fvec sigma22 = w*st.XYInfo.C10;
  fvec sigma23 = w*st.XYInfo.C11 + w1*vINF;
  // initialize covariance matrix 
  track.C00= sigma21; 
  track.C10= sigma22;      track.C11= sigma23;
/*  track.C00= vINF; 
  track.C10= ZERO;      track.C11= vINF;*/
  track.C20= ZERO;         track.C21= ZERO;      track.C22= vINF;
  track.C30= ZERO;         track.C31= ZERO;      track.C32= ZERO;    track.C33= vINF;
  track.C40= ZERO;         track.C41= ZERO;      track.C42= ZERO;    track.C43= ZERO;     track.C44= vINF;

  track.x = w*x + w1*track.x;
  track.y = w*y + w1*track.y;
  track.NDF = -3.0;
  track.chi2 = ZERO;
}

void L1Algo::FilterLast( L1TrackPar &track,fvec &x, fvec &y, fvec &w, L1Station &st )
{
  fvec w1 = ONE - w;
  fvec sigma21 = w*st.XYInfo.C00 + w1*vINF;
  fvec sigma22 = w*st.XYInfo.C10;
  fvec sigma23 = w*st.XYInfo.C11 + w1*vINF;
  // initialize covariance matrix 
  track.C00= sigma21; 
  track.C10= sigma22;      track.C11= sigma23; 
  track.C20= ZERO;         track.C21= ZERO;      track.C22= vINF;
  track.C30= ZERO;         track.C31= ZERO;      track.C32= ZERO;    track.C33= vINF;
  track.C40= ZERO;         track.C41= ZERO;      track.C42= ZERO;    track.C43= ZERO;     track.C44= ONE;

  track.x = w*x + w1*track.x;
  track.y = w*y + w1*track.y;
  track.NDF = -3.0;
  track.chi2 = ZERO;
}


void L1Algo::Filter( L1TrackPar &T, L1UMeasurementInfo &info, fvec &u , fvec &w)
{
  register fvec wi, zeta, zetawi, HCH;
  register fvec F0, F1, F2, F3, F4;
  register fvec K1, K2, K3, K4;

  zeta = info.cos_phi*T.x + info.sin_phi*T.y - u;

  // F = CH'
  F0 = info.cos_phi*T.C00 + info.sin_phi*T.C10;
  F1 = info.cos_phi*T.C10 + info.sin_phi*T.C11;

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

  F2 = info.cos_phi*T.C20 + info.sin_phi*T.C21;
  F3 = info.cos_phi*T.C30 + info.sin_phi*T.C31;
  F4 = info.cos_phi*T.C40 + info.sin_phi*T.C41;

  wi = 1./(info.sigma2 +HCH);

  wi *= w;

  zetawi = zeta *wi;
  T.chi2 +=  zeta * zetawi ;
  T.NDF  += w;

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

  T.x  -= F0*zetawi;
  T.y  -= F1*zetawi;
  T.tx -= F2*zetawi;
  T.ty -= F3*zetawi;
  T.qp -= F4*zetawi;

  T.C00-= F0*F0*wi;
  T.C10-= K1*F0;
  T.C11-= K1*F1;
  T.C20-= K2*F0;
  T.C21-= K2*F1;
  T.C22-= K2*F2;
  T.C30-= K3*F0;
  T.C31-= K3*F1;
  T.C32-= K3*F2;
  T.C33-= K3*F3;
  T.C40-= K4*F0;
  T.C41-= K4*F1;
  T.C42-= K4*F2;
  T.C43-= K4*F3;
  T.C44-= K4*F4;
}
/*
Int_t L1Algo::ExtrapolateRK4
( 
 const fvec T_in [], // input track parameters (x,y,tx,ty,Q/p)
 const fvec C_in [], // input covariance matrix
 fvec       T_out[], // output track parameters
 fvec       C_out[], // output covariance matrix
 fvec       z_out  , // extrapolate to this z position
 fvec       qp0    , // use Q/p linearisation at this value
 FairField       *MF       // magnetic field
 )
{
  //
  // 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 c_light = 0.000299792458;
  
  static fvec a[4] = {0.0, 0.5, 0.5, 1.0};
  static fvec c[4] = {1.0/6.0, 1.0/3.0, 1.0/3.0, 1.0/6.0};
  static fvec b[4] = {0.0, 0.5, 0.5, 1.0};
  
  Int_t 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_in[4];
  fvec z_in  = T_in[5];
  fvec h     = z_out - z_in;
  fvec hC    = h * c_light;
  x0[0] = T_in[0]; x0[1] = T_in[1];
  x0[2] = T_in[2]; x0[3] = T_in[3];
  //
  //   Runge-Kutta step
  //

  Int_t step;
  Int_t i;
  
  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 point[3] = { x[0], x[1], z_in  + a[step] * h };
    fvec B[3];
    if ( MF ) MF->GetFieldValue( point, B );
    else { B[0] = B[1] = B[2] = 0.; }

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

    Ax_tx[step] = Ax[step]*tx*I_tx2ty21 + (ty*B[0]-2.0*tx*B[1])*tx2ty2qp;
    Ax_ty[step] = Ax[step]*ty*I_tx2ty21 + (tx*B[0]+B[2])*tx2ty2qp;
    Ay_tx[step] = Ay[step]*tx*I_tx2ty21 + (-ty*B[1]-B[2])*tx2ty2qp;
    Ay_ty[step] = Ay[step]*ty*I_tx2ty21 + (-tx*B[1]+2.0*ty*B[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

  for(i=0; i < 4; ++i) {
    T_out[i]=x0[i]+c[0]*k[i]+c[1]*k[4+i]+c[2]*k[8+i]+c[3]*k[12+i];
  }
  T_out[4]=T_in[4];
  T_out[5]=z_out;
  //
  //     Derivatives    dx/dqp
  //

  x0[0] = 0.0; x0[1] = 0.0; x0[2] = 0.0; x0[3] = 0.0;

  //
  //   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.0; x0[1] = 0.0; x0[2] = 1.0; x0[3] = 0.0;

  //
  //   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.0;
  J[14] = 0.0;

  //     Derivatives    dx/ty
  //

  x0[0] = 0.0; x0[1] = 0.0; x0[2] = 0.0; x0[3] = 1.0;

  //
  //   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.;

  // extrapolate inverse momentum

  T_out[4] = qp_in;
  
  fvec dqp = qp_in - qp0;
  
  { for( Int_t i=0; i<4; i++ )
    {
      T_out[i]+=J[5*4+i]*dqp;
    }
  }
   
  //          covariance matrix transport 
  
  if ( C_in&&C_out ) CbmKFMath::multQtSQ( 5, J, C_in, C_out);
  return 0;
}    // end of RK4

void L1Algo::multQtSQ( Int_t N, const fvec Q[], const fvec S[], fvec S_out[] )
{
  fvec A[N*N];
  
  for( Int_t i=0, n=0; i<N; i++ ){
    for( Int_t j=0; j<N; j++, ++n ){
      A[n] = 0 ;
      for( Int_t k=0; k<N; ++k ) A[n]+= S[indexS(i,k)] * Q[ k*N+j];
    }
  }
  
  for( Int_t i=0; i<N; i++ ){
    for( Int_t j=0; j<=i; j++ ){
      Int_t n = indexS(i,j);
      S_out[n] = 0 ;
      for( Int_t k=0; k<N; k++ )  S_out[n] += Q[ k*N+i ] * A[ k*N+j ];
    }
  }
}*/