#include <iostream>
#include <exception>
#include <cmath>

#include <cometEventLoop.hxx>
#include <ICOMETLog.hxx>

#include <TMatchingUtils.hxx>
#include <IValidationUtils.hxx>
#include <IReconGlobalUtils.hxx>
#include <IOARuntimeParameters.hxx>
#include <ISubdetectorsRecon.hxx>

#include <ICalibManager.hxx>
#include <IFGDChannelCalibrator.hxx>
#include <ITPCChannelCalibrator.hxx>

#include <IRecPackManager.hxx>
#include <recpack/KalmanFitter.h>




class IComputeEfficiencyLoop: public COMET::ICOMETEventLoopFunction {

protected:

  TMatchingUtils fMatchingUtils;
  IReconGlobalUtils fGlobalReconUtils;
  ISubdetectorsRecon fSubRecon;
  KalmanFitter *_filterer;

  std::string fRecPackVerb;
  int ftmanVerb;
  int fConverterVerb;
  int fpmanVerb;


  double fMaxMatchingChi2;

  TFile *fOutFile;

  TTree *fTree; 

  // tree variables
  int frun;                 ///< Run number
  int fsubrun;              ///< Subrun number
  int fevent;               ///< Event number
  double fmatchchi2[2];     ///< Matching chi2 (track1-track2 in index 0, track2-track3 in index 1)
  int faddfgdhits;          ///< 0 if matching tracks, 1 if matching tracks to add FGD hits
  int fnhits[3];            ///< Number of hits in each track being matched
  int fdet[3];              ///< Detector number (1=TPC1,2=TPC2, 3=TPC3, 4=FGD1, 5=FGD2)
  double fchi2track[3];     ///< Chi2 of the track fits
  double fchi2ndoftrack[3]; ///< Chi2 / DOF of the track fits
  double fxtrack[3];        ///< x position of the track fits
  double fytrack[3];        ///< y position of the track fits
  double fztrack[3];        ///< z position of the track fits
  double ftx[3];            ///< x component of direction vector of track fits
  double fty[3];            ///< y component of direction vector of track fits
  double ftz[3];            ///< z component of direction vector of track fits
  double fptrack[3];        ///< total momentum of the track fits
  double ftime[3];          ///< time of the track fits
  double fq[3];             ///< charge of the track fits
  double fccorr[3];         ///< for TPC pids put the corrected truncated charge here
  double fpullele[3];       ///< for TPC pids put the electron pull here
  double fpullmuon[3];      ///< for TPC pids put the muon pull here
  double fpullproton[3];    ///< for TPC pids put the proton pull here

public:

  IComputeEfficiencyLoop();
  
  virtual ~IComputeEfficiencyLoop() {}

  bool operator () (COMET::ICOMETEvent& event);
  void Initialize();
  void BeginFile(COMET::IVInputFile *input);	
  void Finalize(COMET::ICOMETOutput *output);
// The following has been commented out so that do not override pass-through
// method defined in the event loop base class. 
//  void EndFile(COMET::IVInputFile *input){}
  bool SetOption(std::string option, std::string value="");

  void ProcessContainers(COMET::IHandle<COMET::IReconObjectContainer> objects1, 
       COMET::IHandle<COMET::IReconObjectContainer> objects2,
       COMET::IHandle<COMET::IReconObjectContainer> objects3);



  void ProcessContainers(COMET::IHandle<COMET::IReconObjectContainer> objects1, 
       COMET::IHitSelection& hits2,
       COMET::IHandle<COMET::IReconObjectContainer> objects3,
       int sense);


  void FillTree(COMET::IHandle<COMET::IReconBase> object1, COMET::IHandle<COMET::IReconBase> object2, COMET::IHandle<COMET::IReconBase> object3, double chi2ndf_13, double chi2ndf_12 );

  int GetDetectorNumber(COMET::IHandle<COMET::IReconBase> object);

  bool PropagateToSurface(COMET::IHandle<COMET::IReconBase> t1, const TVector3& pos, const TVector3& normal,  const TVector3& axis, COMET::IReconState& tstate);

  void MergeResults(const COMET::IAlgorithmResult& from, COMET::IAlgorithmResult& into);

  COMET::IHandle<COMET::IReconBase> MatchFgdTpc(COMET::IHandle<COMET::IReconBase> tpcObject, 
            COMET::IHitSelection fgdHits, 
            std::set< COMET::IHandle<COMET::IHit> > &matchedFgdHits, int sense, double& avg_chi2ndf);

};

//*************************************************
IComputeEfficiencyLoop::IComputeEfficiencyLoop() {
//*************************************************

  COMET::ICOMETLog::SetLogLevel(COMET::ICOMETLog::QuietLevel);
  COMET::ICOMETLog::SetLogLevel("eff",COMET::ICOMETLog::VerboseLevel);    
  COMET::ICOMETLog::SetLogLevel("eff",COMET::ICOMETLog::QuietLevel);    

  fRecPackVerb ="00000";
  ftmanVerb = 0;
  fConverterVerb = 0;
  fpmanVerb = 0;


  COMET::ICalibManager::Get().AddCalibrator(new COMET::IFGDChannelCalibrator());
  COMET::ICalibManager::Get().AddCalibrator(new COMET::ITPCChannelCalibrator());

}

