//----------------------------------------------------------------------------
// Implementation of the AliKFParticleSIMD class
// .
// @author  S.Gorbunov, I.Kisel
// @version 1.0
// @since   13.05.07
// 
// Class to reconstruct and store the decayed particle parameters.
// The method is described in CBM-SOFT note 2007-003, 
// ``Reconstruction of decayed particles based on the Kalman filter'', 
// http://www.gsi.de/documents/DOC-2007-May-14-1.pdf
//
// This class is ALICE interface to general mathematics in AliKFParticleSIMDCore
// 
//  -= Copyright &copy ALICE HLT Group =-
//____________________________________________________________________________


#include "AliKFParticleSIMD.h"
#include "TDatabasePDG.h"
#include "TParticlePDG.h"
#include "AliVTrack.h"
#include "AliVVertex.h"

fvec AliKFParticleSIMD::fgBz = -5.;  //* Bz compoment of the magnetic field

static const fvec Zero = 0;
static const fvec One = 1;

AliKFParticleSIMD::AliKFParticleSIMD( const AliKFParticleSIMD &d1, const AliKFParticleSIMD &d2, Bool_t gamma )
{
  if (!gamma) {
    AliKFParticleSIMD mother;
    mother+= d1;
    mother+= d2;
    *this = mother;
  } else
    ConstructGamma(d1, d2);
}

void AliKFParticleSIMD::Create( const fvec Param[], const fvec Cov[], fvec Charge, Int_t PID )
{
  // Constructor from "cartesian" track, PID hypothesis should be provided
  //
  // Param[6] = { X, Y, Z, Px, Py, Pz } - position and momentum
  // Cov [21] = lower-triangular part of the covariance matrix:
  //
  //                (  0  .  .  .  .  . )
  //                (  1  2  .  .  .  . )
  //  Cov. matrix = (  3  4  5  .  .  . ) - numbering of covariance elements in Cov[]
  //                (  6  7  8  9  .  . )
  //                ( 10 11 12 13 14  . )
  //                ( 15 16 17 18 19 20 )
  fvec C[21];
  for( int i=0; i<21; i++ ) C[i] = Cov[i];

  TParticlePDG* particlePDG = TDatabasePDG::Instance()->GetParticle(PID);
  Double_t mass = (particlePDG) ? particlePDG->Mass() :0.13957;

  AliKFParticleBaseSIMD::Initialize( Param, C, Charge, mass );
}

AliKFParticleSIMD::AliKFParticleSIMD( const AliVTrack *track, Int_t PID )
{
  // Constructor from ALICE track, PID hypothesis should be provided

  Double_t r[3];
  Double_t C[21];

  for(Int_t iPart = 0; iPart<fvecLen; iPart++)
  {
    track[iPart].XvYvZv(r);
    for(Int_t i=0; i<3; i++)
      fP[i][iPart] = r[i];
    track[iPart].PxPyPz(r);
    for(Int_t i=0; i<3; i++)
      fP[i+3][iPart] = r[i];
    fQ[iPart] = track[iPart].Charge();
    track[iPart].GetCovarianceXYZPxPyPz( C );
    for(Int_t i=0; i<21; i++)
      fC[i][iPart] = C[i];
  }
  Create(fP,fC,fQ,PID);
}

AliKFParticleSIMD::AliKFParticleSIMD( const AliKFParticle &p, Int_t PID )
{
  for(Int_t i=0; i<6; i++)
    fP[i] = p.GetParameter(i);
  fQ = p.GetQ();
  for(Int_t i=0; i<21; i++)
    fC[i] = p.GetCovariance(i);

  Create(fP,fC,fQ,PID);
}

AliKFParticleSIMD::AliKFParticleSIMD( AliKFParticle *p[fvecLen], Int_t PID )
{
  for(int iPart=0; iPart<fvecLen; iPart++)
  {
    for(Int_t i=0; i<6; i++)
      fP[i][iPart] = p[iPart]->GetParameter(i);
    fQ[iPart] = p[iPart]->GetQ();
    for(Int_t i=0; i<21; i++)
      fC[iPart][i] = p[iPart]->GetCovariance(i);
  }
  Create(fP,fC,fQ,PID);
}

AliKFParticleSIMD::AliKFParticleSIMD( const AliVVertex &vertex )
{
  // Constructor from ALICE vertex

  Double_t r[3];
  Double_t C[21];

  vertex.GetXYZ( r );
  for(Int_t i=0; i<3; i++)
    fP[i] = r[i];
  vertex.GetCovarianceMatrix( C );
  for(Int_t i=0; i<21; i++)
    fC[i] = C[i];
  fChi2 = vertex.GetChi2();
  fNDF = 2*vertex.GetNContributors() - 3;
  fQ = Zero;
  fAtProductionVertex = 0;
  fIsLinearized = 0;
  fSFromDecay = 0;
}

