#include "IDatum.hxx"
#include "IRealDatum.hxx"
#include "TPrincipal.h"
#include "IGeomInfo.hxx"
#include "IECALGeom.hxx"

#include "IG4PrimaryVertex.hxx"
#include "IG4PrimaryParticle.hxx"
#include "IG4Trajectory.hxx"

#include <IG4Trajectory.hxx>
#include <IG4HitSegment.hxx>
#include <ITFBmcDigit.hxx>
#include <IGeomIdManager.hxx>
#include <TrackingUtils.hxx>
#include <ReconObjectUtils.hxx>

#include <TPrincipal.h>

#include <vector>

#include <IRecPackManager.hxx>

#include "ITrackerECALPi0ReconModule.hxx"

#include <IEventFolder.hxx>

namespace {
  const COMET::IReconBase::Status AllSubDetectors[19] = { COMET::IReconBase::kTPC1, COMET::IReconBase::kTPC2, COMET::IReconBase::kTPC3, COMET::IReconBase::kFGD1, COMET::IReconBase::kFGD2, COMET::IReconBase::kP0D,
                                                       COMET::IReconBase::kDSECal, COMET::IReconBase::kTopSMRD, COMET::IReconBase::kBottomSMRD, COMET::IReconBase::kLeftSMRD, COMET::IReconBase::kRightSMRD,
                                                       COMET::IReconBase::kTopPECal, COMET::IReconBase::kBottomPECal, COMET::IReconBase::kLeftPECal, COMET::IReconBase::kRightPECal, COMET::IReconBase::kTopTECal,
                                                       COMET::IReconBase::kBottomTECal, COMET::IReconBase::kLeftTECal, COMET::IReconBase::kRightTECal };
}

ClassImp(COMET::ITrackerECALPi0ReconModule);
ClassImp(COMET::ITrackerECALPi0ReconModule::IPi0);
ClassImp(COMET::ITrackerECALPi0ReconModule::IPi0Photon);
ClassImp(COMET::ITrackerECALPi0ReconModule::ITruthClusteredParticle);
ClassImp(COMET::ITrackerECALPi0ReconModule::IFailedGlobalObject);
//ClassImp(COMET::ITrackerECALPi0ReconModule::IReconECalShower);
//ClassImp(COMET::ITrackerECALPi0ReconModule::TECALReconVertex);
//ClassImp(COMET::ITrackerECALPi0ReconModule::IReconECalObject);
//ClassImp(COMET::ITrackerECALPi0ReconModule::TECALRecon2DObject);

#define CVSTAG "\
  $Name: v5r19 $"
#define CVSID  "\
  $Id: ITrackerECALPi0ReconModule.cxx,v 1.7 2013/02/04 12:52:52 db211 Exp $"

COMET::ITrackerECALPi0ReconModule::ITrackerECALPi0ReconModule(const char *name,
const char *title) {

  SetNameTitle(name, title);
  // Disable this module by default:
  fIsEnabled = kFALSE;
  fDescription = "Tracker ECAL recon output";
  fCVSTagName = CVSTAG;
  fCVSID = CVSID;

  // Tree variables
  fPi0 = new TClonesArray("COMET::ITrackerECALPi0ReconModule::IPi0", 200);
  fTruth = new TClonesArray("COMET::ITrackerECALPi0ReconModule::ITruthClusteredParticle", 200);
  fFailedGlobalObject = new TClonesArray("COMET::ITrackerECALPi0ReconModule::IFailedGlobalObject", 200);

  COMET::IOADatabase::Get().RegisterGeometryCallback(new IMyChangeClass(this));
}


COMET::ITrackerECALPi0ReconModule::~ITrackerECALPi0ReconModule() {}

Bool_t COMET::ITrackerECALPi0ReconModule::ProcessFirstEvent(COMET::ICOMETEvent& event) {
  return true;
}


void COMET::ITrackerECALPi0ReconModule::InitializeModule() {

}


void COMET::ITrackerECALPi0ReconModule::InitializeBranches() {

  fOutputTree->Branch("NPi0", &fNPi0, "NPi0/I", fBufferSize);
  // A TClonesArray of IReconECalObject objects
  fOutputTree->Branch("Pi0", &fPi0, fBufferSize, fSplitLevel);
  fOutputTree->Branch("TrueTraj", &fTruth, fBufferSize, fSplitLevel);

  fOutputTree->Branch("NFailedGlobalObject", &fNFailedGlobalObject, "NFailedGlobalObject/I", fBufferSize);
  fOutputTree->Branch("FailedGlobalObject", &fFailedGlobalObject, fBufferSize, fSplitLevel);

}


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

  int det=0;

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

  return det;

}

COMET::IHandle<COMET::IAlgorithmResult> COMET::ITrackerECALPi0ReconModule::ProcessBunches(const COMET::IAlgorithmResult& input) {


  //Declare object to hold the algorithm output
  COMET::IHandle<COMET::IAlgorithmResult> result(new COMET::IAlgorithmResult(this->GetName(),
        "Pi0BunchSplitter"));

  std::map<int, COMET::IReconObjectContainer*> module_bunch_clusters;
  for(int b = -23; b < 23; ++b) {
    module_bunch_clusters[b] = new COMET::IReconObjectContainer(Form("pi0_split_%d",b+1), "pi0_split");
  }


  COMET::IHandle<COMET::IReconObjectContainer> allObjects
    = input.GetResultsContainer("final");
    
  

  
  for (COMET::IReconObjectContainer::const_iterator t = allObjects->begin();
      t != allObjects->end(); ++t) {
    int bunch = -1;
    if((*t)->UsesDetector(COMET::IReconBase::kDSECal) && GetDetectorNumber(*t) < 10) {
      bunch = GetBunch(*t);
      module_bunch_clusters[bunch]->push_back(*t);
    }
  }
  for(std::map<int, COMET::IReconObjectContainer*>::iterator b = module_bunch_clusters.begin(); b != module_bunch_clusters.end(); ++b) {
    if(b->second->empty()) {
      delete b->second;
    }
    else {
      result->AddResultsContainer(b->second);
    }
  }
  return result;
}

