// -------------------------------------------------------------------------
// -----                   PndLmdGeaneTask source file                 -----
// -----                 Created 18/07/08  by T.Stockmanns             -----
// -----           modified for Lmd by M. Michel & A.Karavdina         -----
// -------------------------------------------------------------------------
// libc includes
#include <iostream>

// Root includes
#include "TROOT.h"
#include "TClonesArray.h"
#include "TVector3.h"
#include "TTree.h"
#include <TMatrixDSym.h>


// framework includes
#include "FairRootManager.h"
#include "PndLmdGeaneTask.h"
#include "FairRun.h"
#include "FairRuntimeDb.h"
#include "FairHit.h"
#include "PndMCTrack.h"
#include "FairBaseParSet.h"
#include "TGeant3.h"
#include "FairTrackParH.h"
#include "FairTrackParP.h"
#include "TDatabasePDG.h"
#include "PndTrack.h"
#include "FairRunAna.h"
#include "PndMultiField.h"
// PndSds includes
#include "PndSdsMCPoint.h"
#include "PndSdsHit.h"
#include "PndSdsMergedHit.h"
//PndLmd includes
#include "PndLinTrack.h"
#include <vector>
#include <map>


// -----   Default constructor   -------------------------------------------
PndLmdGeaneTask::PndLmdGeaneTask() : FairTask("Geane Task for PANDA Lmd"), fEventNr(0), fUseMVDPoint(false)
{
  //tprop = new TNtuple();
}
// -------------------------------------------------------------------------


PndLmdGeaneTask::PndLmdGeaneTask(Double_t pBeam,TVector3 IP): FairTask("Geane Task for PANDA Lmd"), fEventNr(0), fUseMVDPoint(false)
{
  //  fMomLim = 1e-5*pBeam;
  fMomLim = 1e1*pBeam;
  fPDGid=-2212;
  fPbeam = pBeam;
  cout<<"Beam Momentum for particle with PDGid#"<<fPDGid<<" this run is "<<fPbeam<<endl;
  vtx = IP;
  cout<<"Interaction Point:"<<endl;
  vtx.Print();
  // tprop = new TNtuple("tprop","forward MC vs. backward rec","xrec:yrec:zrec:prec:thetarec:phirec:xmc:ymc:zmc:pmc:thetamc:phimc") ;
  tprop = new TTree("tprop","forward MC vs. backward rec with Mag.Field comp@MC point");
  tprop->Branch("xrec",&fxrec);
  tprop->Branch("yrec",&fyrec);
  tprop->Branch("zrec",&fzrec);
  tprop->Branch("xrec_err",&fxrec_err);
  tprop->Branch("yrec_err",&fyrec_err);
  tprop->Branch("zrec_err",&fzrec_err);
   tprop->Branch("pxrec",&fpxrec);
  tprop->Branch("pyrec",&fpyrec);
  tprop->Branch("pzrec",&fpzrec);
  tprop->Branch("pxrec_err",&fpxrec_err);
  tprop->Branch("pyrec_err",&fpyrec_err);
  tprop->Branch("pzrec_err",&fpzrec_err);
  tprop->Branch("thetarec_err",&fthetarec_err);
  tprop->Branch("phirec_err",&fphirec_err);

  tprop->Branch("xmc",&fxmc);
  tprop->Branch("ymc",&fymc);
  tprop->Branch("zmc",&fzmc);
  tprop->Branch("pxmc",&fpxmc);
  tprop->Branch("pymc",&fpymc);
  tprop->Branch("pzmc",&fpzmc);

  tprop->Branch("xmcb",&fxmcb);
  tprop->Branch("ymcb",&fymcb);
  tprop->Branch("zmcb",&fzmcb);
  tprop->Branch("pxmcb",&fpxmcb);
  tprop->Branch("pymcb",&fpymcb);
  tprop->Branch("pzmcb",&fpzmcb);
  tprop->Branch("thetamcb",&fthetamcb);
  tprop->Branch("phimcb",&fphimcb);
  tprop->Branch("pmcb",&fpmcb);

  tprop->Branch("xmclmd",&fxmclmd);
  tprop->Branch("ymclmd",&fymclmd);
   tprop->Branch("zmclmd",&fzmclmd);
  tprop->Branch("thetarec",&fthetarec);
  tprop->Branch("phirec",&fphirec);
  tprop->Branch("prec",&fprec);
  tprop->Branch("thetamc",&fthetamc);
  tprop->Branch("phimc",&fphimc);
  tprop->Branch("pmc",&fpmc);
  tprop->Branch("thetamclmd",&fthetamclmd);
  tprop->Branch("phimclmd",&fphimclmd);
  tprop->Branch("pmclmd",&fpmclmd);

  // // tprop->Branch("xmc_err",&fxmc_err);
  // // tprop->Branch("ymc_err",&fymc_err);
  // // tprop->Branch("xmclmd_err",&fxmclmd_err);
  // // tprop->Branch("ymclmd_err",&fymclmd_err);
 
  tprop->Branch("vrec",&fvrec);
  tprop->Branch("wrec",&fwrec);
  tprop->Branch("tvrec",&ftvrec);
  tprop->Branch("twrec",&ftwrec);
  tprop->Branch("vrec_err",&fvrec_err);
  tprop->Branch("wrec_err",&fwrec_err);
  tprop->Branch("tvrec_err",&ftvrec_err);
  tprop->Branch("twrec_err",&ftwrec_err);
  tprop->Branch("vmc",&fvmc);
  tprop->Branch("wmc",&fwmc);
  tprop->Branch("tvmc",&ftvmc);
  tprop->Branch("twmc",&ftwmc);
  // tprop->Branch("vmc_err",&fvmc_err);
  // tprop->Branch("wmc_err",&fwmc_err);
  // tprop->Branch("tvmc_err",&ftvmc_err);
  // tprop->Branch("twmc_err",&ftwmc_err);
  // tprop->Branch("twmclmd_err",&ftwmclmd_err);
  tprop->Branch("vmclmd",&fvmclmd);
  tprop->Branch("wmclmd",&fwmclmd);
  tprop->Branch("tvmclmd",&ftvmclmd);
  tprop->Branch("twmclmd",&ftwmclmd);
  // tprop->Branch("vmclmd_err",&fvmclmd_err);
  // tprop->Branch("wmclmd_err",&fwmclmd_err);
  // tprop->Branch("tvmclmd_err",&ftvmclmd_err);
  // tprop->Branch("twmclmd_err",&ftwmclmd_err);

  tprop->Branch("Bx",&fbx);
  tprop->Branch("By",&fby);
  tprop->Branch("Bz",&fbz);
 
 tprop->Branch("phiMCip",&fphiMCip);
 tprop->Branch("thetaMCip",&fthetaMCip);
}


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

