// -------------------------------------------------------------------------
// -----                CbmStsHitProducerIdeal source file             -----
// -----                  Created 10/01/06  by V. Friese               -----
// -------------------------------------------------------------------------


#include "TClonesArray.h"
#include "TArrayD.h"
#include "TGeoManager.h"

#include "CbmRootManager.h"
#include "CbmRunAna.h"
#include "CbmRuntimeDb.h"

#include "PndMvdIdealTrackFinderTask.h"

#include "PndMvdDigiPixel.h"
#include "PndMvdPixelCluster.h"


// -----   Default constructor   -------------------------------------------
PndMvdIdealTrackFinderTask::PndMvdIdealTrackFinderTask() :
  CbmTask("MVD Ideal Track Finding Task")
{
   fHitBranch = "MVDClusterHit";
   fClusterBranch = "MVDClusterCand";
   fDigiBranch = "MVDPixelHit";
   fMcBranch = "MVDPoint";
   fTrackBranch = "MCTrack";
}


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


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

// -----   Initialization  of Parameter Containers -------------------------
void PndMvdIdealTrackFinderTask::SetParContainers()
{
  // Get Base Container
/*  
  CbmRunAna* ana = CbmRunAna::Instance();
  CbmRuntimeDb* rtdb=ana->GetRuntimeDb();
  fGeoPar = (PndMvdGeoPar*)(rtdb->getContainer("PndMvdGeoPar"));
*/
}

InitStatus PndMvdIdealTrackFinderTask::ReInit()
{
  
  InitStatus stat=kERROR;
  return stat;
  
  /*
  CbmRunAna* ana = CbmRunAna::Instance();
  CbmRuntimeDb* rtdb=ana->GetRuntimeDb();
  fGeoPar=(PndMvdGeoPar*)(rtdb->getContainer("PndMvdGeoPar"));
  
  return kSUCCESS;
  */
}

// -----   Public method Init   --------------------------------------------
InitStatus PndMvdIdealTrackFinderTask::Init() 
{
  
  CbmRootManager* ioman = CbmRootManager::Instance();

  if ( ! ioman ) 
    {
      std::cout << "-E- PndMvdIdealTrackFinderTask::Init: "
     << "RootManager not instantiated!" << std::endl;
      return kFATAL;
    }
    
  // Get input array
  fHitArray = (TClonesArray*) ioman->GetObject(fHitBranch);
  if ( !fHitArray){
    std::cout << "-W- PndMvdIdealTrackFinderTask::Init: " << "No hitArray!" << std::endl;
    return kERROR;
  }
  
  fClusterArray = (TClonesArray*) ioman->GetObject(fClusterBranch);
  if ( !fHitArray){
    std::cout << "-W- PndMvdIdealTrackFinderTask::Init: " << "No clusterArray!" << std::endl;
    return kERROR;
  }

  fDigiArray = (TClonesArray*) ioman->GetObject(fDigiBranch);
  if ( !fHitArray){
    std::cout << "-W- PndMvdIdealTrackFinderTask::Init: " << "No digiArray!" << std::endl;
    return kERROR;
  }

  fMcArray = (TClonesArray*) ioman->GetObject(fMcBranch);
  if ( !fHitArray){
    std::cout << "-W- PndMvdIdealTrackFinderTask::Init: " << "No mcArray!" << std::endl;
    return kERROR;
  }

  fTrackArray = (TClonesArray*) ioman->GetObject(fTrackBranch);
  if ( !fHitArray){
    std::cout << "-W- PndMvdIdealTrackFinderTask::Init: " << "No trackArray!" << std::endl;
    return kERROR;
  }
  

  
  fTrackCandArray = new TClonesArray("TrackCand");
  ioman->Register("MVDTrackCand", "MVD", fTrackCandArray, kTRUE);
  
  std::cout << "-I- PndMvdIdealTrackFinderTask: Initialisation successfull" << std::endl;
  return kSUCCESS;
}
// -------------------------------------------------------------------------