//*************************************************
bool IComputeEfficiencyLoop::SetOption(std::string option, std::string value) {
//*************************************************

  COMET::ICOMETLog::LogPriority verb[4] = {COMET::ICOMETLog::QuietLevel, COMET::ICOMETLog::LogLevel, COMET::ICOMETLog::InfoLevel, COMET::ICOMETLog::VerboseLevel};

  int value2 = atoi(value.c_str());
  if (value2 <1) value2=0;
  if (value2 >3) value2=3;

  if (option == "v"){
    COMET::ICOMETLog::SetLogLevel("eff",verb[value2]);
  }
  else if (option == "vglob"){
    COMET::ICOMETLog::SetLogLevel("ReconGlobal",verb[value2]);
  }
  else if (option == "vorp"){
    COMET::ICOMETLog::SetLogLevel("oaRecPack",verb[value2]);
  }
  else if (option == "vtr"){
    COMET::ICOMETLog::SetLogLevel("ReconTracker",verb[value2]);
  }
  else if (option == "vfgd"){
    COMET::ICOMETLog::SetLogLevel("ReconFGD",verb[value2]);
  }
  else if (option == "vrp"){
    fRecPackVerb = value;
  }
  else if (option == "vtman"){
    ftmanVerb = atoi(value.c_str());
  }
  else if (option == "vconv"){
    fConverterVerb = atoi(value.c_str());
  }
  else if (option == "vpman"){
    fpmanVerb = atoi(value.c_str());
  }    
  else return false;

  return true;

}

//*****************************************************************************
void IComputeEfficiencyLoop::BeginFile (COMET::IVInputFile *input){
//*****************************************************************************


  // add a new RecpackManager 
  COMET::tman().add_manager("computeEff");
  COMET::tman().select_manager("computeEff");

  // only surfaces with measurements are tried inside a volume
  COMET::rpman("computeEff").navigation_svc().navigator(RP::particle_helix).set_unique_surface(true);

  // select the Kalman Filter as fitting algorithm
  COMET::rpman("computeEff").fitting_svc().select_fitter(RP::kalman);

  // set a big local chi2 cut for global reconstruction
  //  COMET::rpman("computeEff").fitting_svc().retrieve_fitter<KalmanFitter>(RP::kalman,RP::particle_helix).set_max_local_chi2ndf(EGeo::inf());

  _filterer = &(COMET::rpman("computeEff").fitting_svc().retrieve_fitter<KalmanFitter>(RP::kalman,RP::particle_helix));
     
  _filterer->set_max_local_chi2ndf(1000.);



  // enable energy loss correction
  COMET::rpman("computeEff").model_svc().enable_correction(RP::particle_helix, RP::eloss, true);

  // dums the RecPack Setup
  if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::VerboseLevel)
    std::cout << COMET::tman().GetSetup() << std::endl;  

  // Changes the RecPack verbosity (5 digits = fitting, navigation, model, matching, simulation. Minimum is 0, maximum is 3)
  COMET::tman("computeEff").SetVerbosity(fRecPackVerb);   

  if(COMET::ICOMETLog::GetLogLevel("ReconFGD") >= COMET::ICOMETLog::VerboseLevel)
    COMET::tman("ReconFGD").SetVerbosity(fRecPackVerb);   
  
  // the IRecPackManager verbosity
  COMET::tman("computeEff").SetVerbosity(ftmanVerb);    

  
  // the IRecPackConverters verbosity
  COMET::converter().SetVerbosity(fConverterVerb);    
  
  // the IPIDManager verbosity
  COMET::pman().SetVerbosity(fpmanVerb);    


}


//*****************************************************************************
void IComputeEfficiencyLoop::Finalize (COMET::ICOMETOutput* output){
//*****************************************************************************

  fOutFile->Write();

}

//*****************************************************************************
void IComputeEfficiencyLoop::Initialize (){
//*****************************************************************************


  fOutFile = new TFile("eff.root","recreate");

  fTree = new TTree("eff","recon efficiency" );
  fTree->Branch("run",&frun,"run/I");
  fTree->Branch("subrun",&fsubrun,"subrun/I");
  fTree->Branch("event",&fevent,"event/I");
  fTree->Branch("matchchi2", &fmatchchi2,"matchchi2[2]/D");
  fTree->Branch("addfgdhits",&faddfgdhits,"addfgdhits/I");
  fTree->Branch("nhits", &fnhits,"nhits[3]/I");
  fTree->Branch("det", &fdet,"det[3]/I");
  fTree->Branch("chi2",&fchi2track,"chi2[3]/D");
  fTree->Branch("chi2ndof",&fchi2ndoftrack,"chi2ndof[3]/D");
  fTree->Branch("x",&fxtrack,"x[3]/D");
  fTree->Branch("y",&fytrack,"y[3]/D");
  fTree->Branch("z",&fztrack,"z[3]/D");
  fTree->Branch("tx",&ftx,"tx[3]/D");
  fTree->Branch("ty",&fty,"ty[3]/D");
  fTree->Branch("tz",&ftz,"tz[3]/D");
  fTree->Branch("p",&fptrack,"p[3]/D");
  fTree->Branch("t",&ftime,"t[3]/D");
  fTree->Branch("q",&fq,"q[3]/D");
  fTree->Branch("ccorr",&fccorr,"ccorr[3]/D");
  fTree->Branch("pulle",&fpullele,"pulle[3]/D");
  fTree->Branch("pullmu",&fpullmuon,"pullmu[3]/D");
  fTree->Branch("pullp",&fpullproton,"pullp[3]/D");

  fMaxMatchingChi2 = 10000;

  fMatchingUtils.SetChi2NdfCut(fMaxMatchingChi2);

}

