// -------------------------------------------------------------------------
// -----                 CbmStsFitPerformanceTask source file             -----
// -----                  Created 02/02/05  by E. Kryshen              -----
// -------------------------------------------------------------------------

#include "CbmStsFitPerformanceTask.h"
#include "CbmStsKFTrackFitter.h"
#include "CbmPVFinderKF.h"
#include "CbmStsKFSecondaryVertexFinder.h"
#include "CbmKF.h"
#include "CbmKFMath.h"
#include "CbmKFTrack.h"
#include "CbmKFVertex.h"
#include "CbmMCTrack.h"
#include "CbmStsTrack.h"
#include "CbmStsTrackMatch.h"
#include "CbmStsHit.h"
#include "CbmRootManager.h"
#include "CbmMCApplication.h"
#include "iostream.h"
#include "iomanip.h"
#include "TVector3.h"
#include "TF1.h"
#include "TParticlePDG.h"
#include "TDatabasePDG.h"

#include "CbmKFPrimaryVertexFinder.h"
#include "CbmKFPrimaryVertexFinder.h"
#include "CbmKF.h"
#include "CbmKFTrackInterface.h"
#include "CbmKFVertexInterface.h"
#include "CbmKFTrack.h"
#include "CbmKFVertex.h"

#include "CbmStsTrack.h"
#include "CbmVertex.h"


// -------------------------------------------------------------------------

void writedir2current( TObject *obj ){
  if( !obj->IsFolder() ) obj->Write();
  else{
    TDirectory *cur = gDirectory;
    TDirectory *sub = cur->mkdir(obj->GetName());
    sub->cd();
    TList *listSub = ((TDirectory*)obj)->GetList();
    TIter it(listSub);
    while( TObject *obj1=it() ) writedir2current(obj1);
    cur->cd();
  }
}

void CbmStsFitPerformanceTask::CreateTrackHisto(TH1D* hist[10], const char* name, const char* title ){
  struct {    
    char *name;
    char *title;
    Int_t n;
    Double_t l,r;
  } Table[10]= 
    { 
      {"x",  "Residual X [#mum]",                   100, -100., 100.},
      {"y",  "Residual Y [#mum]",                   100, -100., 100.},
      {"tx", "Residual Tx [mrad]",                  100,   -2.,   2.},
      {"ty", "Residual Ty [mrad]",                  100,   -2.,   2.},
      {"P",  "Resolution P/Q [100%]",               100,   -.1,  .1 },
      {"px", "Pull X [residual/estimated_error]",   100,  -10.,  10.},
      {"py", "Pull Y [residual/estimated_error]",   100,  -10.,  10.},
      {"ptx","Pull Tx [residual/estimated_error]",  100,  -10.,  10.},
      {"pty","Pull Ty [residual/estimated_error]",  100,  -10.,  10.},
      {"pQP","Pull Q/P [residual/estimated_error]", 100,  -10.,  10.}
    };

  for( int i=0; i<10; i++ ){
    char n[225], t[255];
    sprintf(n,"%s_%s",name,Table[i].name);
    sprintf(t,"%s %s",title,Table[i].title);
    hist[i] = new TH1D(n,t, Table[i].n, Table[i].l, Table[i].r);
  }
}

void CbmStsFitPerformanceTask::CreateVertexHisto(TH1D* hist[9], const char* name, const char* title ){
  struct {    
    char *name;
    char *title;
    Int_t n;
    Double_t l,r;
  } Table[9]= 
    { 
      {"x",  "Residual X [#mum]",                   100, -5., 5.},
      {"y",  "Residual Y [#mum]",                   100, -5., 5.},
      {"z",  "Residual Z [#mum]",                   100, -40., 40.},
      {"px", "Pull X [residual/estimated_error]",   100,  -10.,  10.},
      {"py", "Pull Y [residual/estimated_error]",   100,  -10.,  10.},
      {"pz", "Pull Z [residual/estimated_error]",   100,  -10.,  10.},
      {"chi2","Chi2/NDF",                           100,  0.,  10.},
      {"prob","Prob(Chi2,NDF)",                     100,  0.,  1.},
      {"ntracks","N tracks",                        100,  0.,  1000.},
    };

  for( int i=0; i<9; i++ ){
    char n[225], t[255];
    sprintf(n,"%s_%s",name,Table[i].name);
    sprintf(t,"%s %s",title,Table[i].title);
    hist[i] = new TH1D(n,t, Table[i].n, Table[i].l, Table[i].r);
  }
}