void AliKFParticleSIMD::GetExternalTrackParam( const AliKFParticleBaseSIMD &p, Double_t X[fvecLen], Double_t Alpha[fvecLen], Double_t P[5][fvecLen] )
{
  // Conversion to AliExternalTrackParam parameterization

  fvec cosA = p.GetPx(), sinA = p.GetPy(); 
  fvec pt = sqrt(cosA*cosA + sinA*sinA);
  fvec pti = Zero;
  fvec mask = fvec(pt < fvec(1.e-4));
  pti = (!mask) & (One/pt);
  cosA = if3(mask, One, cosA*pti);
  sinA = if3(mask, One, sinA*pti);

  fvec AlphaSIMD = atan2SIMD(sinA,cosA);
  fvec XSIMD     = p.GetX()*cosA + p.GetY()*sinA;
  fvec PSIMD[5];
  PSIMD[0]= p.GetY()*cosA - p.GetX()*sinA;
  PSIMD[1]= p.GetZ();
  PSIMD[2]= Zero;
  PSIMD[3]= p.GetPz()*pti;
  PSIMD[4]= p.GetQ()*pti;

  for(int iPart=0; iPart<fvecLen; iPart++)
  {
    X[iPart] = XSIMD[iPart];
    Alpha[iPart] = AlphaSIMD[iPart];
    P[0][iPart] = PSIMD[0][iPart];
    P[1][iPart] = PSIMD[1][iPart];
    P[2][iPart] = PSIMD[2][iPart];
    P[3][iPart] = PSIMD[3][iPart];
    P[4][iPart] = PSIMD[4][iPart];
  }
}

fvec AliKFParticleSIMD::GetDistanceFromVertexXY( const fvec vtx[], const fvec Cv[], fvec &val, fvec &err ) const
{
  //* Calculate DCA distance from vertex (transverse impact parameter) in XY
  //* v = [xy], Cv=[Cxx,Cxy,Cyy ]-covariance matrix

  fvec ret = Zero;
  
  fvec mP[8];
  fvec mC[36];
  
  Transport( GetDStoPoint(vtx), mP, mC );  

  fvec dx = mP[0] - vtx[0];
  fvec dy = mP[1] - vtx[1];
  fvec px = mP[3];
  fvec py = mP[4];
  fvec pt = sqrt(px*px + py*py);
  fvec ex=Zero, ey=Zero;
  fvec mask = fvec( pt < fvec(1.e-4) );
  ret = mask;
  pt = if3(mask, One, pt);
  ex = (!mask) & (px/pt);
  ey = (!mask) & (py/pt);
  val = if3(mask, fvec(1.e4), dy*ex - dx*ey);

  fvec h0 = -ey;
  fvec h1 = ex;
  fvec h3 = (dy*ey + dx*ex)*ey/pt;
  fvec h4 = -(dy*ey + dx*ex)*ex/pt;
  
  err = 
    h0*(h0*GetCovariance(0,0) + h1*GetCovariance(0,1) + h3*GetCovariance(0,3) + h4*GetCovariance(0,4) ) +
    h1*(h0*GetCovariance(1,0) + h1*GetCovariance(1,1) + h3*GetCovariance(1,3) + h4*GetCovariance(1,4) ) +
    h3*(h0*GetCovariance(3,0) + h1*GetCovariance(3,1) + h3*GetCovariance(3,3) + h4*GetCovariance(3,4) ) +
    h4*(h0*GetCovariance(4,0) + h1*GetCovariance(4,1) + h3*GetCovariance(4,3) + h4*GetCovariance(4,4) );

  if( Cv ){
    err+= h0*(h0*Cv[0] + h1*Cv[1] ) + h1*(h0*Cv[1] + h1*Cv[2] ); 
  }

  err = sqrt(fabs(err));

  return ret;
}

fvec AliKFParticleSIMD::GetDistanceFromVertexXY( const fvec vtx[], fvec &val, fvec &err ) const
{
  return GetDistanceFromVertexXY( vtx, 0, val, err );
}


fvec AliKFParticleSIMD::GetDistanceFromVertexXY( const AliKFParticleSIMD &Vtx, fvec &val, fvec &err ) const 
{
  //* Calculate distance from vertex [cm] in XY-plane

  return GetDistanceFromVertexXY( Vtx.fP, Vtx.fC, val, err );
}

