// $Id: AliHLTTPCCATrackParam.cxx,v 1.5 2010/08/26 15:05:52 ikulakov Exp $
// **************************************************************************
// This file is property of and copyright by the ALICE HLT Project          *
// ALICE Experiment at CERN, All rights reserved.                           *
//                                                                          *
// Primary Authors: Sergey Gorbunov <sergey.gorbunov@kip.uni-heidelberg.de> *
//                  Ivan Kisel <kisel@kip.uni-heidelberg.de>                *
//                  for The ALICE HLT Project.                              *
//                                                                          *
// Developed by:   Igor Kulakov <I.Kulakov@gsi.de>                          *
//                 Maksym Zyzak <M.Zyzak@gsi.de>                            *
//                                                                          *
// Permission to use, copy, modify and distribute this software and its     *
// documentation strictly for non-commercial purposes is hereby granted     *
// without fee, provided that the above copyright notice appears in all     *
// copies and that both the copyright notice and this permission notice     *
// appear in the supporting documentation. The authors make no claims       *
// about the suitability of this software for any purpose. It is            *
// provided "as is" without express or implied warranty.                    *
//                                                                          *
//***************************************************************************


#include "AliHLTTPCCATrackParamArBB.h"
#include "AliHLTTPCCATrackLinearisationArBB.h"
#include <iostream>

//
// Circle in XY:
//
// kCLight = 0.000299792458;
// Kappa = Bz*kCLight*QPt;
// R  = 1/CAMath::Abs(Kappa);
// Xc = X - sin(Phi)/Kappa;
// Yc = Y + cos(Phi)/Kappa;
//

using arbb::select;

void AliHLTTPCCATrackParamArBB::GetS( dense<f32> x, dense<f32> y, f32 cBz, dense<f32> &dS )
{
  //* Get XY path length to the given point
  dense<f32> k  = cBz * fP[4] ;
  dense<f32> ex = fSignCosPhi*arbb::sqrt(1.f - fP[2]*fP[2]);
  dense<f32> ey = fP[2];

  x -= fX;
  y -= fP[0];
  dS = select( arbb::abs(k) > 1.e-4, atan2( k * dS, 1.f + k * ( x * ey - y * ex ) ) / k, x * ex + y * ey);
}

void AliHLTTPCCATrackParamArBB::GetDist2( i32 iTrack, i32 jTrack, f32 &Dist2 )
{
  f32 dx = fX[iTrack] - fX[jTrack];
  f32 dy = fP[0][iTrack] - fP[0][jTrack];
  f32 dz = fP[1][iTrack] - fP[1][jTrack];
  Dist2 = dx*dx + dy*dy + dz*dz;
}

void AliHLTTPCCATrackParamArBB::GetDist2( dense<i32> iTrack, dense<i32> jTrack, dense<f32> &Dist2 )
{
  dense<f32> dx = gather(fX,iTrack,0) - gather(fX,jTrack,0);
  dense<f32> dy = gather(fP[0],iTrack,0) - gather(fP[0],jTrack,0);
  dense<f32> dz = gather(fP[1],iTrack,0) - gather(fP[1],jTrack,0);
  Dist2 = dx*dx + dy*dy + dz*dz;
}

void AliHLTTPCCATrackParamArBB::GetDist2( const dense<i32> &iTrack, const AliHLTTPCCATrackParamArBB& a, const dense<i32> &jTrack, dense<f32> &Dist2 )
{
  dense<f32> dx = gather(fX,iTrack,0) - gather(a.fX,jTrack,0);
  dense<f32> dy = gather(fP[0],iTrack,0) - gather(a.fP[0],jTrack,0);
  dense<f32> dz = gather(fP[1],iTrack,0) - gather(a.fP[1],jTrack,0);
  Dist2 = dx*dx + dy*dy + dz*dz;
}

void AliHLTTPCCATrackParamArBB::GetDistXZ2( i32 iTrack, i32 jTrack, f32 &DistXZ2 )
{
  f32 dx = fX[iTrack] - fX[jTrack];
  f32 dz = fP[1][iTrack] - fP[1][jTrack];
  DistXZ2 = dx*dx + dz*dz;
}

void AliHLTTPCCATrackParamArBB::GetDistXZ2( dense<i32> iTrack, dense<i32> jTrack, dense<f32> &DistXZ2 )
{
  dense<f32> dx = gather(fX,iTrack,0) - gather(fX,jTrack,0);
  dense<f32> dz = gather(fP[1],iTrack,0) - gather(fP[1],jTrack,0);
  DistXZ2 = dx*dx + dz*dz;
}

void AliHLTTPCCATrackParamArBB::GetDistXZ2( const dense<i32> &iTrack, const AliHLTTPCCATrackParamArBB& a, const dense<i32> &jTrack, dense<f32> &DistXZ2 )
{
  dense<f32> dx = gather(fX,iTrack,0) - gather(a.fX,jTrack,0);
  dense<f32> dz = gather(fP[1],iTrack,0) - gather(a.fP[1],jTrack,0);
  DistXZ2 = dx*dx + dz*dz;
}

void AliHLTTPCCATrackParamArBB::GetDistXZ2( const dense<i32> &iTrack, const AliHLTTPCCATrackParamArBB& a, const dense<i32> &jTrack,
                                            const dense<f32> &Alpha,dense<f32> &DistXZ2 )
{
  const dense<f32> cA = arbb::cos( Alpha );
  const dense<f32> sA = arbb::sin( Alpha );

  const dense<f32> x = ( gather(a.fX,jTrack,0)*cA + gather(a.fP[0],jTrack,0)*sA );
  
  dense<f32> dx = gather(fX,iTrack,0) - x;
  dense<f32> dz = gather(fP[1],iTrack,0) - gather(a.fP[1],jTrack,0);
  DistXZ2 = dx*dx + dz*dz;  
}

