#include "IFgdOnlyModule.hxx"
#include <ITrackState.hxx>
#include "IReconCluster.hxx"
#include "IRealDatum.hxx"
#include "IIntegerDatum.hxx"
#include "TrackTruthInfo.hxx"
#include <cmath>
#include <IFGDPid.hxx>


ClassImp(COMET::IFgdOnlyModule);
ClassImp(COMET::IFgdOnlyModule::IFgd2DIsoTrack);
COMET::IFgdOnlyModule::IFgd2DIsoTrack::~IFgd2DIsoTrack() {}
ClassImp(COMET::IFgdOnlyModule::IFgd3DIsoTrack);
COMET::IFgdOnlyModule::IFgd3DIsoTrack::~IFgd3DIsoTrack() {}
ClassImp(COMET::IFgdOnlyModule::IFgd2DCluster);
COMET::IFgdOnlyModule::IFgd2DCluster::~IFgd2DCluster() {}
ClassImp(COMET::IFgdOnlyModule::IFgd3DShowerHyp);
COMET::IFgdOnlyModule::IFgd3DShowerHyp::~IFgd3DShowerHyp() {}
ClassImp(COMET::IFgdOnlyModule::IFgd3DShowerPID);
COMET::IFgdOnlyModule::IFgd3DShowerPID::~IFgd3DShowerPID() {}

#define CVSTAG "\
  $Name: v5r19 $"
#define CVSID  "\
"  

double getTrackLength(COMET::IHandle<COMET::IReconTrack> track)
{
  const COMET::IReconNodeContainer &nodes = track->GetNodes();
  if (nodes.size() < 2)
    return -1;
  
  // horrendous syntax begins here
  COMET::IHandle<COMET::ITrackState> start = (*(nodes.begin()))->GetState();
  COMET::IHandle<COMET::ITrackState> stop = (*(nodes.end()-1))->GetState();
  // horrendous syntax ends here
  if(start && stop){
    TVector3 beginning = start->GetPosition().Vect();
    TVector3 end = stop->GetPosition().Vect();
    return (beginning - end).Mag();
  }
  else{
    return -1;
  }
}


COMET::IFgdOnlyModule::IFgdOnlyModule(const char *name, const char *title)
{
  fTrackAlgorithms.push_back("_Radon2");
  fTrackAlgorithms.push_back("_RECONSBCAT");
  fTrackAlgorithms.push_back("_Radon");
  fTrackAlgorithms.push_back("_flexRadon");
  fTrackAlgorithms.push_back("_P0DHough");

  fPids.push_back("_Proton");
  fPids.push_back("_Pion");
  fPids.push_back("_Muon");

  SetNameTitle(name, title);

  fIsEnabled = kTRUE;
  fDescription = "FGD Recon Output";
  fCVSTagName = CVSTAG;
  fCVSID = CVSID;

  fNXZTracks=new int[fTrackAlgorithms.size()];
  fNYZTracks=new int[fTrackAlgorithms.size()];
  fNXYZTracks=new int[fTrackAlgorithms.size()];
  fNXZTracksAllFGD=new int[fTrackAlgorithms.size()];
  fNYZTracksAllFGD=new int[fTrackAlgorithms.size()];
  fNXYZTracksAllFGD=new int[fTrackAlgorithms.size()];
  
  fXZTracks=new TClonesArray*[fTrackAlgorithms.size()];
  fYZTracks=new TClonesArray*[fTrackAlgorithms.size()];
  fXYZTracks=new TClonesArray*[fTrackAlgorithms.size()];
  fXZTracksAllFGD=new TClonesArray*[fTrackAlgorithms.size()];
  fYZTracksAllFGD=new TClonesArray*[fTrackAlgorithms.size()];
  fXYZTracksAllFGD=new TClonesArray*[fTrackAlgorithms.size()];
  
  for(unsigned int i=0;i<fTrackAlgorithms.size();++i){
    fNXZTracks[i]=0;
    fXZTracks[i]=new TClonesArray("COMET::IFgdOnlyModule::IFgd2DIsoTrack", 200);
    fNYZTracks[i]=0;
    fYZTracks[i]=new TClonesArray("COMET::IFgdOnlyModule::IFgd2DIsoTrack", 200);
    fNXYZTracks[i]=0;
    fXYZTracks[i]=new TClonesArray("COMET::IFgdOnlyModule::IFgd3DIsoTrack", 200);
    
    fNXZTracksAllFGD[i]=0;
    fXZTracksAllFGD[i]=new TClonesArray("COMET::IFgdOnlyModule::IFgd2DIsoTrack", 200);
    fNYZTracksAllFGD[i]=0;
    fYZTracksAllFGD[i]=new TClonesArray("COMET::IFgdOnlyModule::IFgd2DIsoTrack", 200);
    fNXYZTracksAllFGD[i]=0;
    fXYZTracksAllFGD[i]=new TClonesArray("COMET::IFgdOnlyModule::IFgd3DIsoTrack", 200);
  }

  fN3DShowers = 0;
  f3DShowers = new TClonesArray("COMET::IFgdOnlyModule::IFgd3DShowerPID", 200);

  fN2DClustersXZ = 0;
  f2DClustersXZ = new TClonesArray("COMET::IFgdOnlyModule::IFgd2DCluster", 200);
  f2DClustersYZ = 0;
  f2DClustersYZ = new TClonesArray("COMET::IFgdOnlyModule::IFgd2DCluster", 200);
}