int COMET::ITrackerECALPi0ReconModule::GetBunch(COMET::IHandle<COMET::IReconBase> object) {
  int run = COMET::IEventFolder::GetCurrentEvent()->GetRunId();
  double deltaMC = 0;
  if(run == 0) {}
  else if(run < 4300) { // run 31
    deltaMC = 103.66;
  }
  else if(run < 4600) { // run 32
    deltaMC = 85.79;
  }
  else if(run < 4900) { // run 33
    deltaMC = 78.28;
  }
  else if(run < 5200) { // run 34
    deltaMC = 88.3;
  }
  else if(run > 6000) {
    if(run < 7000) { // run 36
      deltaMC = 102.13;
    }
    else if(run < 7720) { // run 37
      deltaMC = 237.9;
    }
    else if(run < 7800) { // run 38
      deltaMC = 243.58;
    }
  }
  if(object->UsesDetector(COMET::IReconBase::kDSECal)) {
    object = ReconObjectUtils::GetConstituentInDetector(object, COMET::IReconBase::kDSECal);
  }
  else if(object->UsesDetector(COMET::IReconBase::kLeftTECal)) {
    object = ReconObjectUtils::GetConstituentInDetector(object, COMET::IReconBase::kLeftTECal);
  }
  else if(object->UsesDetector(COMET::IReconBase::kRightTECal)) {
    object = ReconObjectUtils::GetConstituentInDetector(object, COMET::IReconBase::kRightTECal);
  }
  else if(object->UsesDetector(COMET::IReconBase::kTopTECal)) {
    object = ReconObjectUtils::GetConstituentInDetector(object, COMET::IReconBase::kTopTECal);
  }
  else if(object->UsesDetector(COMET::IReconBase::kBottomTECal)) {
    object = ReconObjectUtils::GetConstituentInDetector(object, COMET::IReconBase::kBottomTECal);
  }
  //COMET::IHandle<COMET::IReconState> firstState = TrackingUtils::GetFirstState(*object);
  double t = COMET::IHandle<COMET::IReconPID>(object)->GetPosition().T() - deltaMC;
  double firstbcentre = 2744.73;
  int b = -23;
  float firstbbegin = firstbcentre - 582./2. + (b+1)*582.;
  while(b++ < 23) {
    if(t < firstbbegin) break;
    firstbbegin += 582.;
  }
  return b;
}

bool MaxEnergy(COMET::IHandle<COMET::IReconPID> a, COMET::IHandle<COMET::IReconPID> b) {
  double eA = a->GetConstituents()->front()->GetConstituents()->front()->Get<COMET::IRealDatum>("EMEnergyFitResult")->GetVector()[1];
  double eB = b->GetConstituents()->front()->GetConstituents()->front()->Get<COMET::IRealDatum>("EMEnergyFitResult")->GetVector()[1];
  return eA > eB;
}

double COMET::ITrackerECALPi0ReconModule::GetAngularResolution(double incangle, double energy) {
  return 0.09259 / TMath::Sqrt(energy / unit::GeV); // for now, no ang dependence
}

COMET::IHandle<COMET::IAlgorithmResult> COMET::ITrackerECALPi0ReconModule::ProcessCandidates(const COMET::IAlgorithmResult& input) {


  //Declare object to hold the algorithm output
  COMET::IHandle<COMET::IAlgorithmResult> result(new COMET::IAlgorithmResult(this->GetName(),
        "Pi0ClusterMatching"));

  COMET::IReconObjectContainer module_bunch_clusters;



  for(COMET::IAlgorithmResult::const_iterator dvi = input.begin();
      dvi != input.end(); ++dvi){
    std::string class_name =  (*dvi)->ClassName();
    std::string module_name = (*dvi)->GetName();
    if(class_name == "COMET::IReconObjectContainer") {
      if (module_name.find("pi0_split") == std::string::npos) continue;

      COMET::IReconObjectContainer* module_bunch_clusters_x = dynamic_cast<COMET::IReconObjectContainer*>(*dvi);
      COMET::IReconObjectContainer& module_bunch_clusters = *module_bunch_clusters_x;


      if(module_bunch_clusters.size() > 1) {
        //std::cout << "mbc size: " << module_bunch_clusters.size() << std::endl;
        std::sort(module_bunch_clusters.begin(), module_bunch_clusters.end(), MaxEnergy);
        std::vector<ClusterParams> ParamVec;
        for(COMET::IReconObjectContainer::iterator RecObj = module_bunch_clusters.begin(); RecObj != module_bunch_clusters.end(); ++RecObj) {
          ParamVec.push_back(GetClusterParams(*RecObj));
        }
        // If matching possible, generate required variables for each object and store in vectors

        //Declare objects to hold the individual cluster matching likelihoods
        //std::map<COMET::IReconObjectContainer::iterator, std::map<COMET::IReconObjectContainer, double> > likelihood_matrix;
        double likelihood_matrix[module_bunch_clusters.size()][module_bunch_clusters.size()];
        //std::vector<double> likelihood_umatch_View0, likelihood_umatch_View1;

        //Get likelihoods for each cluster pair to match
        for(unsigned int i = 0; i < module_bunch_clusters.size() - 1; ++i) {
          for(unsigned int j = i+1; j < module_bunch_clusters.size(); ++j) {
            COMETVerbose("Comparing clusters " << i << " & " << j);
            likelihood_matrix[i][j] = CompareClusters(ParamVec[i],ParamVec[j]);
          }
        }        


        //Fill two vectors with ID numbers for the clusters in each view
        //and generate a list of all possible matching hypotheses.
        std::vector<int> ViewIDs;

        for(unsigned int i = 0; i < module_bunch_clusters.size(); ++i) {
          ViewIDs.push_back(i);
        }

        std::vector<std::vector < std::pair<int,int> > > match_combos;

        match_combos = GetMatches(ViewIDs);

        //Generate a total likelihood for each hypothesis by multiplying the
        //likelihoods for each match and unmatched cluster.
        std::vector<double> hyp_likelihoods;
        double total_likelihood=0;

        unsigned int best_hyp=0;
        double best_likelihood=0;
        //std::cout << "match combos size: " << match_combos.size() << std::endl;

        for(unsigned int i=0;i<match_combos.size();i++) {
          double likelihood=1;
          for(unsigned int j=0;j<match_combos[i].size();j++) {
            int ID_View0 = match_combos[i][j].first;
            int ID_View1 = match_combos[i][j].second;
            likelihood*=likelihood_matrix[ID_View0][ID_View1];
          }
          hyp_likelihoods.push_back(likelihood);
          if(likelihood>best_likelihood) {
            best_likelihood=likelihood;
            best_hyp=i;
          }

          total_likelihood+=likelihood;
        }
        COMETVerbose("Best match combo " << best_hyp);

        //Normalise total likelihood to 1
        for(unsigned int i=0;i<hyp_likelihoods.size();i++) {
          hyp_likelihoods[i]/=total_likelihood;
        }

        //Create a IReconObjectContainer containing all matched and unmatched clusters,
        //for each hypothesis scoring likelihood>0.1
        for(unsigned int i=0;i<match_combos.size();i++) {
          COMETVerbose("Match combo " << i);
          if(hyp_likelihoods[i]>0.1 || i == best_hyp) {
            COMET::IReconObjectContainer* recon_module_bunch_clusters;

            if(i==best_hyp) {
              recon_module_bunch_clusters = new COMET::IReconObjectContainer("pi0_match", "pi0_match");
            }
            else {
              recon_module_bunch_clusters = new COMET::IReconObjectContainer("alt_pi0_match","alt_pi0_match");
            }

            //Iterate over all clusters in the hypothesis
            //std::cout << "match combos["<<i<<"] size: " << match_combos[i].size() << std::endl;
            for(unsigned int j=0;j<match_combos[i].size();j++) {
              int ID_View0 = match_combos[i][j].first;
              int ID_View1 = match_combos[i][j].second;


              COMETVerbose("Make a pi0 vertex containing raw IDs "<<ID_View0<<", "<<ID_View1);                                        
              //Add hits to matched and unmatched clusters


              //Add the cluster ID from each view to the object. This will be -1 if no
              //cluster from the view has been included (ie unmatched cluster).
              //Likelihood is currently added to all clusters.

              //COMET::IHandle<COMET::IReconVertex> recon_vertex = MakeReconVertex(module_bunch_clusters[ID_View0], module_bunch_clusters[ID_View1], ParamVec[ID_View0].View, ParamVec[ID_View1].View, ParamVec[ID_View0].EMEnergy, ParamVec[ID_View1].EMEnergy, ParamVec[ID_View0].Time, ParamVec[ID_View1].Time);
              COMET::IHandle<COMET::IReconVertex> recon_vertex = MakeReconVertex(ParamVec[ID_View0], ParamVec[ID_View1]);


              recon_vertex->AddConstituent(module_bunch_clusters[ID_View0]);
              recon_vertex->AddConstituent(module_bunch_clusters[ID_View1]);
              recon_module_bunch_clusters->push_back(recon_vertex);

            }
            COMETVerbose("Total hypothesis likelihood is "<<hyp_likelihoods[i]<<std::endl);
            result->AddResultsContainer(recon_module_bunch_clusters);
          }
        }
      }
    }
  }
  return result;
}