void AliHLTTPCCATrackParamArBB::GetDCAPoint( dense<f32> x, dense<f32> y, dense<f32> z, dense<f32> &xp, dense<f32> &yp, dense<f32> &zp, f32 cBz )
{
  //* Get the track point closest to the (x,y,z)

  const f32 TwoPi = 6.283185307179586f;

  dense<f32> x0 = fX;
  dense<f32> y0 = fP[2];
  dense<f32> k  = cBz*fP[4] ;
  dense<f32> ex = fSignCosPhi*arbb::sqrt(1.f - fP[2]*fP[2]);
  dense<f32> ey = fP[2];
  dense<f32> dx = x - x0;
  dense<f32> dy = y - y0;
  dense<f32> ax = dx * k + ey;
  dense<f32> ay = dy * k - ex;
  dense<f32> a = arbb::sqrt( ax * ax + ay * ay );
  xp = x0 + ( dx - ey * ( ( dx * dx + dy * dy ) * k - 2 * ( -dx * ey + dy * ex ) ) / ( a + 1 ) ) / a;
  yp = y0 + ( dy + ex * ( ( dx * dx + dy * dy ) * k - 2 * ( -dx * ey + dy * ex ) ) / ( a + 1 ) ) / a;
  dense<f32> s;
  GetS( x, y, cBz, s );
  zp = fP[1] + fP[3] * s;
  dense<f32> dZ = select( arbb::abs(k) > 1.e-2, fP[3]* TwoPi / k , 0.f );
  zp = select(dZ > .1f, zp + arbb::round( ( z - zp ) / dZ ) * dZ, zp); // TODO improve (correct) the rounding with i32
}

//*
//*  Multiple scattering and energy losses
//*

void AliHLTTPCCATrackParamArBB::ApproximateBetheBloch( dense<f32> beta2, dense<f32> &approximation)
{
  //------------------------------------------------------------------
  // This is an approximation of the Bethe-Bloch formula with
  // the density effect taken i32o account at beta*gamma > 3.5
  // (the approximation is reasonable only for solid materials)
  //------------------------------------------------------------------

  const dense<f32> beta2_beta21i = beta2 / ( 1.f - beta2 );
  approximation = select( beta2_beta21i > 12.25,
                          0.153e-3 / beta2 * ( 9.94223 + 0.5 * arbb::log( beta2_beta21i ) - beta2 ),
                          0.153e-3 / beta2 * ( 8.6895 + arbb::log( beta2_beta21i ) - beta2 ));
  approximation = select( beta2 < 1.f, approximation, 0.f);
}

void AliHLTTPCCATrackParamArBB::BetheBlochGeant( dense<f32> bg2, dense<f32> &approximation,
                                                 f32 kp0, f32 kp1, f32 kp2, f32 kp3, f32 kp4)
{
  //
  // This is the parameterization of the Bethe-Bloch formula inspired by Geant.
  //
  // bg2  - (beta*gamma)^2
  // kp0 - density [g/cm^3]
  // kp1 - density effect first junction point
  // kp2 - density effect second junction point
  // kp3 - mean excitation energy [GeV]
  // kp4 - mean Z/A
  //
  // The default values for the kp* parameters are for silicon.
  // The returned value is in [GeV/(g/cm^2)].
  //

  const f32 mK  = 0.307075e-3; // [GeV*cm^2/g]
  const f32 me  = 0.511e-3;    // [GeV/c^2]
  const f32 rho = kp0;
  const f32 x0  = kp1 * 2.303;
  const f32 x1  = kp2 * 2.303;
  const f32 mI  = kp3;
  const f32 mZA = kp4;
  const dense<f32> maxT = 2.f * me * bg2;    // neglecting the electron mass

  //*** Density effect
  const dense<f32> x = 0.5f * arbb::log( bg2 );
  const f32 lhwI = arbb::log( 28.816 * 1e-9 * arbb::sqrt( rho * mZA ) / mI );
  dense<f32> r = ( x1 - x ) / ( x1 - x0 );
  dense<f32> d2 = select(x > x0, lhwI + x - 0.5 + ( 0.5 - lhwI - x0 ) * r * r * r, 0.f); // x1 > x0 !!!
  d2 = select(x > x1, lhwI + x - 0.5, d2);

  approximation = mK*mZA*( 1 + bg2 ) / bg2*( 0.5f * arbb::log( 2.f*me*bg2*maxT / ( mI*mI ) ) - bg2 / ( 1 + bg2 ) - d2 );
}

void AliHLTTPCCATrackParamArBB::BetheBlochSolid( dense<f32> bg, dense<f32> &approximation )
{
  //------------------------------------------------------------------
  // This is an approximation of the Bethe-Bloch formula,
  // reasonable for solid materials.
  // All the parameters are, in fact, for Si.
  // The returned value is in [GeV]
  //------------------------------------------------------------------
  BetheBlochGeant( bg, approximation );
}

void AliHLTTPCCATrackParamArBB::BetheBlochGas( dense<f32> bg, dense<f32> &approximation )
{
  //------------------------------------------------------------------
  // This is an approximation of the Bethe-Bloch formula,
  // reasonable for gas materials.
  // All the parameters are, in fact, for Ne.
  // The returned value is in [GeV]
  //------------------------------------------------------------------

  const f32 rho = 0.9e-3;
  const f32 x0  = 2.;
  const f32 x1  = 4.;
  const f32 mI  = 140.e-9;
  const f32 mZA = 0.49555;
  BetheBlochGeant( bg, approximation, rho, x0, x1, mI, mZA );
}