//*****************************************************************************
bool IComputeEfficiencyLoop::operator () (COMET::ICOMETEvent& event){
//*****************************************************************************


  int evtno = event.GetContext().GetEvent();
  IFGDTruthInfo::Get().SetEventInfo(event);
  std::cout << "Compute efficiency: processing event # " << evtno << std::endl;

  frun   = event.GetContext().GetRun();          
  fsubrun= event.GetContext().GetSubRun(); 
  fevent = evtno;

    // Get TPC hits   
  COMET::IHandle<COMET::IHitSelection> tpc = event.GetHitSelection("tpc");
  if( !tpc ) { 
    if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::LogLevel)
      std::cout << " NO TPC hits found --> Try to calibrate ..." << std::endl;     
    COMET::ICalibManager::Get().Calibrate("tpc");
  }
  tpc = event.GetHitSelection("tpc");

  if (tpc){
    if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::LogLevel)
      std::cout << tpc->size() << " TPC hits" << std::endl;     
    if (tpc->size() > 500){
      if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::LogLevel)
  std::cout << " Too many TPC hits --> skip event !!!" << std::endl;     
      return true;
    }
  }

  // Get FGD hits   
  COMET::IHandle<COMET::IHitSelection> fgd = event.GetHitSelection("fgd");
  if( !fgd ) { 
    if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::LogLevel)
      std::cout << " NO FGD hits found --> Try to calibrate ..." << std::endl;     
    COMET::ICalibManager::Get().Calibrate("fgd");
  }

  fgd = event.GetHitSelection("fgd");
  if (fgd){
    if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::LogLevel)
      std::cout << fgd->size() << " FGD hits" << std::endl;     
    if (fgd->size() > 80){
      if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::LogLevel)
  std::cout << " Too many FGD hits --> skip event !!!" << std::endl;     
      return true;
    }
  }


  // Check whether reconstruction results already exists
  COMET::IHandle<COMET::IReconObjectContainer> tpcObjectsCheck     = COMET::tman().GetContainer(event, "ReconTPC", "TPCPids");  
  
  // if not run ReconTracker
  if (!tpcObjectsCheck){
      fSubRecon.RunTrackerRecon(event);
  }

  // What are these two different results for???
  COMET::IHandle<COMET::IAlgorithmResult> fgdResult(new COMET::IAlgorithmResult("fgdResult","fgdResult"));

  int ntimebins = fgdUtils::getNumberTimeBins(event);
  if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::LogLevel)
    std::cout << "Number of FGD time bins in this event: " << ntimebins << std::endl;
  for(int ibin = 0 ; ibin < ntimebins; ibin++){

    // Get the FGD result for this time bin
    COMET::IHandle<COMET::IAlgorithmResult> FgdBinResult = fgdUtils::getTimeBinResult(event,ibin);
    if(!FgdBinResult) continue;

    // Add Fgd result in this time bin to fgdResult
    MergeResults(*FgdBinResult,*fgdResult);

  }

  if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::VerboseLevel){
    if (event.GetFit("ReconTPC"))
      COMET::tman().DumpResult(*(event.GetFit("ReconTPC")));

    //    if (event.GetFit("ReconFGD"))
    //      COMET::tman().DumpResult(*(event.GetFit("ReconFGD")));
    if (fgdResult)
      COMET::tman().DumpResult(*fgdResult);

    if (event.GetFit("ReconTracker"))
      COMET::tman().DumpResult(*(event.GetFit("ReconTracker")));
    
    if (event.GetFit("ReconECal"))
      COMET::tman().DumpResult(*(event.GetFit("ReconECal")));

    if (event.GetFit("ReconP0DECal"))
      COMET::tman().DumpResult(*(event.GetFit("ReconP0DECal")));
    
    if (event.GetFit("ReconP0D"))
      COMET::tman().DumpResult(*(event.GetFit("ReconP0D")));
    
    if (event.GetFit("ReconSMRD"))
      COMET::tman().DumpResult(*(event.GetFit("ReconSMRD")));
  }

  COMET::IHandle<COMET::IReconObjectContainer> tpcObjects     = COMET::tman().GetContainer(event, "ReconTPC", "TPCPids");
  //  COMET::IHandle<COMET::IReconObjectContainer> fgdObjects     = COMET::tman().GetContainer(event, "ReconFGD", "fittedFgdTpcReconObjects");
  COMET::IHandle<COMET::IReconObjectContainer> fgdObjects     = fgdResult->GetResultsContainer("FGDPids");
  //COMET::IHandle<COMET::IReconObjectContainer> fgdObjects     = fgdResult->GetResultsContainer("fittedFgdTpcReconObjects");


  if (tpcObjects){
    COMET::IReconObjectContainer::const_iterator pp;  
    for(pp = tpcObjects->begin(); pp != tpcObjects->end(); pp++){
      COMET::IHandle<COMET::IReconBase> object = *pp;  	 
      object->ClearStatus(COMET::IReconBase::kRan);
    }
  }

  //  fGlobalReconUtils.PrepareObjects("FGD", fgdObjects);

  // Select TPC1, TPC2 and TPC3 objects
  COMET::IHandle<COMET::IReconObjectContainer> TPC1Objects(new COMET::IReconObjectContainer("TPC1"));
  COMET::IHandle<COMET::IReconObjectContainer> TPC2Objects(new COMET::IReconObjectContainer("TPC2"));
  COMET::IHandle<COMET::IReconObjectContainer> TPC3Objects(new COMET::IReconObjectContainer("TPC3"));
  if (tpcObjects){
    COMET::tman().GetObjectsInDetector(*tpcObjects, COMET::IReconBase::kTPC1, *TPC1Objects);
    COMET::tman().GetObjectsInDetector(*tpcObjects, COMET::IReconBase::kTPC2, *TPC2Objects);
    COMET::tman().GetObjectsInDetector(*tpcObjects, COMET::IReconBase::kTPC3, *TPC3Objects);
  }

  // Select FGD1 and FGD2 objects
  COMET::IHandle<COMET::IReconObjectContainer> FGD1Objects(new COMET::IReconObjectContainer("FGD1"));
  COMET::IHandle<COMET::IReconObjectContainer> FGD2Objects(new COMET::IReconObjectContainer("FGD2"));
  if (fgdObjects){
    COMET::tman().GetObjectsInDetector(*fgdObjects, COMET::IReconBase::kFGD1, *FGD1Objects);
    COMET::tman().GetObjectsInDetector(*fgdObjects, COMET::IReconBase::kFGD2, *FGD2Objects);
  }

  if(0){
  std::cout<<" TPC1objects="<<TPC1Objects->size()
     <<" TPC2objects="<<TPC2Objects->size()
     <<" TPC3objects="<<TPC3Objects->size()
     <<" FGD1objects="<<FGD1Objects->size()
     <<" FGD2objects="<<FGD2Objects->size()<<std::endl;
  if (fgdObjects) fgdObjects->ls();
  }

  // compute the efficiency of TPC2 using TPC1-TPC3 as reference
  // alternately look at TPC1, TPC3 efficiency using TPC2 as reference
  faddfgdhits = 0;
  ProcessContainers(TPC1Objects, TPC2Objects, TPC3Objects);

  // alternately try different orders: 
  // TPC1 eff:
  ProcessContainers(TPC2Objects, TPC1Objects, TPC3Objects);
  // TPC3 eff:
  ProcessContainers(TPC2Objects, TPC3Objects, TPC1Objects);
  
  // note that FGD1Objects and FGD2Objects never get filled at the moment, so use the
  // hit matching that follows if(0)
  if (0){
  // compute the efficiency of FGD1 using TPC1-TPC2 as reference
  // alternate way to look at TPC1, TPC2 efficiency using FGD1 and other TPC 
  faddfgdhits = 0;
  ProcessContainers(TPC1Objects, FGD1Objects, TPC2Objects);

  // compute the efficiency of FGD2 using TPC2-TPC3 as reference
  // alternate way to look at TPC2, TPC3 efficiency using FGD2 and other TPC
  faddfgdhits = 0;
  ProcessContainers(TPC2Objects, FGD2Objects, TPC3Objects);
  }
  // sort the FGD hits into separate FGD1 and FGD2 lists
  COMET::IHitSelection fgd1Hits, fgd2Hits;
  if (fgd){
    for (COMET::IHitSelection::const_iterator hit = fgd->begin(); 
   hit != fgd->end(); hit++) {
      if (COMET::IGeomInfo::FGD().IsInFGD2((*hit)->GetPosition())) { // then it's in FGD2
  fgd2Hits.push_back(*hit);
      } else { // it's in FGD1
  fgd1Hits.push_back(*hit);
      }
      
    }
  }
  if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::VerboseLevel){
    std::cout<<" - FGD1 Hits:   " << fgd1Hits.size() << std::endl;
    std::cout<<" - FGD2 Hits:   " << fgd2Hits.size() << std::endl;
  }

  // compute the efficiency of FGD hit association, alternate
  // way to look at TPC efficiency using TPC-FGD matched track
  int sense=1;
  faddfgdhits = 1;
  ProcessContainers(TPC1Objects, fgd1Hits, TPC2Objects, sense); //FGD1 hit assoc to TPC1, TPC2 eff using TPC1-FGD1
  ProcessContainers(TPC2Objects, fgd2Hits, TPC3Objects, sense); //FGD2 hit assoc to TPC2, TPC3 eff using TPC2-FGD2
  sense=-1;
  ProcessContainers(TPC2Objects, fgd1Hits, TPC1Objects, sense); //FGD1 hit assoc to TPC2, TPC1 eff using TPC2-FGD1
  ProcessContainers(TPC3Objects, fgd2Hits, TPC2Objects, sense); //FGD2 hit assoc to TPC3, TPC2 eff using TPC3-FGD2
 
  return true;
}


