//----------------------------------------------------------------------------
// Implementation of the AliKFParticle 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 AliKFParticleCore
// 
//  -= Copyright &copy ALICE HLT Group =-
//____________________________________________________________________________

#include "AliKFParticleTest.h"
#include "AliKFParticle.h"
#include "AliKFParticleSIMD.h"

#include <TTree.h>
#include <TFile.h>
#include <TMath.h>
#include <TDirectory.h>
#include <TH1D.h>
#include <TDatabasePDG.h>
#include <TParticle.h>
#include <TAxis.h>
#include <TStopwatch.h>
#include "TDatabasePDG.h"

#include "AliRun.h"
#include "AliStack.h"
#include "AliESDEvent.h"
#include "AliTracker.h"
#include "AliKFParticle.h"
#include "AliKFVertex.h"

#include <iostream>
using std::vector;
using std::cout;
using std::endl;

ClassImp(AliKFParticleTest)

struct TESDTrackInfo
{
  AliKFParticle fParticle; //* assigned KFParticle
  Int_t TrackID;
  Int_t mcPDG;             //* Monte Carlo PDG code
  Int_t mcMotherID;        //* Monte Carlo ID of its mother
};

struct KFTestTrack
{
  KFTestTrack():fX(0),fAngle(0),U01(0),U02(0),U03(0),U04(0),U12(0),U13(0),U14(0),U23(0),U24(0),U34(0),D00(0),D11(0),D22(0),D33(0),D44(0),
                iU01(0), iU02(0), iU03(0), iU04(0), iU12(0), iU13(0), iU14(0), iU23(0), iU24(0), iU34(0)
  {
    for(int i=0; i<5; i++) fPar[i] = 0;
    for(int i=0; i<5; i++) fParEigen[i] = 0;
    for(int i=0; i<15; i++) fCov[i] = 0;
  }
  KFTestTrack(AliESDtrack &T):fX(0),fAngle(0),U01(0),U02(0),U03(0),U04(0),U12(0),U13(0),U14(0),U23(0),U24(0),U34(0),D00(0),D11(0),D22(0),D33(0),D44(0),
                              iU01(0), iU02(0), iU03(0), iU04(0), iU12(0), iU13(0), iU14(0), iU23(0), iU24(0), iU34(0)
  {
    SetFromESDTrack(T);
  }

  void SetFromESDTrack(AliESDtrack &T);
  void TransformVector(Double_t vec[5]);

  Double_t fX;
  Double_t fAngle;
  // UtDU decomposition of the Covariance matrix
  Double_t U01, U02, U03, U04,
                U12, U13, U14,
                     U23, U24,
                          U34,
      D00, D11, D22, D33, D44;

    Double_t iU01, iU02, iU03, iU04,
                iU12, iU13, iU14,
                     iU23, iU24,
                          iU34;
      
  Double_t fPar[5];
  Double_t fCov[15];

  Double_t fParEigen[5];
};

void KFTestTrack::SetFromESDTrack(AliESDtrack &T)
{
  fX = T.GetX();
  fAngle = T.GetAlpha();
  for(int i=0; i<5; i++) fPar[i] = T.GetParameter()[i];
  for(int i=0; i<15; i++) fCov[i] = T.GetCovariance()[i];

  D44 = fCov[14];
  const Double_t iD44 = 1./D44;
  const Double_t &DU43 = fCov[13];
  const Double_t &DU42 = fCov[12];
  const Double_t &DU41 = fCov[11];
  const Double_t &DU40 = fCov[10];
  U34 = DU43*iD44;
  U24 = DU42*iD44;
  U14 = DU41*iD44;
  U04 = DU40*iD44;

  D33 = fCov[9] - U34*DU43;
  const Double_t iD33 = 1.f/D33;
  const Double_t DU32 = fCov[8] - U34*DU42;
  const Double_t DU31 = fCov[7] - U34*DU41;
  const Double_t DU30 = fCov[6] - U34*DU40;
  U23 = DU32*iD33;
  U13 = DU31*iD33;
  U03 = DU30*iD33;

  D22 = fCov[5] - U23*DU32 - U24*DU42;
  const Double_t iD22 = 1.f/D22;
  const Double_t DU21 = fCov[4] - U23*DU31 - U24*DU41;
  const Double_t DU20 = fCov[3] - U23*DU30 - U24*DU40;
  U12 = DU21*iD22;
  U02 = DU20*iD22;

  D11 = fCov[2] - U12*DU21 - U13*DU31 - U14*DU41;
  const Double_t DU10 = fCov[1] - U12*DU20 - U13*DU30 - U14*DU40;
  U01 = DU10/D11;

  D00 = fCov[0] - U01*DU10 - U02*DU20 - U03*DU30 - U04*DU40;

  for(int i=0; i<5; i++) fParEigen[i] = fPar[i];
  
  iU01 = -U01;
  iU12 = -U12;
  iU23 = -U23;
  iU34 = -U34;
  
  iU02 = -U02+U01*U12;
  iU13 = -U13+U12*U23;
  iU24 = -U24+U23*U34;
  
  iU03 = -U03 + U01*U13 + U23*U02 - U23*U01*U12;
  iU14 = -U14 + U12*U24 + U34*U13 - U34*U12*U23;
  
  iU04 = -U04 + U01*U14 + U02*U24 - U01*U12*U24 +U03*U34 - U01*U13*U34 - U02*U23*U34 + U01*U12*U23*U34;
  
  TransformVector(fParEigen);
}

void KFTestTrack::TransformVector(Double_t vec[5])
{
  vec[0] = vec[0];
  vec[1] = vec[0] * iU01 + vec[1];
  vec[2] = vec[0] * iU02 + vec[1] * iU12 + vec[2];
  vec[3] = vec[0] * iU03 + vec[1] * iU13 + vec[2] * iU23 + vec[3];
  vec[4] = vec[0] * iU04 + vec[1] * iU14 + vec[2] * iU24 + vec[3] * iU34 + vec[4];
}