COMET::IFgdOnlyModule::~IFgdOnlyModule() {
//  if(fFGDPid){
//      delete fFGDPid;
//      fFGDPid = NULL;
//  }
}

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

void COMET::IFgdOnlyModule::InitializeBranches()
{
  char buf[100];
  char bufI[100];

  for(unsigned int i=0;i<fTrackAlgorithms.size();++i){
    
    const char* algoName=fTrackAlgorithms[i];

    sprintf(buf,"NXZTracks%s",algoName);
    sprintf(bufI,"%s/I",buf);
    fOutputTree->Branch(buf,
      &fNXZTracks[i], bufI,
      fBufferSize);

    sprintf(buf,"NYZTracks%s",algoName);
    sprintf(bufI,"%s/I",buf);
    fOutputTree->Branch(buf,
      &fNYZTracks[i], bufI,
      fBufferSize);

    sprintf(buf,"NXYZTracks%s",algoName);
    sprintf(bufI,"%s/I",buf);
    fOutputTree->Branch(buf,
      &fNXYZTracks[i], bufI,
      fBufferSize);

    sprintf(buf,"NXZTracksAllFGD%s",algoName);
    sprintf(bufI,"%s/I",buf);
    fOutputTree->Branch(buf,
      &fNXZTracksAllFGD[i], bufI,
      fBufferSize);

    sprintf(buf,"NYZTracksAllFGD%s",algoName);
    sprintf(bufI,"%s/I",buf);
    fOutputTree->Branch(buf,
      &fNYZTracksAllFGD[i], bufI,
      fBufferSize);

    sprintf(buf,"NXYZTracksAllFGD%s",algoName);
    sprintf(bufI,"%s/I",buf);
    fOutputTree->Branch(buf,
      &fNXYZTracksAllFGD[i], bufI,
      fBufferSize);
    

    sprintf(buf,"XZTracks%s",algoName);
    fOutputTree->Branch(buf,
      &fXZTracks[i],
      fBufferSize, fSplitLevel);

    sprintf(buf,"YZTracks%s",algoName);
    fOutputTree->Branch(buf,
      &fYZTracks[i],
      fBufferSize, fSplitLevel);

    sprintf(buf,"XYZTracks%s",algoName);
    fOutputTree->Branch(buf,
      &fXYZTracks[i],
      fBufferSize, fSplitLevel);

    sprintf(buf,"XZTracksAllFGD%s",algoName);
    fOutputTree->Branch(buf,
      &fXZTracksAllFGD[i],
      fBufferSize, fSplitLevel);
    
    sprintf(buf,"YZTracksAllFGD%s",algoName);
    fOutputTree->Branch(buf,
      &fYZTracksAllFGD[i],
      fBufferSize, fSplitLevel);
    
    sprintf(buf,"XYZTracksAllFGD%s",algoName);
    fOutputTree->Branch(buf,
      &fXYZTracksAllFGD[i],
      fBufferSize, fSplitLevel);
  }

  fOutputTree->Branch("N3DShowers",
          &fN3DShowers, "N3DShowers/I",
          fBufferSize);
  fOutputTree->Branch("N2DClustersXZ",
          &fN2DClustersXZ, "N2DClustersXZ/I",
          fBufferSize);
  fOutputTree->Branch("N2DClustersYZ",
          &fN2DClustersYZ, "N2DClustersYZ/I",
          fBufferSize);

  fOutputTree->Branch("FGD3DShowers",
          &f3DShowers,
          fBufferSize, fSplitLevel);
  fOutputTree->Branch("FGD2DClustersXZ",
          &f2DClustersXZ,
          fBufferSize, fSplitLevel);
  fOutputTree->Branch("FGD2DClustersYZ",
          &f2DClustersYZ,
          fBufferSize, fSplitLevel);
}

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

  for(unsigned int i=0;i<fTrackAlgorithms.size();++i){
    fNXZTracks[i]=0;
    fXZTracks[i]->Clear();
    
    fNYZTracks[i]=0;
    fYZTracks[i]->Clear();
    
    fNXYZTracks[i] = 0;
    fXYZTracks[i]->Clear();
    
    fNXZTracksAllFGD[i] = 0;
    fXZTracksAllFGD[i]->Clear();
    
    fNYZTracksAllFGD[i] = 0;
    fYZTracksAllFGD[i]->Clear();
    
    fNXYZTracksAllFGD[i] = 0;
    fXYZTracksAllFGD[i]->Clear();
  }

  fN3DShowers = 0;
  f3DShowers->Clear();

  fN2DClustersXZ = 0;
  f2DClustersXZ->Clear();
  fN2DClustersYZ = 0;
  f2DClustersYZ->Clear();

  COMET::IHandle<COMET::IAlgorithmResult> FgdResult = event.GetFit("ReconFGD");

  // Returning false will lead to this module being disabled - need to decide 
  // if we want this to happen if don't find one "ReconFGD" fit result
  if (!FgdResult)
    return true;

  char buf[100];

  for(unsigned int i=0;i<fTrackAlgorithms.size();++i){
    
    const char *algoName=fTrackAlgorithms[i];
    
    sprintf(buf,"xzFgdReconTracks%s",algoName);
    COMET::IHandle<COMET::IReconObjectContainer> xzReconTracks = 
      FgdResult->GetResultsContainer(buf);
    sprintf(buf,"yzFgdReconTracks%s",algoName);
    COMET::IHandle<COMET::IReconObjectContainer> yzReconTracks =
      FgdResult->GetResultsContainer(buf);
    sprintf(buf,"fittedFgdReconTracks%s",algoName);
    COMET::IHandle<COMET::IReconObjectContainer> xyzReconTracks =
      FgdResult->GetResultsContainer(buf);
    
    sprintf(buf,"xzFgdReconTracks_allFGD%s",algoName);
    COMET::IHandle<COMET::IReconObjectContainer> xzReconTracksAllFGD = 
      FgdResult->GetResultsContainer(buf);
    sprintf(buf,"yzFgdReconTracks_allFGD%s",algoName);
    COMET::IHandle<COMET::IReconObjectContainer> yzReconTracksAllFGD =
      FgdResult->GetResultsContainer(buf);
    sprintf(buf,"fittedFgdReconTracks_allFGD%s",algoName);
    COMET::IHandle<COMET::IReconObjectContainer> xyzReconTracksAllFGD =
      FgdResult->GetResultsContainer(buf);


    if(xzReconTracks){  
      for (COMET::IReconObjectContainer::iterator track = xzReconTracks->begin();
     track != xzReconTracks->end(); track++) {
  COMET::IHandle<COMET::IReconTrack> tr = *track;
  if (!tr)
    continue;
  COMET::IHandle<COMET::ITrackState> state = tr->GetState();
  if (!state)
    continue;

  IFgd2DIsoTrack *TempTrack = new((*(fXZTracks[i]))[fNXZTracks[i]++]) IFgd2DIsoTrack;
  Fill2DIsoTrack(tr, state, TempTrack, 'X');
      }
    }
    if(yzReconTracks){
      for (COMET::IReconObjectContainer::iterator track = yzReconTracks->begin();
     track != yzReconTracks->end(); track++) {
  COMET::IHandle<COMET::IReconTrack> tr = *track;
  if (!tr)
    continue;
  COMET::IHandle<COMET::ITrackState> state = tr->GetState();
  if (!state)
    continue;

  IFgd2DIsoTrack *TempTrack = new((*(fYZTracks[i]))[fNYZTracks[i]++]) IFgd2DIsoTrack;
  Fill2DIsoTrack(tr, state, TempTrack, 'Y');
      }
    }
    
    if(xyzReconTracks){
      for (COMET::IReconObjectContainer::iterator track = xyzReconTracks->begin();
     track != xyzReconTracks->end(); track++) {
  COMET::IHandle<COMET::IReconTrack> tr = *track;
  if (!tr)
    continue;
  COMET::IHandle<COMET::ITrackState> state = tr->GetState();
  if (!state)
    continue;

  IFgd3DIsoTrack *TempTrack = new((*(fXYZTracks[i]))[fNXYZTracks[i]++]) IFgd3DIsoTrack;
  Fill3DIsoTrack(tr, state, TempTrack);
      }
    }
    
    // Tracks using all the FGD hits
    if(xzReconTracksAllFGD){  
      for (COMET::IReconObjectContainer::iterator track = xzReconTracksAllFGD->begin();
     track != xzReconTracksAllFGD->end(); track++) {
  COMET::IHandle<COMET::IReconTrack> tr = *track;
  if (!tr)
    continue;
  COMET::IHandle<COMET::ITrackState> state = tr->GetState();
  if (!state)
    continue;

  IFgd2DIsoTrack *TempTrack = new((*(fXZTracksAllFGD[i]))[fNXZTracksAllFGD[i]++]) IFgd2DIsoTrack;
  Fill2DIsoTrack(tr, state, TempTrack, 'X');
      }
    }
    if(yzReconTracksAllFGD){
      for (COMET::IReconObjectContainer::iterator track = yzReconTracksAllFGD->begin();
     track != yzReconTracksAllFGD->end(); track++) {
  COMET::IHandle<COMET::IReconTrack> tr = *track;
  if (!tr)
    continue;
  COMET::IHandle<COMET::ITrackState> state = tr->GetState();
  if (!state)
    continue;

  IFgd2DIsoTrack *TempTrack = new((*(fYZTracksAllFGD[i]))[fNYZTracksAllFGD[i]++]) IFgd2DIsoTrack;
  Fill2DIsoTrack(tr, state, TempTrack, 'Y');
      }
    }
    
    if(xyzReconTracksAllFGD){
      for (COMET::IReconObjectContainer::iterator track = xyzReconTracksAllFGD->begin();
     track != xyzReconTracksAllFGD->end(); track++) {
  COMET::IHandle<COMET::IReconTrack> tr = *track;
  if (!tr)
    continue;
  COMET::IHandle<COMET::ITrackState> state = tr->GetState();
  if (!state)
    continue;
  
  IFgd3DIsoTrack *TempTrack = new((*(fXYZTracksAllFGD[i]))[fNXYZTracksAllFGD[i]++]) IFgd3DIsoTrack;
  Fill3DIsoTrack(tr, state, TempTrack);
      }
    }
  }// end loop over track algorithms

  COMET::IHandle<COMET::IReconObjectContainer> fgd3DShowers = FgdResult->GetResultsContainer("FGDShowerPIDs");
  std::vector<COMET::IHandle<COMET::IReconObjectContainer> > fgd2DClustersXZ, fgd2DClustersYZ;
  fgd2DClustersXZ.push_back(FgdResult->GetResultsContainer("combi_basic_fgd1_xz"));
  fgd2DClustersYZ.push_back(FgdResult->GetResultsContainer("combi_basic_fgd1_yz"));
  fgd2DClustersXZ.push_back(FgdResult->GetResultsContainer("combi_basic_fgd2_xz"));
  fgd2DClustersYZ.push_back(FgdResult->GetResultsContainer("combi_basic_fgd2_yz"));

  // FGD Showers
  if (fgd3DShowers) {
    COMETVerbose("Have fgd3DShowers");
    for (COMET::IReconObjectContainer::iterator pidIter = fgd3DShowers->begin();
         pidIter != fgd3DShowers->end(); pidIter++) {
      COMET::IHandle<COMET::IReconPID> pid = *pidIter;

      IFgd3DShowerPID *TempObject = new((*f3DShowers)[fN3DShowers++]) IFgd3DShowerPID;
      Fill3DShower(TempObject, pid);
    }
  }

  // 2D Constituents of 3D Showers
  for (std::vector<COMET::IHandle<COMET::IReconObjectContainer> >::iterator it = fgd2DClustersXZ.begin(); it != fgd2DClustersXZ.end(); it++) {
    COMET::IHandle<COMET::IReconObjectContainer> roc = (*it);
    if (roc) {
      for (COMET::IReconObjectContainer::iterator objIter = roc->begin(); objIter != roc->end(); objIter++) {
  COMET::IHandle<COMET::IReconCluster> cluster = *objIter;
        IFgd2DCluster *TempObject = new((*f2DClustersXZ)[fN2DClustersXZ++]) IFgd2DCluster;
        Fill2DCluster(TempObject, cluster);
      }
    }
  }
  for (std::vector<COMET::IHandle<COMET::IReconObjectContainer> >::iterator it = fgd2DClustersYZ.begin(); it != fgd2DClustersYZ.end(); it++) {
    COMET::IHandle<COMET::IReconObjectContainer> roc = (*it);
    if (roc) {
      for (COMET::IReconObjectContainer::iterator objIter = roc->begin(); objIter != roc->end(); objIter++) {
  COMET::IHandle<COMET::IReconCluster> cluster = *objIter;
        IFgd2DCluster *TempObject = new((*f2DClustersYZ)[fN2DClustersYZ++]) IFgd2DCluster;
        Fill2DCluster(TempObject, cluster);
      }
    }
  }

  return true;
}