//*****************************************************************************
void IComputeEfficiencyLoop::ProcessContainers(COMET::IHandle<COMET::IReconObjectContainer> objects1, 
                COMET::IHandle<COMET::IReconObjectContainer> objects2,
                COMET::IHandle<COMET::IReconObjectContainer> objects3){
//*****************************************************************************


  /* 
     Match objects in detector 1 (objects1) with objects in detector 3 (objects3) to define a reference track
     that goes from detector 1 to detector 3. 
     For the good matches found try to find an object in a intermediate detector (objects 2). 
     In this way we can compute the efficiency of detector2
  */

  
  if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::InfoLevel){
    std::cout << " - objects 1: " << objects1->size() << std::endl;
    std::cout << " - objects 2: " << objects2->size() << std::endl;
    std::cout << " - objects 3: " << objects3->size() << std::endl;
  }
  

  HyperVector resHV;
  // dimension of helix model is 6
  // x,y,z,dx/dz,dy/dz,q/p
  int P[6] ={1,1,1,1,1,0};  // do not mach the momentum

  std::map< COMET::IHandle<COMET::IReconBase> , COMET::IHandle<COMET::IReconBase> > matches13;
  std::map< COMET::IHandle<COMET::IReconBase> , COMET::IHandle<COMET::IReconBase> >::iterator it;
  std::map< COMET::IHandle<COMET::IReconBase> , double > matches13_chi2ndf;
  COMET::IHandle<COMET::IReconObjectContainer> objectsMatched1(new COMET::IReconObjectContainer("matched13"));

  fMatchingUtils.Matcher().Reset();  

  // do all possible matching combinations between objects1 and objects3 
  fMatchingUtils.MatchObjects("1","3", objects1, objects3, P);

  if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::InfoLevel)
    std::cout << fMatchingUtils.Matcher() << std::endl;
  
  // loop over all matches found in increasing matching chi2 order
  for (int im=0;im<fMatchingUtils.Matcher().nMatches();im++){

    // apply chi2/ndf cut
    //    if (fMatchingUtils.Matcher().GetChi2Ndf(im) < fMaxMatchingChi2){
      
      COMET::IHandle<COMET::IReconBase> object1 = fMatchingUtils.Matcher().GetObject1(im);
      COMET::IHandle<COMET::IReconBase> object3 = fMatchingUtils.Matcher().GetObject2(im);
      double chi2ndf_13=fMatchingUtils.Matcher().GetChi2Ndf(im);

      //  save only one match per object (the best match)
      bool found = false;
      for (it = matches13.begin(); it!=matches13.end();it++){
  if (it->first == object1) found = true;
      }
      if (!found){
  matches13[object1]=object3;
  matches13_chi2ndf[object1]=chi2ndf_13;
  objectsMatched1->push_back(object1);
      }

      //    }
  }


  fMatchingUtils.Matcher().Reset();  

  // Now do all posible matching combinations between objects previously matched (reference track) and objects2
  fMatchingUtils.MatchObjects("1","2", objectsMatched1, objects2, P);

  // loop over all matches found in increasing matching chi2 order
  for (int im=0;im<fMatchingUtils.Matcher().nMatches();im++){

      COMET::IHandle<COMET::IReconBase> object1 = fMatchingUtils.Matcher().GetObject1(im);
      COMET::IHandle<COMET::IReconBase> object2 = fMatchingUtils.Matcher().GetObject2(im);
      COMET::IHandle<COMET::IReconBase> object3 = matches13[object1];
      double chi2ndf_12=fMatchingUtils.Matcher().GetChi2Ndf(im);
      double chi2ndf_13= matches13_chi2ndf[object1];


      // Fill the tree with:
      // - The objects in the refference detectors (1 and 3) 
      // - The object in the detector whose eff we want to calculate (2)
      // - The matching chi2/ndf between the objects in the reference detectors
      // - The matching chi2/ndf between the object in the reference detector and the object in the actual detector
      FillTree(object1,object2, object3, chi2ndf_13, chi2ndf_12);


  }


}