void AliHLTTPCCATrackParamArBB::CalculateFitParameters( AliHLTTPCCATrackFitParamArBB &par, f32 mass)
{
  const dense<f32> p2 = ( 1.f + fP[3] * fP[3] );
  const dense<f32> k2 = fP[4] * fP[4];
  const f32 mass2 = mass * mass;
  const dense<f32> beta2 = p2 / ( p2 + mass2 * k2 );

  const dense<f32> pp2 = select( k2 > 1.e-8f , p2 / k2 , 10000.f); // impuls 2

    //par.fBethe = BetheBlochGas( pp2/mass2);
  dense<f32> pm = pp2 / mass2;
  ApproximateBetheBloch( pm, par.fBethe );
  par.fE = arbb::sqrt( pp2 + mass2 );
  par.fTheta2 = 198.81e-6f / ( beta2 * pp2 ); // 14.1^2 * 1e-6 
  par.fEP2 = par.fE / pp2;  // have tried reduce number of "/", but it was slower. (may be bacause of additional of constants = memory)
  // Approximate energy loss fluctuation (M.Ivanov)

  const f32 knst = 0.07; // To be tuned.
  par.fSigmadE2 = knst * par.fEP2 * fP[4];
  par.fSigmadE2 = par.fSigmadE2 * par.fSigmadE2;

  par.fK22 = p2;
  par.fK33 = par.fK22 * par.fK22;
  par.fK43 = fP[3] * fP[4] * par.fK22;
  par.fK44 = (p2 - 1.f) * k2;
}

void AliHLTTPCCATrackParamArBB::CorrectForMeanMaterial( dense<boolean> &mask, dense<f32> xOverX0, dense<f32> xTimesRho, AliHLTTPCCATrackFitParamArBB par )
{
  //------------------------------------------------------------------
  // This function corrects the track parameters for the crossed material.
  // "xOverX0"   - X/X0, the thickness in units of the radiation length.
  // "xTimesRho" - is the product length*density (g/cm^2).
  //------------------------------------------------------------------
  //Energy losses************************

  const dense<f32> dE = par.fBethe * xTimesRho;
  const dense<f32> corr = ( 1.f - par.fEP2 * dE );

//  if ( CAMath::Abs( dE ) > 0.3 * par.fE ) return 0; //30% energy loss is too much!
//  if ( corr < 0.3 || corr > 1.3 ) return 0;

  dense<boolean> mask_loc = mask && (!( arbb::abs( dE ) > 0.3f * par.fE ) && !( corr < 0.3f || corr > 1.3f ));

  _if(any(mask_loc))
  {
    fP[4] = select( mask_loc, fP[4]*corr, fP[4] );
    fC[10] = select( mask_loc, fC[10]*corr, fC[10] );
    fC[11] = select( mask_loc, fC[11]*corr, fC[11] );
    fC[12] = select( mask_loc, fC[12]*corr, fC[12] );
    fC[13] = select( mask_loc, fC[13]*corr, fC[13] );
    fC[14] = select( mask_loc, fC[14]*corr, fC[14] );
    fC[14] = select( mask_loc, fC[14] + par.fSigmadE2 * arbb::abs( dE ), fC[14] );

  //Multiple scattering******************

    const dense<f32> theta2 = par.fTheta2 * arbb::abs( xOverX0 );
    fC[5] = select( mask_loc, fC[5] + theta2 * par.fK22 * ( 1.f - fP[2] * fP[2] ), fC[5] );
    fC[9] = select( mask_loc, fC[9] + theta2 * par.fK33, fC[9] );
    fC[13] = select( mask_loc, fC[13] + theta2 * par.fK43, fC[13] );
    fC[14] = select( mask_loc, fC[14] + theta2 * par.fK44, fC[14] );
  }_end_if
}

//*
//* Transport routines
//*