AliKFParticleTest::AliKFParticleTest():fInpFileName(),
                                       fOutFileName(""),
                                       fTestParticlesPDG(421)
{
  //resolutions and Pulls of KFParticle parameters
  TString ResNames[8] = {"resX", "resY", "resZ", "resPx", "resPy", "resPz", "resE", "resM"};
  TString PullNames[8] = {"pullX", "pullY", "pullZ", "pullPx", "pullPy", "pullPz", "pullE", "pullM"};
  Float_t XMax[8] = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1};
  TString StrSimd = "SIMD";
  for(int i=0; i<8; i++)
  {
    fhKFParticle[i]    = new TH1F(ResNames[i].Data(), ResNames[i].Data(), 60, -XMax[i], XMax[i]);
    fhKFParticle[8+i]  = new TH1F(PullNames[i].Data(), PullNames[i].Data(), 60, -6, 6);
    fhKFParticleSIMD[i]    = new TH1F((ResNames[i]+StrSimd).Data(), (ResNames[i]+StrSimd).Data(), 60, -XMax[i], XMax[i]);
    fhKFParticleSIMD[8+i]  = new TH1F((PullNames[i]+StrSimd).Data(), (PullNames[i]+StrSimd).Data(), 60, -6, 6);
  }

  fhKFParticle[16]  = new TH1F("resolution_eta", "resolution_eta", 200, -20, 20);
  fhKFParticle[17]  = new TH1F("residual_eta", "residual_eta", 200, -0.1, 0.1);
  fhKFParticle[18]  = new TH1F("pull_eta", "pull_eta", 200, -10, 10);

  fhKFParticle[19]  = new TH1F("res_ip_abs", "res_ip_abs", 200, -0.5e-13, 0.5e-13);
  fhKFParticle[20]  = new TH1F("res_ip", "res_ip", 200, -0.5e-13, 0.5e-13);
  fhKFParticle[21]  = new TH1F("res_ip_P", "res_ip_P", 200, -5, 5);
  fhKFParticle[22]  = new TH1F("res_ip_P_all", "res_ip_P_all", 200, -5, 5);

  fhKFParticle[23]  = new TH1F("delta_ip", "delta_ip", 200, -15, 15);
  fhKFParticle[24]  = new TH1F("delta_ip_cv", "delta_ip_cv", 200, -15, 15);
  
  fhKFParticle[25] = new TH1F("massAll","AliKFParticle test", 500,0.,3.);
  fhKFParticle[25]->SetXTitle("invariant mass [GeV]");
  fhKFParticle[26] = new TH1F("massMulti","V^{+-} contributions", 500,-1.1,3);
  fhKFParticle[27] = new TH1F("massV0","V^{0} signal", 500,-1.1,3);
  fhKFParticle[25]->SetLineColor(8);
  fhKFParticle[26]->SetLineColor(kMagenta);
  fhKFParticle[27]->SetLineColor(kBlue);
  fhKFParticle[28] = new TH1F("massBG","BG mass [GeV]", 500, 0., 3.);

  fhKFParticle[29] = new TH1F("SigmaMass","Sigma Mass [GeV]", 60, 0., 0.2);
  fhKFParticle[30] = new TH1F("SigmaE","Sigma E [GeV]", 60, 0., 0.2);
  fhKFParticle[31] = new TH1F("probV0","probV0", 100, 0., 1.);

  fhKFParticleSIMD[16]  = new TH1F("resolution_etaSIMD", "resolution_etaSIMD", 200, -20, 20);
  fhKFParticleSIMD[17]  = new TH1F("residual_etaSIMD", "residual_etaSIMD", 200, -0.1, 0.1);
  fhKFParticleSIMD[18]  = new TH1F("pull_etaSIMD", "pull_etaSIMD", 200, -10, 10);

  fhKFParticleSIMD[19]  = new TH1F("res_ip_absSIMD", "res_ip_absSIMD", 200, -0.5e-13, 0.5e-13);
  fhKFParticleSIMD[20]  = new TH1F("res_ipSIMD", "res_ipSIMD", 200, -0.5e-13, 0.5e-13);
  fhKFParticleSIMD[21]  = new TH1F("res_ip_PSIMD", "res_ip_PSIMD", 200, -5, 5);
  fhKFParticleSIMD[22]  = new TH1F("res_ip_P_allSIMD", "res_ip_P_allSIMD", 200, -5, 5);

  fhKFParticleSIMD[23]  = new TH1F("delta_ipSIMD", "delta_ipSIMD", 200, -15, 15);
  fhKFParticleSIMD[24]  = new TH1F("delta_ip_cvSIMD", "delta_ip_cvSIMD", 200, -15, 15);
  
  fhKFParticleSIMD[25] = new TH1F("massAllSIMD","AliKFParticle testSIMD", 500,0.,3.);
  fhKFParticleSIMD[25]->SetXTitle("invariant mass [GeV]");
  fhKFParticleSIMD[26] = new TH1F("massMultiSIMD","V^{+-} contributions", 500,-1.1,3);
  fhKFParticleSIMD[27] = new TH1F("massV0SIMD","V^{0} signal", 500,-1.1,3);
  fhKFParticleSIMD[25]->SetLineColor(8);
  fhKFParticleSIMD[26]->SetLineColor(kMagenta);
  fhKFParticleSIMD[27]->SetLineColor(kBlue);
  fhKFParticleSIMD[28] = new TH1F("massBGSIMD","BG mass [GeV]", 500, 0., 3.);

  TString ResNamesInput[5] = {"resYInput", "resZInput", "resSinPhiInput", "resDzDsInput", "resQPtInput"};
  TString PullNamesInput[5] = {"pullYInput", "pullZInput", "pullSinPhiInput", "pullDzDsInput", "pullQPtInput"};
  Double_t XMaxInput[5] = {0.1, 0.1, 0.05, 0.05, 0.1};
  for(int i=0; i<5; i++)
  {
    fhKFInputTracks[i]    = new TH1F(ResNamesInput[i].Data(), ResNamesInput[i].Data(), 60, -XMaxInput[i], XMaxInput[i]);
    fhKFInputTracks[5+i]  = new TH1F(PullNamesInput[i].Data(), PullNamesInput[i].Data(), 60, -6, 6);
  }

}