//Calculate the cluster parameters (front, back, middle, charge, EVR)
COMET::ITrackerECALPi0ReconModule::ClusterParams COMET::ITrackerECALPi0ReconModule::GetClusterParams(COMET::IHandle<COMET::IReconPID> cluster) {
  ClusterParams result;
  result.Time = -1e10;
  result.TrShVal = 999.;
  result.EMHadVal = 999.;
  result.EMEnergy = -1.;
  result.View = kOrientUndefined;

  COMET::IReconBase::Status dets[5] ={COMET::IReconBase::kLeftTECal, COMET::IReconBase::kRightTECal, COMET::IReconBase::kTopTECal, COMET::IReconBase::kBottomTECal, 
    COMET::IReconBase::kDSECal };

  // Get constituents in any of the calorimeters
  for (int i = 0;i<5;i++){
    if (!cluster->UsesDetector(dets[i])) continue;

    COMET::IHandle<COMET::IReconBase> ecalObject = ReconObjectUtils::GetConstituentInDetector(cluster, dets[i]);

    if(i == 4) result.View = kOrientZ;
    else if(i < 2) result.View = kOrientX;
    else result.View = kOrientY;

    //COMETVerbose("About to check ecalObject");
    //ecalObject->ls();

    COMET::IHandle<COMET::IReconBase> ecalObject2 = ecalObject->GetConstituents()->front();

    //COMETVerbose("About to check ecalObject2");
    //ecalObject2->ls();

    result.Time = COMET::IHandle<COMET::IReconPID>(ecalObject)->GetPosition().T();
    //COMET::IHandle<COMET::IReconPID>(ecalObject2)->GetPosition().T();
    if(ecalObject2->Get< COMET::IRealDatum >("TrShval")){ 
      result.TrShVal = ecalObject2->Get< COMET::IRealDatum >("TrShval")->GetValue();
    }
    if(ecalObject2->Get< COMET::IRealDatum >("EMHadVal")){ 
      result.EMHadVal = ecalObject2->Get< COMET::IRealDatum >("EMHadVal")->GetValue();
    }

    COMET::IHandle<COMET::IReconBase> ecalObject3 = ecalObject2->GetConstituents()->front();
    if(ecalObject3->Get< COMET::IRealDatum >("EMEnergyFitResult")){ 
      result.EMEnergy = ecalObject3->Get< COMET::IRealDatum >("EMEnergyFitResult")->GetVector().front();
    }

    //COMETVerbose(ecalObject3->ClassName());
    //ecalObject3->ls();

    //COMET::IECALThrustFit::GetThrustParams3DFromCombi(ecalObject3, result.Thrust, result.Origin, result.Direction);
    if(ecalObject2->Get< COMET::IRealDatum >("Thrust")) {
      std::vector<double> thrust = ecalObject2->Get< COMET::IRealDatum >("Thrust")->GetVector();
      result.Thrust = thrust[0];
      result.Origin.SetXYZ(thrust[1], thrust[2], thrust[3]);
      result.Direction.SetXYZ(thrust[4], thrust[5], thrust[6]);
    }

    /*
    COMET::IHandle<COMET::IRealDatum> datum(new COMET::IRealDatum("Thrust", result.Thrust));
    datum->push_back(result.Origin.X());
    datum->push_back(result.Origin.Y());
    datum->push_back(result.Origin.Z());
    datum->push_back(result.Direction.X());
    datum->push_back(result.Direction.Y());
    datum->push_back(result.Direction.Z());
    cluster->AddDatum(datum);
    */
  }
  return result;
}


//Return a likelihood for two clusters to match. Each failed cut multiplies the
//likelihood by an exponential factor with coefficient proportional to the amount
//that the cut was missed by.
double COMET::ITrackerECALPi0ReconModule::CompareClusters(ClusterParams cl1, ClusterParams cl2) {
  bool pass = true;
  double likelihood=1.;

  //if(fTimeThreshold > 0.) {
    double dt = TMath::Abs(cl1.Time - cl2.Time);
    if(dt > 400.*unit::ns) {
      pass = false;
      likelihood *= TMath::Exp(-(dt - 400.*unit::ns)/(400.*unit::ns));
      COMETVerbose("\tTime\t" << dt << " FAIL");
    }
    else COMETVerbose("\tTime\t" << dt);
  //}

  //if(fTrShThreshold > 0.) {
    if(cl1.TrShVal > 0.5) {
      pass = false;
      likelihood *= TMath::Exp(-(cl1.TrShVal - 0.5)/0.5);
      COMETVerbose("\tTrShVal\t" << cl1.TrShVal << " FAIL");
    }
    else COMETVerbose("\tTrShVal\t" << cl1.TrShVal);
  //}

  COMETVerbose("Match likelihood "<<likelihood);
  return likelihood;
}

//Recursive algorithm to return all possible sets of matched pairs and
//unmatched clusters.
std::vector<std::vector < std::pair<int,int> > > COMET::ITrackerECALPi0ReconModule::GetMatches
(std::vector<int> Set1) {

  std::vector<std::vector < std::pair<int,int> > > result;

  if(Set1.size() > 1) {
    std::vector<int>::iterator last = Set1.end(); --last;
    for(std::vector<int>::iterator i = Set1.begin(); i != last; ++i) {
      std::vector<int>::iterator next = i; ++next;
      for(std::vector<int>::iterator j = next; j != Set1.end(); ++j) {
        std::vector<std::pair<int, int> > result2;
        result2.push_back(std::pair<int, int>(*i, *j));
        result.push_back(result2);
      }
    }
  }

  return result;
}