void AliHLTTPCCATrackParamArBB::TransportToX( dense<boolean> &mask, dense<f32> x, AliHLTTPCCATrackLinearisationArBB &t0, f32 cBz, dense<f32> &DL, f32 maxSinPhi)
{
  //* Transport the track parameters to X=x, using linearization at t0, and the field value Bz
  //* maxSinPhi is the max. allowed value for |t0.SinPhi()|
  //* linearisation of trajectory t0 is also transported to X=x,
  //* returns 1 if OK
  //*

  const dense<f32> ex = t0.fCosPhi;
  const dense<f32> ey = t0.fSinPhi;
  const dense<f32> k  = t0.fQPt * cBz;
  const dense<f32> dx = x - fX;

  const dense<f32> ey1 = k * dx + ey;

  // check for intersection with X=x

  mask = mask && ( arbb::abs( ey1 ) < maxSinPhi );

  dense<f32> ex1 = arbb::sqrt( 1.f - ey1 * ey1 );
  ex1 = select(ex < 0.f, -ex1, ex1);

  const dense<f32> dx2 = dx * dx;
  const dense<f32> ss = ey + ey1;
  const dense<f32> cc = ex + ex1;

  mask = mask && ( ( arbb::abs(cc) > 1.e-4 && arbb::abs(ex) > 1.e-4 && arbb::abs(ex1) > 1.e-4 ) );

  _if(any(mask))
  {
    const dense<f32> cci = 1.f / cc;
    const dense<f32> exi = 1.f / ex;
    const dense<f32> ex1i = 1.f / ex1;
  
    const dense<f32> tg = ss * cci; // tan((phi1+phi)/2)

    const dense<f32> dy = dx * tg;
    dense<f32> dl = dx * arbb::sqrt( 1.f + tg * tg );

    dl = select(cc < 0.f, -dl, dl);
    dense<f32> dSin = dl * k * 0.5;
    dSin = select( dSin > 1.f, 1.f, dSin);
    dSin = select( dSin < -1.f, -1.f, dSin);
    const dense<f32> dS = select( arbb::abs( k ) > 1.e-4 , ( 2.f * arbb::asin( dSin ) / k ) , dl);
    const dense<f32> dz = dS * t0.fDzDs;

    DL = -dS * arbb::sqrt( 1.f + t0.fDzDs * t0.fDzDs );

    const dense<f32> d0 =  fP[2] - t0.fSinPhi;
    const dense<f32> d1 =  fP[3] - t0.fDzDs;
    const dense<f32> d2 =  fP[4] - t0.fQPt ;

    //f32 H0[5] = { 1,0, h2,  0, h4 };
    //f32 H1[5] = { 0, 1, 0, dS,  0 };
    //f32 H2[5] = { 0, 0, 1,  0, dxBz };
    //f32 H3[5] = { 0, 0, 0,  1,  0 };
    //f32 H4[5] = { 0, 0, 0,  0,  1 };

    const dense<f32> h2 = dx * ( 1.f + ey * ey1 + ex * ex1 ) * exi * ex1i * cci;
    const dense<f32> h4 = dx2 * ( cc + ss * ey1 * ex1i ) * cci * cci * cBz;
    const dense<f32> dxBz = dx * cBz;

    t0.fCosPhi = select(mask, ex1, t0.fCosPhi);
    t0.fSinPhi = select(mask, ey1, t0.fSinPhi);

    fX    = select(mask, fX + dx, fX);
    fP[0] = select(mask, fP[0] + dy + h2 * d0 + h4 * d2,fP[0]);
    fP[1] = select(mask, fP[1] + dz + dS * d1, fP[1]);
    fP[2] = select(mask, t0.fSinPhi + d0 + dxBz * d2, fP[2]);
    fP[2] = select((arbb::abs(fP[2]) > maxSinPhi) && mask,t0.fSinPhi,fP[2]);

    const dense<f32> c00 = fC[0];
    const dense<f32> c10 = fC[1];
    const dense<f32> c11 = fC[2];
    const dense<f32> c20 = fC[3];
    const dense<f32> c21 = fC[4];
    const dense<f32> c22 = fC[5];
    const dense<f32> c30 = fC[6];
    const dense<f32> c31 = fC[7];
    const dense<f32> c32 = fC[8];
    const dense<f32> c33 = fC[9];
    const dense<f32> c40 = fC[10];
    const dense<f32> c41 = fC[11];
    const dense<f32> c42 = fC[12];
    const dense<f32> c43 = fC[13];
    const dense<f32> c44 = fC[14];

    fC[0] = select(mask, c00  + h2 * h2 * c22 + h4 * h4 * c44 + 2.f * ( h2 * c20 + h4 * c40 + h2 * h4 * c42 ), fC[0] );

    fC[1] = select(mask, c10 + h2 * c21 + h4 * c41 + dS * ( c30 + h2 * c32 + h4 * c43 ), fC[1]);
    fC[2] = select(mask, c11 + 2.f * dS * c31 + dS * dS * c33, fC[2]);

    fC[3] = select(mask, c20 + h2 * c22 + h4 * c42 + dxBz * ( c40 + h2 * c42 + h4 * c44 ), fC[3]);
    fC[4] = select(mask, c21 + dS * c32 + dxBz * ( c41 + dS * c43 ), fC[4]);
    fC[5] = select(mask, c22 + 2.f * dxBz * c42 + dxBz * dxBz * c44, fC[5]);

    fC[6] = select(mask, c30 + h2 * c32 + h4 * c43, fC[6]);
    fC[7] = select(mask, c31 + dS * c33, fC[7]);
    fC[8] = select(mask, c32 + dxBz * c43, fC[8]);
//    fC[9] = c33;

    fC[10] = select(mask, c40 + h2 * c42 + h4 * c44, fC[10]);
    fC[11] = select(mask, c41 + dS * c43, fC[11]);
    fC[12] = select(mask, c42 + dxBz * c44, fC[12]);
//    fC[13] = c43;
//    fC[14] = c44;
  }_end_if
}