//*****************************************************************************
void IComputeEfficiencyLoop::ProcessContainers(COMET::IHandle<COMET::IReconObjectContainer> objects1, 
                COMET::IHitSelection& hits2,
                COMET::IHandle<COMET::IReconObjectContainer> objects3,
                int sense){
//*****************************************************************************


  /* 
     Match objects in detector 1 (objects1) with objects in detector 3 (objects3) to define a reference track
     that goes from detector 1 to detector 3. 
     
     Match objects in detector 1 (objects1) with hits in detector 2 (hits2) to define a reference track
     that goes from detector 1 to detector 2. 

     In this way we can compute the efficiency of detector 2 and detector 3
  */

  
  if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::InfoLevel){
    std::cout << " - objects 1: " << objects1->size() << std::endl;
    std::cout << " - hits 2:    " << hits2.size()    << std::endl;
    std::cout << " - objects 3: " << objects3->size() << std::endl;
  }

  HyperVector resHV;
  // dimension of helix model is 6
  // x,y,z,dx/dz,dy/dz,q/p
  int P[6] ={1,1,1,1,1,0};  // do not mach the momentum

  std::map< COMET::IHandle<COMET::IReconBase> , COMET::IHandle<COMET::IReconBase> > matches13;
  std::map< COMET::IHandle<COMET::IReconBase> , COMET::IHandle<COMET::IReconBase> >::iterator it;
  std::map< COMET::IHandle<COMET::IReconBase> , double > matches13_chi2ndf;
  COMET::IHandle<COMET::IReconObjectContainer> objectsMatched1(new COMET::IReconObjectContainer("matched13"));

  fMatchingUtils.Matcher().Reset();  

  // do all possible matching combinations between objects1 and objects3 
  fMatchingUtils.MatchObjects("1","3", objects1, objects3, P);

  if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::InfoLevel)
    std::cout << fMatchingUtils.Matcher() << std::endl;
  
  // loop over all matches found in increasing matching chi2 order
  for (int im=0;im<fMatchingUtils.Matcher().nMatches();im++){

    // apply chi2/ndf cut
    //    if (fMatchingUtils.Matcher().GetChi2Ndf(im) < fMaxMatchingChi2){
      
      COMET::IHandle<COMET::IReconBase> object1 = fMatchingUtils.Matcher().GetObject1(im);
      COMET::IHandle<COMET::IReconBase> object3 = fMatchingUtils.Matcher().GetObject2(im);
      double chi2ndf_13=fMatchingUtils.Matcher().GetChi2Ndf(im);

      //  save only one match per object (the best match)
      bool found = false;
      for (it = matches13.begin(); it!=matches13.end();it++){
  if (it->first == object1) found = true;
      }
      if (!found){
  matches13[object1]=object3;
  matches13_chi2ndf[object1]=chi2ndf_13;
  objectsMatched1->push_back(object1);
      }

      //    }
  }


  // do all possible matching combinations between objects1 and hits2
  COMET::IReconObjectContainer::const_iterator pp1;

  //-------- Loop over DET1 objects -------------------
  for(pp1 = objects1->begin(); pp1 != objects1->end(); pp1++){

    COMET::IHandle<COMET::IReconBase> object1 = *pp1;  
    if (!object1) continue;

    // Set of all hits that have already been matched.
    std::set< COMET::IHandle<COMET::IHit> > used;

    double avg_chi2ndf;
    COMET::IHandle<COMET::IReconBase> object2 = MatchFgdTpc(object1, hits2, used,sense,avg_chi2ndf);
    double chi2ndf_12=avg_chi2ndf;


    COMET::IHandle<COMET::IReconBase> object3;
    double chi2ndf_13 = 0;
    bool found = false;
    for (it = matches13.begin(); it!=matches13.end();it++){
      if (it->first == object1) found = true;
    }
    if (found){    
      object3 = matches13[object1];
      chi2ndf_13 = matches13_chi2ndf[object1];
    }

      // Fill the tree with:
      // - The objects in the refference detectors (1 and 3) 
      // - The object in the detector whose eff we want to calculate (2)
      // - The matching chi2/ndf between the objects in the reference detectors
      // - The matching chi2/ndf between the object in the reference detector and the object in the actual detector
    FillTree(object1,object2, object3, chi2ndf_12, chi2ndf_13);


  }

}



