//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class TBTrackInitTask
//      see TBTrackInitTask.hh for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Maxence Vandenbroucke    TUM            (original author)
//-----------------------------------------------------------

// Panda Headers ----------------------

// This Class' Header ------------------
#include "TBTrackInitTask.h"

// C/C++ Headers ----------------------
#include <map>

// Collaborating Class Headers --------
#include "FairRootManager.h"
#include "FairRuntimeDb.h"
#include "TClonesArray.h"
#include "TpcRiemannHit.h"

#include "TpcDigiPar.h"

#include "GFTrackCand.h"
#include "GFTrack.h"
#include "RKTrackRep.h"
#include "McIdCollection.h"
#include "TVector3.h"
#include "FairMCPoint.h"
#include "FairRunAna.h"
#include "GFDetPlane.h"
#include "TDatabasePDG.h"

#include "FairField.h"
#include "PndConstField.h"
#include "PndMultiField.h"
#include "PndFieldAdaptor.h"
#include "GFFieldManager.h"

#include "TFile.h"
#include "TVector3.h"
#include "TMath.h"
#include"TDatabasePDG.h"

#include <cmath>

using namespace std;

// Class Member definitions -----------
#define PDGDEFAULT 13

ClassImp(TBTrackInitTask)

TBTrackInitTask::TBTrackInitTask()
: FairTask("TB Pattern Reco"), 
  _persistence(kFALSE),
  
  _clusterSIBranchName("TBSICluster"),
  _clusterGEMBranchName("TBGEMCluster"),
  _riemannBranchName("RiemannTBTrack"),
  _trackBranchName("TBTrackPreFit"),

  _smoothing(true),
  _pdg(PDGDEFAULT),
  fdefmom(160.),//GeV?
  Bz(0),
  counter(0),
  fDEBUG(kFALSE)
{
}

TBTrackInitTask::~TBTrackInitTask(){}


InitStatus
TBTrackInitTask::Init()
{
  //Get ROOT Manager
  FairRootManager* ioman= FairRootManager::Instance();
  if(ioman==0){
    Error("TBTrackInitTask::Init","RootManager not instantiated!");
    return kERROR;
  }
  
  // get input arrays
  _clusterSIArray=(TClonesArray*) ioman->GetObject(_clusterSIBranchName);
  if(_clusterSIArray==0){
    Error("TBTrackInitTask::Init","Cluster-SI-array not found!");
    return kERROR;
  }
  
  _clusterGEMArray=(TClonesArray*) ioman->GetObject(_clusterGEMBranchName);
  if(_clusterGEMArray==0){
    Error("TBTrackInitTask::Init","Cluster-array not found!");
    return kERROR;
  }
  
  
  // create and register output array
  _trackArray = new TClonesArray("GFTrack");
  ioman->Register(_trackBranchName,"GenFit",_trackArray,_persistence);


  //get the magnetic field for curvature seeding
  FairField* field=FairRunAna::Instance()->GetField();
  bool CField = dynamic_cast<PndConstField*>(field);
  bool MField = dynamic_cast<PndMultiField*>(field);
  GFFieldManager::getInstance()->init(new PndFieldAdaptor(field));
  if(MField) {
    Double_t O[3], B[3];
    O[0]=0; O[1]=0; O[2]=0;
    field->GetFieldValue(O,B);
    Bz=B[2];
    std::cerr<<"TpcTrackInitTask: "<<"No const field! Curvature seeding not valid... Setting Bz = "<<Bz<<std::endl;
  }
  else if(CField) {
    Bz=field->GetBz(0.,0.,0.);
    std::cerr<<"TpcTrackInitTask: "<<"const field! Setting Bz = "<<Bz<<std::endl;
  }
  else{
    Bz=20.;
    std::cerr<<"TpcTrackInitTask: "<<"default setting Bz = "<<Bz<<std::endl;
  }
  
  return kSUCCESS;
}


void
TBTrackInitTask::SetParContainers() {
  
  std::cout<<"TpcClusterFinderTask::SetParContainers"<<std::endl;
  std::cout.flush();
  
  // Get run and runtime database
  FairRun* run = FairRun::Instance();
  if ( ! run ) Fatal("SetParContainers", "No analysis run");
  
  FairRuntimeDb* db = run->GetRuntimeDb();
  if ( ! db ) Fatal("SetParContainers", "No runtime database");
  
  // Get Tpc digitisation parameter container
  //  fpar= (TpcDigiPar*) db->getContainer("TpcDigiPar");
  //  if (! fpar ) Fatal("SetParContainers", "TpcDigiPar not found");
}