void CbmStsFitPerformanceTask::CreateD0Histo(TH1D* hist[15], const char* name, const char* title ){
  struct {    
    char *name;
    char *title;
    Int_t n;
    Double_t l,r;
  } Table[14]= 
    { 
      {"x",  "Residual X [#mum]",                   100, -100., 100.},
      {"y",  "Residual Y [#mum]",                   100, -100., 100.},
      {"z",  "Residual Z [#mum]",                   100, -500., 500.},
      {"px", "Pull X [residual/estimated_error]",   100,  -10.,  10.},
      {"py", "Pull Y [residual/estimated_error]",   100,  -10.,  10.},
      {"pz", "Pull Z [residual/estimated_error]",   100,  -10.,  10.},
      {"chi2","Chi2/NDF",                           100,  0.,  10.},
      {"prob","Prob(Chi2,NDF)",                     100,  0.,  1.},
      {"mass","Residual Mass",                      100,  -.1,  .1},
      {"pmass", "Pull Mass [residual/estimated_error]",   100,  -10.,  10.},
      {"KaonP", "Kaon P resolution",                100,  -.05,  .05},
      {"PionP", "Pion P resolution",                100,  -.05,  .05},
      {"KaonP0", "Kaon P resolution before fit",    100,  -.05,  .05},
      {"PionP0", "Pion P resolutionbefore fit",     100,  -.05,  .05}
    };

  for( int i=0; i<14; i++ ){
    char n[225], t[255];
    sprintf(n,"%s_%s",name,Table[i].name);
    sprintf(t,"%s %s",title,Table[i].title);
    hist[i] = new TH1D(n,t, Table[i].n, Table[i].l, Table[i].r);
  }
}


// -----   Standard constructor   ------------------------------------------
CbmStsFitPerformanceTask::CbmStsFitPerformanceTask(const char* name, Int_t iVerbose )
                 :CbmTask(name,iVerbose){
  fTrackAnalysis=1;
  fVertexAnalysis=0;
  fD0Analysis=0;
}
// -------------------------------------------------------------------------



// -----   Destructor   ----------------------------------------------------
CbmStsFitPerformanceTask::~CbmStsFitPerformanceTask(){
}