void AliHLTTPCCATrackParamArBB::TransportToX( dense<boolean> &mask, dense<f32> x, dense<f32> sinPhi0, dense<f32> cosPhi0,  f32 cBz, f32 maxSinPhi)
{
  //* Transport the track parameters to X=x, using linearization at phi0 with 0 curvature,
  //* and the field value Bz
  //* maxSinPhi is the max. allowed value for |t0.SinPhi()|
  //* linearisation of trajectory t0 is also transported to X=x,
  //* returns 1 if OK
  //*

  const dense<f32> dx = x - fX;

  mask = mask && ( arbb::abs( cosPhi0 ) > 1.e-4 );
  const dense<f32> exi = 1. / cosPhi0;
  const dense<f32> dxBz = dx * cBz;
  const dense<f32> dS = dx * exi;
  const dense<f32> h2 = dS * exi * exi;
  const dense<f32> h4 = .5 * h2 * dxBz;

  //const float H0[5] = { 1,0, h2,  0, h4 };
  //const float H1[5] = { 0, 1, 0, dS,  0 };
  //const float H2[5] = { 0, 0, 1,  0, dxBz };
  //const float H3[5] = { 0, 0, 0,  1,  0 };
  //const float H4[5] = { 0, 0, 0,  0,  1 };

  const dense<f32> sinPhi = fP[2] + dxBz * fP[4];
  mask = mask && (arbb::abs( sinPhi ) < arbb::abs(maxSinPhi));
  
  _if(any(mask))
  {
    fX    = select(mask, fX + dx, fX);
    fP[0] =  select(mask, fP[0] + dS * sinPhi0 + h2 * ( fP[2] - sinPhi0 )  +   h4 * fP[4], fP[0]);
    fP[1] =  select(mask, fP[1] + dS * fP[3], fP[1]);
    fP[2] =  select(mask, sinPhi, fP[2]);

    const dense<f32> c00 = fC[0];
    const dense<f32> c10 = fC[1];
    const dense<f32> c11 = fC[2];
    const dense<f32> c20 = fC[3];
    const dense<f32> c21 = fC[4];
    const dense<f32> c22 = fC[5];
    const dense<f32> c30 = fC[6];
    const dense<f32> c31 = fC[7];
    const dense<f32> c32 = fC[8];
    const dense<f32> c33 = fC[9];
    const dense<f32> c40 = fC[10];
    const dense<f32> c41 = fC[11];
    const dense<f32> c42 = fC[12];
    const dense<f32> c43 = fC[13];
    const dense<f32> c44 = fC[14];

    fC[0] =  select(mask, c00  + h2 * h2 * c22 + h4 * h4 * c44 + 2.f * ( h2 * c20 + h4 * c40 + h2 * h4 * c42 ), fC[0]);

    fC[1] =  select(mask, c10 + h2 * c21 + h4 * c41 + dS * ( c30 + h2 * c32 + h4 * c43 ), fC[1]);
    fC[2] =  select(mask, c11 + 2.f * dS * c31 + dS * dS * c33, fC[2]);

    fC[3] =  select(mask, c20 + h2 * c22 + h4 * c42 + dxBz * ( c40 + h2 * c42 + h4 * c44 ), fC[3]);
    fC[4] =  select(mask, c21 + dS * c32 + dxBz * ( c41 + dS * c43 ), fC[4]);
    fC[5] =  select(mask, c22 + 2.f * dxBz * c42 + dxBz * dxBz * c44, fC[5]);

    fC[6] =  select(mask, c30 + h2 * c32 + h4 * c43, fC[6]);
    fC[7] =  select(mask, c31 + dS * c33, fC[7]);
    fC[8] =  select(mask, c32 + dxBz * c43, fC[8]);
//    fC[9] = c33;

    fC[10] =  select(mask, c40 + h2 * c42 + h4 * c44, fC[10]);
    fC[11] =  select(mask, c41 + dS * c43, fC[11]);
    fC[12] =  select(mask, c42 + dxBz * c44, fC[12]);
//    fC[13] = c43;
//    fC[14] = c44;
  }_end_if
}

void AliHLTTPCCATrackParamArBB::TransportToX( dense<boolean> &mask, dense<f32> x, f32 cBz, f32 maxSinPhi)
{
  AliHLTTPCCATrackLinearisationArBB t0( *this );
  dense<f32> DL;
  TransportToX( mask, x, t0, cBz, DL, maxSinPhi );
}


void AliHLTTPCCATrackParamArBB::TransportToX( dense<boolean> &mask, const dense<f32> &x, const dense<f32> &sinPhi0, f32 Bz, f32 maxSinPhi)
{ // supposed to be used only when C has zeros!!!!!!!
  //* Transport the track parameters to X=x, using linearization at phi0 with 0 curvature,
  //* and the field value Bz
  //* maxSinPhi is the max. allowed value for |t0.SinPhi()|
  //* linearisation of trajectory t0 is also transported to X=x,
  //* returns 1 if OK
  //*
  const usize taskSize = mask.length();

  const dense<f32> ey = sinPhi0;
  const dense<f32> dx = x - X();
  const dense<f32> exi = rsqrt( 1 - ey * ey ); // RSqrt

  const dense<f32> dxBz = dx * Bz;
  const dense<f32> dS = dx * exi;
  const dense<f32> h2 = dS * exi * exi;
  const dense<f32> h4 = .5f * h2 * dxBz;

///mvz start 23.01.2010
//  const dense<f32> sinPhi = SinPhi() * (sfloat_v( Vc::One ) - 0.5f * dxBz * QPt() *dxBz * QPt()/ ( sfloat_v( Vc::One ) - SinPhi()*SinPhi() )) + dxBz * QPt();
  const dense<f32> sinPhi = SinPhi() + dxBz * QPt();
///mvz end 23.01.2010

  mask = mask && arbb::abs( exi ) <= 1.e4f;
  mask = mask && ( (arbb::abs( sinPhi ) <= maxSinPhi) || (maxSinPhi <= 0.f) );


  fX   = select( mask, fX + dx, fX );
  SetPar( 0, fP[0] + dS * ey + h2 * ( SinPhi() - ey )  +   h4 * QPt(), mask);
  SetPar( 1, fP[1] + dS * DzDs(), mask );
  SetPar( 2, sinPhi, mask );


  const dense<f32> c00 = fC[0];
  const dense<f32> c11 = fC[2];
  const dense<f32> c20 = fC[3];
  //const dense<f32> c21 = fC[4];
  const dense<f32> c22 = fC[5];
  //const dense<f32> c30 = fC[6];
  const dense<f32> c31 = fC[7];
  //const dense<f32> c32 = fC[8];
  const dense<f32> c33 = fC[9];
  const dense<f32> c40 = fC[10];
  //const dense<f32> c41 = fC[11];
  const dense<f32> c42 = fC[12];
  //const dense<f32> c43 = fC[13];
  const dense<f32> c44 = fC[14];

  const dense<f32> two = fill( 2.f, taskSize );

  SetCov( 0, fC[0] + h2 * h2 * c22 + h4 * h4 * c44
                   + two * ( h2 * c20 + h4 * ( c40 + h2 * c42 ) ), mask );


  //fC[1] ( mask ) += h2 * c21 + h4 * c41 + dS * ( c30 + h2 * c32 + h4 * c43 );
  SetCov( 2, fC[2] + dS * ( two * c31 + dS * c33 ), mask );
  COUT_ARBB_ALL( "cout - par 2 ", fC[2] );
  COUT_ARBB_ALL( "cout - dS ", dS );
  COUT_ARBB_ALL( "cout - c31 ", c31 );
  COUT_ARBB_ALL( "cout - c33 ", c33 );
  
  SetCov( 3, fC[3] + h2 * c22 + h4 * c42 + dxBz * ( c40 + h2 * c42 + h4 * c44 ), mask );
  
  //fC[4] ( mask ) += dS * c32 + dxBz * ( c41 + dS * c43 );
  const dense<f32> dxBz_c44 = dxBz * c44;
  SetCov( 12, fC[12] + dxBz_c44, mask );
  SetCov( 5, fC[5] + dxBz * ( two * c42 + dxBz_c44 ), mask );
  
  //fC[6] ( mask ) += h2 * c32 + h4 * c43;
  SetCov( 7, fC[7] + dS * c33, mask );
  //fC[8] ( mask ) += dxBz * c43;
  //fC[9] ( mask ) = c33;

  SetCov( 10, fC[10] + h2 * c42 + h4 * c44, mask );
  //fC[11]( mask ) += dS * c43;
  //fC[13]( mask ) = c43;
  //fC[14]( mask ) = c44;

//  return mask;
}