void
TBTrackInitTask::Exec(Option_t* opt)
{
  if(fDEBUG) std::cout << "TBTrackInitTask::Exec; Event Number: " << counter++ << std::endl;
  
  // Reset output Arrays
  if(_trackArray==0) Fatal("TBTrackInitTask::Exec)","No TrackArray");
  _trackArray->Delete();
  
  int NTR=0;
  //Trak init with Silicon clusters
  TVector3 s1x(0,0,-5000),s2x(0,0,-5000),s1y(0,0,-5000),s2y(0,0,-5000);
  TVector3 s1xerr(0,0,-5000),s2xerr(0,0,-5000),s1yerr(0,0,-5000),s2yerr(0,0,-5000);
  unsigned int s1xi=0,s2xi=0,s1yi=0,s2yi=0;//cluster counter for each plane
  int s1xindex=-1,s2xindex=-1,s1yindex=-1,s2yindex=-1;//Cluster index
  Int_t ns=_clusterSIArray->GetEntriesFast();
  for(Int_t is=0;is<ns;++is){
    TBSICluster* sic=(TBSICluster*)_clusterSIArray->At(is);
    if(sic->amp()<15) continue; // noise cut ...
    if(fDEBUG) std::cout << "sic->detname() " << sic->detname() << sic->amp() << std::endl;
    if(sic->detname()=="SI01X1__") {s1xi++; s1x=sic->xyz(); s1xerr=sic->xyzerr();s1xindex=is;}
    if(sic->detname()=="SI01Y1__") {s1yi++; s1y=sic->xyz(); s1yerr=sic->xyzerr();s1yindex=is;}
    if(sic->detname()=="SI02X1__") {s2xi++; s2x=sic->xyz(); s2xerr=sic->xyzerr();s2xindex=is;}
    if(sic->detname()=="SI02Y1__") {s2yi++; s2y=sic->xyz(); s2yerr=sic->xyzerr();s2yindex=is;}
    //    if(s1xi>1 || s1yi>1 || s2xi>1 || s2yi>1 ) break;
  }
  if(fDEBUG) std::cout << "Ncluter found : SI1X=" <<s1xi << " SI2X=" <<s2xi << " SI1Y=" <<s1yi << " SI2Y=" <<s2yi<< std::endl;
  TVector3 sl_pos, sl_dir; //seed for GFtrack
  if(s1xi==1 && s1yi==1 && s2xi==1 && s2yi==1 ) {//if one and only cluster per si plane
    TVector3 posi1= TVector3(s1x.x(),s1y.y(),0.5*(s1x.z()+s1y.z()));
    TVector3 posi1err= TVector3(s1xerr.x(),s1yerr.y(),0.5*(s1xerr.z()+s1yerr.z()));
    TVector3 posi2= TVector3(s2x.x(),s2y.y(),0.5*(s2x.z()+s2y.z()));
    TVector3 posi2err= TVector3(s2xerr.x(),s2yerr.y(),0.5*(s2xerr.z()+s2yerr.z()));
    sl_dir = posi2-posi1;
    sl_pos = posi1;
    if(sl_dir.Mag())
      sl_dir*=fdefmom/sl_dir.Mag(); //normalization to default momemtum (perhaps...)
    else{     Error("TBTrackInitTask::Exec","Silicon track with no momentum");    return;}
    if(fDEBUG) std::cout << "sl_dir.Mag()="  << sl_dir.Mag() <<std::endl;
    TVector3 sl_direrr = posi1err-posi2err; //?????
    TVector3 sl_poserr = posi1err;//??????
   //TVector3 sl_dir(0,0,1);TVector3 sl_direrr(0.1,0.1,0.1);TVector3 sl_pos(0,0,0);TVector3 sl_poserr(0.2,0.2,0.2);
    //BUILD Candidate list, sorted in Z, first GEM1, than SI1,SI2 and then GEM2
    //Find nearest GEM clusters
    TVector3 g1x(-5000,-5000,-5000),g2x(-5000,-5000,-5000),g1y(-5000,-5000,-5000),g2y(-5000,-5100,-5000);
    int g1xindex=-1,g2xindex=-1,g1yindex=-1,g2yindex=-1;//Cluster index
    Int_t ng=_clusterGEMArray->GetEntriesFast();
    for(Int_t ig=0;ig<ng;++ig){
      TBGEMCluster* gic=(TBGEMCluster*)_clusterGEMArray->At(ig);
      double alpha = (gic->z()-sl_pos.Z())/sl_dir.Z();
      TVector3 res = sl_pos+alpha*sl_dir;//manual extrapolation to gem plane of the cluster
      if(fDEBUG) 
	{std::cout << "Res (" << res.x() << ", " << res.y() << ", " << res.z() << ")" <<               std::endl;
	  std::cout << "g0 (" << gic->x() << ", " << gic->y() << ", " << gic->z() << ")" <<gic->detname() << g1xindex 
		    << " FABS == "<<fabs(gic->x()-res.x()) <<" < "<<fabs(g1x.x()-res.x()) << std::endl;}
      if(gic->detname()=="GM01X1__")
	if(fabs(gic->x()-res.x())< fabs(g1x.x()-res.x())){ std::cout << "in GM01x" << ig<< std::endl;g1x=gic->xyz(); g1xindex=ig;}
      if(gic->detname()=="GM01Y1__")
	if(fabs(gic->y()-res.y())< fabs(g1y.y()-res.y())) { g1y=gic->xyz(); g1yindex=ig;}
      if(gic->detname()=="GM02X1__")
	if(fabs(gic->x()-res.x())< fabs(g2x.x()-res.x())) { g2x=gic->xyz(); g2xindex=ig;}
      if(gic->detname()=="GM02Y1__")
	if(fabs(gic->y()-res.y())< fabs(g2y.y()-res.y())) { g2y=gic->xyz(); g2yindex=ig;}
    }
#if 0
    if(g1xindex>-1){
      TBGEMCluster* gic=(TBGEMCluster*)_clusterGEMArray->At(g1xindex);
      std::cout << "g1x (" << gic->x() << ", " << gic->y() << ", " << gic->z() << ")" << std::endl;
    }
    if(g1yindex>-1){
      TBGEMCluster* gic=(TBGEMCluster*)_clusterGEMArray->At(g1yindex);
      std::cout << "g1y (" << gic->x() << ", " << gic->y() << ", " << gic->z() << ")" << std::endl;
    }
    if(g2xindex>-1){
      TBGEMCluster* gic=(TBGEMCluster*)_clusterGEMArray->At(g2xindex);
      std::cout << "g2x (" << gic->x() << ", " << gic->y() << ", " << gic->z() << ")" << std::endl;
    }
    if(g2yindex>-1){
      TBGEMCluster* gic=(TBGEMCluster*)_clusterGEMArray->At(g2yindex);
      std::cout << "g2y (" << gic->x() << ", " << gic->y() << ", " << gic->z() << ")" << std::endl;
    }
    if(s1xindex>-1){
      TBSICluster* gic=(TBSICluster*)_clusterSIArray->At(s1xindex);
      std::cout << "s1x (" << gic->x() << ", " << gic->y() << ", " << gic->z() << ")" << std::endl;
    }    
    if(s1yindex>-1){
      TBSICluster* gic=(TBSICluster*)_clusterSIArray->At(s1yindex);
      std::cout << "s1y (" << gic->x() << ", " << gic->y() << ", " << gic->z() << ")" << std::endl;
    }    
    if(s2xindex>-1){
      TBSICluster* gic=(TBSICluster*)_clusterSIArray->At(s2xindex);
      std::cout << "s2x (" << gic->x() << ", " << gic->y() << ", " << gic->z() << ")" << std::endl;
    }    
    if(s2yindex>-1){
      TBSICluster* gic=(TBSICluster*)_clusterSIArray->At(s2yindex);
      std::cout << "s2y (" << gic->x() << ", " << gic->y() << ", " << gic->z() << ")" << std::endl;
    }    
#endif
    //FILL CANDIDATES IN Z ORDER
    GFTrackCand* cand=new GFTrackCand();
    if(g1xindex>-1) cand->addHit(FairRootManager::Instance()->GetBranchId("TBGEMCluster"),g1xindex);
    if(g1yindex>-1) cand->addHit(FairRootManager::Instance()->GetBranchId("TBGEMCluster"),g1yindex);
    if(s1xindex>-1) cand->addHit(FairRootManager::Instance()->GetBranchId("TBSICluster"),s1xindex);
    if(s1yindex>-1) cand->addHit(FairRootManager::Instance()->GetBranchId("TBSICluster"),s1yindex);
    if(s2xindex>-1) cand->addHit(FairRootManager::Instance()->GetBranchId("TBSICluster"),s2xindex);
    if(s2yindex>-1) cand->addHit(FairRootManager::Instance()->GetBranchId("TBSICluster"),s2yindex);
    if(g2xindex>-1) cand->addHit(FairRootManager::Instance()->GetBranchId("TBGEMCluster"),g2xindex);
    if(g2yindex>-1) cand->addHit(FairRootManager::Instance()->GetBranchId("TBGEMCluster"),g2yindex);
    
    double alpha = (-1.*sl_pos.Z())/sl_dir.Z();
    TVector3 newsl = sl_pos+alpha*sl_dir;//manual extrapolation to origin for seed
    
    // set seed values to cands
    cand->setCurv(0.);
    //    cand->setDip(trk->dip());
    cand->setComplTrackSeed(newsl, sl_dir, _pdg, sl_poserr, sl_direrr);//sl_pos
    if(fDEBUG) std::cout << "Seed: pos=(" << newsl.x() <<", "<<newsl.y() <<", "<<newsl.z() <<") and  dir=("
			 << sl_dir.x() <<", "<<sl_dir.y() <<", "<<sl_dir.z() <<")" <<std::endl;
    
    //RK TRACKREP
    RKTrackRep* rkrep = new RKTrackRep(newsl, sl_dir, sl_poserr, sl_direrr, _pdg);

    //GF track
    GFTrack* gftrk=new((*_trackArray)[_trackArray->GetEntriesFast()]) GFTrack(rkrep);
    gftrk->setCandidate(*cand); // here the candidate is copied!
    NTR++;
    if(fDEBUG) {gftrk->Print(); std::cout << "gcand =" << cand->getNHits() << "gftrack hits= " << gftrk->getCand().getNHits() <<std::endl; cand->Print();}
    
    //SMOOTHING
    if(_smoothing) gftrk->setSmoothing(true);
    
  }// end loop over tracks
  
  
  std::cout<<"TBTrackInitTask::Exec:: " <<NTR<<" tracks setup."<<std::endl;
  
}