// -----   Init CbmStsFitPerformanceTask task   -------------------------------
InitStatus CbmStsFitPerformanceTask::Init(){
  fEvent = 0;

  TDirectory *curdir = gDirectory;
  histodir = gDirectory->mkdir("StsFitPerformance");
  histodir->cd();

  gDirectory->mkdir("TrackFit");
  gDirectory->cd("TrackFit");
  {
    fhChi2 = new TH1D("hChi2","hChi2",100,0,10);
    fhProb = new TH1D("hProb","hProb",100,0,1.0);

    gDirectory->mkdir("AtFirstHit");  
    gDirectory->cd("AtFirstHit");
    CreateTrackHisto( fhFrst, "fst", "at first STS Hit" );
    gDirectory->cd("..");
    gDirectory->mkdir("AtLastHit");
    gDirectory->cd("AtLastHit");
    CreateTrackHisto( fhLast, "lst", "at last  STS Hit" );
    gDirectory->cd("..");
    gDirectory->mkdir("AtImpactPoint");
    gDirectory->cd("AtImpactPoint");
    CreateTrackHisto( fhImp,  "imp", "at impact point" );
    gDirectory->cd("..");
    gDirectory->mkdir("InTheMiddle");
    gDirectory->cd("InTheMiddle");
    CreateTrackHisto( fhMid,  "mid", "at 4th station" );
    gDirectory->cd("..");
    gDirectory->mkdir("FittedToVertex");
    gDirectory->cd("FittedToVertex");
    CreateTrackHisto( fhVfit, "vfit", "for vertex fitted track" );
    gDirectory->cd("..");
  
    gDirectory->mkdir("PSlidesOnP");
    gDirectory->cd("PSlidesOnP");
    fhPq[0] = new TH1D("Pq0","Resolution P/Q at impact point, 0<=P<1" , 100,  -.1 ,  .1 );
    fhPq[1] = new TH1D("Pq1","Resolution P/Q at impact point, 1<=P<2" , 100,  -.1 ,  .1 );
    fhPq[2] = new TH1D("Pq2","Resolution P/Q at impact point, 2<=P<3" , 100,  -.1 ,  .1 );
    fhPq[3] = new TH1D("Pq3","Resolution P/Q at impact point, 3<=P<4" , 100,  -.1 ,  .1 );
    fhPq[4] = new TH1D("Pq4","Resolution P/Q at impact point, 4<=P<5" , 100,  -.1 ,  .1 );
    fhPq[5] = new TH1D("Pq5","Resolution P/Q at impact point, 5<=P<6" , 100,  -.1 ,  .1 );
    fhPq[6] = new TH1D("Pq6","Resolution P/Q at impact point, 6<=P<7" , 100,  -.1 ,  .1 );
    fhPq[7] = new TH1D("Pq7","Resolution P/Q at impact point, 7<=P<8" , 100,  -.1 ,  .1 );
    fhPq[8] = new TH1D("Pq8","Resolution P/Q at impact point, 8<=P<9" , 100,  -.1 ,  .1 );
    fhPq[9] = new TH1D("Pq9","Resolution P/Q at impact point, 9<=P<10" , 100,  -.1 ,  .1 );
    fhPq[10] = new TH1D("Pq10","Resolution P/Q at impact point, 10<=P<11" , 100,  -.1 ,  .1 );
    gDirectory->cd("..");
  }
  gDirectory->cd("..");

  gDirectory->mkdir("VertexFit");  
  gDirectory->cd("VertexFit");
  {
    CreateVertexHisto( fhVtx, "vtx", "for Primary Vertex" );
    gDirectory->mkdir("VertexOnNTracks");
    gDirectory->cd("VertexOnNTracks");
    for( int i=0; i<13; i++){
      char name[225], namedir[225], title[225];
      if( i==0 ){
	sprintf(namedir,"AllTracks");
	sprintf(name,"Vall");
	sprintf(title,"for Primary Vertex on all tracks");
      }else{
	sprintf(namedir,"%iTracks",i*50);
	sprintf(name,"V%i",i*50);
	sprintf(title,"for Primary Vertex on %i tracks",i*50);
      }
      gDirectory->mkdir(namedir);
      gDirectory->cd(namedir);
      CreateVertexHisto( fhV[i], name, title );
      gDirectory->cd("..");
    }
    gDirectory->cd("..");
  }
  gDirectory->cd("..");

  gDirectory->mkdir("D0Fit");
  gDirectory->cd("D0Fit");
  {
    gDirectory->mkdir("No constraints");
    gDirectory->cd("No constraints");  
    CreateD0Histo( fhD0[0], "D0", "for D0 Sec. Vertex" );
    gDirectory->cd("..");
    gDirectory->mkdir("Topological constraint");
    gDirectory->cd("Topological constraint");
    CreateD0Histo( fhD0[1], "D0T", "for D0 Topo Sec. Vertex" );
    gDirectory->cd("..");
    gDirectory->mkdir("Mass constraint");
    gDirectory->cd("Mass constraint");
    CreateD0Histo( fhD0[2], "D0M", "for D0 Mass Sec. Vertex" );
    gDirectory->cd("..");
    gDirectory->mkdir("Mass+Topological constraint");
    gDirectory->cd("Mass+Topological constraint");
    CreateD0Histo( fhD0[3], "D0TM", "for D0 Topo+Mass Sec. Vertex" );
    gDirectory->cd("..");  
  }
  gDirectory->cd("..");
  
  curdir->cd();
  return ReInit();
}