void COMET::IFgdOnlyModule::Fill2DIsoTrack(COMET::IHandle<COMET::IReconTrack> tr, COMET::IHandle<COMET::ITrackState> state, IFgd2DIsoTrack *isoTrack, char Axis){

      isoTrack->AlgorithmName = tr->GetAlgorithmName();
      
      COMET::IHandle<COMET::IHitSelection> hits = tr->GetHits();
      isoTrack->NHits = hits->size();
  
      double charge = 0;
      for (COMET::IHitSelection::iterator hit = hits->begin();
     hit != hits->end(); hit++) {
        charge += (*hit)->GetCharge();
      }
      isoTrack->SumCharge = charge;
  
      TLorentzVector posn = state->GetPosition();
      isoTrack->Origin = posn;
      TLorentzVector dposn = state->GetPositionVariance();
      isoTrack->OriginVariance = dposn;
  
      TVector3 dir = state->GetDirection();
      isoTrack->Direction = dir;
  
      if (Axis == 'X')
        isoTrack->Angle = atan2(dir.X(), dir.Z());
      if (Axis == 'Y')
        isoTrack->Angle = atan2(dir.Y(), dir.Z());

      isoTrack->Range = getTrackLength(tr);
  
      for (COMET::IHitSelection::iterator hit = hits->begin();
     hit != hits->end(); hit++) {
        TVector3 *pos = new TVector3((*hit)->GetPosition());
        isoTrack->HitPositions.push_back(pos);
      }

      isoTrack->TrajId = TrackTruthInfo::GetG4TrajectoryID(tr, isoTrack->Completeness, isoTrack->Cleanliness);

}