COMET::IHandle<COMET::IReconVertex> COMET::ITrackerECALPi0ReconModule::MakeReconVertex(const ClusterParams& cl1, const ClusterParams& cl2) {
  COMET::IHandle<COMET::IReconVertex> vertex(new COMET::IReconVertex);
  kOrient view1 = cl1.View, view2 = cl2.View;
  //if(clus1 && clus2) {
  kView whichViewToMatch = kViewUndefined;
  switch(view1 | view2) {
    case kOrientX | kOrientX: whichViewToMatch = kViewYX; break; 
    case kOrientY | kOrientY: whichViewToMatch = kViewZY; break; 
    case kOrientZ | kOrientZ: whichViewToMatch = kViewXZ; break; 
    case kOrientX | kOrientY: whichViewToMatch = kViewXY; break; 
    case kOrientY | kOrientZ: whichViewToMatch = kViewYZ; break; 
    case kOrientZ | kOrientX: whichViewToMatch = kViewZX; break; 
    default: break;
  }
  kView remainingView1 = kViewUndefined, remainingView2 = kViewUndefined;
  switch(whichViewToMatch) {
    case kViewZX: {
                    if(view1 == kOrientX) {
                      remainingView1 = kViewYX;
                      remainingView2 = kViewYZ;
                    }
                    else {
                      remainingView1 = kViewYZ;
                      remainingView2 = kViewYX;
                    }
                  }
                  break;
    case kViewYZ: {
                    if(view1 == kOrientY) {
                      remainingView1 = kViewXY;
                      remainingView2 = kViewXZ;
                    }
                    else {
                      remainingView1 = kViewXZ;
                      remainingView2 = kViewXY;
                    }
                  }
                  break;
    case kViewXY: {
                    if(view1 == kOrientX) {
                      remainingView1 = kViewZX;
                      remainingView2 = kViewZY;
                    }
                    else {
                      remainingView1 = kViewZY;
                      remainingView2 = kViewZX;
                    }
                  }
                  break;
    case kViewYX: {
                    remainingView1 = remainingView2 = kViewZX;
                  }
                  break;
    case kViewZY: {
                    remainingView1 = remainingView2 = kViewXY;
                  }
                  break;
    case kViewXZ: {
                    remainingView1 = remainingView2 = kViewYZ;
                  }
                  break;
    default: break;
  }
  TVector3 vtx = MatchThrusts(cl1, cl2, whichViewToMatch);
  TVector3 var = MatchThrustVariance(vertex, cl1, cl2, vtx, whichViewToMatch);
  std::pair<double, double> thirdCoordVar = MatchThirdViewInterval(vtx, cl1, remainingView1);
  switch(whichViewToMatch) {
    case kViewXZ:
    case kViewZX: vtx.SetY(thirdCoordVar.first); var.SetY(thirdCoordVar.second); break;
    case kViewZY:
    case kViewYZ: vtx.SetX(thirdCoordVar.first); var.SetX(thirdCoordVar.second); break;
    case kViewYX:
    case kViewXY: vtx.SetZ(thirdCoordVar.first); var.SetZ(thirdCoordVar.second); break;
    default: break;
  }
  COMET::IHandle<COMET::IRealDatum> datum = vertex->Get<COMET::IRealDatum>("Variance");
  if(datum) datum->push_back(thirdCoordVar.second);
  COMET::IHandle<COMET::IVertexState> st = vertex->GetState();
  st->SetPosition(vtx.X(), vtx.Y(), vtx.Z(), std::min(cl1.Time, cl2.Time));
  vertex->AddDatum(new COMET::IRealDatum("deltaT", "Time difference between 2 clusters", std::abs(cl1.Time - cl2.Time)));
  st->SetPositionVariance(var.X(), var.Y(), var.Z(), 0.);
  //COMETInfo("Making vertex: " << vtx.X() << ", " << vtx.Y() << ", " << vtx.Z() << ", " << std::min(cl1.Time, cl2.Time));
  AddKinematicsToVertex(vertex, vtx, cl1, cl2);
  //COMETInfo("pi0 mass: " << mass);

  return vertex;
}

double COMET::ITrackerECALPi0ReconModule::AddKinematicsToVertex(COMET::IHandle<COMET::IReconVertex> vertex, const TVector3& pos, const ClusterParams& cl1, const ClusterParams& cl2) {
  TVector3 d1 = cl1.Origin - pos;
  TVector3 d2 = cl2.Origin - pos;
  d1.SetMag(cl1.EMEnergy);
  d2.SetMag(cl2.EMEnergy);
  TVector3 p = d1 + d2;
  TLorentzVector P(d1 + d2, cl1.EMEnergy + cl2.EMEnergy);
  double m = P.M();
  COMET::IHandle<COMET::IRealDatum> datum(new COMET::IRealDatum("pi0kinem", "mass & 4-mom of pi0 candidate", m));
  datum->push_back(P.E());
  datum->push_back(P.X());
  datum->push_back(P.Y());
  datum->push_back(P.Z());
  vertex->AddDatum(datum);
  return m;
}

const TVector3 COMET::ITrackerECALPi0ReconModule::MatchThrusts(const ClusterParams& cl1, const ClusterParams& cl2, kView view) {
  const TVector3& c1a = cl1.Direction;
  const TVector3& c2a = cl2.Direction;
  const TVector3& c1o = cl1.Origin;
  const TVector3& c2o = cl2.Origin;
  double m1, m2, c1, c2, x = 0., y = 0., z = 0.;
  if(view == kViewXZ || view == kViewZX) {
    m1 = c1a.Z()/c1a.X();
    m2 = c2a.Z()/c2a.X();
    c1 = c1o.Z() - m1 * c1o.X();
    c2 = c2o.Z() - m2 * c2o.X();
    x = (c2 - c1) / (m1 - m2);
    z = m1 * x + c1;
  }
  else if(view == kViewYZ || view == kViewZY) {
    m1 = c1a.Z()/c1a.Y();
    m2 = c2a.Z()/c2a.Y();
    c1 = c1o.Z() - m1 * c1o.Y();
    c2 = c2o.Z() - m2 * c2o.Y();
    y = (c2 - c1) / (m1 - m2);
    z = m1 * y + c1;
  }
  else if(view == kViewXY || view == kViewYX) {
    m1 = c1a.Y()/c1a.X();
    m2 = c2a.Y()/c2a.X();
    c1 = c1o.Y() - m1 * c1o.X();
    c2 = c2o.Y() - m2 * c2o.X();
    x = (c2 - c1) / (m1 - m2);
    y = m1 * x + c1;
  }
  return TVector3(x, y, z);
}