// -----   ReInit CbmStsFitPerformanceTask task   -------------------------------
InitStatus CbmStsFitPerformanceTask::ReInit(){

  CbmRootManager* fManger =CbmRootManager::Instance();	
  fMCTrackArray     = (TClonesArray*) fManger->GetObject("MCTrack");
  fStsPointArray    = (TClonesArray*) fManger->GetObject("STSPoint");
  fRecStsTrackArray = (TClonesArray*) fManger->GetObject("STSTrack");
  fHitArray = (TClonesArray*) fManger->GetObject("STSHit");
  fPrimaryVertex = (CbmVertex*) fManger->GetObject("PrimaryVertex");
  fSTSTrackMatch = (TClonesArray*) fManger->GetObject("STSTrackMatch");
  return kSUCCESS;
}
// -------------------------------------------------------------------------



// -----   Exec CbmStsFitPerformanceTask task   -------------------------------
void CbmStsFitPerformanceTask::Exec(Option_t * option){
  cout << "Event: " << ++fEvent << " ";

  Int_t nTracks=fRecStsTrackArray->GetEntriesFast();
  cout << "  nTracks: " << nTracks << endl;

  if( !fSTSTrackMatch ) return;

  Double_t mc[6];

  Int_t Quality[nTracks];

  for (Int_t iTrack=0; iTrack<nTracks; iTrack++ ){
    Quality[iTrack]=0;
    CbmStsTrack*   track= (CbmStsTrack*) fRecStsTrackArray->At(iTrack);
    if( !track ) continue;
    CbmStsTrackMatch *m = (CbmStsTrackMatch*) fSTSTrackMatch->At(iTrack);
    if( !m ) continue;
    double ratio = 1.*m->GetNofTrueHits()
      /(m->GetNofTrueHits()+m->GetNofWrongHits()+m->GetNofFakeHits());      
    if( ratio<.7 ) continue;
    Int_t mcTrackID = m->GetMCTrackId();    
    if (mcTrackID<0)  continue;
    if (!IsLong(track)) continue;
  
    CbmMCTrack*  mcTrack= (CbmMCTrack*)  fMCTrackArray->At(mcTrackID);
    if( !mcTrack ) continue;
    if (fabs((mcTrack->GetStartVertex()).z())>1.) continue;

    if( mcTrack->GetMotherID() !=-1 ) continue;
    Quality[iTrack]=1;
  }

  if( fTrackAnalysis ){
    for (Int_t iTrack=0;iTrack<nTracks;iTrack++){
      if( Quality[iTrack]<1 ) continue;
      CbmStsTrack*   track= (CbmStsTrack*) fRecStsTrackArray->At(iTrack);
      CbmStsTrackMatch *m = (CbmStsTrackMatch*) fSTSTrackMatch->At(iTrack);
      CbmMCTrack*  mcTrack= (CbmMCTrack*)  fMCTrackArray->At(m->GetMCTrackId());

      // Get MC points;
      vector<CbmStsPoint*> vPoints;
      for( Int_t i=0; i<track->GetNHits(); i++ ){
	Int_t hitID = track->GetHitIndex(i);
	if( hitID<0 ) continue;
	CbmStsHit* hit = (CbmStsHit*) fHitArray->At(hitID);
	if( !hit ) continue;
	Int_t pointID = hit->GetRefIndex(); 
	if( pointID<0 ) continue;
	CbmStsPoint *point = (CbmStsPoint*) fStsPointArray->At(pointID);
	if( !point ) continue;
	vPoints.push_back(point);
      }

      // impact mc track
      Double_t mci[6];
    
      mci[0] = (mcTrack->GetStartVertex()).x();
      mci[1] = (mcTrack->GetStartVertex()).y();
      mci[2] = (mcTrack->GetMomentum()).x() / (mcTrack->GetMomentum()).z();
      mci[3] = (mcTrack->GetMomentum()).y() / (mcTrack->GetMomentum()).z();
      mci[4] = GetCharge(mcTrack)/(mcTrack->GetMomentum()).Mag();
      mci[5] = (mcTrack->GetStartVertex()).z();
      
      if( !vPoints.empty() )
	{
	  // first point
	  
	  FillMCStateVectors(vPoints.front(),mc);
	  FillTrackHisto( mc, track, fhFrst );
	  
	  // last point
	  
	  FillMCStateVectors(vPoints.back(),mc,1);
	  FillTrackHisto( mc, track, fhLast );
	  
	  // impact point
	  
	  FillTrackHisto(mci,track,fhImp);    
	  
	  // 4th station (check smoother)
	  
	  for( vector<CbmStsPoint*>::iterator i=vPoints.begin(); i!=vPoints.end(); i++){
	    int id = (*i)->GetDetectorID();
	    CbmKF &KF = *CbmKF::Instance();
	    if( KF.StsStationIDMap.find(id) == KF.StsStationIDMap.end() ) continue;
	    if( KF.StsStationIDMap[id] != 3 ) continue;
	    FillMCStateVectors(*i,mc,1);
	    FillTrackHisto( mc, track, fhMid );	  
	    break;
	  }	
	}
    
    
      if( fPrimaryVertex && mcTrack->GetMotherID() ==-1 ){      
	
	// fit track to Primary Vertex
	
	CbmStsKFTrackFitter Fitter;
	CbmStsTrack tt(*track);
	Fitter.FitToVertex( &tt, fPrimaryVertex, tt.GetParamFirst() );
	FillTrackHisto(mci,&tt,fhVfit);
      
	// fill momentum resolution 
	
	double z = mci[5];
	CbmTrackParam param;
	Fitter.Extrapolate(track, z, &param);
	int iP= int(fabs(1./mci[4]));
	if( iP>=0 && iP<11 ) fhPq[iP]->Fill( (mci[4]/param.GetQp() - 1.) );      
      }

      fhChi2->Fill(track->GetChi2()/track->GetNDF());
      fhProb->Fill(TMath::Prob(track->GetChi2(),track->GetNDF()));
    }
  }

  if( fPrimaryVertex ){

    // find MC Primary vertex
    
    TVector3 MC_V(0,0,0);
    Bool_t Is_MC_V = 0;
    {
      TVector3 MC_Vcurr(0,0,0);
      Int_t nvtracks=0, nvtrackscurr=0;
      Int_t nMCTracks = fMCTrackArray->GetEntriesFast();
      for (Int_t iTrack=0;iTrack<nMCTracks;iTrack++){
	CbmMCTrack*  mcTrack= (CbmMCTrack*)  fMCTrackArray->At(iTrack);
	if( mcTrack->GetMotherID() !=-1 ) continue; // not primary track
	double z = (mcTrack->GetStartVertex()).z();
	if ( !Is_MC_V || fabs(z-MC_Vcurr.Z())>1.e-7 ){// new vertex
	  Is_MC_V = 1;
	  if( nvtrackscurr > nvtracks ){
	    MC_V = MC_Vcurr;
	    nvtracks = nvtrackscurr;
	  }
	  MC_Vcurr = mcTrack->GetStartVertex();
	  nvtrackscurr = 1;
	}else nvtrackscurr++;      
      }
      if( nvtrackscurr > nvtracks ) MC_V = MC_Vcurr;
    }

    
    if( Is_MC_V ){

      // primary vertex fit performance
 
      FillVertexHisto( MC_V, fPrimaryVertex, fhVtx);
    
      if( fVertexAnalysis ){
	
	CbmPVFinderKF Finder;
	TClonesArray TracksToFit("CbmStsTrack");
	Int_t N=0;
	for (Int_t iTrack=0;iTrack<nTracks;iTrack++){
	  if( Quality[iTrack]<1 ) continue;
	  new( TracksToFit[N] ) CbmStsTrack();
	  *(CbmStsTrack*)TracksToFit.At(N) = *(CbmStsTrack*)fRecStsTrackArray->At(iTrack);
	  N++;
	  if( N%50!=0 ) continue;
	  Int_t i = N/50;
	  if( i>=13 ) continue;
	  CbmVertex V;
	  Finder.FindPrimaryVertex( &TracksToFit, &V );	
	  FillVertexHisto( MC_V, &V, fhV[i] );
	}
	CbmVertex V;
	Finder.FindPrimaryVertex( &TracksToFit, &V );	
	FillVertexHisto( MC_V, &V, fhV[0] );
      } 
    
      // D0 fit performance (if there are)

      if( fD0Analysis&&fabs(MC_V.Z())<1.e-5 ){
	// search for  Kaon
	for (Int_t iK=0;iK<nTracks;iK++){
	  if( Quality[iK]<1 ) continue;
	  CbmStsTrack*   tK= (CbmStsTrack*) fRecStsTrackArray->At(iK);	  
	  CbmStsTrackMatch *m = (CbmStsTrackMatch*) fSTSTrackMatch->At(iK);
	  CbmMCTrack*  mcK = (CbmMCTrack*)  fMCTrackArray->At(m->GetMCTrackId());
	  if( mcK->GetMotherID() !=-1 ) continue; // not primary or D0 track
	  if( abs(mcK->GetPdgCode())!=321 ) continue;	  
	  double zK = ( mcK->GetStartVertex() ).z();
	  if( zK-MC_V.Z()<1.e-5 ) continue; // primary
	  double MCPK = mcK->GetMomentum().Mag();
	  CbmStsKFTrackFitter StsKFFitter;
	  StsKFFitter.DoFit( tK, 321 ); // refit
	  // search for Pion
	  for (Int_t iP=0;iP<nTracks;iP++){
	    if( Quality[iP]<1 ) continue;
	    CbmStsTrack*   tP= (CbmStsTrack*) fRecStsTrackArray->At(iP);
	    m = (CbmStsTrackMatch*) fSTSTrackMatch->At(iP);
	    CbmMCTrack*  mcP = (CbmMCTrack*)  fMCTrackArray->At(m->GetMCTrackId());
	    if( mcP->GetMotherID() !=-1 ) continue; // not primary or D0 track
	    if( abs(mcP->GetPdgCode())!=211 ) continue;	
	    if( mcK->GetPdgCode()*mcP->GetPdgCode() >=0 ) continue; //same charge
	    double zP = ( mcP->GetStartVertex() ).z();
	    if( fabs(zP-zK)>1.e-8 ) continue; // different vertex
	    double MCPP = mcP->GetMomentum().Mag();
	    
	    const double D0 = 1.8645 ;
	    
	    TVector3 mc = mcK->GetStartVertex();

	    CbmStsKFSecondaryVertexFinder SVFinder;
	    SVFinder.AddTrack(tK);
	    SVFinder.AddTrack(tP);

	    for( int iconst=0; iconst<4; iconst++){
	      
	      switch( iconst ){
	      case 0: 
		SVFinder.SetMassConstraint(); // no constraints
		SVFinder.SetTopoConstraint();
		break;
	      case 1: 
		SVFinder.SetMassConstraint();// Topo constraint
		SVFinder.SetTopoConstraint(fPrimaryVertex);
		break;
	      case 2: 
		SVFinder.SetMassConstraint(D0);// Mass constraint
		SVFinder.SetTopoConstraint();
		break;
	      case 3: 
		SVFinder.SetMassConstraint(D0);// Topo + Mass constraint
		SVFinder.SetTopoConstraint(fPrimaryVertex);
		break;
	      default:
		break;
	      }
	      SVFinder.Fit();
	      CbmVertex sv;	    
	      //CbmTrackParam KFitted, PFitted;
	      Double_t mass, mass_err;
	      SVFinder.GetVertex(sv);
	      //SVFinder.GetFittedTrack(0, &KFitted );
	      //SVFinder.GetFittedTrack(1, &PFitted );
	      SVFinder.GetMass(&mass, &mass_err);
	      if( sv.GetNDF()<=0) continue;
	      Double_t dx = sv.GetX() - mc.X();
	      Double_t dy = sv.GetY() - mc.Y();
	      Double_t dz = sv.GetZ() - mc.Z();
	      Double_t sx = sv.GetCovariance(0,0);
	      Double_t sy = sv.GetCovariance(1,1);
	      Double_t sz = sv.GetCovariance(2,2);
	      fhD0[iconst][0]->Fill( 1.E4*dx );
	      fhD0[iconst][1]->Fill( 1.E4*dy );
	      fhD0[iconst][2]->Fill( 1.E4*dz );
	      if ( sx >1.e-20 ) fhD0[iconst][3]->Fill( dx/sqrt(sx) );
	      if ( sy >1.e-20 ) fhD0[iconst][4]->Fill( dy/sqrt(sy) );
	      if ( sz >1.e-20 ) fhD0[iconst][5]->Fill( dz/sqrt(sz) );
	      fhD0[iconst][6]->Fill( sv.GetChi2()/sv.GetNDF() );
	      fhD0[iconst][7]->Fill( TMath::Prob(sv.GetChi2(),sv.GetNDF()) );
	      fhD0[iconst][8]->Fill( (mass-D0) );
	      fhD0[iconst][9]->Fill( (mass-D0)/mass_err );	    
	      //fhD0[iconst][10]->Fill( ( fabs(1./KFitted.GetQp()) - MCPK)/MCPK );
	      //fhD0[iconst][11]->Fill( (fabs(1./PFitted.GetQp()) - MCPP)/MCPP );
	      fhD0[iconst][12]->Fill( ( fabs(1./tK->GetParamFirst()->GetQp()) - MCPK)/MCPK );
	      fhD0[iconst][13]->Fill( ( fabs(1./tP->GetParamFirst()->GetQp()) - MCPP)/MCPP );	    
	    }
	  }
	}
      }    
    }
  }// vertex analysis 
}
// -------------------------------------------------------------------------



