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

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

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

#include "IOADatabase.hxx"
#include "IGeomIdManager.hxx"


/// Tool developed by Anselmo Cervera to deduce alignment constants for each subdetector of COMET. 
/// The output is a TTree, which must be analyzed using a ROOT macro.
/// Unfortunately, there is no prototype macro given by Anselmo...

class IComputeResidualsLoop: public COMET::ICOMETEventLoopFunction {

protected:
  IReconGlobalUtils fUtils;
  ISubdetectorsRecon fSubRecon;

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

  std::string fAlignDetector[2];
  std::string fAlignDetectorTemp[2];

  TFile *fOutFile;

  TTree *fTree; 


  // tree variables
  double fmatchchi2;
  int fnhits[2];
  int fdet[2];
  double fchi2track[2];
  double fchi2ndoftrack[2];
  double fxtrack[2]; 
  double fytrack[2]; 
  double fztrack[2];
  double ftx[2];
  double fty[2]; 
  double fptrack[2];
  double ftime[2];
  double ftimeCath; 
  double fresx,fresy,fresz; 
  double frestx,fresty; 
  int ftbits; 
  int fnhhits; 
  int fnhxhits; 
  int fnhyhits; 
  int fnhzhits; 
  double fhx[100];
  double fhy[100];
  double fhz[100];
  double fht[100];
  double fhq[100];
  double fhdx[100];
  double fhdy[100];
  double fhdz[100];
  int fhisx[100];
  int fhisy[100];
  int fhisz[100];


public:

  IComputeResidualsLoop();
  
  virtual ~IComputeResidualsLoop() {}

  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 );

  void FillTree(COMET::IHandle<COMET::IReconBase> track1, COMET::IHandle<COMET::IReconBase> track2, bool ok, double chi2ndf, const HyperVector& resHV);

  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);

};

//*************************************************
IComputeResidualsLoop::IComputeResidualsLoop() {
//*************************************************

  COMET::ICOMETLog::SetLogLevel(COMET::ICOMETLog::QuietLevel);
  COMET::ICOMETLog::SetLogLevel("oaAlignTools",COMET::ICOMETLog::QuietLevel);    
  
  fRecPackVerb ="00000";
  ftmanVerb = 0;
  fConverterVerb = 0;
  fpmanVerb = 0;

  fAlignDetector[0] = COMET::IOARuntimeParameters::Get().GetParameterS("oaAlignTools.AlignDetector1");
  fAlignDetector[1] = COMET::IOARuntimeParameters::Get().GetParameterS("oaAlignTools.AlignDetector2");

}