const TVector3 COMET::ITrackerECALPi0ReconModule::MatchThrustVariance(COMET::IHandle<COMET::IReconVertex> vertex, const ClusterParams& cl1, const ClusterParams& cl2, const TVector3& pos, kView view) {
  const TVector3& c1a = cl1.Direction;
  const TVector3& c2a = cl2.Direction;
  const TVector3& c1o = cl1.Origin;
  const TVector3& c2o = cl2.Origin;
  kOrient o1 = cl1.View;
  kOrient o2 = cl2.View;
  double x = 0., y = 0., z = 0., mh, ml, gamma = 0., thetah, thetal, alphah, alphal, dh = 0., dl = 0., rhp, rhn, rlp, rln;
  if(view == kViewXZ || view == kViewZX) {
    if(o1 == kOrientZ) mh = c1a.X()/c1a.Z();
    else mh = c1a.Z()/c1a.X();
    TVector3 ph(c1o.X() - pos.X(), 0., c1o.Z() - pos.Z());
    dh = ph.Mag();
    if(o2 == kOrientZ) ml = c2a.X()/c2a.Z();
    else ml = c2a.Z()/c2a.X();
    TVector3 pl(c2o.X() - pos.X(), 0., c2o.Z() - pos.Z());
    dl = pl.Mag();
    gamma = ph.Angle(pl);
  }
  else if(view == kViewYZ || view == kViewZY) {
    if(o1 == kOrientZ) mh = c1a.Y()/c1a.Z();
    else mh = c1a.Z()/c1a.Y();
    mh = c1a.Y()/c1a.Z();
    TVector3 ph(0., c1o.Y() - pos.Y(), c1o.Z() - pos.Z());
    dh = ph.Mag();
    if(o2 == kOrientZ) ml = c2a.Y()/c2a.Z();
    else ml = c2a.Z()/c2a.Y();
    TVector3 pl(0., c2o.Y() - pos.Y(), c2o.Z() - pos.Z());
    dl = pl.Mag();
    gamma = ph.Angle(pl);
  }
  else if(view == kViewXY || view == kViewYX) {
    if(o1 == kOrientY) mh = c1a.X()/c1a.Y();
    else mh = c1a.Y()/c1a.X();
    TVector3 ph(c1o.X() - pos.X(), c1o.Y() - pos.Y(), 0.);
    dh = ph.Mag();
    if(o2 == kOrientY) ml = c2a.X()/c2a.Y();
    else ml = c2a.Y()/c2a.X();
    TVector3 pl(c2o.X() - pos.X(), c2o.Y() - pos.Y(), 0.);
    dl = pl.Mag();
    gamma = ph.Angle(pl);
  }
  thetah = TMath::ATan(mh);
  thetal = TMath::ATan(ml);
  alphah = GetAngularResolution(cl1.ThetaIncidence, cl1.EMEnergy);
  alphal = GetAngularResolution(cl2.ThetaIncidence, cl2.EMEnergy);

  rhp = dl * TMath::Sin(alphal) / TMath::Sin(TMath::Pi() - gamma - alphal);
  rhn = dl * TMath::Sin(alphal) / TMath::Sin(gamma - alphal);
  rlp = dh * TMath::Sin(alphah) / TMath::Sin(TMath::Pi() - gamma - alphah);
  rln = dh * TMath::Sin(alphah) / TMath::Sin(gamma - alphah);

  double rh = (rhp + rhn) / 2.;
  double rl = (rlp + rln) / 2.;

  COMET::IHandle<COMET::IRealDatum> datum(new COMET::IRealDatum("Variance", "Variance along high and low energy axis", rh));
  datum->push_back(rl);
  vertex->AddDatum(datum);

  return TVector3(x, y, z);
}

const std::pair<double, double> COMET::ITrackerECALPi0ReconModule::MatchThirdViewInterval(const TVector3& vtx2d, const ClusterParams& cl, kView view) {
  const TVector3& origin = cl.Origin;
  const TVector3& axis = cl.Direction;
  double m = 0., mh, ml, c, ox = 0., oy = 0., x, yh, yl, theta, res;
  if(view == kViewXZ) {
    x = vtx2d.Z();
    m = axis.X()/axis.Z();
    ox = origin.Z();
    oy = origin.X();
  }
  else if(view == kViewYZ) {
    x = vtx2d.Z();
    m = axis.Y()/axis.Z();
    ox = origin.Z();
    oy = origin.Y();
  }
  else if(view == kViewXY) {
    x = vtx2d.Y();
    m = axis.X()/axis.Y();
    ox = origin.Y();
    oy = origin.X();
  }
  else if(view == kViewZX) {
    x = vtx2d.X();
    m = axis.Z()/axis.X();
    ox = origin.X();
    oy = origin.Z();
  }
  else if(view == kViewZY) {
    x = vtx2d.Y();
    m = axis.Z()/axis.Y();
    ox = origin.Y();
    oy = origin.Z();
  }
  else if(view == kViewYX) {
    x = vtx2d.X();
    m = axis.Y()/axis.X();
    ox = origin.X();
    oy = origin.Y();
  }
  c = oy - m * ox;
  theta = TMath::ATan(m);
  res = GetAngularResolution(cl.ThetaIncidence, cl.EMEnergy);
  mh = TMath::Tan(theta + res);
  ml = TMath::Tan(theta - res);
  yh = mh * ox + c;
  yl = ml * ox + c;
  return std::pair<double, double>(yl, yh);
}