// -----   Public method Init   --------------------------------------------
InitStatus PndLmdGeaneTask::Init()
{

  
  // Get RootManager
  FairRootManager* ioman = FairRootManager::Instance();
  if ( !ioman){
    std::cout << "-E- PndLmdGeaneTask::Init: "<< "RootManager not instantiated!" << std::endl;
    return kFATAL;
  }


  fMCHits = (TClonesArray*) ioman->GetObject("LMDPoint");
  if ( !fMCHits)	{
    std::cout << "-W- PndLmdGeaneTask::Init: "<< "No LMDPoint"<<" array!" << std::endl;
    return kERROR;
  }

 //Get MC tracks
  fMCTracks = (TClonesArray*) ioman->GetObject("MCTrack");
  if ( !fMCTracks)	{
    std::cout << "-W- PndLmdTrkQTask::Init: "<< "No MCTrack"<<" array!" << std::endl;
    return kERROR;
  }

  //  fTracks = (TClonesArray*) ioman->GetObject("LMDTrack");
  fTracks = (TClonesArray*) ioman->GetObject("LMDPndTrack");
  if (!fTracks){
    std::cout << "-W- PndLmdGeaneTask::Init: "<< "No Track" << " array!" << std::endl;
    return kERROR;
  }

  //Get trk cand [needed only for tests!!!]
  fRecCandTracks = (TClonesArray*) ioman->GetObject("LMDTrackCand");
  if ( !fRecCandTracks)	{
    std::cout << "-W- PndLmdGeaneTask::Init: "<< "No LMDTrackCand [needed only for tests!!!]"<<" array!" << std::endl;
    return kERROR;
 }

  //Get rec. hits  [needed only for tests!!!]
  fRecHits = (TClonesArray*) ioman->GetObject("LMDHitsMerged");
  if ( !fRecHits)	{
    std::cout << "-W- PndLmdGeaneTask::Init: "<< "No LMDHitsMerged [needed only for tests!!!]"<<" array!" << std::endl;
    return kERROR;
  }

  fTrackParGeane = new TClonesArray("FairTrackParH");
  ioman->Register("GeaneTrackPar","Geane", fTrackParGeane, kTRUE);

  fTrackParIni = new TClonesArray("FairTrackParH");
  ioman->Register("GeaneTrackIni","Geane", fTrackParIni, kTRUE);

  fTrackParFinal = new TClonesArray("FairTrackParH");
  ioman->Register("GeaneTrackFinal","Geane", fTrackParFinal, kTRUE);


  fDetName = new TClonesArray("TObjString");
  ioman->Register("DetName", "Geane", fDetName, kTRUE);

  fPro = new FairGeanePro();
  fGeoH = PndGeoHandling::Instance();
  FairRun* fRun = FairRun::Instance();
  FairRuntimeDb* rtdb = fRun->GetRuntimeDb();
 
  pndField = FairRunAna::Instance()->GetField();

  return kSUCCESS;
}
// -------------------------------------------------------------------------
void PndLmdGeaneTask::SetParContainers()
{
  // Get Base Container
 /// FairRun* ana = FairRun::Instance();
 // FairRuntimeDb* rtdb=ana->GetRuntimeDb();

}