//*************************************************
bool IComputeResidualsLoop::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("oaAlignTools",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 IComputeResidualsLoop::BeginFile (COMET::IVInputFile *input){
//*****************************************************************************

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

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

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

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

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

  // dums the RecPack Setup
  if(COMET::ICOMETLog::GetLogLevel("oaAlignTools") >= 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("globalRecon").SetVerbosity(fRecPackVerb);   

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

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

}


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

  fOutFile->Write();

}


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


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

  fTree = new TTree("aligntree","detector matching" );
  
  fTree->Branch("matchchi2", &fmatchchi2,"matchchi2/D");
  fTree->Branch("nhits", &fnhits,"nhits[2]/I");
  fTree->Branch("det", &fdet,"det[2]/I");
  fTree->Branch("chi2",&fchi2track,"chi2[2]/D");
  fTree->Branch("chi2ndof",&fchi2ndoftrack,"chi2ndof[2]/D");
  fTree->Branch("x",&fxtrack,"x[2]/D");
  fTree->Branch("y",&fytrack,"y[2]/D");
  fTree->Branch("z",&fztrack,"z[2]/D");
  fTree->Branch("tx",&ftx,"tx[2]/D");
  fTree->Branch("ty",&fty,"ty[2]/D");
  fTree->Branch("p",&fptrack,"p[2]/D");
  fTree->Branch("t",&ftime,"t[2]/D");
  fTree->Branch("resx",&fresx,"resx/D");
  fTree->Branch("resy",&fresy,"resy/D");
  fTree->Branch("resz",&fresz,"resz/D");
  fTree->Branch("restx",&frestx,"restx/D");
  fTree->Branch("resty",&fresty,"resty/D");
  fTree->Branch("timeCath",&ftimeCath,"timeCath/D");
  fTree->Branch("tbits",&ftbits,"tbits/I");
  fTree->Branch("nhhits",&fnhhits,"nhhits/I");
  fTree->Branch("nhxhits",&fnhyhits,"nhxhits/I");
  fTree->Branch("nhyhits",&fnhyhits,"nhyhits/I");
  fTree->Branch("nhzhits",&fnhzhits,"nhzhits/I");
  fTree->Branch("hx",fhx,"hx[nhhits]/D");
  fTree->Branch("hy",fhy,"hy[nhhits]/D");
  fTree->Branch("hz",fhz,"hz[nhhits]/D");
  fTree->Branch("ht",fhz,"ht[nhhits]/D");
  fTree->Branch("hq",fhq,"hq[nhhits]/D");
  fTree->Branch("hdx",fhdx,"hdx[nhhits]/D");
  fTree->Branch("hdy",fhdy,"hdy[nhhits]/D");
  fTree->Branch("hdz",fhdz,"hdz[nhhits]/D");
  fTree->Branch("hisx",fhisx,"hisx[nhhits]/I");
  fTree->Branch("hisy",fhisy,"hisy[nhhits]/I");
  fTree->Branch("hisz",fhisz,"hisz[nhhits]/I");


}

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


  int evtno = event.GetContext().GetEvent();
  IFGDTruthInfo::Get().SetEventInfo(event);
  if(evtno%10 == 0)
    std::cout << "Compute residuals: processing event # " << evtno << std::endl;

  if (fAlignDetector[0].find("TPC") != std::string::npos  ||  
      fAlignDetector[1].find("TPC") != std::string::npos  ||  
      fAlignDetector[0].find("FGD") != std::string::npos  ||  
      fAlignDetector[1].find("FGD") != std::string::npos  ||  
      fAlignDetector[0] == "TRACKER"  ||  fAlignDetector[1] == "TRACKER" || fAlignDetector[0] == "ALL" )
    fSubRecon.RunTrackerRecon(event);
  if (fAlignDetector[0] == "ECAL"     ||  fAlignDetector[1] == "ECAL"    || fAlignDetector[0] == "ALL")
    fSubRecon.RunEcalRecon(event);
  if (fAlignDetector[0] == "P0DECAL"  ||  fAlignDetector[1] == "P0DECAL" || fAlignDetector[0] == "ALL")
    fSubRecon.RunP0DEcalRecon(event);
  if (fAlignDetector[0] == "P0D"      ||  fAlignDetector[1] == "P0D"     || fAlignDetector[0] == "ALL")
    fSubRecon.RunP0dRecon(event);
  if (fAlignDetector[0] == "SMRD"     ||  fAlignDetector[1] == "SMRD"    || fAlignDetector[0] == "ALL")
    fSubRecon.RunSmrdRecon(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("oaAlignTools") >= 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("oaAlignTools") >= 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> objects[2];


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

  // 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 (fAlignDetector[0] !="ALL"){
    for (int d=0;d<2;d++){
      if      (fAlignDetector[d] == "TRACKER")
  objects[d]    = COMET::tman().GetContainer(event, "ReconTracker", "final");  
      else if (fAlignDetector[d] == "ECAL")
  objects[d]    = COMET::tman().GetContainer(event, "ReconECal", "final");  
      else if (fAlignDetector[d] == "P0DECAL")
  objects[d]    = COMET::tman().GetContainer(event, "ReconP0DECal", "final");  
      else if (fAlignDetector[d] == "P0D")
  objects[d]    = COMET::tman().GetContainer(event, "ReconP0D", "final");  
      else if (fAlignDetector[d] == "SMRD")
  objects[d]    = COMET::tman().GetContainer(event, "ReconSMRD", "final");  
      else if (fAlignDetector[d] == "TPC1")
  objects[d]    = TPC1Objects;
      else if (fAlignDetector[d] == "TPC2")
  objects[d]    = TPC2Objects;
      else if (fAlignDetector[d] == "TPC3")
  objects[d]    = TPC3Objects;
      else if (fAlignDetector[d] == "FGD1")
  objects[d]    = FGD1Objects;      
      else if (fAlignDetector[d] == "FGD2")
  objects[d]    = FGD2Objects;
      else if (fAlignDetector[d] == "TPC")
  objects[d]    = tpcObjects;
      else if (fAlignDetector[d] == "FGD")
  objects[d]    = fgdObjects;


      if (fAlignDetector[d] != "TRACKER" && 
    fAlignDetector[d] != "TPC"     &&
    fAlignDetector[d] != "TPC1"     &&
    fAlignDetector[d] != "TPC2"     &&
    fAlignDetector[d] != "TPC3")
  fUtils.PrepareObjects(fAlignDetector[d],objects[d]);      
    }

    
    fAlignDetectorTemp[0] = fAlignDetector[0];    
    fAlignDetectorTemp[1] = fAlignDetector[1];    
    ProcessContainers(objects[0], objects[1]);
  }  
  else{


    COMET::IHandle<COMET::IReconObjectContainer> trackerObjects = COMET::tman().GetContainer(event, "ReconTracker", "final");  
    COMET::IHandle<COMET::IReconObjectContainer> ecalObjects    = COMET::tman().GetContainer(event, "ReconECal",    "final");  
    COMET::IHandle<COMET::IReconObjectContainer> smrdObjects    = COMET::tman().GetContainer(event, "ReconSMRD",    "final");  
    COMET::IHandle<COMET::IReconObjectContainer> p0dVertices    = COMET::tman().GetContainer(event, "ReconP0D",     "final");  

    fUtils.PrepareObjects("ECAL",ecalObjects);
    fUtils.PrepareObjects("SMRD",smrdObjects);

    COMET::IHandle<COMET::IReconObjectContainer> p0dObjects(new COMET::IReconObjectContainer("p0d"));
    fUtils.GetObjectsFromVertices(p0dVertices,p0dObjects);
    fUtils.PrepareObjects("P0D",p0dObjects);


    fAlignDetectorTemp[0]="TRACKER";
    fAlignDetectorTemp[1]="ECAL";
    ProcessContainers(trackerObjects, ecalObjects);
    
    fAlignDetectorTemp[0]="TRACKER";
    fAlignDetectorTemp[1]="P0D";
    ProcessContainers(trackerObjects, p0dObjects);
    
    fAlignDetectorTemp[0]="TRACKER";
    fAlignDetectorTemp[1]="SMRD";
    ProcessContainers(trackerObjects, smrdObjects);

    fAlignDetectorTemp[0]="P0D";
    fAlignDetectorTemp[1]="SMRD";    
    ProcessContainers(p0dObjects, smrdObjects);


    fAlignDetectorTemp[0]="TPC1";
    fAlignDetectorTemp[1]="FGD1";
    ProcessContainers(TPC1Objects, FGD1Objects);

    fAlignDetectorTemp[0]="TPC2";
    fAlignDetectorTemp[1]="FGD1";
    ProcessContainers(TPC2Objects, FGD1Objects);

    fAlignDetectorTemp[0]="TPC2";
    fAlignDetectorTemp[1]="FGD2";
    ProcessContainers(TPC2Objects, FGD2Objects);

    fAlignDetectorTemp[0]="TPC3";
    fAlignDetectorTemp[1]="FGD2";
    ProcessContainers(TPC3Objects, FGD2Objects);


  }
 
  return true;
}


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


  if(COMET::ICOMETLog::GetLogLevel("oaAlignTools") >= COMET::ICOMETLog::InfoLevel){
    std::cout << "Matching:  " << fAlignDetectorTemp[0] << "-" << fAlignDetectorTemp[1] << std::endl;
  }

  double chi2ndf = 0;
  HyperVector resHV;
  bool ok;
  int P[6] ={1,1,1,1,1,0};  // do not mach the momentum


  if (!objects2 || !objects1){
    if(COMET::ICOMETLog::GetLogLevel("oaAlignTools") >= COMET::ICOMETLog::InfoLevel){
      if (!objects1)
  std::cout << " Container for " << fAlignDetectorTemp[0] << " objects does not exist" << std::endl;
      if (!objects2 )
  std::cout << " Container for " << fAlignDetectorTemp[1] << " objects does not exist" << std::endl;
    }
    return;
  }

  if(COMET::ICOMETLog::GetLogLevel("oaAlignTools") >= COMET::ICOMETLog::InfoLevel){
    std::cout << " #objects1: " << objects1->size() << std::endl;
    std::cout << " #objects2: " << objects2->size() << std::endl;
  }

  if( !objects1 || objects1->size() == 0) return;

  COMET::IReconObjectContainer::const_iterator pp1;
  COMET::IReconObjectContainer::const_iterator pp2;

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

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

    if (fAlignDetectorTemp[1] == "ECAL")
      if (!(object1->UsesDetector(object1->kTPC3))) continue;   // select TPC3 objects

    if (fAlignDetectorTemp[1] == "P0D")
      if (!(object1->UsesDetector(object1->kTPC1))) continue;   // select TPC3 objects

    if(COMET::ICOMETLog::GetLogLevel("oaAlignTools") >= COMET::ICOMETLog::InfoLevel)
      std::cout <<  "   - " << iobject1 << "  "  << object1 << std::endl;

    //-------- Loop over DET2 objects -------------------
    int iobject2=0;
    for(pp2 = objects2->begin(); pp2 != objects2->end(); pp2++, iobject2++){

      COMET::IHandle<COMET::IReconBase> object2 = *pp2;  
      if (!object2) continue;

      if (fAlignDetectorTemp[1] == "ECAL")
  if ( !(object2->UsesDetector(object1->kDSECal))) continue;  // Select DsEcal objects

      if(COMET::ICOMETLog::GetLogLevel("oaAlignTools") >= COMET::ICOMETLog::InfoLevel)
  std::cout << "     - " << iobject2 << "  "  << object2 << std::endl;

      // Check that the objects are different
      if (GetPointer(object2) == GetPointer(object1)) continue;

      COMET::IHandle<COMET::IReconPID> pid1 = object1;
      COMET::IHandle<COMET::IReconPID> pid2 = object2;

      COMET::IHandle<COMET::IReconTrack> track1 = object1;
      COMET::IHandle<COMET::IReconTrack> track2 = object2;

      // Match the DET1 and DET2 objects
      if (track1 && track2)
  ok = COMET::tman().MatchObjects(*track1,*track2, P, chi2ndf,resHV);
      else if (pid1 && track2)
  ok = COMET::tman().MatchObjects(*pid1,*track2, P, chi2ndf,resHV);
      else if (track1 && pid2)
  ok = COMET::tman().MatchObjects(*track1,*pid2, P, chi2ndf,resHV);
      else if (pid1 && pid2)
  ok = COMET::tman().MatchObjects(*pid1,*pid2, P, chi2ndf,resHV);
      else
  continue;

      if (ok ){
  if(COMET::ICOMETLog::GetLogLevel("oaAlignTools") >= COMET::ICOMETLog::InfoLevel)
    std::cout << "       ---> matching chi2/ndof: = " << chi2ndf << " res = " << print(resHV.vector()) << std::endl;

      }
      else
  if(COMET::ICOMETLog::GetLogLevel("oaAlignTools") >= COMET::ICOMETLog::VerboseLevel)
    std::cout << "       ---> unsuccessful matching !!!" << std::endl;      

      FillTree(object1,object2,ok, chi2ndf,resHV);


    }    

  }

}

