//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Conversion of HeliTracks into GFtrack objects
//      plus initialization
//
// Environment:
//      Software developed for the GEM-TPC in FOPI
//
// Author List:
//      Francesco Cusanno    TUM            (original author)
//      Felix Boehmer, E18   TUM            (Overhaul)
//      Paul Buehler,        SMI            (adapted for Helitron/Plawa tracks)
//
//-----------------------------------------------------------

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

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

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

// Collaborating Class Headers --------
#include "FairRootManager.h"
#include "TClonesArray.h"
#include "FopiEvent.h"
#include "HeliHit.h"
#include "HeliTrack.h"
#include "PlawaTrack.h"
#include "FopiForwardHit.h"
#include "PseudoSpacePoint.h"

#include "GFTrackCand.h"
#include "GFTrack.h"
#include "RKTrackRep.h"
#include "TVector3.h"
#include "FairRunAna.h"

#include "TDatabasePDG.h"


// Class Member definitions -----------

ClassImp(HeliPlawaTrackInitTask)

HeliPlawaTrackInitTask::HeliPlawaTrackInitTask()
  : fFopiEventName("FopiEvent"),
    fHeliHitBranchName("HeliHit"), 
    fHeliTrackBranchName("HeliTrack"),
    fPlawaHitBranchName("PlawaTrack"), 
    fTrackOutBranchName("HeliPlawaTrackPreFit"),
    fdrMax(100.),
    fwithMainVertex(kFALSE),
    fWithPlawaHit(kTRUE),
    fminHPmatch(0.),
    fPersistence(kTRUE),
    fVerbose(kFALSE)
{	

  fRad = 3.1415926/180.;
  fPosErr = TVector3(1.,1.,0.1);
  fMomErr = TVector3(0.1,0.1,0.1);
  fResScaleFac = 1;
  fwivScale = kFALSE;
  fHRes = TVector3(-1.,-1.,0.1);
  fPRes = TVector3(1.0,1.0,1.0);

}   

HeliPlawaTrackInitTask::~HeliPlawaTrackInitTask()
{;}


InitStatus
HeliPlawaTrackInitTask::Init()
{
  //Get ROOT Manager
  FairRootManager* ioman= FairRootManager::Instance();
  if(ioman==NULL){
    Error("HeliPlawaTrackInitTask::Init","RootManager not instantiated!");
    return kERROR;
  }

  // get input arrays    
  fFopiEventArray=(TClonesArray*) ioman->GetObject(fFopiEventName);
  if(fFopiEventArray==0) {
    Error("TpcHelMatchingTask::Init","FOPI event not found!");
    return kERROR;
  } 

  fHeliHitArray=(TClonesArray*) ioman->GetObject(fHeliHitBranchName);
  if(fHeliHitArray==NULL){
    Error("HeliPlawaTrackInitTask::Init","HeliHit-Array not found!");
    return kERROR;
  }

 fHeliTrackArray=(TClonesArray*) ioman->GetObject(fHeliTrackBranchName);
  if(fHeliTrackArray==NULL){
    Error("HeliPlawaTrackInitTask::Init","HeliTrack-Array not found!");
    return kERROR;
  }

  fPlawaHitArray=(TClonesArray*) ioman->GetObject(fPlawaHitBranchName);
  if(fPlawaHitArray==NULL){
    Error("HeliPlawaTrackInitTask::Init","PlawaHit-Array not found!");
    return kERROR;
  }

  // create and register output array
  fTrackOutArray = new TClonesArray("GFTrack");
  ioman->Register(fTrackOutBranchName,"Tpc",fTrackOutArray,fPersistence);
  
  return kSUCCESS;
}


//helper functor
Bool_t 
sortingbyz(const FopiForwardHit lh, const FopiForwardHit rh) {
  
  // return(lh.GetGlobalPos().Perp() < rh.GetGlobalPos().Perp());
  return(lh.GetGlobalPos().Z() < rh.GetGlobalPos().Z());
}

