// -------------------------------------------------------------------------
// -----                  CbmLitTrdTrackFinderSts 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 "CbmTrdPoint.h"
#include "CbmTrdHit.h"
#include "CbmStsTrack.h"
#include "CbmTrdTrack.h"

#include "base/CbmTrackPropagator.h"
#include "base/CbmTrackUpdate.h"
#include "base/CbmTrackExtrapolator.h"
#include "CbmLitKalmanFilter.h"
#include "CbmLitTrackPropagator.h"
#include "CbmLitLineTrackExtrapolator.h"
#include "CbmLitRK4TrackExtrapolator.h"
#include "CbmLitParabolicTrackExtrapolator.h"
//#include "base/CbmTrackPropagatorGeane.h"

#include "CbmLitEnvironment.h"


#include "CbmLitTrdTrackFinderSts.h"



// ----------------------- Default constructor ---------------------------
CbmLitTrdTrackFinderSts::CbmLitTrdTrackFinderSts()
{
  // fIsStandalone = false;
}
// -----------------------------------------------------------------------


// --------------------------- Destructor --------------------------------
CbmLitTrdTrackFinderSts::~CbmLitTrdTrackFinderSts()
{
   //TODO delete it in the base class
   delete fPropagator;
   delete fExtrapolator;
   delete fPropagatorToDet;
   delete fExtrapolatorToDet;
   delete fFilter;
}
// -----------------------------------------------------------------------


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

   CbmRootManager* rootMgr = CbmRootManager::Instance();
   if(NULL == rootMgr) 
      TObject::Fatal("CbmLitTrdTrackFinderSts::Init", "ROOT manager is not instantiated"); 
   
   // Get pointer to the STS track array
   fSeedTracksArray = (TClonesArray*) rootMgr->GetObject("STSTrack");
   if(NULL == fSeedTracksArray)
      TObject::Fatal("CbmLitTrdTrackFinderSts::Init", "no STS track array");
   
   fExtrapolatorToDet = new CbmLitRK4TrackExtrapolator();
   fPropagatorToDet = new CbmLitTrackPropagator(fExtrapolatorToDet);
   //fPropagatorToDet = new CbmTrackPropagatorGeane();
   if(NULL == fPropagatorToDet) 
      TObject::Fatal("CbmLitTrdTrackFinderSts::Init", "propagator to detector is not instantiated");
   
   fPropagatorToDet->Properties().SetProperty("fMass",0.139);
   fPropagatorToDet->Properties().SetProperty("fApplyEnergyLoss", false);
   fPropagatorToDet->Properties().SetProperty("fEnergyLoss", 0.00354);
   fPropagatorToDet->Properties().SetProperty("fFms", 1.1);
   fPropagatorToDet->Initialize();
   
   
   fExtrapolator = new CbmLitLineTrackExtrapolator();
   fPropagator = new CbmLitTrackPropagator(fExtrapolator);
  // fPropagator = new CbmTrackPropagatorGeane();
   if(NULL == fPropagator) 
      TObject::Fatal("CbmLitTrdTrackFinderSts::Init", "propagator is not initialized");
    
   fPropagator->Properties().SetProperty("fMass",0.139);
   fPropagator->Properties().SetProperty("fApplyEnergyLoss", false);
   fPropagator->Properties().SetProperty("fEnergyLoss", 0.00354);
   fPropagator->Properties().SetProperty("fFms", 1.1);
   fPropagator->Initialize();
   
   
   fFilter = new CbmLitKalmanFilter();
   if(NULL == fFilter) 
      TObject::Fatal("CbmLitTrdTrackFinderSts::Init", "filter is not initialized");
   fFilter->Initialize();
   
   // Determine the TRD geometry
   ReadDetectorGeometry();

   fHits.clear();
   fHits.resize(fNofLayers);
}
// -----------------------------------------------------------------------


// ------------------- Determines the TRD geometry ------------------------
void CbmLitTrdTrackFinderSts::ReadDetectorGeometry()
{
   CbmLitEnvironment* env = CbmLitEnvironment::Instance();
   
   env->GetTrdLayout(fNofStations, fNofLayers, fNofLayersPerStation, fLayerZPos);

   fMaxErrX.resize(fNofLayers);
   fMaxErrY.resize(fNofLayers);
}
// -----------------------------------------------------------------------