bool COMET::ITrackerECALPi0ReconModule::FillTree(COMET::ICOMETEvent& event) {

  // Initialise
  fNPi0 = 0;
  fNTruth = 0;
  fNFailedGlobalObject = 0;

  fPi0->Clear();
  fTruth->Clear();

  //Check for failed global objects
  COMET::IHandle<COMET::IAlgorithmResult> global = event.GetFit("ReconGlobal");
  if(global) {
    COMET::IHandle<COMET::IReconObjectContainer> pids = global->GetResultsContainer("final");
    if(pids) {
      Int_t PidId = -1;
      for(COMET::IReconObjectContainer::iterator pid = pids->begin(); pid != pids->end(); ++pid) {
        
        // try to get the state of the object. If it dosen't exists skip the object. 
        try{ 
          (*pid)->GetState(); 
        }
        catch(COMET::EReconObject e) { COMETWarn("IReconBase with no state. Skip object !!"); continue; }

        PidId++;
        
        bool isIso = false;
        COMET::IReconBase::Status subdet = 0;
        for(int sub = 0; sub < 19; ++sub) {
          if(isIso && (*pid)->UsesDetector(AllSubDetectors[sub])) {
            isIso = false;
            break;
          }
          if(!isIso && (*pid)->UsesDetector(AllSubDetectors[sub])) {
            isIso = true;
            subdet = AllSubDetectors[sub];
          }
        }
        if(!isIso) continue;

        COMET::IHandle<COMET::IReconBase> obj = ReconObjectUtils::GetConstituentInDetector(*pid, subdet);
        COMET::IHandle<COMET::IReconState> firstState = TrackingUtils::GetFirstState(*obj);
        if(firstState && TrackingUtils::GetPosition(firstState).Vect().Mag() > 0.) continue;

        IFailedGlobalObject* globalObject = new((*fFailedGlobalObject)[fNFailedGlobalObject++]) IFailedGlobalObject;

        globalObject->PidId = PidId;
        
        std::string cln = obj->ClassName();
        if(cln.find("COMET::IReconPID") != std::string::npos) {
          globalObject->Time = COMET::IHandle<COMET::IReconPID>(obj)->GetPosition().T();
        }
        else if(cln.find("COMET::IReconTrack") != std::string::npos) {
          globalObject->Time = COMET::IHandle<COMET::IReconTrack>(obj)->GetPosition().T();
        }
        else if(cln.find("COMET::IReconShower") != std::string::npos) {
          globalObject->Time = COMET::IHandle<COMET::IReconShower>(obj)->GetPosition().T();
        }
        else if(cln.find("COMET::IReconCluster") != std::string::npos) {
          globalObject->Time = COMET::IHandle<COMET::IReconCluster>(obj)->GetPosition().T();
        }
        else if(cln.find("COMET::IReconVertex") != std::string::npos) {
          globalObject->Time = COMET::IHandle<COMET::IReconVertex>(obj)->GetPosition().T();
        }
        else {
          globalObject->Detector = -1;
          continue;
        }

        switch(subdet) {
          case COMET::IReconBase::kTPC1: globalObject->Detector = 1; globalObject->Subdetector = 1; break;
          case COMET::IReconBase::kTPC2: globalObject->Detector = 2; globalObject->Subdetector = 2; break;
          case COMET::IReconBase::kTPC3: globalObject->Detector = 3; globalObject->Subdetector = 3; break;
          case COMET::IReconBase::kFGD1: globalObject->Detector = 4; globalObject->Subdetector = 1; break;
          case COMET::IReconBase::kFGD2: globalObject->Detector = 5; globalObject->Subdetector = 2; break;
          case COMET::IReconBase::kP0D: globalObject->Detector = 6; globalObject->Subdetector = 1; break;
          case COMET::IReconBase::kDSECal: globalObject->Detector = 7; globalObject->Subdetector = 9; break;
          case COMET::IReconBase::kTopSMRD: globalObject->Detector = 8; globalObject->Subdetector = 1; break;
          case COMET::IReconBase::kBottomSMRD: globalObject->Detector = 8; globalObject->Subdetector = 2; break;
          case COMET::IReconBase::kLeftSMRD: globalObject->Detector = 8; globalObject->Subdetector = 3; break;
          case COMET::IReconBase::kRightSMRD: globalObject->Detector = 8; globalObject->Subdetector = 4; break;
          case COMET::IReconBase::kLeftPECal: globalObject->Detector = 9; globalObject->Subdetector = 1; break;
          case COMET::IReconBase::kRightPECal: globalObject->Detector = 9; globalObject->Subdetector = 2; break;
          case COMET::IReconBase::kTopPECal: globalObject->Detector = 9; globalObject->Subdetector = 3; break;
          case COMET::IReconBase::kBottomPECal: globalObject->Detector = 9; globalObject->Subdetector = 4; break;
          case COMET::IReconBase::kLeftTECal: globalObject->Detector = 9; globalObject->Subdetector = 5; break;
          case COMET::IReconBase::kRightTECal: globalObject->Detector = 9; globalObject->Subdetector = 6; break;
          case COMET::IReconBase::kTopTECal: globalObject->Detector = 9; globalObject->Subdetector = 7; break;
          case COMET::IReconBase::kBottomTECal: globalObject->Detector = 9; globalObject->Subdetector = 8; break;
        }
      }
    }
  }
  
  GetTrajectories(event);

  // Getting the final Algorithm Result from ReconECal.
  COMET::IHandle<COMET::IAlgorithmResult> bunches = ProcessBunches(*global);
  COMET::IHandle<COMET::IAlgorithmResult> candidates = ProcessCandidates(*bunches);

  if(candidates)
  for(COMET::IDataVector::iterator dataIter = candidates->begin();
  dataIter != candidates->end(); ++dataIter) {
    std::string class_name = (*dataIter)->ClassName();
    TString object_name = (*dataIter)->GetName();

    // Looking for ObjectContainers that are labelled correctly
    if(class_name.find("COMET::IReconObjectContainer") != std::string::npos && object_name.Contains("pi0_match")) {
      COMET::IReconObjectContainer* reconContainer =
        dynamic_cast<COMET::IReconObjectContainer*>(*dataIter);

      if(!reconContainer)
        COMETError("Dynamic cast of ECALPid ReconObjectContainer didn't work");
      // Getting bunch number.
      std::string std_object_name = (*dataIter)->GetName();
      std::string::size_type idx = std_object_name.find_last_of('_');
      std::string bunch_id = std_object_name.substr(idx+1);

      // Loop looking for IReconPID's in the top recon conatiner
      for(COMET::IReconObjectContainer::iterator reconIter =
        reconContainer->begin();
      reconIter != reconContainer->end();  ++reconIter) {

        std::string reco_class_name = (*reconIter)->ClassName();
        
        // Get IReconPID's
        if(reco_class_name.find("COMET::IReconVertex") != std::string::npos) {
          IPi0* pi0 = new((*fPi0)[fNPi0++]) IPi0;
      
          COMET::IHandle<COMET::IReconVertex> vtx = (*reconIter);
          pi0->DeltaT = vtx->Get<COMET::IRealDatum>("deltaT")->GetValue();
          pi0->Position = vtx->GetPosition();
          COMET::IHandle<COMET::IReconObjectContainer> conContainer = vtx->GetConstituents();
          
          bool first = true;
          
          for(COMET::IReconObjectContainer::iterator conIter =
                conContainer->begin();
              conIter != conContainer->end();  ++conIter) { 
              
            if(first) pi0->PhotonHigh.Fill(*conIter, this);
            else pi0->PhotonLow.Fill(*conIter, this);
            
            first = false;
          }
          TVector3 O1 = pi0->PhotonHigh.ThrustOrigin - pi0->Position.Vect();
          O1.SetMag(pi0->PhotonHigh.Energy);
          pi0->PhotonHigh.Momentum = O1;
          TVector3 O2 = pi0->PhotonLow.ThrustOrigin - pi0->Position.Vect();
          O2.SetMag(pi0->PhotonLow.Energy);
          pi0->PhotonLow.Momentum = O2;
          pi0->OpeningAngle = O1.Angle(O2);
          
          O1.SetMag(1.);
          O2.SetMag(1.);
          pi0->PhotonHigh.VertexResolution = pi0->PhotonHigh.ThrustAxis.Dot(O1);
          pi0->PhotonLow.VertexResolution = pi0->PhotonLow.ThrustAxis.Dot(O2);
          
          COMET::IHandle<COMET::IRealDatum> datum = vtx->Get<COMET::IRealDatum>("pi0kinem");
          if(datum) {
            const std::vector<double>& mom = datum->GetVector();
            pi0->Mass = mom[0];
            pi0->Energy = mom[1];
            pi0->Momentum.SetXYZ(mom[2], mom[3], mom[4]);
          }
          datum = vtx->Get<COMET::IRealDatum>("Variance");
          if(datum) {
            const std::vector<double>& var = datum->GetVector();
            pi0->ResAlongHigh = var[0];
            pi0->ResAlongLow = var[1];
            pi0->ResThird = var[2];
          }
        }
      }
    }
  }
  return true;
}