// -----   Finish CbmStsFitPerformanceTask task   -----------------------------
void CbmStsFitPerformanceTask::Finish(){
  writedir2current(histodir);
}
// -------------------------------------------------------------------------



// -------------------------------------------------------------------------
void CbmStsFitPerformanceTask::FillTrackHisto(const Double_t mc[6], CbmStsTrack *track, TH1D* hist[10]  )
{
  CbmStsKFTrackFitter Fitter;
  double z = mc[5];
  CbmTrackParam param;
  Fitter.Extrapolate(track, z, &param);
  double t[6],c[15];
  CbmKFMath::CopyTrackParam2TC( &param, t, c );

  hist[0]->Fill(1.E4*(t[0] - mc[0]));
  hist[1]->Fill(1.E4*(t[1] - mc[1]));
  hist[2]->Fill( 1.E3*(t[2] - mc[2]) );
  hist[3]->Fill( 1.E3*(t[3] - mc[3]) );
  if ( fabs( t[4] )>1.e-10 ) hist[4]->Fill( (mc[4]/t[4] - 1.) );

  if ( c[ 0] >1.e-20 ) hist[5]->Fill( ( t[0] - mc[0] )/sqrt(c[ 0]) );
  if ( c[ 2] >1.e-20 ) hist[6]->Fill( ( t[1] - mc[1] )/sqrt(c[ 2]) );
  if ( c[ 5] >1.e-20 ) hist[7]->Fill( ( t[2] - mc[2] )/sqrt(c[ 5]) );
  if ( c[ 9] >1.e-20 ) hist[8]->Fill( ( t[3] - mc[3] )/sqrt(c[ 9]) );
  if ( c[14] >1.e-20 ) hist[9]->Fill( ( t[4] - mc[4] )/sqrt(c[14]) );
}
// -------------------------------------------------------------------------