fvec AliKFParticleSIMD::GetDistanceFromVertexXY( const AliVVertex &Vtx, fvec &val, fvec &err ) const 
{
  //* Calculate distance from vertex [cm] in XY-plane

  return GetDistanceFromVertexXY( AliKFParticleSIMD(Vtx), val, err );
}

fvec AliKFParticleSIMD::GetDistanceFromVertexXY( const fvec vtx[] ) const
{
  //* Calculate distance from vertex [cm] in XY-plane
  fvec val, err;
  GetDistanceFromVertexXY( vtx, 0, val, err );
  return val;
}

fvec AliKFParticleSIMD::GetDistanceFromVertexXY( const AliKFParticleSIMD &Vtx ) const 
{
  //* Calculate distance from vertex [cm] in XY-plane

  return GetDistanceFromVertexXY( Vtx.fP );
}

fvec AliKFParticleSIMD::GetDistanceFromVertexXY( const AliVVertex &Vtx ) const 
{
  //* Calculate distance from vertex [cm] in XY-plane

  return GetDistanceFromVertexXY( AliKFParticleSIMD(Vtx).fP );
}

fvec AliKFParticleSIMD::GetDistanceFromParticleXY( const AliKFParticleSIMD &p ) const 
{
  //* Calculate distance to other particle [cm]

  fvec dS, dS1;
  GetDStoParticleXY( p, dS, dS1 );   
  fvec mP[8], mC[36], mP1[8], mC1[36];
  Transport( dS, mP, mC ); 
  p.Transport( dS1, mP1, mC1 ); 
  fvec dx = mP[0]-mP1[0]; 
  fvec dy = mP[1]-mP1[1]; 
  return sqrt(dx*dx+dy*dy);
}

fvec AliKFParticleSIMD::GetDeviationFromParticleXY( const AliKFParticleSIMD &p ) const 
{
  //* Calculate sqrt(Chi2/ndf) deviation from other particle

  fvec dS, dS1;
  GetDStoParticleXY( p, dS, dS1 );   
  fvec mP1[8], mC1[36];
  p.Transport( dS1, mP1, mC1 ); 

  fvec d[2]={ fP[0]-mP1[0], fP[1]-mP1[1] };

  fvec sigmaS = .1+10.*sqrt( (d[0]*d[0]+d[1]*d[1] )/
					(mP1[3]*mP1[3]+mP1[4]*mP1[4] )  );

  fvec h[2] = { mP1[3]*sigmaS, mP1[4]*sigmaS };       
  
  mC1[0] +=h[0]*h[0];
  mC1[1] +=h[1]*h[0]; 
  mC1[2] +=h[1]*h[1]; 

  return GetDeviationFromVertexXY( mP1, mC1 )*sqrt(2./1.);
}


fvec AliKFParticleSIMD::GetDeviationFromVertexXY( const fvec vtx[], const fvec Cv[] ) const 
{
  //* Calculate sqrt(Chi2/ndf) deviation from vertex
  //* v = [xyz], Cv=[Cxx,Cxy,Cyy,Cxz,Cyz,Czz]-covariance matrix

  fvec val, err;
  fvec problem = GetDistanceFromVertexXY( vtx, Cv, val, err );
  fvec ret = fvec(1.e4);
  fvec mask = fvec(problem | fvec(err<fvec(1.e-20)));
  ret = if3(mask, ret, val/err);
  return ret;
}


fvec AliKFParticleSIMD::GetDeviationFromVertexXY( const AliKFParticleSIMD &Vtx ) const  
{
  //* Calculate sqrt(Chi2/ndf) deviation from vertex
  //* v = [xyz], Cv=[Cxx,Cxy,Cyy,Cxz,Cyz,Czz]-covariance matrix

  return GetDeviationFromVertexXY( Vtx.fP, Vtx.fC );
}

fvec AliKFParticleSIMD::GetDeviationFromVertexXY( const AliVVertex &Vtx ) const 
{
  //* Calculate sqrt(Chi2/ndf) deviation from vertex
  //* v = [xyz], Cv=[Cxx,Cxy,Cyy,Cxz,Cyz,Czz]-covariance matrix

  AliKFParticleSIMD v(Vtx);
  return GetDeviationFromVertexXY( v.fP, v.fC );
}