//*****************************************************
COMET::IHandle<COMET::IReconBase> IComputeEfficiencyLoop::MatchFgdTpc(COMET::IHandle<COMET::IReconBase> tpcObject, 
                     COMET::IHitSelection fgdHits, 
                     std::set< COMET::IHandle<COMET::IHit> > &matchedFgdHits, 
                     int sense, double& avg_chi2ndf){
//*****************************************************

  std::string type="PID";
  std::string FGDtype="Track";

  double fMaxModulesToSkip=1;

  double _inichi2=30;
  double _subchi2=30;

  if(fgdHits.size() == 0)
    return COMET::IHandle<COMET::IReconBase>();
  bool ok;

  bool filtering_status = true;
  
  if(!tpcObject->CheckStatus( tpcObject->kSuccess  )) return COMET::IHandle<COMET::IReconBase>();


  if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::InfoLevel){
    std::cout << "Match TPC object and FGD hits. Sense = " << sense << std::endl;
    std::cout << " - " << tpcObject << std::endl;
    std::cout << "   - # fgd hits: " << fgdHits.size() << std::endl;
  }

  
  // this is just a protection to avoid very low momentum particles
  //    if (fabs(tpcObject->GetCurvature())>1/(50*MeV)) 
  //        return COMET::IHandle<COMET::TReconObject>();
  
  // take either the first or the last point of the track
  // depending on the sense
  //  int seed_index;
  COMET::IReconState seedState;
  if (sense == 1)
    seedState = *(COMET::tman().GetLastState(*tpcObject));
  else 
    seedState = *(COMET::tman().GetFirstState(*tpcObject));
  
  // create the TReconObjects
  COMET::IHandle<COMET::IReconBase> object3D  = COMET::converter().CreateNewObject(FGDtype);
  
  // Figure out which is the first x/y planes that 
  // we expect to see hits in.
  int expect_x_plane=-9999, expect_y_plane=-9999;
  bool isFGD1 = COMET::IGeomInfo::FGD().IsInFGD1((*(fgdHits.begin()))->GetPosition());
  
  if(isFGD1 && sense ==1){
    expect_x_plane = COMET::IGeomInfo::FGD().GetFirstFGD1Plane("X");
    expect_y_plane = COMET::IGeomInfo::FGD().GetFirstFGD1Plane("Y");
  }else if(isFGD1 && sense == -1){
    expect_x_plane = COMET::IGeomInfo::FGD().GetLastFGD1Plane("X");
    expect_y_plane = COMET::IGeomInfo::FGD().GetLastFGD1Plane("Y");
  }else if(!isFGD1 && sense ==1){
    expect_x_plane = COMET::IGeomInfo::FGD().GetFirstFGD2Plane("X");
    expect_y_plane = COMET::IGeomInfo::FGD().GetFirstFGD2Plane("Y");
  }else if(!isFGD1 && sense == -1){
    expect_x_plane = COMET::IGeomInfo::FGD().GetLastFGD2Plane("X");
    expect_y_plane = COMET::IGeomInfo::FGD().GetLastFGD2Plane("Y");
  }
  
  
  COMET::IHitSelection currentMatchedHits;
  
  if(sense ==1)
    std::sort(fgdHits.begin(), fgdHits.end(), fgdUtils::zHitComparison);
  else
    std::sort(fgdHits.begin(), fgdHits.end(), fgdUtils::revZHitComparison);
  
  // Hold on to the previous set of matched hits for veto purposes
  std::set< COMET::IHandle<COMET::IHit> > prevMatched(matchedFgdHits);
  
  // Use a larger chi-squared to find the first matching
  // hit.  Then decrease for subsequent matching.
  // Not clear why this is necessary.
  _filterer->set_max_local_chi2ndf(_inichi2);
  bool xhitseen = false, yhitseen = false, changedchi2 = false;   
  avg_chi2ndf=0;
  
  for (COMET::IHitSelection::iterator tt = fgdHits.begin(); 
       tt != fgdHits.end(); tt++) {
    COMET::IHit& hit = **tt;
        
    // Figure out how many modules there are between current hit
    // and last matched hit (in this projection).  Factor of 2 
    // accounts for conversion between modules and planes.
    int plane  = COMET::IGeomInfo::FGD().ActivePlane((hit).GetPosition().Z());
    int module_diff=0;    
    if((hit).IsXHit())
      module_diff = sense*(plane - expect_x_plane)/2;
    else if((hit).IsYHit())
      module_diff = sense*(plane - expect_y_plane)/2;
    
    // Matching should not allow track to skip more than fMaxModulesToSkip module
    if(module_diff > 1 + fMaxModulesToSkip){
      filtering_status = false;	
      break;
    }
        
    // compute the matching chi2 between the hit and the state
    // filter the current hit (if ok=true the hit has been added to the track)
    double chi2ndf=0;
    bool ok2 = false;
    double fCovarianceFactor=5;
    if (filtering_status)
      ok2 = COMET::tman().FilterHit(hit, seedState, sense, fCovarianceFactor, FGDtype, *object3D,chi2ndf);


    if (ok2 && filtering_status){
      avg_chi2ndf += chi2ndf;
      currentMatchedHits.push_back(*tt);
      matchedFgdHits.insert(*tt);
      if((hit).IsXHit()) expect_x_plane = plane;      
      if((hit).IsYHit()) expect_y_plane = plane;  	
      if (!changedchi2) {
  if (!xhitseen && (hit).IsXHit()) xhitseen = true;
  if (!yhitseen && (hit).IsYHit()) yhitseen = true;
  if (xhitseen && yhitseen) {
    _filterer->set_max_local_chi2ndf(_subchi2);
    changedchi2 = true;	  
  }
      }
      
      // This step seems unnecessary: why can't we just use the last 
      // fitted node from the TReconObject?

      Trajectory traj;
      if (filtering_status)
  seedState = *(COMET::tman().GetLastState(*object3D));
    }
  }
  
  // Need to reset chi^2 before final kalman fit
  if(_subchi2 > _inichi2)
    _filterer->set_max_local_chi2ndf(_subchi2);
  else
    _filterer->set_max_local_chi2ndf(_inichi2);
  
  
  COMET::IReconState seedStateRefit;

  COMET::IReconNodeContainer nodes3D = object3D->GetNodes();
  if (nodes3D.size() > 0){

    if (isFGD1) {
      object3D->AddDetector(COMET::IReconBase::kFGD1);
    } else {
      object3D->AddDetector(COMET::IReconBase::kFGD2);
    }

    // ----- Fit the FGD track -------------

    seedStateRefit = *(COMET::tman().GetFirstState(*object3D));
    ok = COMET::tman().FitObject(FGDtype,seedStateRefit, sense, *object3D);

    // Add the list of hits to the TReconObject (RecPack doesn't do this).
    COMET::IHitSelection *new_hits = new COMET::IHitSelection(currentMatchedHits);
    object3D->AddHits(new_hits);

    if(COMET::ICOMETLog::GetLogLevel("eff") >= COMET::ICOMETLog::InfoLevel){
      std::cout << "     --> " << object3D << std::endl;
    }

    avg_chi2ndf /= ((double)(nodes3D.size()));
            
    return object3D;
    
  }    

  return COMET::IHandle<COMET::IReconBase>();
 
}