void COMET::IFgdOnlyModule::Fill3DIsoTrack(COMET::IHandle<COMET::IReconTrack> tr, COMET::IHandle<COMET::ITrackState> state, IFgd3DIsoTrack *isoTrack){

      IFGDPid fFGDPid;

      isoTrack->AlgorithmName = tr->GetAlgorithmName();

      COMET::IHandle<COMET::IHitSelection> hits = tr->GetHits();
      isoTrack->NHits = hits->size();
  
      double charge = 0;
      for (COMET::IHitSelection::iterator hit = hits->begin();
     hit != hits->end(); hit++) {
        charge += (*hit)->GetCharge();
      }
      isoTrack->SumCharge = charge;
  
      TLorentzVector posn = state->GetPosition();
      isoTrack->Origin = posn;
      TLorentzVector dposn = state->GetPositionVariance();
      isoTrack->OriginVariance = dposn;
  
      TVector3 dir = state->GetDirection();
      isoTrack->Direction = dir;
  
      isoTrack->Range = getTrackLength(tr);

      COMET::IHandle<COMET::IReconPID> pid=fFGDPid.ApplyPID(tr);

      COMET::IHandle<COMET::IRealDatum> buf;
      buf=pid->Get<COMET::IRealDatum>("fgdPidWgt_EvsXPull_Muon");
      if(buf){
  isoTrack->muonPull=buf->GetValue();
      }
      else{
  isoTrack->muonPull=1000.;
      }
      buf=pid->Get<COMET::IRealDatum>("fgdPidWgt_EvsXPull_Pion");
      if(buf){
  isoTrack->pionPull=buf->GetValue();
      }
      else{
  isoTrack->pionPull=1000.;
      }
      buf=pid->Get<COMET::IRealDatum>("fgdPidWgt_EvsXPull_Proton");
      if(buf){
  isoTrack->protonPull=buf->GetValue();
      }
      else{
  isoTrack->protonPull=1000.;
      }
      COMETVerbose("Muon pull "<<isoTrack->muonPull);
      COMETVerbose("Pion pull "<<isoTrack->pionPull);
      COMETVerbose("Proton pull "<<isoTrack->protonPull);
      
      // for (COMET::IHitSelection::iterator hit = hits->begin();
      // 	 hit != hits->end(); hit++) {
      //   TVector3 *pos = new TVector3((*hit)->GetPosition());
      //   if ((*hit)->IsXHit())
      // 	isoTrack->XZHitPositions.push_back(pos);
      //   else
      // 	isoTrack->YZHitPositions.push_back(pos);
      // }

      isoTrack->TrajId = TrackTruthInfo::GetG4TrajectoryID(tr, isoTrack->Completeness, isoTrack->Cleanliness);
}

