// -------------------------------------------------------------------------
// -----                  CbmLitMuchTrackFinder source file               -----
// -----                  Created 06/07/06  by A. Lebedev               -----
// -------------------------------------------------------------------------
 


#include <iostream>
#include <set>

#include "TClonesArray.h"

#include "CbmRootManager.h"
#include "CbmRunAna.h"
#include "CbmBaseParSet.h"
#include "CbmRuntimeDb.h"
#include "CbmDetector.h"
#include "CbmMuchHit.h"
#include "CbmStsTrack.h"
#include "CbmMuchTrack.h"
#include "CbmStsTrackMatch.h"
#include "CbmMCTrack.h"

#include "CbmLitEnvironment.h"

#include "CbmLitMuchTrackFinder.h"


// ----------------------- Default constructor ---------------------------
CbmLitMuchTrackFinder::CbmLitMuchTrackFinder()
{
   fEvents = 0;
   fVerbose = 1;
}
// -----------------------------------------------------------------------


// --------------------------- Destructor --------------------------------
CbmLitMuchTrackFinder::~CbmLitMuchTrackFinder()
{

}
// -----------------------------------------------------------------------

// ------------------------- Initialisation ------------------------------
void CbmLitMuchTrackFinder::Init()
{
   // Activate data branches

   CbmRootManager* rootMgr = CbmRootManager::Instance();
   if(NULL == rootMgr) 
      TObject::Fatal("CbmLitMuchTrackFinder::Init","ROOT manager is not instantiated");
   
   // Get pointer to the STS track array
   fSeedTracksArray = (TClonesArray*) rootMgr->GetObject("STSTrack");
   if(NULL == fSeedTracksArray) 
      TObject::Fatal("CbmLitMuchTrackFinder::Init","no STS track array");
  
  //fMuchPoints=(TClonesArray *) CbmRootManager::Instance()->GetObject("MuchPoint");

 // fMCTracks =(TClonesArray *) CbmRootManager::Instance()->GetObject("MCTrack");
  
 // fStsTrackMatch = (TClonesArray*) CbmRootManager::Instance()->GetObject("STSTrackMatch");
   
   
   fExtrapolatorToDet = fExtrapolator = new CbmLitRK4TrackExtrapolator();
   //ExtrapolatorToDet = fExtrapolator = new CbmLitParabolicTrackExtrapolator();
   fExtrapolatorToDet->Initialize();
   fPropagatorToDet = fPropagator = new CbmLitTrackPropagator(fExtrapolator);
   if(NULL == fPropagator) 
      TObject::Fatal("CbmLitMuchTrackFinder::Init","propagator is not instantiated");
   
   fPropagatorToDet->Properties().SetProperty("fMass",0.105);
   fPropagatorToDet->Properties().SetProperty("fApplyEnergyLoss", true);
   fPropagatorToDet->Properties().SetProperty("fEnergyLoss", 0.00354);
   fPropagatorToDet->Properties().SetProperty("fFms", 1.25);

   fPropagator->Initialize();
  
   fFilter = new CbmLitKalmanFilter();
   if(NULL == fFilter) 
      TObject::Fatal("CbmLitMuchTrackFinder::Init","filter is not instantiated");
   fFilter->Initialize();
    
   // Determine the Much geometry
   ReadDetectorGeometry();

   fMaxErrX.resize(fNofLayers);
   fMaxErrY.resize(fNofLayers);
   
   
   fSigmaX.resize(fNofLayers);
   fSigmaY.resize(fNofLayers);
   
   if (fNofLayers == 15) {
      fSigmaX[0] = 2.83e-1;  fSigmaY[0] = 3.35e-1;
      fSigmaX[1] = 1.45e-1;  fSigmaY[1] = 2.78e-1;
      fSigmaX[2] = 1.31e-1;  fSigmaY[2] = 2.76e-1;
      fSigmaX[3] = 3.95e-1;  fSigmaY[3] = 5.33e-1;
      fSigmaX[4] = 1.88e-1;  fSigmaY[4] = 4.12e-1;
      fSigmaX[5] = 1.74e-1;  fSigmaY[5] = 3.85e-1;
      fSigmaX[6] = 4.71e-1;  fSigmaY[6] = 6.50e-1;
      fSigmaX[7] = 1.91e-1;  fSigmaY[7] = 3.83e-1;
      fSigmaX[8] = 1.80e-1;  fSigmaY[8] = 3.67e-1;
      fSigmaX[9] = 8.68e-1;  fSigmaY[9] = 1.05;
      fSigmaX[10] = 2.43e-1; fSigmaY[10] = 4.49e-1;
      fSigmaX[11] = 2.15e-1; fSigmaY[11] = 4.20e-1;
      fSigmaX[12] = 1.46;    fSigmaY[12] = 1.75;
      fSigmaX[13] = 3.90e-1; fSigmaY[13] = 7.68e-1;
      fSigmaX[14] = 3.65e-1; fSigmaY[14] = 7.27e-1;   
   } else 
   if (fNofLayers == 10) {
      fSigmaX[0]=0.300774;   fSigmaY[0]=0.345188;
      fSigmaX[1]=0.194035;   fSigmaY[1]=0.35252;
      fSigmaX[2]=0.402636;   fSigmaY[2]=0.569883;
      fSigmaX[3]=0.250647;   fSigmaY[3]=0.48694;
      fSigmaX[4]=0.476763;   fSigmaY[4]=0.689394;
      fSigmaX[5]=0.266849;   fSigmaY[5]=0.465514;
      fSigmaX[6]=0.874739;   fSigmaY[6]=1.09516;
      fSigmaX[7]=0.358771;   fSigmaY[7]=0.549629;
      fSigmaX[8]=1.46335;   fSigmaY[8]=1.73062;
      fSigmaX[9]=0.541059;   fSigmaY[9]=0.929868;
   }
 
}

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