// -----   Public method Exec   --------------------------------------------
void PndLmdGeaneTask::Exec(Option_t* opt)
{
  // if(fVerbose>5){
  //   if((fTracks->GetEntries())!=(fMCTracks->GetEntries()))
  //     return;
  //     }
  // cout<<"PndLmdGeaneTask::Exec starts!"<<endl;
  std::map<int, std::vector<int> > mcHitMap;//Track ->  MCHits
  fTrackParGeane->Delete();
  fTrackParIni->Delete();
  fTrackParFinal->Delete();
  fDetName->Delete();

  mcHitMap = AssignHitsToTracks();


  if(fVerbose>2){
    cout<<" ---- Info: "<<  fEventNr<<endl;
  }
  fEventNr++;
  //Charge & mass of particle
  Int_t PDGCode = -2212; //antiproton
  //  Int_t PDGCode = fPDGid;
  TDatabasePDG *fdbPDG = TDatabasePDG::Instance();
  TParticlePDG *fParticle = fdbPDG->GetParticle(PDGCode);
  Double_t  fCharge = fParticle->Charge()/3.;
  //  cout<<"fCharge = "<<fCharge<<endl;

  //go through all tracks
  int glI = fTracks->GetEntriesFast();
  // cout<<"glI = "<<glI<<endl;
  Int_t counterGeaneTrk = 0;

  for (Int_t i = 0; i<glI;i++){ 

    //  cout<<"PndLmdGeaneTask::Exec for track#"<<i<<endl;
      TVector3 StartPos, StartPosErr, StartMom, StartMomErr, StartO, StartU, StartV;
      int p = 0;
    
      PndTrack* recTrack = (PndTrack*)(fTracks->At(i));
      FairTrackParP fFittedTrkP = recTrack->GetParamFirst();
      int ierr0;
      FairTrackParH fFittedTrkH(&fFittedTrkP,ierr0);
      //      cout<<"CONVERTION Parabola->Helix ierr = "<<ierr0<<endl;
     
      TVector3 PosRecLMD(fFittedTrkH.GetX(),fFittedTrkH.GetY(),fFittedTrkH.GetZ());
      if(fFittedTrkH.GetZ()>1130) continue;// TEST: skip trks from 2nd plane. TODO: check are they fine???
      TVector3 MomRecLMD(fFittedTrkH.GetPx(),fFittedTrkH.GetPy(),fFittedTrkH.GetPz());
      MomRecLMD *=fPbeam/MomRecLMD.Mag();//external assumption about mom magnitude
      fFittedTrkH.SetPx(MomRecLMD.X()); fFittedTrkH.SetPy(MomRecLMD.Y()); fFittedTrkH.SetPz(MomRecLMD.Z());
      double covMARS[6][6];
      fFittedTrkH.GetMARSCov(covMARS);
      TVector3 errMomRecLMD(sqrt(covMARS[0][0]),sqrt(covMARS[1][1]),sqrt(covMARS[2][2]));
      TVector3 errPosRecLMD(sqrt(covMARS[3][3]),sqrt(covMARS[4][4]),sqrt(covMARS[5][5]));

      StartPos = PosRecLMD;
      StartPosErr = errPosRecLMD;
      StartMom = MomRecLMD;
      StartMomErr = errMomRecLMD;

      //Make true2 track from MC info on 1st plane of LMD-----------
      FairTrackParP *fStartMCLMD = new FairTrackParP();
      if(fVerbose>5 &&  (fTracks->GetEntries()==fMCTracks->GetEntries())){
      int candID = recTrack->GetRefIndex();
      PndTrackCand *trkcand = (PndTrackCand*)fRecCandTracks->At(candID);    
      PndTrackCandHit candhit = (PndTrackCandHit)(trkcand->GetSortedHit(0));//1st hit info
      Int_t hitID = candhit.GetHitId();
      PndSdsMergedHit* myHit = (PndSdsMergedHit*)(fRecHits->At(hitID));
      int mcrefbot = myHit->GetSecondMCHit();
      int mcreftop = myHit->GetRefIndex();
      PndSdsMCPoint* MCPointHit;
      if(mcreftop>=0){
	MCPointHit = (PndSdsMCPoint*)(fMCHits->At(mcreftop));
      }
      else{
	MCPointHit = (PndSdsMCPoint*)(fMCHits->At(mcrefbot));
      }
      
      TVector3 PosMClmd =  MCPointHit->GetPosition();
      double pxTrue =  MCPointHit->GetPx();
      double pyTrue =  MCPointHit->GetPy();
      double pzTrue =  MCPointHit->GetPz();
      TVector3 MomMClmd(pxTrue,pyTrue,pzTrue);
      TVector3 dirMClmd = MomMClmd;
      dirMClmd *=1./(MomMClmd.Mag());
      MomMClmd *=fPbeam/(MomMClmd.Mag());
      double xMClmdNew =  PosMClmd.X()-(dirMClmd.X()*0.010);
      double yMClmdNew =  PosMClmd.Y()-(dirMClmd.Y()*0.010);
      double zMClmdNew  = PosMClmd.Z()-(dirMClmd.Z()*0.010);
      PosMClmd.SetXYZ(xMClmdNew, yMClmdNew,zMClmdNew);  //shift 100 mkm to avoid adding MC errors during back propagation;
      TVector3 ocMC(0,0,0); //define plane perpendicular to z-axis in IP
      TVector3 djMC(1.,0.,0.);
      TVector3 dkMC(0.,1.,0.);
      TVector3 PosMClmderr(0.,0.,0.);
      TVector3 MomMClmderr(0.,0.,0.);
      fStartMCLMD = new FairTrackParP(PosMClmd, MomMClmd, PosMClmderr, MomMClmderr, fCharge,ocMC,djMC,dkMC);
      //      fStartMCLMD = new FairTrackParH(PosMClmd, MomMClmd, PosMClmderr, MomMClmderr, fCharge);
      }
      //-------------------------------------------------------------------------------

      // // ///Propagate to the PCA (a space point) in 7 steps ---------------------------------
      // // //Comment: seems back propagation in one step (for 11 m) is too much
      // // //try smoothing it by small steps, which follow mag.field
      // const int nstep=7;
      // double zbend[nstep]={661, 660.5, 660., 659, 319, 316, 220};//entarance and exit mag.field
      
      // // //TEST: 8 steps from LMD to IP
      // const int nstep=8;
      // double zbend[nstep]={fFittedTrkP.GetZ(), 661, 660.5, 660., 659, 319, 316, 220};//entarance and exit mag.field

      // //TEST: many-many steps -------------------------------------------------
      // const int nstep0=10;
      // //      double zbend0[nstep0]={fFittedTrkP.GetZ(), 661, 660.5, 660., 659, 319, 316, 220,10};//entarance and exit mag.field
      // double zbend0[nstep0]={fFittedTrkP.GetZ(), 660, 602, 450, 342, 283, 248, 180, 100, 1};//entarance and exit mag.field
      //  //      TEST for backward and forward propagation: more steps!
      //      const int nintstep = 100;
      // // const int nintstep = 10;
      // const int nstep=nstep0*nintstep;
      // double zbend[nstep];
      //  for(int is=1;is<=nstep0;is++){
      // 	 const double z0=zbend0[is-1];
      // 	 const double z1=zbend0[is];
      // 	 const double zstep=(z0-z1)/nintstep;
      // 	 //	 cout<<"is = "<<is<<": z0 = "<<z0<<" z1 = "<<z1<<" zstep = "<<zstep<<endl;
      // 	 for(int js=0;js<nintstep;js++){
      // 	   int curint = (is-1)*nintstep+js;
      // 	   zbend[curint]=z0-zstep*js;
      // 	   //   cout<<"zbend["<<curint<<"]="<<zbend[curint]<<endl;
      // 	 }
      //  }
      //  //(end) many-many steps -------------------------------------------------
      //TEST: many-many steps -------------------------------------------------
      //      const int nstep0=9;
      const int nstep0=10;
      //      double zbend0[nstep0]={fFittedTrkH.GetZ()-0.010, 661, 660.5, 660., 659, 319, 316, 220, 0};//entarance and exit mag.field
      //      double zbend0[nstep0]={fFittedTrkH.GetZ(), 661, 660.5, 660., 659, 319, 316, 220, 0};//entarance and exit mag.field
      double zbend0[nstep0]={fFittedTrkH.GetZ(), fFittedTrkH.GetZ()-5., 661, 660.5, 660., 659, 319, 316, 220, 0};//entarance and exit mag.field
       //      TEST for backward and forward propagation: more steps!
      //      const int nintstep = 100;
      const int nintstep = 50;
      //      const int nintstep = 1;
      const int nstep=(nstep0-1)*nintstep;
      double zbend[nstep];
       for(int is=1;is<nstep0;is++){
      	 const double z0=zbend0[is-1];
      	 const double z1=zbend0[is];
      	 const double zstep=(z0-z1)/nintstep;
      	 //	 cout<<"is = "<<is<<": z0 = "<<z0<<" z1 = "<<z1<<" zstep = "<<zstep<<endl;
      	 for(int js=0;js<nintstep;js++){
      	   int curint = (is-1)*nintstep+js;
      	   zbend[curint]=z0-zstep*js;
	   //cout<<"zbend["<<curint<<"]="<<zbend[curint]<<endl;
      	 }
       }
       //   zbend[nstep-1] = zbend0[nstep0-1];
       //(end) many-many steps -------------------------------------------------


 vector<double> vxmc(nstep),vymc(nstep),vzmc(nstep),vpxmc(nstep),vpymc(nstep),vpzmc(nstep),vthetamc(nstep),vphimc(nstep),vpmc(nstep),vvmc(nstep),vwmc(nstep),vtvmc(nstep),vtwmc(nstep),vvmc_err(nstep),vwmc_err(nstep),vtvmc_err(nstep),vtwmc_err(nstep);
       FairTrackParP *fStartMC = new FairTrackParP();
 // FairTrackParH *fStartMC = new FairTrackParH();
       if(fVerbose>2){
 	 cout<<"------------------------------------------"<<endl;      
 	 cout<<"StartPos:"<<endl;
 	 StartPos.Print();
 	 cout<<"StartPosErr:"<<endl;
 	 StartPosErr.Print();

 	 cout<<"StartMom: "<<StartMom.Mag()<<endl;
 	 StartMom.Print();
 	 cout<<"StartMomErr: "<<StartMomErr.Mag()<<endl;
 	 StartMomErr.Print();     
 	 cout<<""<<endl;

 	 if(fVerbose>5 &&  (fTracks->GetEntries()==fMCTracks->GetEntries())){ //check results for MC track propagation to LMD plane
	   FairTrackParP *fStartPst = new FairTrackParP(fFittedTrkP);
	   // FairTrackParH *fStartPst = new FairTrackParH(fFittedTrkH);
 	   // TVector3 istLMD(fStartPst->GetIVer());
 	   // TVector3 jstLMD(fStartPst->GetJVer());
 	   // TVector3 kstLMD(fStartPst->GetKVer());
 	   // //	   cout<<"i,j,k of LMD trk:"<<endl;
 	   // //	   istLMD+=StartPos;
 	   // istLMD=StartPos;
 	   // // istLMD.Print();
 	   // // jstLMD.Print();
 	   // // kstLMD.Print();
 	  TVector3 ocMC(0,0,0); //define plane perpendicular to z-axis in IP
 	  TVector3 djMC(1.,0.,0.);
 	  TVector3 dkMC(0.,1.,0.);
 	  djMC.SetMag(1);
 	  dkMC.SetMag(1);

	  
 	   PndMCTrack* mctrk = (PndMCTrack*)(fMCTracks->At(i));
 	   TVector3 MomMC = mctrk->GetMomentum();
 	   TVector3 PosMC = mctrk->GetStartVertex();
 	   TVector3 MomMCerr(0.,0.,0.);
 	   TVector3 PosMCerr(0.,0.,0.);
 	   if(PosMC.Z()>0){
 	     cout<<"!!! Achtung: "<<PosMC.Z()<<endl;
 	     break;
 	   }

	   fStartMC = new FairTrackParP(PosMC, MomMC, PosMCerr, MomMCerr, fCharge,ocMC,djMC,dkMC);
	   //   fStartMC = new FairTrackParH(PosMC, MomMC, PosMCerr, MomMCerr, fCharge);
 	   for(int sj=nstep-1;sj>-1;sj--){
	     //   cout<<"MC forward: to z["<<sj<<"]="<<zbend[sj]<<endl;
 	     bool isPropMC;
	     TVector3 fmomStepMC = fStartMC->GetMomentum();//TEST: set back mom value
	     // cout<<"current MC mom: "<<endl;
	     // fmomStepMC.Print();
	     if(fabs(fmomStepMC.Mag()-fPbeam)>fMomLim){
	       fmomStepMC*=fPbeam/fmomStepMC.Mag();
	       fStartMC->SetPx(fmomStepMC.X());
	       fStartMC->SetPy(fmomStepMC.Y());
	       fStartMC->SetPz(fmomStepMC.Z());
	     }
	     FairTrackParP *fResMC = PropToPlane(fStartMC,zbend[sj],+1,isPropMC);//forward propagation
	     //   FairTrackParH *fResMC = PropToPCA(fStartMC,zbend[sj],+1,isPropMC);//forward propagation
 	     if(isPropMC){
 	       delete fStartMC;
 	       fStartMC=fResMC;
 	       vxmc[sj]= fResMC->GetX();
 	       vymc[sj]= fResMC->GetY();
 	       vzmc[sj]= fResMC->GetZ();
	       //      cout<<"Save zmc = "<<vzmc[sj]<<endl;
	       vpxmc[sj]= fResMC->GetPx();
 	       vpymc[sj]= fResMC->GetPy();
 	       vpzmc[sj]= fResMC->GetPz();
 	       TVector3 finDirMC(fResMC->GetPx(),fResMC->GetPy(),fResMC->GetPz());
 	       vpmc[sj]= finDirMC.Mag();
 	       vthetamc[sj]= finDirMC.Theta();
 	       vphimc[sj]= finDirMC.Phi();

 	       vvmc[sj] = fResMC->GetV();
 	       vwmc[sj] = fResMC->GetW();
 	       vtvmc[sj] = fResMC->GetTV();
 	       vtwmc[sj] = fResMC->GetTW();
 	       vvmc_err[sj] = fResMC->GetDV();
 	       vwmc_err[sj] = fResMC->GetDW();
 	       vtvmc_err[sj] = fResMC->GetDTV();
 	       vtwmc_err[sj] = fResMC->GetDTW();
	   
 	       //   cout<<"Next step;)"<<endl;
 	     }
 	     else break;
 	   }
 	 }
       }
       if(abs(fStartMC->GetZ()-fFittedTrkH.GetZ())>10 && fVerbose>5){
       	 cout<<"abs(fStartMC->GetZ()-fFittedTrkH.GetZ()) = "<<abs(fStartMC->GetZ()-fFittedTrkH.GetZ())
       	     <<" fStartMC->GetZ() = "<<fStartMC->GetZ()<<" fFittedTrkH.GetZ() =  "<<fFittedTrkH.GetZ()<<endl;
       	 cout<<"MC particle wasn't propagated to LMD plane ????"<<endl;
       }
            if(abs(fStartMC->GetZ()-fFittedTrkH.GetZ())>10 && fVerbose>5) break;//MC particle wasn't propagated to LMD plane ????
	    //MC forward propagated, to check if forward and backward give similar results
	    FairTrackParP *fStartPstMCB = new FairTrackParP(*fStartMC);//MC forward propagated to  LMD, should be treated as REC

      TClonesArray& clref1 = *fTrackParIni;
      Int_t size1 = clref1.GetEntriesFast();

     
      FairTrackParP *fStartPst = new FairTrackParP(fFittedTrkP);//REC
      FairTrackParP *fStartPstMCLMD= new FairTrackParP(*fStartMCLMD);//MC in LMD, should be treated as REC

    

      //      for(int js=0;js<nstep;js++){
      for(int js=1;js<nstep;js++){
	//	cout<<"step#"<<js<<endl;
 	bool isProp;
 	//	cout<<"REC backward "<<endl;
	TVector3 fmomStep = fStartPst->GetMomentum();//TEST: set back mom value
	if(fabs(fmomStep.Mag()-fPbeam)>fMomLim){
	  fmomStep*=fPbeam/fmomStep.Mag();
	  fStartPst->SetPx(fmomStep.X());
	  fStartPst->SetPy(fmomStep.Y());
	  fStartPst->SetPz(fmomStep.Z());
	}
 	FairTrackParP*  fResPst  = PropToPlane(fStartPst,zbend[js],-1,isProp);//back propagation
	if(js==0){
	  isProp=true;
	  fResPst = fStartPst;
	}
	if(!isProp){
	  cout<<"js = "<<js<<" to zbend = "<<zbend[js]<<endl;
	  cout<<"smth went wrong with back-propagation for REC trk!"<<endl;
	}
 	if(isProp){
	  //  delete fStartPst;
 	  fStartPst = fResPst;
 	  if(fVerbose>5){

	    TVector3 fmomStepMCLMD = fStartPstMCLMD->GetMomentum();//TEST: set back mom value
	    if(fabs(fmomStepMCLMD.Mag()-fPbeam)>fMomLim){
	      fmomStepMCLMD*=fPbeam/fmomStepMCLMD.Mag();
	      fStartPstMCLMD->SetPx(fmomStepMCLMD.X());
	      fStartPstMCLMD->SetPy(fmomStepMCLMD.Y());
	      fStartPstMCLMD->SetPz(fmomStepMCLMD.Z());
	    }
	    // 	    FairTrackParH*  fResPstMCLMD  = PropToPCA(fStartPstMCLMD,zbend[js],-1,isPropMClmd);//back propagation
 	    bool isPropMClmd;
 	    FairTrackParP*  fResPstMCLMD  = PropToPlane(fStartPstMCLMD,zbend[js],-1,isPropMClmd);//back propagation
 	    bool isPropMCB;
 	    FairTrackParP*  fResPstMCB  = PropToPlane(fStartPstMCB,zbend[js],-1,isPropMCB);//back propagation of forward propagated track
	   
	    if(js==0){//"back-propagation" to exacly the same point
	      isPropMCB = true;
	      isPropMClmd = true;
	      fResPstMCB = fStartPstMCB;
	      fResPstMCLMD = fStartPstMCLMD;
	    }
	    if(!isPropMCB){
	      cout<<"js = "<<js<<endl;
	      cout<<"Trk with z="<<fStartPstMCB->GetZ()<<" to zbend = "<<zbend[js]<<endl;
	      cout<<"smth went wrong with back-propagation for forward propagated trk!"<<endl;
	    }
	    if(!isPropMClmd){
	      cout<<"js = "<<js<<endl;
	      cout<<"Trk with z="<<fStartPstMCB->GetZ()<<" to zbend = "<<zbend[js]<<endl;
	      cout<<"smth went wrong with back-propagation for MCLMD trk!"<<endl;
	    }
	    if(isPropMClmd && isPropMCB){
	    // 	    if(isPropMClmd){
 	      TVector3 finPosMCLMD(fResPstMCLMD->GetX(),fResPstMCLMD->GetY(),fResPstMCLMD->GetZ());
 	      TVector3 finDirMCLMD(fResPstMCLMD->GetPx(),fResPstMCLMD->GetPy(),fResPstMCLMD->GetPz());

 	      TVector3 finPosMCB(fResPstMCB->GetX(),fResPstMCB->GetY(),fResPstMCB->GetZ());//back propagation of forward propagated track
 	      TVector3 finDirMCB(fResPstMCB->GetPx(),fResPstMCB->GetPy(),fResPstMCB->GetPz());

 	      TVector3 finPos(fResPst->GetX(),fResPst->GetY(),fResPst->GetZ());
	      // cout<<"RESULT: "<<endl;
	      // finPos.Print();

 	      TVector3 finDir(fResPst->GetPx(),fResPst->GetPy(),fResPst->GetPz());

 	      //     double pnt[3]={finPosMC.X(),finPosMC.Y(),finPosMC.Z()}; //Position where to get field strength
 	      double pnt[3]={vxmc[js],vymc[js],vzmc[js]};
 	      double Bf[3]; //result goes here
 	      // retrieve the field from the framework
 	      pndField->Field(pnt, Bf);  //[kGs]
 	      fbx = Bf[0];
 	      fby = Bf[1];
 	      fbz = Bf[2];
 	      fxrec = finPos.X();
 	      fyrec = finPos.Y();
	      fzrec = finPos.Z();
	      //   cout<<"fzrec = "<<fzrec<<endl;
 	      fxrec_err = fResPst->GetDX();
 	      fyrec_err = fResPst->GetDY();
 	      fzrec_err = fResPst->GetDZ();
	      fpxrec =  fResPst->GetPx();
	      fpyrec =  fResPst->GetPy();
	      fpzrec =  fResPst->GetPz();
 	      fpxrec_err = fResPst->GetDPx();
 	      fpyrec_err = fResPst->GetDPy();
 	      fpzrec_err = fResPst->GetDPz();


 	      fprec = finDir.Mag();

	      int ierr;
	      FairTrackParH *fResPstH= new FairTrackParH(fResPst,ierr);
 	      fthetarec = 0.5*(TMath::Pi()) - fResPstH->GetLambda();
 	      fphirec = fResPstH->GetPhi();
 	      fthetarec_err = fResPstH->GetDLambda();
 	      fphirec_err = fResPstH->GetDPhi();
	      delete fResPstH;
 	      fxmc = vxmc[js];
 	      fymc = vymc[js];
 	      fzmc = vzmc[js];
	      fpxmc = vpxmc[js];
 	      fpymc = vpymc[js];
 	      fpzmc = vpzmc[js];
 	      fpmc = vpmc[js];
 	      fthetamc = vthetamc[js];
 	      fphimc = vphimc[js];
	    

 	      fxmclmd = finPosMCLMD.X();
 	      fymclmd = finPosMCLMD.Y();
	      fzmclmd = finPosMCLMD.Z();
 	      fpmclmd = finDirMCLMD.Mag();
	      FairTrackParH *fResPstMCLMDH= new FairTrackParH(fResPstMCLMD,ierr);
 	      fthetamclmd = 0.5*(TMath::Pi()) - fResPstMCLMDH->GetLambda();
 	      fphimclmd = fResPstMCLMDH->GetPhi();
	      delete fResPstMCLMDH;

	      fxmcb = finPosMCB.X();
 	      fymcb = finPosMCB.Y();
	      fzmcb = finPosMCB.Z();
 	      fpmcb = finDirMCB.Mag();
	      FairTrackParH *fResPstMCBH= new FairTrackParH(fResPstMCB,ierr);
 	      fthetamcb = 0.5*(TMath::Pi()) - fResPstMCBH->GetLambda();
 	      fphimcb = fResPstMCBH->GetPhi();
	      delete fResPstMCBH;

 	      fvmc = vvmc[js];
 	      fwmc = vwmc[js];
 	      ftvmc = vtvmc[js];
 	      ftwmc = vtwmc[js];
 	   

 	      fvrec = fResPst->GetV();
 	      fwrec = fResPst->GetW();
 	      ftvrec = fResPst->GetTV();
 	      ftwrec = fResPst->GetTW();
 	      fvrec_err = fResPst->GetDV();
 	      fwrec_err = fResPst->GetDW();
 	      ftvrec_err = fResPst->GetDTV();
 	      ftwrec_err = fResPst->GetDTW();
 	      fvmclmd = fResPstMCLMD->GetV();
 	      fwmclmd = fResPstMCLMD->GetW();
 	      ftvmclmd = fResPstMCLMD->GetTV();
 	      ftwmclmd = fResPstMCLMD->GetTW();
 	    
	      fphiMCip = vphimc[nstep-1];
	      fthetaMCip = vthetamc[nstep-1];

 	      tprop->Fill();
 	      delete fStartPstMCLMD;
 	      fStartPstMCLMD = fResPstMCLMD;
 	      fStartPstMCB = fResPstMCB;
     	    }
 	    else
 	      break;
 	  }
 	}
 	else
 	  break;
      }

      ///and now Propagate to the PCA (a space point) in one step ---------------------------------
      int ierr=0;
      FairTrackParH *fStart = new (clref1[size1]) FairTrackParH(fStartPst,ierr);
      //    FairTrackParH *fStart = new (clref1[size1]) FairTrackParH(*fStartPst);
      TClonesArray& clref = *fTrackParGeane;
      Int_t size = clref.GetEntriesFast();
      FairTrackParH *fRes = new(clref[size]) FairTrackParH();
      fPro->SetPoint(vtx);
      fPro->PropagateToPCA(1,-1);// back-propagate to point
      Bool_t isProp = fPro->Propagate(fStart, fRes, PDGCode);
     
      // ///----------------------------------------------------------------------
      delete  fStartPst;
     
      ///----------------------------------------------------------------------

      //------- TEST of calculation errors -------
      TVector3 gPos(fRes->GetX(),fRes->GetY(),fRes->GetZ());
      TVector3 gMom(fRes->GetPx(),fRes->GetPy(),fRes->GetPz());

      TVector3 gErrPos(fRes->GetDX(),fRes->GetDY(),fRes->GetDZ());
      TVector3 gErrMom(fRes->GetDPx(),fRes->GetDPy(),fRes->GetDPz());
      // cout<<" "<<endl;
      if(fVerbose>2){
	//      cout<<"================= %%%% ===================="<<endl;
      
      cout<<"gPos:"<<endl;
      gPos.Print();
      // cout<<"difference between final and initial point = "<<(-StartPos.Mag()+gPos.Mag())<<endl;
      cout<<"gErrPos:"<<endl;
      gErrPos.Print();
      //  cout<<"difference between final and initial point error = "<<(-StartPosErr.Mag()+gErrPos.Mag())<<endl;

      cout<<"gMom: "<<gMom.Mag()<<endl;
      gMom.Print();   
      // cout<<"difference between initial and final momentum = "<<(StartMom.Mag()-gMom.Mag())<<endl;
      cout<<"gErrMom: "<<gErrMom.Mag()<<endl;
      // cout<<"difference between initial and final momentum error= "<<(StartMomErr.Mag()-gErrMom.Mag())<<endl;
      gErrMom.Print();
      if(fVerbose>5 &&  (fTracks->GetEntries()==fMCTracks->GetEntries())){
	PndMCTrack* mctrk = (PndMCTrack*)(fMCTracks->At(i));
	int motherID = mctrk->GetMotherID();
	if(motherID>=0) break;
	TVector3 MomMC = mctrk->GetMomentum();
	TVector3 PosMC = mctrk->GetStartVertex();
	// cout<<"mcPos:"<<endl;
	// PosMC.Print();
	// cout<<"mcMom: "<<MomMC.Mag()<<endl;
	// MomMC.Print();   

	double pnt[3]={PosMC.X(),PosMC.Y(),PosMC.Z()}; //Position where to get field strength
	double Bf[3]; //result goes here
	// retrieve the field from the framework

	pndField->Field(pnt, Bf);  //[kGs]
	//cout<<"^^^^ Bf = ("<<Bf[0]<<", "<<Bf[1]<<", "<<Bf[2]<<")"<<endl;
	fbx = Bf[0];
	fby = Bf[1];
	fbz = Bf[2];
	fxrec = gPos.X();
	fyrec = gPos.Y();
	fzrec = gPos.Z();
	fprec = gMom.Mag();
	fthetarec = gMom.Theta();
	fphirec = gMom.Phi();
	fxmc = PosMC.X();
	fymc = PosMC.Y();
	fzmc = PosMC.Z();
	fpmc = MomMC.Mag();
	fthetamc = MomMC.Theta();
	fphimc = MomMC.Phi();

	FairTrackParH *fStartMClmd = new FairTrackParH(fStartPstMCLMD,ierr);
	//	FairTrackParH *fStartMClmd = new FairTrackParH(*fStartPstMCLMD);
	fPro->SetPoint(vtx);
	fPro->PropagateToPCA(1,-1);// back-propagate to point
	FairTrackParH *fResMCLMD = new FairTrackParH();
	Bool_t isPropMC = fPro->Propagate(fStartMClmd, fResMCLMD, PDGCode);
	TVector3 gPosMC(fResMCLMD->GetX(),fResMCLMD->GetY(),fResMCLMD->GetZ());
	TVector3 gMomMC(fResMCLMD->GetPx(),fResMCLMD->GetPy(),fResMCLMD->GetPz());
	fxmclmd = gPosMC.X();
	fymclmd = gPosMC.Y();
	fzmclmd = gPosMC.Z();
	fpmclmd = gMomMC.Mag();
	fthetamclmd = gMomMC.Theta();
	fphimclmd = gMomMC.Phi();
	tprop->Fill();
	delete  fStartPstMCLMD;
      }
      cout<<"================= %%%% ===================="<<endl;
      }
      //------------------------------------------
      
      if(isProp == kTRUE){
	new((*fTrackParFinal)[counterGeaneTrk]) FairTrackParH(*(fRes)); //save Track
	//	new((*fTrackParFinal)[counterGeaneTrk]) FairTrackParP(*(fRes)); //save Track
	counterGeaneTrk++;
	if(fVerbose>2) cout<<"***** isProp TRUE *****"<<endl;
      }
      else{
	if(fVerbose>2){
	cout<<"!!! Back-propagation with GEANE didn't return result !!!"<<endl;
	cout<<"StartPos:"<<endl;
	StartPos.Print();
	cout<<"StartPosErr:"<<endl;
	StartPosErr.Print();
	
	cout<<"StartMom: "<<StartMom.Mag()<<endl;
	StartMom.Print();
	cout<<"StartMomErr: "<<StartMomErr.Mag()<<endl;
	StartMomErr.Print();  
	}
	new((*fTrackParFinal)[counterGeaneTrk]) FairTrackParH(); //save NULL
	//	new((*fTrackParFinal)[counterGeaneTrk]) FairTrackParP(); //save NULL
	counterGeaneTrk++;
      }
  }
  // fMCTracks->Delete();
  // fMCHits->Delete();
  if(fVerbose>2) cout<<"PndLmdGeaneTask::Exec END!"<<endl;
}