void COMET::IFgdOnlyModule::Fill2DCluster(IFgd2DCluster *TempObject, COMET::IHandle<COMET::IReconCluster> cluster) {
  TempObject->Status = cluster->GetStatus();
  TempObject->NDOF = cluster->GetNDOF();
  TempObject->Quality = cluster->GetQuality();
  TempObject->Position = cluster->GetPosition();
  TempObject->EDeposit = cluster->GetEDeposit();
  TempObject->NumHits = cluster->GetHits()->size();

  TempObject->Range = cluster->Get<COMET::IRealDatum> ("range")->GetValue();
  TempObject->AvgHitTime = cluster->Get<COMET::IRealDatum> ("avgHitTime")->GetValue();

  std::vector<double> startPos = cluster->Get<COMET::IRealDatum> ("startPosition")->GetVector();
  TempObject->StartPosition = TVector3(startPos[0], startPos[1], startPos[2]);
  std::vector<double> endPos = cluster->Get<COMET::IRealDatum> ("endPosition")->GetVector();
  TempObject->EndPosition = TVector3(endPos[0], endPos[1], endPos[2]);
  std::vector<double> dir = cluster->Get<COMET::IRealDatum> ("pcaDirection")->GetVector();
  TempObject->PCADirection = TVector3(dir[0], dir[1], dir[2]);

  TempObject->Trajectories = TrackTruthInfo::GetG4Trajectories(*(cluster->GetHits()));
}