// ------------------- Determines the Much geometry ------------------------
void CbmLitMuchTrackFinder::ReadDetectorGeometry()
{
   CbmLitEnvironment* env = CbmLitEnvironment::Instance();
   
   env->GetMuchLayout(fNofStations, fNofLayers, fNofLayersPerStation, fLayerZPos);
   
}
// -----------------------------------------------------------------------



// -------------------- Algorithm implementation -------------------------
Int_t CbmLitMuchTrackFinder::DoFind(TClonesArray* hitArray,
                                      TClonesArray* trackArray)
{


  // return 0;
   
   std::cout << "==============================================================================" << std::endl;
   std::cout << "-I- CbmLitMuchTrackFinder::DoFind: ACTIVE" << std::endl;
   
   
   fHitArray = hitArray;

  // fMuchTracks.clear();
 //  fFoundTracks.clear();
   fSeedsIdSet.clear();

   fHitIdMap.clear();
  // fUsedHitsSet.clear();
   fHits.clear();
   fHits.resize(fNofLayers);
   
   fApplyUpdateInLayer = true;   
   fPrecalcSearchRegions = false;
   fNofSharedHits = 3;
   fBeginStation = 0;
   fEndStation = fNofStations - 1;
   fMaxNofMissingHitsInStation = 0;
   fSigmaCoef = 3.;
   
   ArrangeHits();

   CreateTracksFromSeeds();

   ExtrapolateToDetector();
 

   
   fNofIter = 1;
   
   for (Int_t iIter = 0; iIter < fNofIter; iIter++) {
       
      if (fVerbose > 1) {
         std::cout << "-I- CbmLitMuchTrackFinder::DoFind: "
              << "iteration " << iIter << std::endl;
      }
   
      
    //  if (iIter == 0) SetIterPar(3, 0, fNofMuchStations - 1, 0);
    //  if (iIter == 1) SetIterPar(5, 0, fNofMuchStations - 2, 0);
    //  if (iIter == 2) SetIterPar(5, 1, fNofMuchStations - 1, 0);
    //  if (iIter == 3) SetIterPar(5, 0, fNofMuchStations - 1, 1);
    
            
    //  if (iIter != 0) ArrangeHits();
      
      InitTracks();

      TrackFollowing();
     
   //   RemoveHits();

      CopyToOutput(trackArray);
   
    //  for(UInt_t i = 0; i < fMuchTracks.size(); i++)
    //       delete fMuchTracks[i];
    //  fMuchTracks.clear();

   //   for (Int_t i = 0; i < fNofMuchLayers; i++) 
   //   fHits[i].clear();
      //fHits.clear();
   }

   //fUsedHitsSet.clear();
  // fHits.clear();
  //fSeedsIdSet.clear();
   
   for(std::vector<CbmLitTrack*>::iterator iTrack = fTracksS.begin();
      iTrack != fTracksS.end(); iTrack++) {
      delete (*iTrack);
   }
   fTracksS.clear();

   
   fTracks.clear();
   fFoundTracks.clear();
    
   std::cout << "-I- CbmLitMuchTrackFinder::DoFind : "
             << trackArray->GetEntriesFast() << " Much tracks found." << std::endl;

   fEvents++;

   std::cout << "-I- CbmLitMuchTrackFinder : "
             << fEvents << " events processed" << std::endl;
       
   std::cout << "----------------------------------------------------" << std::endl;

   return trackArray->GetEntriesFast();
  
}
// -----------------------------------------------------------------------