//*****************************************************************************
void IComputeEfficiencyLoop::FillTree(COMET::IHandle<COMET::IReconBase> object1, 
             COMET::IHandle<COMET::IReconBase> object2, 
             COMET::IHandle<COMET::IReconBase> object3, 
             double chi2ndf_13, 
             double chi2ndf_12 ){
//*****************************************************************************

  std::cout << "Fill tree: " << std::endl;
  std::cout << "  - chi2ndf: " << chi2ndf_12 << "  " << chi2ndf_13 << std::endl;


  bool ok;
  
  fmatchchi2[0] = chi2ndf_13;
  fmatchchi2[1] = chi2ndf_12;

  // ------ Track properties -------------------------------


  COMET::IHandle<COMET::IReconBase> objects[3];
  objects[0] =object1;
  objects[1] =object2;
  objects[2] =object3;

  for (int i=0;i<3;i++){

    fnhits[i] = 0;
    fdet[i] = 0;
    fchi2track[i] = 0;                 
    fchi2ndoftrack[i] = 0;
    fxtrack[i] = 0;
    fytrack[i] = 0;
    fztrack[i] = 0;
    ftime[i] = 0;
    ftx[i] = 0;
    fty[i] = 0;
    fptrack[i] = 0;
    fq[i] = 0;
    fccorr[i] = 0;
    fpullele[i] = 0;
    fpullmuon[i] = 0;
    fpullproton[i] = 0;


    if (!objects[i]) continue;

    //std::cout << "  - " << objects[i] << std::endl;

    fnhits[i] = objects[i]->GetHits()->size();
    fdet[i] = GetDetectorNumber(objects[i]);

    if (objects[i]->CheckStatus(COMET::IReconBase::kSuccess)){
      
      TLorentzVector pos = COMET::tman().GetPosition(objects[i]);
      TVector3 dir = COMET::tman().GetDirection(objects[i]);
      
      fchi2track[i] = objects[i]->GetQuality();
                 
      if (objects[i]->GetNDOF() !=0)
  fchi2ndoftrack[i] = objects[i]->GetQuality()/objects[i]->GetNDOF();
      else
  fchi2ndoftrack[i] = 0; 
      
      fxtrack[i] = pos.X();
      fytrack[i] = pos.Y();
      fztrack[i] = pos.Z();
      ftime[i] = pos.T();
      ftx[i] = dir.X();
      fty[i] = dir.Y();
      ftz[i] = dir.Z();
      fptrack[i] = COMET::tman().GetMomentum(objects[i]);
      fq[i] = COMET::tman().GetCharge(objects[i]);

      // check if it is a pid?
      COMET::IHandle<COMET::IReconPID> apid = objects[i];
      if (apid){
  // check for electron pull, etc.
  if (apid->Get<COMET::IRealDatum>("tpcPid_Ccorr") )
    fccorr[i] = apid->Get<COMET::IRealDatum>("tpcPid_Ccorr")->GetValue();
  if (apid->Get<COMET::IRealDatum>("tpcPid_PullEle") )
    fpullele[i] = apid->Get<COMET::IRealDatum>("tpcPid_PullEle")->GetValue();
  if (apid->Get<COMET::IRealDatum>("tpcPid_PullMuon") )
    fpullmuon[i] = apid->Get<COMET::IRealDatum>("tpcPid_PullMuon")->GetValue();
  if (apid->Get<COMET::IRealDatum>("tpcPid_PullProton") )
    fpullproton[i] = apid->Get<COMET::IRealDatum>("tpcPid_PullProton")->GetValue();
      }
    }
  }
  
  fTree->Fill();

}