void COMET::IFgdOnlyModule::Fill3DShower(IFgd3DShowerPID *TempObject, COMET::IHandle<COMET::IReconPID> pid) {
  COMETVerbose("Filling next shower");
  COMET::IReconObjectContainer altCon = pid->GetAlternates();
  if (altCon.size() != 1) {
    COMETWarn("Expect 1 alternative hypothesis for FGD Shower PID, have " << altCon.size());
    return;
  }

  COMET::IHandle<COMET::IReconPID> alt = altCon.at(0);
  if (!alt) {
    COMETWarn("Unable to get alternate hypothesis");
    return;
  }

  COMET::IHandle<COMET::IReconObjectContainer> cons = pid->GetConstituents();
  if (cons->size() != 1) {
    COMETWarn("Expect 1 constituent for FGD Shower PID, have " << cons->size());
    return;
  }

  COMET::IHandle<COMET::IReconShower> shower = cons->at(0);
  if (!shower) {
    COMETWarn("Couldn't cast constituent to IReconShower");
    return;
  }

  // Ensure that hypothesis 1 corresponds to the forward-going hypothesis.
  if (pid->GetDirection().Z() > 0) {
    TempObject->Hyp1 = Fill3DShowerHyp(pid);
    TempObject->Hyp2 = Fill3DShowerHyp(alt);
  } else {
    TempObject->Hyp1 = Fill3DShowerHyp(alt);
    TempObject->Hyp2 = Fill3DShowerHyp(pid);
  }

  TempObject->Status = pid->GetStatus();
  TempObject->NDOF = pid->GetNDOF();
  TempObject->Quality = pid->GetQuality();
  TempObject->NumHits = pid->GetHits()->size();
  TempObject->MatchingLikelihood3D = shower->Get<COMET::IRealDatum> ("MatchingLikelihood3D")->GetValue();
  std::vector<double> pcaValues = shower->Get<COMET::IRealDatum> ("pcaValues")->GetVector();
  TempObject->PCAEigenValues = TVector3(pcaValues[0], pcaValues[1], pcaValues[2]);
  TempObject->Circularity = shower->Get<COMET::IRealDatum> ("Circularity")->GetValue();
  TempObject->Trajectories = TrackTruthInfo::GetG4Trajectories(*(pid->GetHits()));
}