void AliHLTTPCCATrackParamArBB::TransportToXWithMaterial( dense<boolean> &mask, dense<f32> x, AliHLTTPCCATrackLinearisationArBB &t0,
                                                          AliHLTTPCCATrackFitParamArBB &par, f32 cBz, f32 maxSinPhi)
{
  //* Transport the track parameters to X=x  taking i32o account material budget

  const f32 kRho = 1.54e-3f;//1.025e-3 ;//0.9e-3;
//  const f32 kRadLen = 29.532;//28.94;
  //const f32 kRhoOverRadLen = kRho / kRadLen;
  const f32 kRhoOverRadLen = 7.68e-5f;
  dense<f32> dl(x.length());
  TransportToX( mask, x, t0, cBz, dl, maxSinPhi );
  CorrectForMeanMaterial( mask, dl*kRhoOverRadLen, dl*kRho, par );
}

void AliHLTTPCCATrackParamArBB::TransportToXWithMaterial( dense<boolean> &mask, dense<f32> x, AliHLTTPCCATrackFitParamArBB &par, f32 cBz, f32 maxSinPhi)
{
  AliHLTTPCCATrackLinearisationArBB t0( *this );
  TransportToXWithMaterial( mask, x, t0, par, cBz, maxSinPhi );
}

void AliHLTTPCCATrackParamArBB::TransportToXWithMaterial( dense<boolean> &mask, dense<f32> x, f32 cBz, f32 maxSinPhi)
{
  //* Transport the track parameters to X=x taking into account material budget

  AliHLTTPCCATrackFitParamArBB par;
  CalculateFitParameters( par );
  TransportToXWithMaterial( mask, x, par, cBz, maxSinPhi );
}

//*
//* Rotation
//*

void AliHLTTPCCATrackParamArBB::RotateXY( dense<f32> alpha, dense<f32> &x, dense<f32> &y, dense<f32> &sin )
{
  const dense<f32> cA = arbb::cos( alpha );
  const dense<f32> sA = arbb::sin( alpha );

  x = ( fX*cA + fP[0]*sA );
  y = (-fX*sA + fP[0]*cA );
  sin = -fSignCosPhi*arbb::sqrt(1.f - fP[2]*fP[2]) * sA + fP[2] * cA;
}

void AliHLTTPCCATrackParamArBB::RotateXY( dense<f32> alpha, dense<f32> &x, dense<f32> &y, dense<f32> &sin, const i32 &iTr )
{
  const dense<f32> cA = arbb::cos( alpha );
  const dense<f32> sA = arbb::sin( alpha );

  x = ( fX[iTr]*cA + fP[0][iTr]*sA );
  y = (-fX[iTr]*sA + fP[0][iTr]*cA );
  sin = -fSignCosPhi[iTr]*arbb::sqrt(1.f - fP[2][iTr]*fP[2][iTr]) * sA + fP[2][iTr] * cA;
}