void CbmStsFitPerformanceTask::FillVertexHisto( TVector3 &mc, CbmVertex *V, TH1D* hist[8]  )
{
  Double_t dx = V->GetX() - mc.X() ;
  Double_t dy = V->GetY() - mc.Y() ;
  Double_t dz = V->GetZ() - mc.Z() ;
  Double_t s2x = V->GetCovariance(0,0);
  Double_t s2y = V->GetCovariance(1,1);
  Double_t s2z = V->GetCovariance(2,2);
	  
  hist[0]->Fill(1.E4*dx);
  hist[1]->Fill(1.E4*dy);
  hist[2]->Fill(1.E4*dz);
  if ( s2x >1.e-20 ) hist[3]->Fill( dx/sqrt(s2x) );
  if ( s2y >1.e-20 ) hist[4]->Fill( dy/sqrt(s2y) );
  if ( s2z >1.e-20 ) hist[5]->Fill( dz/sqrt(s2z) );
  hist[6]->Fill( V->GetChi2()/V->GetNDF() );
  hist[7]->Fill( TMath::Prob(V->GetChi2(),V->GetNDF()));
  hist[8]->Fill( V->GetNTracks() );		
}

// -------------------------------------------------------------------------
void CbmStsFitPerformanceTask::FillMCStateVectors( CbmStsPoint* point, Double_t mc[], Bool_t out){
  if( !point ) return;
  Int_t mcTrackID = point->GetTrackID();  
  CbmMCTrack* mcTrack = (CbmMCTrack*) fMCTrackArray->At(mcTrackID);
  if( !mcTrack ) return;
  Double_t q=GetCharge(mcTrack);

  // Get MC state vector
  TVector3 r,p;
  if( !out ){
    point->Momentum(p);    
    point->Position(r);    
  }else{
    point->MomentumOut(p);    
    point->PositionOut(r);    
  }
  mc[2]=p.x()/p.z();
  mc[3]=p.y()/p.z();
  mc[4]=q/p.Mag();  
  mc[0]=r.x();
  mc[1]=r.y();  
  mc[5]=r.z();
}
 
// -----   GetCharge   ----------------------------------------------------
Double_t CbmStsFitPerformanceTask::GetCharge(CbmMCTrack* mcTrack){
  Double_t q;
  Int_t pdgCode = mcTrack->GetPdgCode();
  TParticlePDG* particlePDG = TDatabasePDG::Instance()->GetParticle(pdgCode);
  if (particlePDG) q = particlePDG->Charge()/3.; else q = 0;
  return q;
}


// -------------------------------------------------------------------------
Bool_t CbmStsFitPerformanceTask::IsLong(CbmStsTrack* track){
  Int_t nHits = track->GetNHits();
  if (nHits <4) return 0;
  Int_t stmin=1000, stmax=-1000;
  Int_t st,iHit,hitID;
  for (iHit=0;iHit<nHits; iHit++){
    hitID=track->GetHitIndex(iHit);
    st=((CbmStsHit*) fHitArray->At(hitID))->GetDetectorID();
    if (st<stmin) stmin=st;
    if (st>stmax) stmax=st;
  }
  if (stmax-stmin+1<4) return 0;
  return 1;
}

ClassImp(CbmStsFitPerformanceTask)