AliKFParticleTest::~AliKFParticleTest()
{
  for(int i=0; i<cNHistos; i++)
  {
    if(fhKFParticle[i])
      delete fhKFParticle[i];
    if(fhKFParticleSIMD[i])
      delete fhKFParticleSIMD[i];
  }
  for(int i=0; i<10; i++)
  {
    if(fhKFInputTracks[i])
      delete fhKFInputTracks[i];
  }
}

void AliKFParticleTest::SelectParticles(AliESDEvent *event, vector<TESDTrackInfo> &ESDTrackInfo)
{
  if( !gAlice ) return;
  AliRunLoader *rl = AliRunLoader::Instance(); 

  AliStack *stack = rl->Stack();
  if( !stack ) return;

  ESDTrackInfo.clear();

  Int_t nESDTracks=event->GetNumberOfTracks();

  //* Fill ESDTrackInfo array
//  std::cout << "nTracks  " << nESDTracks << std::endl;
  for (Int_t iTr=0; iTr<nESDTracks; iTr++)
  {
    TESDTrackInfo info;
    info.mcPDG = -1;
    info.mcMotherID = -1;
    info.TrackID = iTr;
    //* track quality check

    AliESDtrack *pTrack = event->GetTrack(iTr);    
    if( !pTrack  ) continue;
    Int_t PDG = ( pTrack->GetSigned1Pt() <0 ) ?321 :211;
    //* take MC PDG  
    Int_t mcID = TMath::Abs(pTrack->GetLabel());
    TParticle * part = stack->Particle(TMath::Abs(mcID));

    if( part )
    {
      info.mcPDG = part->GetPdgCode();
      PDG = info.mcPDG;
      if( mcID>=0 ) info.mcMotherID = part->GetFirstMother();
    }

      // select only particles from interesting decays
    Bool_t IsTestPart = 0;
    for(Int_t i=0; i<static_cast<Int_t>(fTestParticlesPDG.size()); i++)
      if(info.mcMotherID >= 0 && stack->Particle(info.mcMotherID)->GetPdgCode() == fTestParticlesPDG[i])
        IsTestPart = 1;

    if(!IsTestPart) continue;
    //* Construct KFParticle for the track
    info.fParticle = AliKFParticle( *pTrack, PDG );
//    info.fParticle = AliKFParticle( *pTrack, 211 );

    ESDTrackInfo.push_back(info);
  }
//cout << ESDTrackInfo.size() << endl;
}

void AliKFParticleTest::CheckInputTracks(AliESDEvent *event, vector<TESDTrackInfo> &ESDTrackInfo)
{
  static int NCovNeg = 0;
  static int NCorBig = 0;
  static int NCorAll = 0;
  static int NEv = 0;
  NEv++;

  if( !gAlice ) return;
  AliRunLoader *rl = AliRunLoader::Instance(); 

  AliStack *stack = rl->Stack();
  if( !stack ) return;

  //Check Residuals and Pulls for all tracks

  Int_t nESDTracks=event->GetNumberOfTracks();
  for (Int_t iTr=0; iTr<nESDTracks; iTr++)
  {
    AliESDtrack *pTrack = event->GetTrack(iTr);    
    if( !pTrack  ) continue;
    TParticle * part = stack->Particle(TMath::Abs(pTrack->GetLabel()));
    if( !part ) continue;

    Double_t cosA = TMath::Cos( pTrack->GetAlpha() );
    Double_t sinA = TMath::Sin( pTrack->GetAlpha() );
    Double_t mcX =  part->Vx() * cosA + part->Vy() * sinA;
    Double_t mcY = -part->Vx() * sinA + part->Vy() * cosA;
    Double_t mcZ =  part->Vz();
    Double_t mcEx =  part->Px() * cosA + part->Py() * sinA;
    Double_t mcEy = -part->Px() * sinA + part->Py() * cosA;
    Double_t mcEz =  part->Pz();
    Double_t mcEt = TMath::Sqrt( mcEx * mcEx + mcEy * mcEy );
    Double_t mcSinPhi = mcEy / mcEt;
    Double_t mcDzDs = mcEz / mcEt;
    Double_t mcQ=0;
    if ( part->GetPdgCode() < 9999999 )
      mcQ = TDatabasePDG::Instance()->GetParticle(part->GetPdgCode())->Charge()/3.0;
    Double_t mcQPt = mcQ/part->Pt();

    Double_t MCPar[5] = {mcY, mcZ, mcSinPhi, mcDzDs, mcQPt},
             Error[5] = {pTrack->GetSigmaY2(),pTrack->GetSigmaZ2(),pTrack->GetSigmaSnp2(),pTrack->GetSigmaTgl2(),pTrack->GetSigma1Pt2()};

    for(int i=0; i<5; i++)
    {
      Double_t res = pTrack->GetParameter()[i] - MCPar[i];
      fhKFInputTracks[i]->Fill(res);
      if(Error[i] >= 0.)
        fhKFInputTracks[i+5]->Fill(res/TMath::Sqrt(Error[i]));
    }

    if((pTrack->GetCovariance()[0] <=0.) ||
       (pTrack->GetCovariance()[2] <=0.) ||
       (pTrack->GetCovariance()[5] <=0.) ||
       (pTrack->GetCovariance()[9] <=0.) ||
       (pTrack->GetCovariance()[14] <=0.) ) NCovNeg++;
    if((pTrack->GetCovariance()[1]*pTrack->GetCovariance()[1] > pTrack->GetCovariance()[0]*pTrack->GetCovariance()[2]) ||
       (pTrack->GetCovariance()[3]*pTrack->GetCovariance()[3] > pTrack->GetCovariance()[0]*pTrack->GetCovariance()[5]) ||
       (pTrack->GetCovariance()[4]*pTrack->GetCovariance()[4] > pTrack->GetCovariance()[2]*pTrack->GetCovariance()[5]) ||
       (pTrack->GetCovariance()[6]*pTrack->GetCovariance()[6] > pTrack->GetCovariance()[0]*pTrack->GetCovariance()[9]) ||
       (pTrack->GetCovariance()[7]*pTrack->GetCovariance()[7] > pTrack->GetCovariance()[2]*pTrack->GetCovariance()[9]) ||
       (pTrack->GetCovariance()[8]*pTrack->GetCovariance()[8] > pTrack->GetCovariance()[5]*pTrack->GetCovariance()[9]) ||
       (pTrack->GetCovariance()[10]*pTrack->GetCovariance()[10] > pTrack->GetCovariance()[0]*pTrack->GetCovariance()[14]) ||
       (pTrack->GetCovariance()[11]*pTrack->GetCovariance()[11] > pTrack->GetCovariance()[2]*pTrack->GetCovariance()[14]) ||
       (pTrack->GetCovariance()[12]*pTrack->GetCovariance()[12] > pTrack->GetCovariance()[5]*pTrack->GetCovariance()[14]) ||
       (pTrack->GetCovariance()[13]*pTrack->GetCovariance()[13] > pTrack->GetCovariance()[9]*pTrack->GetCovariance()[14]) ) NCorBig++;
    NCorAll++;

//     KFTestTrack tTrack(*pTrack);
//     tTrack.TransformVector(MCPar);
//     Double_t ErrorD[5] = {tTrack.D00, tTrack.D11,tTrack.D22,tTrack.D33,tTrack.D44};
//     for(int i=0; i<5; i++)
//     {
//       Double_t res = tTrack.fParEigen[i] - MCPar[i];
//       fhKFInputTracks[i]->Fill(res);
//       if(ErrorD[i] >= 0.)
//         fhKFInputTracks[i+5]->Fill(res/TMath::Sqrt(ErrorD[i]));
//     }
  }

  //Check Signal

  Int_t nParticles = ESDTrackInfo.size();

  for( Int_t iTr = 0; iTr<nParticles; iTr++ )
  {
    TESDTrackInfo &info = ESDTrackInfo[iTr];
    AliESDtrack *pTrack = event->GetTrack(info.TrackID);
    KFTestTrack tTrack(*pTrack);

#if 0
    if( ((event->GetEventNumberInFile() == 18) && ((info.TrackID == 24) || (info.TrackID == 30))) ||
        ((event->GetEventNumberInFile() == 506) && ((info.TrackID == 24) || (info.TrackID == 37))) ||
        ((event->GetEventNumberInFile() == 645) && ((info.TrackID == 24) || (info.TrackID == 44))) ||
        ((event->GetEventNumberInFile() == 708) && ((info.TrackID == 8) || (info.TrackID == 30))) ||
        ((event->GetEventNumberInFile() == 920) && ((info.TrackID == 24) || (info.TrackID == 14))) ||
        ((event->GetEventNumberInFile() == 985) && ((info.TrackID == 18) || (info.TrackID == 29))) )
    {
      std::cout << "event " << event->GetEventNumberInFile() << " track " << info.TrackID << std::endl;
      std::cout << " D matrix: D00 = " << tTrack.D00 << " D11 = "<< tTrack.D11 << " D22 = "<< tTrack.D22 << " D33 = "<< tTrack.D33
                << " D44 = "<< tTrack.D44 << std::endl;
      std::cout << "X  " << tTrack.fX << "  Angle  " << tTrack.fAngle << std::endl;
    }
#endif
  }

  if(NEv==1000)
  {
    std::cout << "NCovNeg "<<NCovNeg<<std::endl;
    std::cout << "NCorBig "<<NCorBig<<std::endl;
    std::cout << "NCorAll "<<NCorAll<<std::endl;
  }
}