void COMET::ITrackerECALPi0ReconModule::IPi0Photon::Fill(COMET::IHandle<COMET::IReconPID> cluster, COMET::ITrackerECALPi0ReconModule* parent) {

  COMET::IHandle<COMET::IRealDatum> datum = cluster->Get<COMET::IRealDatum>("Thrust");
  if(datum) {
    std::vector<double> d = datum->GetVector();
    this->Thrust = d[0];
    this->ThrustOrigin.SetXYZ(d[1], d[2], d[3]);
    this->ThrustAxis.SetXYZ(d[4], d[5], d[6]);
  }
  
  
  this->PID = cluster->GetConstituents()->front()->Get<COMET::IRealDatum>("TrShval")->GetValue();
  COMET::IHandle<COMET::IReconCluster> c = cluster->GetConstituents()->front()->GetConstituents()->front();
  this->Position = c->GetPosition();
  this->Energy = c->Get<COMET::IRealDatum>("EMEnergyFitResult")->GetVector()[1];
  this->Module = parent->FindModule(this->Position.Vect()).AsInt();


  this->UpstreamLayer = 36;
  this->DownstreamLayer = -1;
  this->ExtremePlusXHit = -1;
  this->ExtremeMinusXHit = 200;
  this->ExtremePlusYHit = -1;
  this->ExtremeMinusYHit = 200;
  this->NHits = 0;
  COMET::IHandle<COMET::IHitSelection> hits = c->GetHits();
  
  if(!hits) return;
  
  TPrincipal pca(3,"");
  double sumchrg = 0., meanpoint = 0.;
  
  for (COMET::IHitSelection::iterator hit = hits->begin();
       hit != hits->end(); ++hit) {
    try {
      Int_t layerHit = COMET::IGeomInfo::ECAL().GetLayerNumber(*hit);
      Int_t barHit = COMET::IGeomInfo::ECAL().GetBarNumber(*hit);
      this->NHits++;
      if(layerHit < this->UpstreamLayer){
        this->UpstreamLayer = layerHit;
      }
      if(layerHit > this->DownstreamLayer){
        this->DownstreamLayer = layerHit;
      }
      if(layerHit %2 == 0) {
        if(barHit < this->ExtremeMinusXHit) {
          this->ExtremeMinusXHit = barHit;
        }
        if(barHit > this->ExtremePlusXHit) {
          this->ExtremePlusXHit = barHit;
        }
      }
      else {
        if(barHit < this->ExtremeMinusYHit) {
          this->ExtremeMinusYHit = barHit;
        }
        if(barHit > this->ExtremePlusYHit) {
          this->ExtremePlusYHit = barHit;
        }
      }
    } catch(ENoSuchECALModule) {}
    // Obtaining the first and last layers hit.
    double row[3] = { (*hit)->GetPosition().X(),
                      (*hit)->GetPosition().Y(),
                      (*hit)->GetPosition().Z()};
    for (double q=0; q < (*hit)->GetCharge(); ++q) {
      pca.AddRow(row);
    }
    sumchrg += (*hit)->GetCharge();
  }
  
  // Find the principal components of the shower.
  pca.MakePrincipals();

  double posP[] = {0.,0.,0.};
  double posX[3];
  pca.P2X(posP, posX, 3);

  TVector3 origin(posX);
  TVector3 axis((*pca.GetEigenVectors())[0][0], (*pca.GetEigenVectors())[1][0], (*pca.GetEigenVectors())[2][0]);
  for (COMET::IHitSelection::iterator hit = hits->begin();
       hit != hits->end(); ++hit) {
    meanpoint += (*hit)->GetCharge() * axis.Dot((*hit)->GetPosition() - origin);
  }
  this->Pointing = (meanpoint / sumchrg) * axis;
  
  if(parent->fTrajIDs.empty()) return;
  
  std::map<COMET::IGeometryId, std::map<int, double> > fTrueParticlesWeights;
  for(COMET::IHitSelection::iterator hit = hits->begin(); hit != hits->end(); ++hit) {
    COMET::IGeometryId geomId = COMET::IGeomInfo::Get().ECAL().GetModule((*hit)->GetGeomId()).GetGeomId();
    COMET::IHandle<COMET::IReconHit> rhit(*hit);
    if(!rhit) continue;
    int nrhitcons = rhit->GetContributorCount();
    for(int irhitcons = 0; irhitcons < nrhitcons; ++irhitcons) {
      COMET::IHandle<COMET::ISingleHit> shit(rhit->GetContributor(irhitcons));

      if(shit->GetDigitCount() == 0) continue;

      COMET::IDigitProxy proxy = shit->GetDigit();
      COMET::IMCDigit *mcDigit = proxy.As<COMET::IMCDigit>();

      std::vector<COMET::IG4VHit*> contributors = mcDigit->GetContributors();

      for (std::vector<COMET::IG4VHit*>::const_iterator g4Hit = contributors.begin();
           g4Hit != contributors.end(); g4Hit++) {
        COMET::IG4HitSegment* segment = dynamic_cast<COMET::IG4HitSegment*>(*g4Hit);

        if(!segment)  continue; 
        double edep = segment->GetEnergyDeposit();
        std::vector<int> scont = segment->GetContributors();
        for(std::vector<int>::iterator s = scont.begin(); s != scont.end(); ++s) {
          int id = *s;
          bool found = false;
          for(std::map<int, std::vector<int> >::iterator t = parent->fTrajIDs[geomId].begin(); t != parent->fTrajIDs[geomId].end(); ++t) {
            if(find(t->second.begin(), t->second.end(), id) != t->second.end()) {
              fTrueParticlesWeights[geomId][t->first] += edep;
              found = true;
              break;
            }
          }
        }
      }
    }
  }
  if(fTrueParticlesWeights.empty()) return;
  for(std::map<COMET::IGeometryId, std::map<int, double> >::iterator m = fTrueParticlesWeights.begin(); m != fTrueParticlesWeights.end(); ++m) {
    COMET::IGeometryId modId = m->first;
    std::map<int, double>& tpw = m->second;
    for(std::map<int, double>::iterator t = tpw.begin(); t != tpw.end(); ++t) {
      int idInClonesArray = parent->fNTruth;
      COMET::ITrackerECALPi0ReconModule::ITruthClusteredParticle* tcp = new((*(parent->fTruth))[idInClonesArray]) COMET::ITrackerECALPi0ReconModule::ITruthClusteredParticle;
      tcp->Module = modId.AsInt();
      tcp->Id = t->first;
      tcp->EDep = t->second;
      parent->fNTruth++;
      this->TrueParticles.push_back(idInClonesArray);
    }
  } 
}


void COMET::ITrackerECALPi0ReconModule::GetTrajectories(COMET::ICOMETEvent& event) {
  fTrajIDs.clear();
  fTrajPoses.clear();
  
  COMET::IHandle<COMET::IG4TrajectoryContainer> trajs = event.Get<COMET::IG4TrajectoryContainer>("truth/G4Trajectories");
  if (!trajs) return;
  for(COMET::IG4TrajectoryContainer::iterator traj_pair = trajs->begin(); traj_pair != trajs->end(); ++traj_pair) {
    bool first_point = true;
    int id = traj_pair->second.GetTrackId();
    int parent = traj_pair->second.GetParentId();
    for(COMET::IG4Trajectory::Points::iterator traj_point = traj_pair->second.GetTrajectoryPoints().begin();
        traj_point != traj_pair->second.GetTrajectoryPoints().end(); ++traj_point) {
      TVector3 pos = traj_point->GetPosition().Vect();
      fTrajPoses[id].push_back(pos);
    }
    COMET::IG4Trajectory::Points::iterator prev_traj_point = traj_pair->second.GetTrajectoryPoints().begin();
    for(std::map<COMET::IGeometryId, TGeoBBox*>::iterator m = fModuleShapes.begin(); m != fModuleShapes.end(); ++m) {
      COMET::IGeometryId modId = m->first;
      for(COMET::IG4Trajectory::Points::iterator traj_point = traj_pair->second.GetTrajectoryPoints().begin();
          traj_point != traj_pair->second.GetTrajectoryPoints().end(); ++traj_point) {
        TVector3 pos = traj_point->GetPosition().Vect();
        try {
          if(modId != COMET::IGeomInfo::Get().ECAL().GetModule(pos).GetGeomId()) throw COMET::ENoSuchECALModule();
          if(first_point) {
            bool found = false;
            for(std::map<int, std::vector<int> >::iterator primary = fTrajIDs[modId].begin(); primary != fTrajIDs[modId].end(); ++primary) {
              std::vector<int>::iterator match = find(primary->second.begin(), primary->second.end(), parent);
              if(match != primary->second.end()) {
                primary->second.push_back(id);
                found = true;
                break;
              }
            }
            if(!found) {
              fTrajIDs[modId][id].push_back(id);
            }
          }
          else {
            fTrajIDs[modId][id].push_back(id);
          }
          break;
        }
        catch(ENoSuchECALModule) {
          if(traj_point != prev_traj_point) {
            TVector3 pos1 = prev_traj_point->GetPosition().Vect();
            TVector3 pos2 = traj_point->GetPosition().Vect();
            if(DoesLineCrossModules(modId, pos1, pos2)) {
              fTrajIDs[modId][id].push_back(id);
              break;
            }
          }
        }
        first_point = false;
        prev_traj_point = traj_point;
      }
    }
  }
}