void AliHLTTPCCATrackParamArBB::Rotate( dense<boolean> &mask, dense<f32> alpha, f32 maxSinPhi )
{
  //* Rotate the coordinate system in XY on the angle alpha

  const dense<f32> cA = arbb::cos( alpha );
  const dense<f32> sA = arbb::sin( alpha );
  const dense<f32> x = fX, y = fP[0], sP = fP[2], cP = fSignCosPhi*arbb::sqrt(1.f - fP[2]*fP[2]);
  const dense<f32> cosPhi = cP * cA + sP * sA;
  const dense<f32> sinPhi = -cP * sA + sP * cA;

  mask = mask && (arbb::abs( sinPhi ) < maxSinPhi) && arbb::abs( cosPhi ) > 1.e-2f && arbb::abs( cP ) > 1.e-2;

  _if(any(mask))
  {
    const dense<f32> j0 = cP / cosPhi;
    const dense<f32> j2 = cosPhi / cP;

    fX    = select(mask, x*cA + y*sA, fX );
    fP[0] = select(mask, -x*sA + y*cA, fP[0]);
    fSignCosPhi = select(mask, arbb::abs(cosPhi)/cosPhi, fSignCosPhi);
    fP[2] = select(mask, sinPhi, fP[2]);

    //float J[5][5] = { { j0, 0, 0,  0,  0 }, // Y
    //                      {  0, 1, 0,  0,  0 }, // Z
    //                      {  0, 0, j2, 0,  0 }, // SinPhi
    //                    {  0, 0, 0,  1,  0 }, // DzDs
    //                    {  0, 0, 0,  0,  1 } }; // Kappa
    fC[0] = select(mask, fC[0] * j0 * j0, fC[0]);
    fC[1] = select(mask, fC[1]*j0, fC[1]);
    fC[3] = select(mask, fC[3]*j0, fC[3]);
    fC[6] = select(mask, fC[6]*j0, fC[6]);
    fC[10]= select(mask, fC[10]*j0, fC[10]);

    fC[3] = select(mask, fC[3]*j2, fC[3]);
    fC[4] = select(mask, fC[4]*j2, fC[4]);
    fC[5] = select(mask, fC[5]*j2 * j2, fC[5]);
    fC[8] = select(mask, fC[8]*j2, fC[8]);
    fC[12]= select(mask, fC[12]*j2, fC[12]);
  }_end_if
}

void AliHLTTPCCATrackParamArBB::Rotate( dense<boolean> &mask, dense<f32> alpha, AliHLTTPCCATrackLinearisationArBB &t0, f32 maxSinPhi )
{
  //* Rotate the coordinate system in XY on the angle alpha

  const dense<f32> cA = arbb::cos( alpha );
  const dense<f32> sA = arbb::sin( alpha );
  const dense<f32> x0 = fX, y0 = fP[0], sP = t0.fSinPhi, cP = t0.fCosPhi;
  const dense<f32> cosPhi = cP * cA + sP * sA;
  const dense<f32> sinPhi = -cP * sA + sP * cA;

  mask = mask && (arbb::abs( sinPhi ) < maxSinPhi) && arbb::abs( cosPhi ) > 1.e-2f && arbb::abs( cP ) > 1.e-2;

  //float J[5][5] = { { j0, 0, 0,  0,  0 }, // Y
  //                    {  0, 1, 0,  0,  0 }, // Z
  //                    {  0, 0, j2, 0,  0 }, // SinPhi
  //                  {  0, 0, 0,  1,  0 }, // DzDs
  //                  {  0, 0, 0,  0,  1 } }; // Kappa
  _if(any(mask))
  {
    const dense<f32> j0 = cP / cosPhi;
    const dense<f32> j2 = cosPhi / cP;
    const dense<f32> d = fP[2] - sP;

    fX =    select(mask, x0*cA + y0*sA, fX);
    fP[0] = select(mask, -x0*sA + y0*cA, fP[0]);
    t0.fCosPhi = select(mask, cosPhi, t0.fCosPhi);
    t0.fSinPhi = select(mask, sinPhi, t0.fSinPhi);

    fP[2] = select(mask, sinPhi + j2*d, fP[2]);

    fC[0] = select(mask, fC[0]*j0 * j0, fC[0]);
    fC[1] = select(mask, fC[1]*j0, fC[1]);
    fC[3] = select(mask, fC[3]*j0, fC[3]);
    fC[6] = select(mask, fC[6]*j0, fC[6]);
    fC[10]= select(mask, fC[10]*j0, fC[10]);

    fC[3] = select(mask, fC[3]*j2, fC[3]);
    fC[4] = select(mask, fC[4]*j2, fC[4]);
    fC[5] = select(mask, fC[5]*j2 * j2, fC[5]);
    fC[8] = select(mask, fC[8]*j2, fC[8]);
    fC[12]= select(mask, fC[12]*j2, fC[12]);
  }_end_if
}

//*
//* Filtration
//*