void
HeliPlawaTrackInitTask::Exec(Option_t* opt)
{
  if(fVerbose)
    std::cout<<"HeliPlawaTrackInitTask::Exec():"<<std::endl;
  
  // variable definitions
  Int_t NumHelTrackHits = 0;
  Int_t ntracks = -1;
  TVector3 pos0, mom0;
  TMatrixD cov0;  
  Float_t hrsec;
  Double_t theHel, phiHel, momHel, dEdxHel, HelPlaMat;

  std::vector<unsigned int> HELcandIDs;
  std::vector<FopiForwardHit> helihits;

  // Definition of Helitron and Plawa hit covariance matrices
  TMatrixD HHcov = TMatrixD(3,3); 
  Double_t HHcovvals[9] = {0.,0.,0.,0.,0.,0.,0.,0.,pow(fHRes.Z(),2)};
  TMatrixD PHcov = TMatrixD(3,3); 
  Double_t PHcovvals[9] = {
    pow(fPRes.X(),2),0.,0.,0.,pow(fPRes.Y(),2),0.,0.,0.,pow(fPRes.Z(),2)};
  PHcov.SetMatrixArray(PHcovvals,"F");
  TMatrixD MVcov = TMatrixD(3,3); 
  Double_t MVcovvals[9] = {
    pow(0.1,2),0.,0.,0.,pow(0.1,2),0.,0.,0.,pow(0.1,2)};
  MVcov.SetMatrixArray(MVcovvals,"F");

  // Reset output Arrays
  if(fTrackOutArray==NULL)
    Fatal("HeliPlawaTrackInitTask::Exec()","No TrackOutTrackArray");
  fTrackOutArray->Delete();

	// get FOPI event
  FopiEvent *fopiEv = (FopiEvent*) fFopiEventArray->At(0);
  if (fopiEv == 0) return;
	
	Int_t nrun   = fopiEv->GetRunNum();
	Int_t nspill = fopiEv->GetSpillNum();
	Int_t nevent = fopiEv->GetEvtNum();
  if (fVerbose)
    fprintf(stderr,"Event nrun, nspill, nevent: %i %i %i\n",nrun,nspill,nevent);
  
  // transformation Helitron-to-beam 
  Float_t taroff = fopiEv->GetTaroff();
  Float_t phi0   = fopiEv->GetPhi0();
  TVector3 hoff  = fopiEv->GetHoff();
  TVector3 hrot  = fopiEv->GetHrot();
  
	// define globhit and lochit
	FopiForwardHit globhit = FopiForwardHit(1,0,taroff,hoff,hrot);
	FopiForwardHit lochit  = FopiForwardHit(0,0,taroff,hoff,hrot);

  // how many tracks are there
  Int_t NumHelTracks = fopiEv->GetHmul();
  if (fVerbose)
    fprintf(stderr,"Number of Helitron tracks: %i\n",NumHelTracks);
  
  // loop over Helitron tracks
  for (Int_t ii=0; ii<NumHelTracks; ii++) {
		HeliTrack *hsingletrack = (HeliTrack*) fHeliTrackArray->At(ii);
    PlawaTrack *psingletrack = (PlawaTrack*) fPlawaHitArray->At(ii);

		// momentum and Helitron/PLAWA matching
    momHel = hsingletrack->GetHmom();
    HelPlaMat = hsingletrack->GetPHmatch();
    
    // get track hit errors
    TVector3 HRes = fHRes;
    if (fHRes.X()<0.) HRes = TVector3(hsingletrack->GetHdx(),HRes.Y(),HRes.Z());
    if (fHRes.Y()<0.) HRes = TVector3(HRes.X(),hsingletrack->GetHdy(),HRes.Z());
    if (fVerbose) fprintf(stderr,"Hit errors: %f %f\n",HRes.X(),HRes.Y());
    
    // reference sector
    hrsec = hsingletrack->GetHrsec();
    lochit.SetHrsec(hrsec);
    globhit.SetHrsec(hrsec);

    if (fVerbose) {
      fprintf(stderr,"....................................................\n");
      fprintf(stderr,"Helitron track (ref.sec. %i)> mass: %f mom: %f charge: %f\n",
        (Int_t)hrsec,hsingletrack->GetHmass(),momHel,
        hsingletrack->GetHchar());
        fprintf(stderr,"Hit errors: %f %f\n", HRes.X(),HRes.Y());    }
    
    // compute theta and phi from track hits
    HELcandIDs.clear(); 
    HELcandIDs = hsingletrack->GetHelIdHits();
    NumHelTrackHits = HELcandIDs.size();
    if (fVerbose)
      fprintf(stderr,"Number of hits on track: %i\n",NumHelTrackHits);
      
    helihits.clear();
    theHel = 0.;
    phiHel = 0.;
    TVector3 possum = TVector3(0.,0.,0.);
    for (Int_t jj=0; jj<NumHelTrackHits; jj++) {
			HeliHit *hsinglehit = (HeliHit*) fHeliHitArray->At(HELcandIDs[jj]);
      
      // convert to global coordinate system
      lochit.SetPos(0,
        hsinglehit->GetHhx(),hsinglehit->GetHhy(),hsinglehit->GetHhz());
      helihits.push_back(lochit);

      possum += lochit.GetGlobalPos();

    }
    sort (helihits.begin(),helihits.end(),sortingbyz);
    theHel = atan2(possum.Perp(),possum.Z());
    phiHel = atan2(possum.Y(),possum.X());
    
    // define track representations
    // initial state position
    HeliHit *hh = (HeliHit*) fHeliHitArray->At(HELcandIDs[0]);
    lochit.SetPos(0,hh->GetHhx(),hh->GetHhy(),hh->GetHhz());
    pos0 = lochit.GetGlobalPos();
    
    // initial state momentum
    mom0 = TVector3(1.,0.,0.);
    if (fabs(momHel) > 0. && fabs(momHel) < 100.) mom0 *= fabs(momHel);
    mom0.SetTheta(theHel);
    mom0.SetPhi(phiHel);
    TVector3 momerr(mom0.X()*fMomErr.X(),mom0.Y()*fMomErr.Y(),
      mom0.Z()*fMomErr.Z()); 
    
    // particle ID
    Int_t pdg = 211;
    if (momHel<0.) pdg *= -1.;
    
    RKTrackRep* rep1 = new RKTrackRep(pos0,mom0,fPosErr,momerr,pdg);
    RKTrackRep* rep2 = new RKTrackRep(pos0,mom0,fPosErr,momerr,-1*pdg);
    
    if (fVerbose) {
      fprintf(stderr,"Track initialisation \n");
      fprintf(stderr,"pos0: %8.3f %8.3f %8.3f \n",pos0.X(),pos0.Y(),pos0.Z());
      fprintf(stderr,"mom0: %8.3f %8.3f %8.3f \n",mom0.X(),mom0.Y(),mom0.Z());
    }

    // create track on output array
    GFTrack* trk = new GFTrack(rep1);
    trk->addTrackRep(rep2);
    trk->setCardinalRep(0);

    // add MainVertex
    if (fwithMainVertex) {
      globhit.SetPos(1,0.,0.,0.);
      globhit.SetCov(1,MVcov);
      PseudoSpacePoint *MainVertex = new PseudoSpacePoint(&globhit);
      trk->addHit(MainVertex);
    }

    // add Helitron hit points
    for (Int_t kk=0; kk<NumHelTrackHits; kk++) {
      
      // determine hit covariance matrix
      Double_t rnom = helihits[kk].GetLocalPos().Z()*tan(theHel);
      Double_t rmes = helihits[kk].GetLocalPos().Perp();
      Double_t dr = fabs(rnom-rmes);
      
      // is this point added?
      if (dr < fdrMax) {
      
        Double_t efac = 1.0;
        if (fwivScale) efac = dr/HRes.Perp();        
        HHcovvals[0] = pow(fResScaleFac*efac*HRes.X(),2);
        HHcovvals[4] = pow(fResScaleFac*efac*HRes.Y(),2);
        HHcovvals[8] = pow(fResScaleFac*HRes.Z(),2);
        HHcov.SetMatrixArray(HHcovvals,"F");
        helihits[kk].SetCov(0,HHcov);

        PseudoSpacePoint *HeliHit = new PseudoSpacePoint(&(helihits[kk]));
        trk->addHit(HeliHit);
      
        if (fVerbose) {
          TVector3 pos = helihits[kk].GetGlobalPos();
          fprintf(stderr,"HEL pos: %8.3f/%8.3f %8.3f/%8.3f %8.3f/%8.3f\n",
            pos.X(),fResScaleFac*efac*HRes.X(),pos.Y(),
            fResScaleFac*efac*HRes.Y(),pos.Z(),HRes.Z());
        }
      }
      
    }
    // fprintf(stderr,"Number of hits on track: %i\n",trk->getNumHits());
    
    // add matched Plawa point to track
    if (fWithPlawaHit && HelPlaMat>0.) {
      
      Float_t phthe = fRad*psingletrack->GetPhthe();
      Float_t phphi = fRad*psingletrack->GetPhphi();
      Float_t phz = psingletrack->GetPhz();
      Float_t pr = phz*tan(phthe);
      
      globhit.SetPos(1,pr*cos(phphi),pr*sin(phphi),phz);
      globhit.SetCov(1,PHcov);

      PseudoSpacePoint *PlawaHit = new PseudoSpacePoint(&globhit);
      trk->addHit(PlawaHit);
    
      if (fVerbose) {
        TVector3 pos = globhit.GetLocalPos();
        fprintf(stderr,"PLA pos: %8.3f/%8.3f %8.3f/%8.3f %8.3f/%8.3f\n",
          pos.X(),fPRes.X(),pos.Y(),fPRes.Y(),pos.Z(),fPRes.Z());
      }
    }
    
    // create track on output array
    ntracks++;
    new((*fTrackOutArray)[ntracks]) GFTrack(*trk);
    
  }

}