void COMET::ITrackerECALPi0ReconModule::InitialiseModuleShapes() {
  fModuleShapes.clear();
  std::vector<COMET::IECALModule*> modules = COMET::IGeomInfo::Get().ECAL().GetECALModules();
  for(std::vector<COMET::IECALModule*>::iterator m = modules.begin(); m != modules.end(); ++m) {
    COMET::IGeometryId moduleId = (*m)->GetGeomId();
    COMET::IOADatabase::Get().GeomId().CdId(moduleId);
    TGeoNode *node = COMET::IOADatabase::Get().Geometry()->GetCurrentNode();
    TGeoBBox* b = (TGeoBBox*)node->GetVolume()->GetShape();
    Double_t local[] = {0.,0.,0.};
    Double_t master[] = {0.,0.,0.};
    Double_t mastervec[] = {0., 0., 0.};
    COMET::IOADatabase::Get().Geometry()->LocalToMaster(local, master);
    local[0] = b->GetDX();
    local[1] = b->GetDY();
    local[2] = b->GetDZ();
    COMET::IOADatabase::Get().Geometry()->LocalToMasterVect(local, mastervec);
    TGeoBBox *b2 = new TGeoBBox(TMath::Abs(mastervec[0]), TMath::Abs(mastervec[1]), TMath::Abs(mastervec[2]), master);
    //COMETError("Module x: " << master[0] << " y: " << master[1] << " z: " << master[2] << " dx: " << mastervec[0] << " dy: " << mastervec[1] << " dz: " << mastervec[2]);
    fModuleShapes[moduleId] = b2;
  }
}

bool COMET::ITrackerECALPi0ReconModule::DoesLineCrossModules(COMET::IGeometryId modId, const TVector3& p1, const TVector3& p2) {
  TVector3 dp = p2 - p1;
  if(fModuleShapes.find(modId) == fModuleShapes.end()) return false;
  TGeoBBox* m = fModuleShapes[modId];
  double zmin, zmax, ymin, ymax, xmin, xmax, d, x, y, z;
  zmin = m->GetOrigin()[2] - m->GetDZ();
  zmax = m->GetOrigin()[2] + m->GetDZ();
  ymin = m->GetOrigin()[1] - m->GetDY();
  ymax = m->GetOrigin()[1] + m->GetDY();
  xmin = m->GetOrigin()[0] - m->GetDX();
  xmax = m->GetOrigin()[0] + m->GetDX();
  if(TMath::Abs(p2.Z() - p1.Z()) > 0) {
    d = (zmin - p1.Z()) / (p2.Z() - p1.Z());
    if(TMath::Abs(d) < 1.) {
      TVector3 intercept = p1 + d * dp;
      x = intercept.X();
      y = intercept.Y();
      if(x > xmin && x < xmax && y > ymin && y < ymax) return true;
    }
    d = (zmax - p1.Z()) / (p2.Z() - p1.Z());
    if(TMath::Abs(d) < 1.) {
      TVector3 intercept = p1 + d * dp;
      x = intercept.X();
      y = intercept.Y();
      if(x > xmin && x < xmax && y > ymin && y < ymax) return true;
    }
  }
  if(TMath::Abs(p2.Y() - p1.Y()) > 0) {
    d = (ymin - p1.Y()) / (p2.Y() - p1.Y());
    if(TMath::Abs(d) < 1.) {
      TVector3 intercept = p1 + d * dp;
      x = intercept.X();
      z = intercept.Z();
      if(x > xmin && x < xmax && z > zmin && z < zmax) return true;
    }
    d = (ymax - p1.Y()) / (p2.Y() - p1.Y());
    if(TMath::Abs(d) < 1.) {
      TVector3 intercept = p1 + d * dp;
      x = intercept.X();
      z = intercept.Z();
      if(x > xmin && x < xmax && z > zmin && z < zmax) return true;
    }
  }
  if(TMath::Abs(p2.X() - p1.X()) > 0) {
    d = (xmin - p1.X()) / (p2.X() - p1.X());
    if(TMath::Abs(d) < 1.) {
      TVector3 intercept = p1 + d * dp;
      y = intercept.Y();
      z = intercept.Z();
      if(y > ymin && y < ymax && z > zmin && z < zmax) return true;
    }
    d = (xmax - p1.X()) / (p2.X() - p1.X());
    if(TMath::Abs(d) < 1.) {
      TVector3 intercept = p1 + d * dp;
      y = intercept.Y();
      z = intercept.Z();
      if(y > ymin && y < ymax && z > zmin && z < zmax) return true;
    }
  }
  return false;
}

COMET::IGeometryId COMET::ITrackerECALPi0ReconModule::FindModule(const TVector3& p) {
  double x = p.X(), y = p.Y(), z = p.Z();
  for(std::map<COMET::IGeometryId, TGeoBBox*>::iterator m = fModuleShapes.begin(); m != fModuleShapes.end(); ++m) {
    COMET::IGeometryId mod = m->first;
    double zmin, zmax, ymin, ymax, xmin, xmax;
    zmin = m->second->GetOrigin()[2] - m->second->GetDZ();
    zmax = m->second->GetOrigin()[2] + m->second->GetDZ();
    ymin = m->second->GetOrigin()[1] - m->second->GetDY();
    ymax = m->second->GetOrigin()[1] + m->second->GetDY();
    xmin = m->second->GetOrigin()[0] - m->second->GetDX();
    xmax = m->second->GetOrigin()[0] + m->second->GetDX();
    if(x >= xmin && x <= xmax && y >= ymin && y <= ymax && z >= zmin && z <= zmax) return mod;
  }
  return COMET::IGeometryId();
}

COMET::ITrackerECALPi0ReconModule::IPi0::~IPi0(){}
COMET::ITrackerECALPi0ReconModule::ITruthClusteredParticle::~ITruthClusteredParticle() {}
COMET::ITrackerECALPi0ReconModule::IPi0Photon::~IPi0Photon() {}
COMET::ITrackerECALPi0ReconModule::IFailedGlobalObject::~IFailedGlobalObject() {}

IMyChangeClass::IMyChangeClass(COMET::ITrackerECALPi0ReconModule* p) : COMET::IOADatabase::IGeometryChange(), fParent(p) {}
IMyChangeClass::~IMyChangeClass() {}
void IMyChangeClass::Callback(const COMET::ICOMETEvent* const) { fParent->InitialiseModuleShapes(); }