// -----   Public method Exec   --------------------------------------------
void PndMvdIdealTrackFinderTask::Exec(Option_t* opt) 
{

  // Reset output array
  if ( ! fTrackCandArray )
    Fatal("Exec", "No trackCandArray");
  fTrackCandArray->Clear();

  Int_t 
    nPoints = fHitArray->GetEntriesFast();

  for (Int_t iPoint = 0; iPoint < nPoints; iPoint++){
  PndMvdHit* myHit = (PndMvdHit*)(fHitArray->At(iPoint));
  Int_t cIndex = myHit->GetRefIndex();
  PndMvdClusterCand* myCluster = (PndMvdClusterCand*)(fClusterArray->At(cIndex));
  
  std::vector<Int_t> digis = GetHitPerCluster(myCluster);
  std::cout << "Digis.size(): " << digis.size() << std::endl;
  for (Int_t iDigis = 0; iDigis < digis.size(); iDigis++){
    PndMvdMCPoint* myPoint = (PndMvdMCPoint*)(fMcArray->At(digis[iDigis]));
    std::cout << *myPoint << std::endl;
    Int_t trackID = myPoint->GetTrackID();
    if (fTrackCandMap[trackID] == 0){
      TrackCand *myTCand = new TrackCand();
      CbmMCTrack* myMCTrack = (CbmMCTrack*)fTrackArray->At(trackID);
      myTCand->setCurv(GetTrackCurvature(myMCTrack));
      myTCand->setDip(GetTrackDip(myMCTrack));
      myTCand->setInverted(false);
      fTrackCandMap[trackID] = myTCand;
    }
    fTrackCandMap[trackID]->addHit(1,iPoint);
  }
  }
  
  PrintResult();
  
  Int_t i = 0;
  for (std::map<Int_t,TrackCand*>::const_iterator ci=fTrackCandMap.begin();
      ci != fTrackCandMap.end(); ci++){
    new((*fTrackCandArray)[i]) TrackCand(*(ci->second));
    i++;
  }
  ClearTrackCandMap();
}

std::vector<Int_t> PndMvdIdealTrackFinderTask::GetHitPerCluster(PndMvdClusterCand* clusterCand)
{
  std::vector<Int_t> result;
  std::vector<Int_t> digiPos = clusterCand->GetClusterList();
  bool isInResult = false;
  for (Int_t i = 0; i < digiPos.size(); i++){
    PndMvdDigiPixel* digiHit = (PndMvdDigiPixel*)(fDigiArray->At(digiPos[i]));
    Int_t mcID = digiHit->GetIndex();
    //std::cout << " -I- GetHitPerCluster: mcID: " << mcID << std::endl;
      for (Int_t j = 0; j < result.size() && isInResult == false; j++){
        //std::cout << "Result: " << result[j] << std::endl;
        if (mcID == result[j])
          isInResult = true;
      }
    if (isInResult == false)
      result.push_back(mcID);
    isInResult = false;
  }
  for (Int_t k = 0; k < result.size(); k++)
    std::cout << " Result: " << k << ": " << result[k] << std::endl;
  return result;
}

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

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

void PndMvdIdealTrackFinderTask::PrintResult()
{
  std::cout << "**** TrackFinding *****" << std::endl;
   for (std::map<Int_t, TrackCand*>::const_iterator ci=fTrackCandMap.begin();
        ci != fTrackCandMap.end(); ci++){
     std::cout << "TrackID: " << ci->first << std::endl;
     TrackCand* trackCand = ci->second;
     for (unsigned int i = 0; i < trackCand->getNHits(); i++){
       unsigned int detId, hitId;
       trackCand->getHit(i, detId, hitId);
      PndMvdHit* myHit = (PndMvdHit*)fHitArray->At(hitId);
      std::cout << *myHit;
     } 
   }
   std::cout << std::endl;
}

void PndMvdIdealTrackFinderTask::ClearTrackCandMap()
{
  for (std::map<Int_t,TrackCand*>::const_iterator ci=fTrackCandMap.begin();
          ci != fTrackCandMap.end(); ci++){
    delete(ci->second);
  }
  fTrackCandMap.clear();
}
ClassImp(PndMvdIdealTrackFinderTask)