fvec AliKFParticleSIMD::GetAngle  ( const AliKFParticleSIMD &p ) const 
{
  //* Calculate the opening angle between two particles

  fvec dS, dS1;
  GetDStoParticle( p, dS, dS1 );   
  fvec mP[8], mC[36], mP1[8], mC1[36];
  Transport( dS, mP, mC ); 
  p.Transport( dS1, mP1, mC1 ); 
  fvec n = sqrt( mP[3]*mP[3] + mP[4]*mP[4] + mP[5]*mP[5] );
  fvec n1= sqrt( mP1[3]*mP1[3] + mP1[4]*mP1[4] + mP1[5]*mP1[5] );
  n*=n1;
  fvec a = Zero;
  fvec mask = fvec(n>fvec(1.e-8));
  a = ( mP[3]*mP1[3] + mP[4]*mP1[4] + mP[5]*mP1[5] )/n;
  a = mask & a;
  mask = fvec( fabs(a) < One);
  fvec aPos = fvec(a>=Zero);
  a = if3(mask, acos(a), if3(aPos, Zero, fvec(3.1415926535)) );
  return a;
}

fvec AliKFParticleSIMD::GetAngleXY( const AliKFParticleSIMD &p ) const 
{
  //* Calculate the opening angle between two particles in XY plane

  fvec dS, dS1;
  GetDStoParticleXY( p, dS, dS1 );   
  fvec mP[8], mC[36], mP1[8], mC1[36];
  Transport( dS, mP, mC ); 
  p.Transport( dS1, mP1, mC1 ); 
  fvec n = sqrt( mP[3]*mP[3] + mP[4]*mP[4] );
  fvec n1= sqrt( mP1[3]*mP1[3] + mP1[4]*mP1[4] );
  n*=n1;
  fvec a = Zero;
  fvec mask = fvec(n>fvec(1.e-8));
  a = ( mP[3]*mP1[3] + mP[4]*mP1[4] )/n;
  a = mask & a;
  mask = fvec( fabs(a) < One);
  fvec aPos = fvec(a>=Zero);
  a = if3(mask, acos(a), if3(aPos, Zero, fvec(3.1415926535)) );
  return a;
}

fvec AliKFParticleSIMD::GetAngleRZ( const AliKFParticleSIMD &p ) const 
{
  //* Calculate the opening angle between two particles in RZ plane

  fvec dS, dS1;
  GetDStoParticle( p, dS, dS1 );   
  fvec mP[8], mC[36], mP1[8], mC1[36];
  Transport( dS, mP, mC ); 
  p.Transport( dS1, mP1, mC1 ); 
  fvec nr = sqrt( mP[3]*mP[3] + mP[4]*mP[4] );
  fvec n1r= sqrt( mP1[3]*mP1[3] + mP1[4]*mP1[4]  );
  fvec n = sqrt( nr*nr + mP[5]*mP[5] );
  fvec n1= sqrt( n1r*n1r + mP1[5]*mP1[5] );
  n*=n1;
  fvec a = Zero;
  fvec mask = fvec(n>fvec(1.e-8));
  a = ( nr*n1r +mP[5]*mP1[5])/n;
  a = mask & a;
  mask = fvec( fabs(a) < One);
  fvec aPos = fvec(a>=Zero);
  a = if3(mask, acos(a), if3(aPos, Zero, fvec(3.1415926535)) );
  return a;
}


/*

#include "AliExternalTrackParam.h"

void AliKFParticleSIMD::GetDStoParticleALICE( const AliKFParticleBaseSIMD &p, 
					   fvec &DS, fvec &DS1 ) 
  const
{ 
  DS = DS1 = 0;   
  fvec x1, a1, x2, a2;
  fvec par1[5], par2[5], cov[15];
  for(int i=0; i<15; i++) cov[i] = 0;
  cov[0] = cov[2] = cov[5] = cov[9] = cov[14] = .001;

  GetExternalTrackParam( *this, x1, a1, par1 );
  GetExternalTrackParam( p, x2, a2, par2 );

  AliExternalTrackParam t1(x1,a1, par1, cov);
  AliExternalTrackParam t2(x2,a2, par2, cov);

  fvec xe1=0, xe2=0;
  t1.GetDCA( &t2, -GetFieldAlice(), xe1, xe2 );
  t1.PropagateTo( xe1, -GetFieldAlice() );
  t2.PropagateTo( xe2, -GetFieldAlice() );

  fvec xyz1[3], xyz2[3];
  t1.GetXYZ( xyz1 );
  t2.GetXYZ( xyz2 );
  
  DS = GetDStoPoint( xyz1 );
  DS1 = p.GetDStoPoint( xyz2 );

  return;
}
*/