void AliKFParticleTest::TestKFParticle(AliESDEvent *event, vector<TESDTrackInfo> &ESDTrackInfo)
{
  if( !gAlice ) return;
  AliRunLoader *rl = AliRunLoader::Instance(); 

  AliStack *stack = rl->Stack();
  if( !stack ) return;
  
  Int_t nParticles = ESDTrackInfo.size();

  const AliESDVertex *PrimVertESD = event->GetPrimaryVertex();
  AliKFVertex PrimVertESDKF(*PrimVertESD);

  for( Int_t iTr = 0; iTr<nParticles; iTr++ ){ //* first daughter

    TESDTrackInfo &info = ESDTrackInfo[iTr];

    for( Int_t jTr = iTr+1; jTr<nParticles; jTr++ ){  //* second daughter

      TESDTrackInfo &jnfo = ESDTrackInfo[jTr];
      if( info.mcMotherID != jnfo.mcMotherID) continue;

//     if( !(((event->GetEventNumberInFile() == 18) && ((info.TrackID == 24) && (jnfo.TrackID == 30))) ||
//          ((event->GetEventNumberInFile() == 506) && ((info.TrackID == 24) && (jnfo.TrackID == 37))) ||
//          ((event->GetEventNumberInFile() == 645) && ((info.TrackID == 24) && (jnfo.TrackID == 44))) ||
//          ((event->GetEventNumberInFile() == 708) && ((info.TrackID == 8)  && (jnfo.TrackID == 30))) ||
//          ((event->GetEventNumberInFile() == 920) && ((info.TrackID == 14) && (jnfo.TrackID == 24))) ||
//          ((event->GetEventNumberInFile() == 985) && ((info.TrackID == 18) && (jnfo.TrackID == 29)))) ) continue;

      //* check for different charge

      if( info.fParticle.GetQ() == jnfo.fParticle.GetQ() ) continue;      

      //* construct V0 mother

      AliKFParticle V0( info.fParticle, jnfo.fParticle );     

      AliKFParticle V0_old;
      V0_old.SetConstructMethod( 0 );
      V0_old += info.fParticle;
      V0_old += jnfo.fParticle; 
//std::cout << "Y    old  " << V0_old.Y() << " new " << V0.Y() << std::endl;
//std::cout << "Mass old  " << V0_old.GetMass() << " new " << V0.GetMass() << std::endl;

      //* check V0 Chi^2

      if( V0.GetNDF()<1 ) continue;
 //     if( TMath::Sqrt(TMath::Abs(V0.GetChi2()/V0.GetNDF())) >3. ) continue;

      //* subtruct daughters from primary vertex 

///      AliKFVertex primVtxCopy = primVtx;

///      if( info.fPrimUsedFlag ) primVtxCopy -= info.fParticle;
///      if( jnfo.fPrimUsedFlag ) primVtxCopy -= jnfo.fParticle;

      //* Check V0 Chi^2 deviation from primary vertex 

  //    if( V0.GetDeviationFromVertex( primVtxCopy ) >3. ) continue;

      //* Add V0 to primary vertex to improve the primary vertex resolution

///      primVtxCopy += V0;      

      //* Set production vertex for V0

///      V0.SetProductionVertex( primVtxCopy );

      //* Check chi^2 for a case

   //   if( TMath::Sqrt( TMath::Abs(V0.GetChi2()/V0.GetNDF()) >3. )) continue;

      //* Get V0 decay length with estimated error

///      Double_t length, sigmaLength;
  //    if( V0.GetDecayLength( length, sigmaLength ) ) continue;

      //* Reject V0 if it decays too close to the primary vertex

 //     if( length  <3.*sigmaLength ) continue;

      //* Get V0 invariant mass and plot it

      Double_t mass, sigmaMass;

//       std::cout << "V0 Cov"<<std::endl;
//       std::cout << V0.GetCovariance( 0 )<<std::endl;
//       std::cout << V0.GetCovariance( 1 )<<"  "<<V0.GetCovariance( 2 )<<std::endl;
//       std::cout << V0.GetCovariance( 3 )<<"  "<<V0.GetCovariance( 4 )<<"  "<<V0.GetCovariance( 5 )<<std::endl;
//       std::cout << V0.GetCovariance( 6 )<<"  "<<V0.GetCovariance( 7 )<<"  "<<V0.GetCovariance( 8 )<<"  "<<V0.GetCovariance( 9 )<<std::endl;
//       std::cout << V0.GetCovariance( 10)<<"  "<<V0.GetCovariance( 11)<<"  "<<V0.GetCovariance( 12)<<"  "<<V0.GetCovariance( 13)<<"  "<<
//                    V0.GetCovariance( 14)<<std::endl;
//       std::cout << V0.GetCovariance( 15)<<"  "<<V0.GetCovariance( 16)<<"  "<<V0.GetCovariance( 17)<<"  "<<V0.GetCovariance( 18)<<"  "<<
//                    V0.GetCovariance( 19)<<"  "<<V0.GetCovariance( 20)<<std::endl;
//       std::cout << V0.GetCovariance( 21)<<"  "<<V0.GetCovariance( 22)<<"  "<<V0.GetCovariance( 23)<<"  "<<V0.GetCovariance( 24)<<"  "<<
//                    V0.GetCovariance( 25)<<"  "<<V0.GetCovariance( 26)<<"  "<<V0.GetCovariance( 27)<<std::endl;
// 
//       std::cout << "V0_old Cov"<<std::endl;
//       std::cout << V0_old.GetCovariance( 0 )<<std::endl;
//       std::cout << V0_old.GetCovariance( 1 )<<"  "<<V0_old.GetCovariance( 2 )<<std::endl;
//       std::cout << V0_old.GetCovariance( 3 )<<"  "<<V0_old.GetCovariance( 4 )<<"  "<<V0_old.GetCovariance( 5 )<<std::endl;
//       std::cout << V0_old.GetCovariance( 6 )<<"  "<<V0_old.GetCovariance( 7 )<<"  "<<V0_old.GetCovariance( 8 )<<"  "<<V0_old.GetCovariance( 9 )<<std::endl;
//       std::cout << V0_old.GetCovariance( 10)<<"  "<<V0_old.GetCovariance( 11)<<"  "<<V0_old.GetCovariance( 12)<<"  "<<V0_old.GetCovariance( 13)<<"  "<<
//                    V0_old.GetCovariance( 14)<<std::endl;
//       std::cout << V0_old.GetCovariance( 15)<<"  "<<V0_old.GetCovariance( 16)<<"  "<<V0_old.GetCovariance( 17)<<"  "<<V0_old.GetCovariance( 18)<<"  "<<
//                    V0_old.GetCovariance( 19)<<"  "<<V0_old.GetCovariance( 20)<<std::endl;
//       std::cout << V0_old.GetCovariance( 21)<<"  "<<V0_old.GetCovariance( 22)<<"  "<<V0_old.GetCovariance( 23)<<"  "<<V0_old.GetCovariance( 24)<<"  "<<
//                    V0_old.GetCovariance( 25)<<"  "<<V0_old.GetCovariance( 26)<<"  "<<V0_old.GetCovariance( 27)<<std::endl;

      if( V0.GetCovariance(6,6) > 0. ) fhKFParticle[30]->Fill( sqrt(V0.GetCovariance(6,6)) );

      
      if( V0.GetMass( mass, sigmaMass ) ) 
      {
        Double_t mo,smo;
        std::cout<<"!!!   old_mass"<<std::endl;
        V0_old.GetMass( mo, smo );
        std::cout <<"old mass = " <<mo<<"  "<<smo<<std::endl;
        std::cout <<"new mass = " <<mass<<std::endl;
        std::cout << "Event Number " << event->GetEventNumberInFile() << std::endl;
        std::cout << "Track Number " << info.TrackID << " and " << jnfo.TrackID << std::endl;
        continue;
      }
      fhKFParticle[25]->Fill(mass);
      
      fhKFParticle[29]->Fill(sigmaMass);

      fhKFParticle[31]->Fill( TMath::Prob( V0.GetChi2(), V0.GetNDF() ) );
      
      TParticle * part = stack->Particle(info.mcMotherID);
      
      Double_t Emc = sqrt(part->P()*part->P() + part->GetMass()*part->GetMass());
      Double_t res[8] = {0,0,0,0,0,0,0,0}, 
               pull[8] = {0,0,0,0,0,0,0,0}, 
               parMC[8] = { part->Vx(), part->Vy(), part->Vz(), part->Px(), part->Py(), part->Pz(), Emc, part->GetMass() };
      for(int iPar=0; iPar < 7; iPar++ )
      {
        Double_t error = V0.GetCovariance(iPar,iPar);
        if(error < 0.) {std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! parameter "<< iPar<<"  error "<<error <<std::endl; error = 1.e20;}
        error = TMath::Sqrt(error);
        res[iPar]  = V0.GetParameter(iPar) - parMC[iPar];
        if(fabs(error) > 1.e-20) pull[iPar] = res[iPar]/error;
      }
      
      res[7] = mass - parMC[7];
      if(fabs(sigmaMass) > 1.e-20) pull[7] = res[7]/sigmaMass;
      
      for(int iPar=0; iPar < 8; iPar++ )
      {
        fhKFParticle[iPar]->Fill(res[iPar]);
        fhKFParticle[8+iPar]->Fill(pull[iPar]);
        
      }

      V0_old.SetProductionVertex(PrimVertESDKF);
//      std::cout << "fC[35]  "<<V0_old.GetCovariance(35) << std::endl;
    }
  }
}

void AliKFParticleTest::TestKFParticleSIMD(AliESDEvent *event, vector<TESDTrackInfo> &ESDTrackInfo)
{
  if( !gAlice ) return;
  AliRunLoader *rl = AliRunLoader::Instance(); 

  AliStack *stack = rl->Stack();
  if( !stack ) return;
  
  Int_t nParticles = ESDTrackInfo.size();
  for( Int_t iTr = 0; iTr<nParticles; iTr++ ){ //* first daughter

    TESDTrackInfo &info = ESDTrackInfo[iTr];

    for( Int_t jTr = iTr+1; jTr<nParticles; jTr++ ){  //* second daughter

      TESDTrackInfo &jnfo = ESDTrackInfo[jTr];
      if( info.mcMotherID != jnfo.mcMotherID) continue;
      
      //* check for different charge

      if( info.fParticle.GetQ() == jnfo.fParticle.GetQ() ) continue;      

      //* construct V0 mother

      AliKFParticleSIMD V0( AliKFParticleSIMD(info.fParticle, info.mcPDG), AliKFParticleSIMD(jnfo.fParticle, jnfo.mcPDG) );

      //* check V0 Chi^2

///      if( V0.GetNDF()<1 ) continue;
 //     if( TMath::Sqrt(TMath::Abs(V0.GetChi2()/V0.GetNDF())) >3. ) continue;

      //* subtruct daughters from primary vertex 

///      AliKFVertex primVtxCopy = primVtx;

///      if( info.fPrimUsedFlag ) primVtxCopy -= info.fParticle;
///      if( jnfo.fPrimUsedFlag ) primVtxCopy -= jnfo.fParticle;

      //* Check V0 Chi^2 deviation from primary vertex 

  //    if( V0.GetDeviationFromVertex( primVtxCopy ) >3. ) continue;

      //* Add V0 to primary vertex to improve the primary vertex resolution

///      primVtxCopy += V0;      

      //* Set production vertex for V0

///      V0.SetProductionVertex( primVtxCopy );

      //* Check chi^2 for a case

   //   if( TMath::Sqrt( TMath::Abs(V0.GetChi2()/V0.GetNDF()) >3. )) continue;

      //* Get V0 decay length with estimated error

///      Double_t length, sigmaLength;
  //    if( V0.GetDecayLength( length, sigmaLength ) ) continue;

      //* Reject V0 if it decays too close to the primary vertex

 //     if( length  <3.*sigmaLength ) continue;

      //* Get V0 invariant mass and plot it

      fvec mass, sigmaMass;
      fvec mask = V0.GetMass(mass,sigmaMass);
      if( mask[0] ) continue;
      fhKFParticleSIMD[25]->Fill(mass[0]);
      
      TParticle * part = stack->Particle(info.mcMotherID);
      
      Double_t Emc = sqrt(part->P()*part->P() + part->GetMass()*part->GetMass());
      Double_t res[8] = {0,0,0,0,0,0,0,0}, 
               pull[8] = {0,0,0,0,0,0,0,0}, 
               parMC[8] = { part->Vx(), part->Vy(), part->Vz(), part->Px(), part->Py(), part->Pz(), Emc, part->GetMass() };
      for(int iPar=0; iPar < 7; iPar++ )
      {
        fvec error = sqrt(V0.GetCovariance(iPar,iPar));
        res[iPar]  = (V0.GetParameter(iPar))[0] - parMC[iPar];
        if(fabs(error[0]) > 1.e-20) pull[iPar] = res[iPar]/error[0];
      }
      
      res[7] = mass[0] - parMC[7];
      if(fabs(sigmaMass[0]) > 1.e-20) pull[7] = res[7]/sigmaMass[0];
      
      for(int iPar=0; iPar < 8; iPar++ )
      {
        fhKFParticleSIMD[iPar]->Fill(res[iPar]);
        fhKFParticleSIMD[8+iPar]->Fill(pull[iPar]);
      }
    }
  }
}

void AliKFParticleTest::TestScalarVsSIMD(AliESDEvent *event, vector<TESDTrackInfo> &ESDTrackInfo)
{
  if( !gAlice ) return;
  AliRunLoader *rl = AliRunLoader::Instance(); 

  AliStack *stack = rl->Stack();
  if( !stack ) return;
  
  Int_t nParticles = ESDTrackInfo.size();
  for( Int_t iTr = 0; iTr<nParticles; iTr++ ){ //* first daughter

    TESDTrackInfo &info = ESDTrackInfo[iTr];

    for( Int_t jTr = iTr+1; jTr<nParticles; jTr++ ){  //* second daughter

      TESDTrackInfo &jnfo = ESDTrackInfo[jTr];
      if( info.mcMotherID != jnfo.mcMotherID) continue;
      
      //* check for different charge

      if( info.fParticle.GetQ() == jnfo.fParticle.GetQ() ) continue;      

      //* construct V0 mother
std::cout << "SIMD"<<std::endl;
      AliKFParticleSIMD V0SIMD( AliKFParticleSIMD(info.fParticle, info.mcPDG), AliKFParticleSIMD(jnfo.fParticle, jnfo.mcPDG) );
std::cout << "Scalar"<<std::endl;
      AliKFParticle V0( info.fParticle, jnfo.fParticle );

      fvec mass, sigmaMass;
      fvec mask = V0SIMD.GetMass(mass,sigmaMass);
      if( mask[0] ) continue;
      fhKFParticleSIMD[25]->Fill(mass[0]);
      
      TParticle * part = stack->Particle(info.mcMotherID);
      
      Double_t Emc = sqrt(part->P()*part->P() + part->GetMass()*part->GetMass());
      Double_t res[8] = {0,0,0,0,0,0,0,0}, 
               pull[8] = {0,0,0,0,0,0,0,0},
               resSIMD[8] = {0,0,0,0,0,0,0,0}, 
               pullSIMD[8] = {0,0,0,0,0,0,0,0},
               parMC[8] = { part->Vx(), part->Vy(), part->Vz(), part->Px(), part->Py(), part->Pz(), Emc, part->GetMass() };
      for(int iPar=0; iPar < 7; iPar++ )
      {
        Double_t error = sqrt(V0.GetCovariance(iPar,iPar));
        res[iPar]  = V0.GetParameter(iPar) - parMC[iPar];
        if(fabs(error) > 1.e-20) pull[iPar] = res[iPar]/error;
        
        fvec errorSIMD = sqrt(V0SIMD.GetCovariance(iPar,iPar));
        resSIMD[iPar]  = (V0SIMD.GetParameter(iPar))[0] - parMC[iPar];
        if(fabs(errorSIMD[0]) > 1.e-20) pullSIMD[iPar] = res[iPar]/errorSIMD[0];
        std::cout << iPar << "  " << res[iPar] << "  " << resSIMD[iPar] << "  "<<error<<"  "<<errorSIMD<<endl;
        int yuyuy=0;
        std::cin>>yuyuy;
      }
      
      resSIMD[7] = mass[0] - parMC[7];
      if(fabs(sigmaMass[0]) > 1.e-20) pullSIMD[7] = resSIMD[7]/sigmaMass[0];
    }
  }
}

void AliKFParticleTest::TestKFParticleSpeed(AliESDEvent *event, float &timer, float &timerSIMD, int &NPart)
{
  vector<TESDTrackInfo> ESDTrackInfoPos;
  vector<TESDTrackInfo> ESDTrackInfoNeg;

  if( !gAlice ) return;
  AliRunLoader *rl = AliRunLoader::Instance(); 

  AliStack *stack = rl->Stack();
  if( !stack ) return;

  Int_t nESDTracks=event->GetNumberOfTracks();

  for (Int_t iTr=0; iTr<nESDTracks; iTr++)
  {
    TESDTrackInfo info;
    info.mcPDG = -1;
    info.mcMotherID = -1;
    info.TrackID = iTr;
    //* track quality check

    AliESDtrack *pTrack = event->GetTrack(iTr);    
    if( !pTrack  ) continue;
    Int_t PDG = ( pTrack->GetSigned1Pt() <0 ) ?321 :211;
    //* take MC PDG  
    Int_t mcID = TMath::Abs(pTrack->GetLabel());
    TParticle * part = stack->Particle(TMath::Abs(mcID));

    if( part )
    {
      info.mcPDG = part->GetPdgCode();
      PDG = info.mcPDG;
      if( mcID>=0 ) info.mcMotherID = part->GetFirstMother();
    }
    //* Construct KFParticle for the track
    info.fParticle = AliKFParticle( *pTrack, PDG );
//    info.fParticle = AliKFParticle( *pTrack, 211 );

    if(info.fParticle.GetQ() < 0) ESDTrackInfoNeg.push_back(info);
    if(info.fParticle.GetQ() > 0) ESDTrackInfoPos.push_back(info);
  }

  Int_t NPos = int(float(ESDTrackInfoPos.size())/float(fvecLen)) * fvecLen;
  Int_t NNeg = int(float(ESDTrackInfoNeg.size())/float(fvecLen)) * fvecLen;

  Int_t NTimes = 1;

  TStopwatch time;
  time.Start();

  for(int iTime=0; iTime<NTimes; iTime++)
  {
    for( Int_t iTr = 0; iTr<NPos; iTr++ ){ //* first daughter
      for( Int_t jTr = 0; jTr<NNeg; jTr++ ){  //* second daughter

        AliKFParticle &info = ESDTrackInfoPos[iTr].fParticle;
        AliKFParticle &jnfo = ESDTrackInfoNeg[jTr].fParticle;

        AliKFParticle V0( info, jnfo );
      }
    }
  }
  time.Stop();

  timer += time.RealTime();

  TStopwatch timeSIMD;
  timeSIMD.Start();
  for(int iTime=0; iTime<NTimes; iTime++)
  {
    for( Int_t iTr = 0; iTr<NPos; iTr+=fvecLen ){ //* first daughter
      for( Int_t jTr = 0; jTr<NNeg; jTr+=fvecLen ){  //* second daughter
        AliKFParticle *info[fvecLen];
        AliKFParticle *jnfo[fvecLen];
        for(int i=0; i<fvecLen; i++)
        {
          info[i]= &(ESDTrackInfoPos[iTr+i].fParticle);
          jnfo[i]= &(ESDTrackInfoNeg[jTr+i].fParticle);
        }

        AliKFParticleSIMD PartPos( info, 211 );
        for(int i=0; i<fvecLen; i++)
        {
          AliKFParticle *jnfoTemp = jnfo[0];
          for(int iVec=0; iVec<fvecLen-1; iVec++)
            jnfo[iVec] = jnfo[iVec+1];
          jnfo[fvecLen-1] = jnfoTemp;

          AliKFParticleSIMD PartNeg( jnfo, 211 );
          AliKFParticleSIMD V0( PartPos, PartNeg );
        }
      }
    }
  }
  timeSIMD.Stop();

  timerSIMD += timeSIMD.RealTime();

  NPart += NPos * NNeg;
}

void AliKFParticleTest::TestKFParticleBG(AliESDEvent *event)
{
  if( !gAlice ) return;
  AliRunLoader *rl = AliRunLoader::Instance(); 

  AliStack *stack = rl->Stack();
  if( !stack ) return;

  Int_t nESDTracks=event->GetNumberOfTracks();
  
  vector<TESDTrackInfo> ESDTrackInfo;
  //* Fill ESDTrackInfo array

  for (Int_t iTr=0; iTr<nESDTracks; iTr++)
  {
    TESDTrackInfo info;
    info.mcPDG = -1;
    info.mcMotherID = -1;
    info.TrackID = iTr;
    //* track quality check

    AliESDtrack *pTrack = event->GetTrack(iTr);    
    if( !pTrack  ) continue;
    Int_t PDG = ( pTrack->GetSigned1Pt() <0 ) ?321 :211;
    //* take MC PDG  
    Int_t mcID = TMath::Abs(pTrack->GetLabel());
    TParticle * part = stack->Particle(TMath::Abs(mcID));

    if( part )
    {
      info.mcPDG = part->GetPdgCode();
      PDG = info.mcPDG;
      if( mcID>=0 ) info.mcMotherID = part->GetFirstMother();
    }

    //* Construct KFParticle for the track
//    info.fParticle = AliKFParticle( *pTrack, PDG );
    info.fParticle = AliKFParticle( *pTrack, 211 );

    ESDTrackInfo.push_back(info);
  }
 
  Int_t nParticles = ESDTrackInfo.size();
  for( Int_t iTr = 0; iTr<nParticles; iTr++ ){ //* first daughter

    TESDTrackInfo &info = ESDTrackInfo[iTr];

    for( Int_t jTr = iTr+1; jTr<nParticles; jTr++ ){  //* second daughter

      TESDTrackInfo &jnfo = ESDTrackInfo[jTr];
      
      //* check for different charge

      if( info.fParticle.GetQ() == jnfo.fParticle.GetQ() ) continue;      

      //* construct V0 mother

     AliKFParticle V0( info.fParticle, jnfo.fParticle );
//       AliKFParticle V0;
//       const AliKFParticle* Daughters[2] = {&info.fParticle,&jnfo.fParticle};
//       V0.Construct(Daughters, 2);
//      V0.SetMassConstraint(0.493667,0);
      //* check V0 Chi^2
      if( V0.GetNDF()<1 ) continue;
      
      Double_t mass, sigmaMass;
      V0.GetMass( mass, sigmaMass );
      fhKFParticle[28]->Fill(mass);
      
    }
  }
}

void AliKFParticleTest::WriteHistos()
{
  TFile *out = new TFile("out.root","recreate");

  for(int i=0; i<cNHistos; i++)
    if(fhKFParticle[i]) fhKFParticle[i]->Write();

  for(int i=0; i<cNHistos; i++)
    if(fhKFParticleSIMD[i]) fhKFParticleSIMD[i]->Write();

  for(int i=0; i<10; i++)
    fhKFInputTracks[i]->Write();

  out->Close();
}

void AliKFParticleTest::RunTest()
{
  for (Int_t ifi=0; ifi<static_cast<Int_t>(fInpFileName.size()); ifi++) {

    TString filename, esdfile;

    filename = fInpFileName[ifi];
    esdfile  = fInpFileName[ifi];

    filename += "/galice.root";
    esdfile  += "/AliESDs.root";
//    sprintf(filename,"%s%s/galice.root",dire,nstring);
//    sprintf(esdfile,"%s%s/AliESDs.root",dire,nstring);

//    cout <<"  Opening "<<filename.Data()<<"\nand ESD "<<esdfile.Data()<<endl;

    if (gAlice) {
      delete AliRunLoader::Instance();
      delete gAlice;
      gAlice=0;
    }

    AliRunLoader *rl = AliRunLoader::Open(filename.Data());

    if ( !rl ) { 
      ::Error("AliKFParticleTest.C","Can not open session !");
      continue;
    }
    if (rl->LoadgAlice()) {
      ::Error("AliKFParticleTest.C","LoadgAlice returned error !");
      delete rl;
      continue;
    }
    if (rl->LoadHeader()) {
      ::Error("AliKFParticleTest.C","LoadHeader returned error !");
      delete rl;
      continue;
    }

    rl->LoadKinematics();
//    AliTracker::SetFieldMap(gAlice->Field(),1);
    AliKFParticle::SetField( -5.f );
    
    // Open file with the ESD
    TFile *ef=TFile::Open(esdfile.Data());

    //Check if the file could be opened
    if (!ef || !ef->IsOpen()) {cout<<"Error open AliESDs.root !\n"; continue ;}

    //create event object
    AliESDEvent *event = new AliESDEvent();

    //Set pointer to the esd tree in the file
    TTree* tree = (TTree*) ef->Get("esdTree");
    
    //check if the tree exists
    if (!tree) {cout<<"no ESD tree found\n"; continue;};
    
    //Set pointer to the esd object in the tree
    event->ReadFromTree(tree);
    
    //Number of events
    Int_t nevents=tree->GetEntriesFast();
    cout << "Number of events: " << nevents << endl;

    float timer=0;
    float timerSIMD=0;
    int NPart = 0;

    for (Int_t iev=0; iev<nevents; iev++){
      tree->GetEvent(iev);
      rl->GetEvent(iev);
      AliKFParticle::SetField( event->GetMagneticField() );
      AliKFParticleSIMD::SetField( event->GetMagneticField() );

      vector<TESDTrackInfo> ESDTrackInfo;

      SelectParticles(event, ESDTrackInfo);
      TestKFParticle(event, ESDTrackInfo);
      CheckInputTracks(event, ESDTrackInfo );
      // TestKFParticleSIMD(event, ESDTrackInfo);
//      TestKFParticleSpeed(event, timer, timerSIMD, NPart);
//      TestScalarVsSIMD(event, ESDTrackInfo);
      TestKFParticleBG(event);
    }

    cout << "NPart  " << NPart << "  timer  " << timer << "  timerSIMD  " << timerSIMD << endl;
    if(NPart ==0 ) NPart = 1;
    cout << "s/Part [s]:  double = " << (timer/NPart) << "  SIMD = " << (timerSIMD/NPart) << endl;
    delete event;
    ef->Close();
  }

  WriteHistos();
}