// -------------------------------------------------------------------------
// -----                PndMvdIdealTrackingTask source file             -----
// -----                  Created 20/03/07  by R.Kliemt               -----
// -------------------------------------------------------------------------
// libc includes
#include <iostream>

// Root includes
#include "TROOT.h"
#include "TString.h"
#include "TClonesArray.h"
#include "TParticlePDG.h"

// framework includes
#include "CbmRootManager.h"
#include "PndMvdIdealTrackingTask.h"
#include "CbmRunAna.h"
#include "CbmRuntimeDb.h"
#include "CbmMCTrack.h"


// PndMvd includes
#include "PndMvdRecoHit.h"
// #include "PndMvdTrackCand.h"
#include "Track.h"
#include "TrackCand.h"
#include "PndMvdHit.h"
#include "PndMvdMCPoint.h"
#include "PndMvdCluster.h"
#include "PndMvdDigi.h"

// -----   Default constructor   -------------------------------------------
PndMvdIdealTrackingTask::PndMvdIdealTrackingTask() :
  CbmTask("Digitization task for PANDA PndMvd") 
{
	fBranchName 	= "";
}
// -------------------------------------------------------------------------



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

// -----   Public method Init   --------------------------------------------
InitStatus PndMvdIdealTrackingTask::Init()
{
  // Get RootManager
  CbmRootManager* ioman = CbmRootManager::Instance();
  if ( ! ioman ) {
    std::cout << "-E- PndMvdIdealTrackingTask::Init: "<< "RootManager not instantiated!" << std::endl;
    return kFATAL;  }

  // Get input array
  fHitArray = (TClonesArray*) ioman->GetObject("MVDHit");
  if ( ! fHitArray ) {
    std::cout << "-W- PndMvdIdealTrackingTask::Init: "<< "No fHitArray!" << std::endl;
    return kERROR;  }

  fClusterPixelArray = (TClonesArray*) ioman->GetObject("MVDClusterCand");
  if ( ! fClusterPixelArray ) {
    std::cout << "-W- PndMvdIdealTrackingTask::Init: "<< "No fClusterPixelArray!" << std::endl;
    return kERROR;  }

  fClusterStripArray = (TClonesArray*) ioman->GetObject("MVDStripClusterCand");
  if ( ! fClusterStripArray ) {
    std::cout << "-W- PndMvdIdealTrackingTask::Init: "<< "No fClusterStripArray!" << std::endl;
    return kERROR;  }

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

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

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

  // Get MCTruth collection
  fMctruthArray=(TClonesArray*) ioman->GetObject("MCTrack");
  if(fMctruthArray==0)  {
    std::cout << "-W- PndMvdIdealTrackingTask::Init: "<< "No McTruth array!" << std::endl;
    return kERROR;  }

  // Create and register output array
  fTrackOutputArray = new TClonesArray("TrackCand");
  ioman->Register("MVDTrackCand", "PndMvd ideal tracklets", fTrackOutputArray, kTRUE);


  return kSUCCESS;
}
// -------------------------------------------------------------------------
void PndMvdIdealTrackingTask::SetParContainers()
{
  // Get Base Container
  CbmRunAna* ana = CbmRunAna::Instance();
  CbmRuntimeDb* rtdb=ana->GetRuntimeDb();

}