COMET::IFgdOnlyModule::IFgd3DShowerHyp COMET::IFgdOnlyModule::Fill3DShowerHyp(COMET::IHandle<COMET::IReconPID> hyp) {
  COMETVerbose("Filling next shower hypothesis");
  IFgd3DShowerHyp output;

  COMET::IHandle<COMET::IReconObjectContainer> cons = hyp->GetConstituents();
  if (cons->size() != 1) {
    COMETWarn("Expect 1 constituent for FGD Shower PID, have " << cons->size());
    return output;
  }

  COMET::IHandle<COMET::IReconShower> shower = cons->at(0);
  if (!shower) {
    COMETWarn("Couldn't cast constituent to IReconShower");
    return output;
  }

  COMET::IHandle<COMET::IShowerState> state = shower->GetState();
  if (!state) {
    COMETWarn("Couldn't get state for shower");
    return output;
  }

  // PID-related datums
  std::vector<double> qAvg = shower->Get<COMET::IRealDatum> ("QAvgInThirds")->GetVector();
  output.QAvgInThirds = TVector3(qAvg[0], qAvg[1], qAvg[2]);
  std::vector<double> spread = shower->Get<COMET::IRealDatum> ("SpreadInThirds")->GetVector();
  output.SpreadInThirds = TVector3(spread[0], spread[1], spread[2]);

  output.EDeposit = shower->GetEDeposit();
  output.ConeAngle = state->GetCone();
  output.ConeAngleVar = state->GetConeVariance();
  output.Direction = state->GetDirection();
  output.DirectionVar = state->GetDirectionVariance();
  output.Position = state->GetPosition();
  output.PositionVar = state->GetPositionVariance();

  return output;
}