//*****************************************************************************
void IComputeResidualsLoop::FillTree(COMET::IHandle<COMET::IReconBase> object1, COMET::IHandle<COMET::IReconBase> object2, bool matchok, double chi2ndf, const HyperVector& resHV  ){
//*****************************************************************************

  bool ok;
  
//  if( nodes.size() < 50 ) continue; 


  // ------ Track-Track matching -------------------------------

  if (matchok){ 
    fmatchchi2=chi2ndf;
    fresx = resHV.vector()[0];
    fresy = resHV.vector()[1];
    fresz = resHV.vector()[2];
    frestx = resHV.vector()[3];
    fresty = resHV.vector()[4];
  }
  else{
    fmatchchi2=0;
    fresx = 0;
    fresy = 0;
    fresz = 0;
    frestx = 0;
    fresty = 0;
  }

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

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

  for (int i=0;i<2;i++){
    TLorentzVector pos = COMET::tman().GetPosition(objects[i]);
    TVector3 dir = COMET::tman().GetDirection(objects[i]);
    
    fnhits[i] = objects[i]->GetHits()->size();
    fdet[i] = GetDetectorNumber(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();
    if (dir.Z() != 0){
      ftx[i] = dir.X()/dir.Z();
      fty[i] = dir.Y()/dir.Z();
    }
    else 
      ftx[i]=fty[i]=0;
    fptrack[i] = COMET::tman().GetMomentum(objects[i]);
  }
    

  ftimeCath=0;
  ftbits=0;


  // ------ Track-Hits residuals -------------------------------

  COMET::IHandle<COMET::IHitSelection> hits = object2->GetHits();

  COMET::IHandle<COMET::IReconPID> pid1 = object1;
  COMET::IHandle<COMET::IReconPID> track1 = object1;

  if (!track1 && !pid1) return; 
  
  fnhhits = fnhyhits = fnhxhits = 0;
  for(COMET::IHitSelection::const_iterator hsel = hits->begin(); hsel != hits->end(); hsel++){
        
    COMET::IHandle<COMET::IReconState> state(new COMET::IPIDState); 
    TVector3 spos = (*hsel)->GetPosition();

    TVector3 normal(0.,0.,1.);
    TVector3 axis  (0.,1.,0.);
    
    if      (object2->UsesDetector(COMET::IReconBase::kTopSMRD) || object2->UsesDetector(COMET::IReconBase::kBottomSMRD) ){
      normal = TVector3(0., 1., 0.);
      axis   = TVector3(0., 0., 1.);
    }
    else if (object2->UsesDetector(COMET::IReconBase::kLeftSMRD) || object2->UsesDetector(COMET::IReconBase::kRightSMRD) ){
      normal = TVector3(1., 0., 0.);
      axis   = TVector3(0., 0., 1.);
    }			    

    if (pid1)
      ok = PropagateToSurface( pid1 , spos, normal, axis, *state);
    else if (track1)
      ok = PropagateToSurface( track1 , spos, normal, axis, *state);
    
    fhx[fnhhits] =  (*hsel)->GetPosition().X();
    fhy[fnhhits] =  (*hsel)->GetPosition().Y();
    fhz[fnhhits] =  (*hsel)->GetPosition().Z();
    fht[fnhhits] =  (*hsel)->GetTime();
    fhq[fnhhits] =  (*hsel)->GetCharge(); 

    if (ok){
      TLorentzVector pos = COMET::tman().GetPosition(state);        
      fhdx[fnhhits] =  pos.X()-(*hsel)->GetPosition().X();
      fhdy[fnhhits] =  pos.Y()-(*hsel)->GetPosition().Y();
      fhdz[fnhhits] =  pos.Z()-(*hsel)->GetPosition().Z();
    }
    else{
      fhdx[fnhhits] =  fhdy[fnhhits] =  fhdz[fnhhits] =0;
    }

    
    fhisx[fnhhits] = fhisy[fnhhits] = fhisz[fnhhits] = 0;
    
    if ( (*hsel)->IsXHit()){ 
      fnhxhits++;
      fhisx[fnhhits] =  1;
    }      
    if ( (*hsel)->IsYHit()){ 	  
      fnhyhits++;
      fhisy[fnhhits] =  1;
    }
    if ( (*hsel)->IsZHit()){ 	  
      fnhzhits++;
      fhisz[fnhhits] =  1;
    }
    
    
    fnhhits++;
    
    if (fnhhits >= 200) break;
    
  }


  fTree->Fill();

}

//*****************************************************
int IComputeResidualsLoop::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;

}


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

  Trajectory traj;

  // convert the track to a RecPack trajectory
  bool ok = COMET::converter().TReconBase_to_Trajectory(*t1, traj);
  if (!ok) return false;



  // convert TVector3 to EVector
  EVector pos2, normal2, axis2;
  ok = COMET::converter().TVector_to_EVector3(pos, pos2);
  ok = COMET::converter().TVector_to_EVector3(normal, normal2);
  ok = COMET::converter().TVector_to_EVector3(axis, axis2);
  if (!ok) return false;

  // build a surface
  Rectangle surf(pos2,normal2, axis2, 1000*cm,1000*cm);

  const std::string surfname = "temp";  
  std::string volname = COMET::converter().get_volume_name(pos);

  surf.set_name("setup_name",volname);

  // add the surface to the setup
  COMET::rpman().geometry_svc().setup().add_surface(volname,surfname, &surf);

  int istate;
  double length;
  State state;
    
  // finds the closest trajectory state to the surface
  ok = COMET::rpman().navigation_svc().closest_state(surf, traj, istate, length, state);
  if (!ok){
    // remove the surface from the setup
    COMET::rpman().geometry_svc().setup().remove_surface(surfname);
    return false;
  }

  // propagate the track to that surface
  ok = COMET::rpman().navigation_svc().propagate(surf, state, length);

  // remove the surface from the setup
  COMET::rpman().geometry_svc().setup().remove_surface(surfname);

  if (!ok) return false;

  // convert the RecPack state to a IReconState
  ok = COMET::converter().State_to_TReconState(state,tstate);

  return ok;

}


//*****************************************************************************
void IComputeResidualsLoop::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) {
//**************************************************
    IComputeResidualsLoop userCode;
    cometEventLoop(argc,argv,userCode);
}