// -------------------- Algorithm implementation -------------------------
Int_t CbmLitTrdTrackFinderSts::DoFind(TClonesArray* hitArray,
                                      TClonesArray* trackArray)
{
   
   std::cout << "==============================================================================" << std::endl;
   std::cout << "-I- CbmLitTrdTrackFinderSts::DoFind: ACTIVE" << std::endl;
   
   
   fHitArray = hitArray;

   fTracks.clear();
   fFoundTracks.clear();
   fSeedsIdSet.clear();


   fUsedHitsSet.clear();

   fHits.clear();
   fHits.resize(fNofLayers);
   
   fApplyUpdateInLayer = false;
   fPrecalcSearchRegions = false;
   fNofSharedHits = 0;
   
   ArrangeHits();

   CreateTracksFromSeeds();

   ExtrapolateToDetector();
 
   
   fNofIter = 4;
   
   for (Int_t iIter = 0; iIter < fNofIter; iIter++) {
       
      if (fVerbose > 1) {
         std::cout << "-I- CbmLitTrdTrackFinderSts::DoFind: "
              << "iteration " << iIter << std::endl;
      }
   
      //fHits.resize(fNofTrdLayers);
      //fHitIdMap.clear();
      
      
      if (iIter == 0) SetIterPar(3, 0, fNofStations - 1, 0);
     // if (iIter == 1) SetIterPar(5, 0, fNofStations - 1, 0);
     // if (iIter == 2) SetIterPar(7, 0, fNofStations - 1, 0);
      
      if (iIter == 2) SetIterPar(5, 0, fNofStations - 2, 0);
      if (iIter == 3) SetIterPar(5, 1, fNofStations - 1, 0);
      if (iIter == 4) SetIterPar(5, 0, fNofStations - 1, 1);
    
            
      if (iIter != 0) ArrangeHits();
      
      InitTracks();

      TrackFollowing();
     
      RemoveHits();

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

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

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

   std::cout << "-I- CbmLitTrdTrackFinderSts::DoFind : "
        << trackArray->GetEntriesFast() << " TRD tracks found." << std::endl;

   fEvents++;

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

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

// ------------Set iteration parameters-----------------------------------
void CbmLitTrdTrackFinderSts::SetIterPar(
                               Double_t SigmaCoef, 
                               Int_t beginStation, 
                               Int_t endStation,
                               Int_t maxNofMissingHitsInStation) 
{
   fSigmaCoef = SigmaCoef;
   fBeginStation = beginStation;
   fEndStation= endStation;
   fMaxNofMissingHitsInStation = maxNofMissingHitsInStation;
   
}
// -----------------------------------------------------------------------

// --------------Creates Trd tracks from Sts tracks-----------------------
void CbmLitTrdTrackFinderSts::CreateTracksFromSeeds()
{

    Int_t nofStsTracks = fSeedTracksArray->GetEntriesFast();

    for(Int_t iStsTrack = 0; iStsTrack < nofStsTracks; iStsTrack++) {
       
       CbmStsTrack* stsTrack = (CbmStsTrack*) fSeedTracksArray->At(iStsTrack);
   
       if (stsTrack == NULL) continue;
       
       if (stsTrack->GetParamLast()->GetQp() == 0) continue;    
       
       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- CbmLitTrdTrackFinderSts::CreateTrdFromSts : " << std::endl
                << nofStsTracks << " STS tracks were loaded, "
                << fTracksS.size() << " TRD tracks were created" << std::endl;
    }
}
// -----------------------------------------------------------------------

// --------------------Copy to output TClonesArray------------------------
void CbmLitTrdTrackFinderSts::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]) CbmTrdTrack((*iTrack)->MapToTrd());
                                    
            fSeedsIdSet.insert((*iTrack)->GetPreviousTrackId());
            
            nofTracks++;
         }
      }
      delete (*iTrack);
   }
   
   //clear array
   fFoundTracks.clear();
}
// -----------------------------------------------------------------------

ClassImp(CbmLitTrdTrackFinderSts);