// -----   Public method Exec   --------------------------------------------
void PndMvdIdealTrackingTask::Exec(Option_t* opt)
{
 	if ( ! fTrackOutputArray ) Fatal("Exec", "No fTrackOutputArray");
 	fTrackOutputArray->Clear();

  // CREATE MONTECARLO MAP
  std::map<Int_t, CbmMCTrack*> mcmap;
  Int_t nmc=fMctruthArray->GetEntriesFast();
  for(Int_t imc=0;imc<nmc;++imc){
    CbmMCTrack* mc=(CbmMCTrack*)fMctruthArray->At(imc);
    mcmap[imc]=mc;
  }

  // BUILD TRACK Candidates
  std::map<Int_t, TrackCand*> trackMap;

  Int_t nPndMvdHits=fHitArray->GetEntriesFast();
  for(Int_t iHit=0;iHit<nPndMvdHits;++iHit)
  {

    PndMvdHit* hit=(PndMvdHit*)fHitArray->At(iHit);
    Int_t clustid=hit->GetRefIndex();
    Int_t digiid,pointid,MCid;
    if(hit->GetBotIndex() < 0){
      digiid=((PndMvdCluster*)fClusterPixelArray->At(clustid))->GetDigiIndex(0);
      pointid=((PndMvdDigi*)fDigiPixelArray->At(digiid))->GetIndex(0);
    }else{
      digiid=((PndMvdCluster*)fClusterStripArray->At(clustid))->GetDigiIndex(0);
      pointid=((PndMvdDigi*)fDigiStripArray->At(digiid))->GetIndex(0);
    }
      PndMvdMCPoint* point = (PndMvdMCPoint*)fPointArray->At(pointid);
      MCid = point->GetTrackID();

    
    // cut on secondaries (deltas) etc
    if(MCid<0)continue;

    TVector3 pos;
    hit->Position(pos);

    // build tracks here:

    // check if track already candidated:
    if(trackMap[MCid]==0)
    {
      Int_t size = fTrackOutputArray->GetEntriesFast();
      //caution this should be an intrinsic assignment, so trackMap and
      //fTrackOutputArray contain the same pointers.
      trackMap[MCid]=new ((*fTrackOutputArray)[size]) TrackCand();

			Int_t pdgcode = mcmap[MCid]->GetPdgCode();
			TParticlePDG pdgpart(pdgcode);
			Int_t mcCharge = (Int_t)pdgpart.Charge();

			TVector3 pos = mcmap[MCid]->GetStartVertex();
			TVector3 mom = mcmap[MCid]->GetMomentum();

			Double_t invp = 1 / mom.Mag();
			Double_t dxdz = invp*mom.x()*mom.z();
			Double_t dydz = invp*mom.y()*mom.z();
			Double_t sigx = 0.01, sigy = 0.01, sigdxdz = 0.01, sigdydz = 0.01, siginvp = 0.01;

			sigx*=pos.x();
			sigy*=pos.y();
			sigdxdz*=dxdz;
			sigdydz*=dydz;
			siginvp*=invp;

			if(fVerbose>1) std::cout<<"PndMvdIdealTrackingTask::Exec(), Particle Info: "
        <<pdgpart.GetName()<<" ("<<pdgcode<<") Q="<<mcCharge<<std::endl;
     
      trackMap[MCid]->setCurv(GetTrackCurvature(mcmap[MCid]));
      trackMap[MCid]->setDip(GetTrackDip(mcmap[MCid]));
      trackMap[MCid]->setInverted(false);


    }

    // add the hit index
//       trackMap[MCid]->addHit(,hit->GetDetectorID(),iHit);
//       PndMvdRecoHit arecohit(hit);

       /// detector number hardcoded TODO: convention
       trackMap[MCid]->addHit(3,iHit);

  }//end for hit


  std::cout<<fTrackOutputArray->GetEntriesFast()
		   <<" Tracks created"<<std::endl;



   
}

Double_t PndMvdIdealTrackingTask::GetTrackCurvature(CbmMCTrack* myTrack)
{
  TVector3 p = myTrack->GetMomentum();
  return (2/TMath::Sqrt(p.Px()*p.Px() + p.Py()*p.Py()));
}

Double_t PndMvdIdealTrackingTask::GetTrackDip(CbmMCTrack* myTrack)
{
  TVector3 p= myTrack->GetMomentum();
  return (p.Mag()/TMath::Sqrt(p.Px()*p.Px() + p.Py()*p.Py()));
}

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


ClassImp(PndMvdIdealTrackingTask);