void AliHLTTPCCATrackParamArBB::Filter( dense<boolean> &mask, const dense<f32> y, const dense<f32> z, const dense<f32> err2Y_, const dense<f32> err2Z_, f32 maxSinPhi )
{
  const dense<f32> c00 = fC[0];
  const dense<f32> c10 = fC[1];
  const dense<f32> c11 = fC[2];
  const dense<f32> c20 = fC[3];
  const dense<f32> c21 = fC[4];
//  f32 c22 = fC[5];
  const dense<f32> c30 = fC[6];
  const dense<f32> c31 = fC[7];
//  f32 c32 = fC[8];
//  f32 c33 = fC[9];
  const dense<f32> c40 = fC[10];
  const dense<f32> c41 = fC[11];
//  f32 c42 = fC[12];
//  f32 c43 = fC[13];
//  f32 c44 = fC[14];

  dense<f32> err2Y = err2Y_;
  dense<f32> err2Z = err2Z_;
//  COUT_ARBB_ALL( " err2Z ", err2Z );
  dense<f32> d = 1.f / ( err2Y*err2Z + err2Y*c11 + err2Z*c00 + c00*c11 - c10*c10 );
  err2Y += c00;
  err2Z += c11;
//  COUT_ARBB_ALL( " err2Z ", err2Z );
  const dense<f32> z0 = y - fP[0];
  const dense<f32> z1 = z - fP[1];
//COUT_ARBB_ALL( "cout z0 ", z0 );
//COUT_ARBB_ALL( "cout z1 ", z1 );

  mask = mask && ( err2Y > 1.e-8f ) && ( err2Z > 1.e-8f );

//  _if( any(mask) )
  {
    const dense<f32> mS0 = err2Z*d;
    const dense<f32> mS1 = -c10*d;
    const dense<f32> mS2 = err2Y*d;

    // K = CHtS

    const dense<f32>
    k00 = c00 * mS0 + c10*mS1,   k01 = c00 * mS1 + c10*mS2,
    k10 = c10 * mS0 + c11*mS1,   k11 = c10 * mS1 + c11*mS2,
    k20 = c20 * mS0 + c21*mS1,   k21 = c20 * mS1 + c21*mS2,
    k30 = c30 * mS0 + c31*mS1,   k31 = c30 * mS1 + c31*mS2,
    k40 = c40 * mS0 + c41*mS1,   k41 = c40 * mS1 + c41*mS2; 

    const dense<f32> sinPhi = fP[2] + k20 * z0 + k21 * z1;

    mask = mask && ( arbb::abs( sinPhi ) < maxSinPhi );

//    _if( any(mask) )
    {
      fNDF  = select(mask, fNDF + 2, fNDF);
      fChi2 = select(mask, fChi2 + mS0 * z0 * z0 + mS2 * z1 * z1 + 2.f * z0 * z1 * mS1, fChi2);

      fP[ 0] = select(mask, fP[0] + k00 * z0 + k01 * z1, fP[0]);
      fP[ 1] = select(mask, fP[1] + k10 * z0 + k11 * z1, fP[1]);
      fP[ 2] = select(mask, sinPhi, fP[2]);
      fP[ 3] = select(mask, fP[3] + k30 * z0 + k31 * z1, fP[3]);
      fP[ 4] = select(mask, fP[4] + k40 * z0 + k41 * z1, fP[4]);

      fC[ 0] = select(mask, fC[ 0] - (k00 * c00 + k01 * c10), fC[0]); //c00
  
      fC[ 1] = select(mask, fC[ 1] - (k10 * c00 + k11 * c10), fC[1]); //c10
      fC[ 2] = select(mask, fC[ 2] - (k10 * c10 + k11 * c11), fC[2]); //c11
  
      fC[ 3] = select(mask, fC[ 3] - (k20 * c00 + k21 * c10), fC[3]); //c20
      fC[ 4] = select(mask, fC[ 4] - (k20 * c10 + k21 * c11), fC[4]); //c21
      fC[ 5] = select(mask, fC[ 5] - (k20 * c20 + k21 * c21), fC[5]); //c22
  
      fC[ 6] = select(mask, fC[ 6] - (k30 * c00 + k31 * c10), fC[6]); //c30
      fC[ 7] = select(mask, fC[ 7] - (k30 * c10 + k31 * c11), fC[7]); //c31
      fC[ 8] = select(mask, fC[ 8] - (k30 * c20 + k31 * c21), fC[8]); //c32
      fC[ 9] = select(mask, fC[ 9] - (k30 * c30 + k31 * c31), fC[9]); //c33

      fC[10] = select(mask, fC[10] - (k40 * c00 + k41 * c10), fC[10]); //c40
      fC[11] = select(mask, fC[11] - (k40 * c10 + k41 * c11), fC[11]); //c41
      fC[12] = select(mask, fC[12] - (k40 * c20 + k41 * c21), fC[12]); //c42
      fC[13] = select(mask, fC[13] - (k40 * c30 + k41 * c31), fC[13]); //c43
      fC[14] = select(mask, fC[14] - (k40 * c40 + k41 * c41), fC[14]); //c44
    }//_end_if
  }//_end_if
}

/*bool AliHLTTPCCATrackParam::Filter( float y, float z, float err2Y, float err2Z, float maxSinPhi )
{
  assert( maxSinPhi > 0.f );
  /// Add the y,z measurement with the Kalman filter

  float
  c00 = fC[ 0],
  c11 = fC[ 2],
  c20 = fC[ 3],
  c31 = fC[ 7],
  c40 = fC[10];

  err2Y += c00;
  err2Z += c11;

  float
  z0 = y - fP[0],
  z1 = z - fP[1];

  if ( ISUNLIKELY( err2Y < 1.e-8f ) || ISUNLIKELY( err2Z < 1.e-8f ) ) return 0;

  float mS0 = 1.f / err2Y;
  float mS2 = 1.f / err2Z;

  // K = CHtS

  float k00, k11, k20, k31, k40;

  k00 = c00 * mS0;
  k20 = c20 * mS0;
  k40 = c40 * mS0;

  k11 = c11 * mS2;
  k31 = c31 * mS2;

  float sinPhi = fP[2] + k20 * z0  ;

  if ( ISUNLIKELY( CAMath::Abs( sinPhi ) >= maxSinPhi ) ) return 0;

  fNDF  += 2;
  fChi2 += mS0 * z0 * z0 + mS2 * z1 * z1 ;

  fP[ 0] += k00 * z0 ;
  fP[ 1] += k11 * z1 ;
  fP[ 2] = sinPhi ;
  fP[ 3] += k31 * z1 ;
  fP[ 4] += k40 * z0 ;

  fC[ 0] -= k00 * c00 ;
  fC[ 3] -= k20 * c00 ;
  fC[ 5] -= k20 * c20 ;
  fC[10] -= k40 * c00 ;
  fC[12] -= k40 * c20 ;
  fC[14] -= k40 * c40 ;

  fC[ 2] -= k11 * c11 ;
  fC[ 7] -= k31 * c11 ;
  fC[ 9] -= k31 * c31 ;

  return 1;
}

AliHLTTPCCATrackParam AliHLTTPCCATrackParam::GetGlobalParam(float alpha) const
{
  AliHLTTPCCATrackParam r = *this;
  r.Rotate( -alpha );
  return r;
}
*/