//*****************************************************
int IComputeEfficiencyLoop::GetDetectorNumber(COMET::IHandle<COMET::IReconBase> object){
//*****************************************************

  int det=0;

  if (object->UsesDetector(COMET::IReconBase::kTPC1)   ) det=1;
  if (object->UsesDetector(COMET::IReconBase::kTPC2)   ) det=det*10+2;
  if (object->UsesDetector(COMET::IReconBase::kTPC3)   ) det=det*10+3;
  if (object->UsesDetector(COMET::IReconBase::kFGD1)   ) det=det*10+4;
  if (object->UsesDetector(COMET::IReconBase::kFGD2)   ) det=det*10+5;
  if (object->UsesDetector(COMET::IReconBase::kDSECal) ) det=det*10+6;
  if (object->UsesDetector(COMET::IReconBase::kTECal) || 
      object->UsesDetector(COMET::IReconBase::kPECal)  ) det=det*10+7;
  if (object->UsesDetector(COMET::IReconBase::kP0D)    ) det=det*10+8;
  if (object->UsesDetector(COMET::IReconBase::kSMRD)   ) det=det*10+9;

  return det;

}

//*****************************************************************************
void IComputeEfficiencyLoop::MergeResults(const COMET::IAlgorithmResult& from, COMET::IAlgorithmResult& into) {
//*****************************************************************************

  for (COMET::IAlgorithmResult::const_iterator item = from.begin(); item != from.end(); ++item) {
    
    TString className((*item)->ClassName());
    TString itemName = (*item)->GetName();
    
    // Ignore all symbolic links.
    // I think that copying these would result in multiple copies of 
    // the hit selections and track containers.
    if(className.Contains("COMET::IDataSymLink"))
      continue;

    // Check if it is HitSelection or TrackContainer. If yes, then copy.
    if(className.Contains("COMET::IHitSelection")){

      COMET::IHandle<COMET::IHitSelection> fromHits = from.Get<COMET::IHitSelection>(itemName);      
      if(!fromHits)
  continue;
      
      COMET::IHandle<COMET::IHitSelection> intoHits = into.Get<COMET::IHitSelection>(itemName);
      if (!intoHits) {
  into.AddHitSelection(new COMET::IHitSelection(*fromHits));
  continue;
      }
      for (COMET::IHitSelection::const_iterator hit = fromHits->begin();
     hit != fromHits->end(); hit++) {
  intoHits->push_back(*hit);
      }
    }
    
    if(className.Contains("COMET::IReconObjectContainer")){
      COMET::IHandle<COMET::IReconObjectContainer> fromRecon = from.Get<COMET::IReconObjectContainer>(itemName);    
      if(!fromRecon)
  continue;
    
      COMET::IHandle<COMET::IReconObjectContainer> intoRecon = into.Get<COMET::IReconObjectContainer>(itemName);
      if (!intoRecon) {
  into.AddResultsContainer(new COMET::IReconObjectContainer(*fromRecon));
  continue;
      }
      for (COMET::IReconObjectContainer::const_iterator obj = fromRecon->begin();
     obj != fromRecon->end(); obj++) {
  intoRecon->push_back(*obj);
      }
    }    
  }
}


//**************************************************
int main(int argc, char **argv) {
//**************************************************
    IComputeEfficiencyLoop userCode;
    cometEventLoop(argc,argv,userCode);
}