void PndLmdGeaneTask::Finish()
{
  if(fVerbose>5){
    TTree *nout1 = tprop->CloneTree();
    nout1->Write();
  }
    //  tprop->Write();
}

FairTrackParP* PndLmdGeaneTask::PropToPlane(FairTrackParP* fStartPst, double zpos,int dir, bool& isProp){
	TVector3 stStartPos(fStartPst->GetX(),fStartPst->GetY(),fStartPst->GetZ());
	if(zpos>1e3){//external assumption about mom magnitude [use it while in BOX and we know mom should be diff]
	  TVector3 MomStartPos(fStartPst->GetPx(),fStartPst->GetPy(),fStartPst->GetPz());
	  if(fabs(MomStartPos.Mag()-fPbeam)>fMomLim){
	    MomStartPos *=fPbeam/MomStartPos.Mag();
	    fStartPst->SetPx(MomStartPos.X()); fStartPst->SetPy(MomStartPos.Y()); fStartPst->SetPz(MomStartPos.Z());//correct mom.magnitude
	  }
	}
	//propagate plane-to-plane
	TVector3 ist(fStartPst->GetIVer());
	TVector3 jst(fStartPst->GetJVer());
	TVector3 kst(fStartPst->GetKVer());
	TVector3 oc(0,0,zpos);
	TVector3 dj(1.,0.,0.);
	TVector3 dk(0.,1.,0.);
	dj.SetMag(1);
	dk.SetMag(1);
	fPro->PropagateFromPlane(jst, kst);//1st detector plane
	fPro->PropagateToPlane(oc,dj,dk);//virtual plane at fixed z
	if(dir<0) fPro->setBackProp();
	FairTrackParP *fResPst = new FairTrackParP();
	isProp = fPro->Propagate(fStartPst, fResPst,fPDGid);
	return fResPst;
}