// --------------Creates Much tracks from Sts tracks-----------------------
void CbmLitMuchTrackFinder::CreateTracksFromSeeds()
{

    Int_t nofStsTracks = fSeedTracksArray->GetEntriesFast();

    for(Int_t iStsTrack = 0; iStsTrack < nofStsTracks; iStsTrack++) {
       
       CbmStsTrack* stsTrack = (CbmStsTrack*) fSeedTracksArray->At(iStsTrack);
   
       if(stsTrack == NULL) continue;
            
      /* 
       CbmStsTrackMatch *m = (CbmStsTrackMatch*) fStsTrackMatch->At(iStsTrack);
       if( !m ) continue;
       Int_t mcTrackID = m->GetMCTrackId();
       if (mcTrackID < 0)  continue;  
       CbmMCTrack*  mcTrack= (CbmMCTrack*)  fMCTracks->At(mcTrackID);
       if( !mcTrack ) continue;
       if( abs(mcTrack->GetPdgCode())!=13 ) continue;
       if(mcTrack->GetMotherId() != -1) continue;
      // std::cout << "Muon track Created :) " << std::endl;       
      */ 
       
       
       CbmLitTrack* track = new CbmLitTrack();
       
       track->SetPreviousTrackId(iStsTrack);
       track->SetParamLast(stsTrack->GetParamLast());
       track->SetChi2(stsTrack->GetChi2());
       track->SetNDF(1);
       fTracksS.push_back(track);
       
      
       track = NULL; ///?!?!?
    }

    if (fVerbose > 1) {
      std::cout << "-I- CbmLitMuchTrackFinder::CreateMuchFromSts : " << std::endl
                << nofStsTracks << " STS tracks were loaded, "
                << fTracksS.size() << " Much tracks created" << std::endl;
    }
}
// -----------------------------------------------------------------------

// --------------------Copy to output TClonesArray------------------------
void CbmLitMuchTrackFinder::CopyToOutput(TClonesArray* trackArray)
{
   Int_t nofTracks = trackArray->GetEntriesFast();
   
   for(std::vector<CbmLitTrack*>::iterator iTrack = fFoundTracks.begin();
         iTrack != fFoundTracks.end(); iTrack++) {
      if( (*iTrack)->GetFlag() == 0) {
         if (fSeedsIdSet.find((*iTrack)->GetPreviousTrackId()) == fSeedsIdSet.end()) {
         
            new ((*trackArray)[nofTracks]) CbmMuchTrack((*iTrack)->MapToMuch());
                                    
            fSeedsIdSet.insert((*iTrack)->GetPreviousTrackId());
            
            nofTracks++;
         }
      }
      delete (*iTrack);
   }
   
   //clear array
   fFoundTracks.clear();
}
// -----------------------------------------------------------------------


ClassImp(CbmLitMuchTrackFinder);