FairTrackParH* PndLmdGeaneTask::PropToPCA(FairTrackParH* fStartPst, double zpos,int dir, bool& isProp){
  //  cout<<"Z@"<<zpos<<" dir propag = "<<dir<<endl;
	TVector3 stStartPos(fStartPst->GetX(),fStartPst->GetY(),fStartPst->GetZ());
	if(zpos>1e3){//external assumption about mom magnitude [use it while in BOX and we know mom should be diff]
	  TVector3 MomStartPos(fStartPst->GetPx(),fStartPst->GetPy(),fStartPst->GetPz());
	  if(fabs(MomStartPos.Mag()-fPbeam)>fMomLim){
	    MomStartPos *=fPbeam/MomStartPos.Mag();
	    fStartPst->SetPx(MomStartPos.X()); fStartPst->SetPy(MomStartPos.Y()); fStartPst->SetPz(MomStartPos.Z());//correct mom.magnitude
	  }
	}
	//propagate pca
	TVector3 oc(fStartPst->GetX(),fStartPst->GetY(),zpos);
	//	if(dir<0) fPro->setBackProp();
	FairTrackParH *fResPst = new FairTrackParH();
	fPro->PropagateToPCA(1,dir);
	fPro->SetPoint(oc);
	//	TVector3 oc1(0,0,zpos-1);
	//	fPro->SetWire(oc, oc1);
	isProp = fPro->Propagate(fStartPst, fResPst,fPDGid);
	if(isProp){
	return fResPst;
	}
	else{
	  cout<<"Oooops! PndLmdGeaneTask::PropToPCA is making you sad!"<<endl;
	  return NULL;
	}
}


std::map<int, std::vector<int> > PndLmdGeaneTask::AssignHitsToTracks()
{
	std::map<int, std::vector<int> > result;
	for (int i = 0; i < fMCHits->GetEntriesFast(); i++){											//get all MC Hits
		PndSdsMCPoint* myPoint = (PndSdsMCPoint*)(fMCHits->At(i));									//sort MCHits with Tracks
		//PndMCTrack* myTrack = (PndMCTrack*)(fMCTracks->At(myPoint->GetTrackID()));
		result[myPoint->GetTrackID()].push_back(i);

	}
	return result;
}

ClassImp(PndLmdGeaneTask);