// This class disabled for now. Will reenable once TOATrack-dependency can be resolved
// as a special case for backward compatibility

#ifdef UNDEFINED_UNTIL_OATRACK_REENABLED
#include <functional>
#include <algorithm>

#include <IDatum.hxx>
#include <IRealDatum.hxx>
#include <IReconTrack.hxx>
#include <IReconShower.hxx>
#include <IReconCluster.hxx>
#include <IReconVertex.hxx>
#include <IReconNode.hxx>

#include <IGeomInfo.hxx>
#include "IG4Trajectory.hxx"
#include <createTOATrack.hxx>

#include "IReconTrackerTOATrackModule.hxx"

ClassImp(COMET::IReconTrackerTOATrackModule);

ClassImp(COMET::IReconTrackerTOATrackModule::ITrackerVertex);
COMET::IReconTrackerTOATrackModule::ITrackerVertex::~ITrackerVertex(){}

ClassImp(COMET::IReconTrackerTOATrackModule::IFgdCluster);
COMET::IReconTrackerTOATrackModule::IFgdCluster::~IFgdCluster(){}

ClassImp(COMET::IReconTrackerTOATrackModule::ITrackerTrack);
COMET::IReconTrackerTOATrackModule::ITrackerTrack::~ITrackerTrack(){}

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

COMET::IReconTrackerTOATrackModule::IReconTrackerTOATrackModule(const char *name, const char *title) :
fUpstreamXFGD1Plane(-9999),
fUpstreamYFGD1Plane(-9999),
fDowstreamXFGD1Plane(-9999),
fDowstreamYFGD1Plane(-9999),
fUpstreamXFGD2Plane(-9999),
fUpstreamYFGD2Plane(-9999),
fDowstreamXFGD2Plane(-9999),
fDowstreamYFGD2Plane(-9999) {
  SetNameTitle(name, title);

  // Enable this module by default:
  fIsEnabled = kTRUE;
  fDescription = "Tracker Recon Output";
  fCVSTagName = CVSTAG;
  fCVSID = CVSID;

  // Tree variables
  fNVertices = 0;
  fVertices = new TClonesArray("COMET::IReconTrackerTOATrackModule::ITrackerVertex", 200);
  fNTracks = 0;
  fTracks = new TClonesArray("COMET::IReconTrackerTOATrackModule::ITrackerTrack", 200);
  fNDelayedClusters = 0;
  fDelayedClusters = new TClonesArray("COMET::IReconTrackerTOATrackModule::IFgdCluster", 200);

}


COMET::IReconTrackerTOATrackModule::~IReconTrackerTOATrackModule() {}

Bool_t COMET::IReconTrackerTOATrackModule::ProcessFirstEvent(COMET::ICOMETEvent& event) {

  // Here we have the first event and the geometry, so can set up module
  // data members using these. The module gets saved in the output file,
  // so these numbers can be accessed by the analysis scripts.
  fFGD1ActiveMin = COMET::IGeomInfo::FGD().FGD1ActiveMin();
  fFGD1ActiveMax = COMET::IGeomInfo::FGD().FGD1ActiveMax();
  fFGD2ActiveMin = COMET::IGeomInfo::FGD().FGD2ActiveMin();
  fFGD2ActiveMax = COMET::IGeomInfo::FGD().FGD2ActiveMax();

  fUpstreamXFGD1Plane = COMET::IGeomInfo::FGD().  GetFirstFGD1Plane("X");
  fUpstreamYFGD1Plane = COMET::IGeomInfo::FGD().  GetFirstFGD1Plane("Y");
  fDowstreamXFGD1Plane = COMET::IGeomInfo::FGD(). GetLastFGD1Plane("X");
  fDowstreamYFGD1Plane = COMET::IGeomInfo::FGD(). GetLastFGD1Plane("Y");
  fUpstreamXFGD2Plane = COMET::IGeomInfo::FGD().  GetFirstFGD2Plane("X");
  fUpstreamYFGD2Plane = COMET::IGeomInfo::FGD().  GetFirstFGD2Plane("Y");
  fDowstreamXFGD2Plane = COMET::IGeomInfo::FGD(). GetLastFGD2Plane("X");
  fDowstreamYFGD2Plane = COMET::IGeomInfo::FGD(). GetLastFGD2Plane("Y");

  return true;
}


void COMET::IReconTrackerTOATrackModule::InitializeModule() {}

void COMET::IReconTrackerTOATrackModule::InitializeBranches() {

  // The number of objects that have been saved.
  fOutputTree->Branch("NVertices",
    &fNVertices, "NVertices/I",
    fBufferSize);
  fOutputTree->Branch("NTracks",
    &fNTracks, "NTracks/I",
    fBufferSize);
  fOutputTree->Branch("NDelayedClusters",
    &fNDelayedClusters, "NDelayedClusters/I",
    fBufferSize);

  // A branch for the clone array of Tracker object information.
  fOutputTree->Branch("Vertices",
    &fVertices,
    fBufferSize, fSplitLevel);
  fOutputTree->Branch("Tracks",
    &fTracks,
    fBufferSize, fSplitLevel);
  fOutputTree->Branch("DelayedClusters",
    &fDelayedClusters,
    fBufferSize, fSplitLevel);
}


// Helper method
// Get the median FGD hit time for this track.
double COMET::IReconTrackerTOATrackModule::MedianTrackTime(COMET::IHandle<COMET::TOATrack> Track) {
  std::vector<double> times;

  // make a vector of all the times for the 'good' hits
  for (COMET::IHitSelection::iterator hit = Track->GetHits().begin();
  hit != Track->GetHits().end(); hit++) {
    if ((COMET::IGeomInfo::FGD().IsInFGD1((*hit)->GetPosition()) ||
    COMET::IGeomInfo::FGD().IsInFGD2((*hit)->GetPosition()))) {
      times.push_back((*hit)->GetTime());
    }
  }

  std::sort(times.begin(), times.end());

  unsigned len = times.size();

  if(len ==0)                                                           // There must be an
    return 0.0;
  else if (len % 2 != 0) {
    return times[(len-1) / 2];
  }
  else {
    return (times[len/2] + times[len/2 - 1]) / 2.0;
  }
}


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

  fNVertices = 0;
  fVertices->Clear();

  fNTracks = 0;
  fTracks->Clear();

  fNDelayedClusters = 0;
  fDelayedClusters->Clear();

  // Get the main result to save.
  COMET::IHandle<COMET::IAlgorithmResult> ReconTracker = event.GetFit("ReconFGD");

  // __________________________________________________________
  // Fill the reconstructed Tracker vertices.

  // Get reconstructed vertices
  COMET::IHandle<COMET::IReconObjectContainer> tvertices
    = ReconTracker->GetResultsContainer("trackerSimpleVertices");

  // Loop over vertices.
  if (tvertices) {
    for (COMET::IReconObjectContainer::iterator v = tvertices->begin();
      v != tvertices->end();
    ++v) {
      if (COMET::IHandle<COMET::IReconVertex> obj = *v) {
        ITrackerVertex* trackerVertex =
          new((*fVertices)[fNVertices++]) ITrackerVertex;

        COMET::IHandle<COMET::IVertexState> objState = obj->GetState();
        trackerVertex->AlgorithmName = obj->GetAlgorithmName();
        trackerVertex->Status = obj->GetStatus();
        trackerVertex->Quality = obj->GetQuality();
        trackerVertex->NDOF = obj->GetNDOF();
        trackerVertex->Position = objState->GetPosition();
        trackerVertex->Variance = objState->GetPositionVariance();
        trackerVertex->Fiducial = 1.0;
      }
    }
  }

  // __________________________________________________________
  // Fill the reconstructed Tracker tracks.

  // Get the results from reconstruction
  // Set of 3D FGD tracks.
  COMET::IHandle<COMET::TOATrackContainer> xyzTracks
    = ReconTracker->GetTrackContainer("fittedFgdTracks");
  // Set of fully matched TPC/FGD tracks.
  COMET::IHandle<COMET::TOATrackContainer> finalFgdTpcTracks
    = ReconTracker->GetTrackContainer("finalFgdTpcTracks");

  // Combine into single TOATrackContainer.
  COMET::TOATrackContainer allTracks;

  if(finalFgdTpcTracks)
    for(COMET::TOATrackContainer::iterator tt = finalFgdTpcTracks->begin();
    tt != finalFgdTpcTracks->end(); tt++ )
  allTracks.push_back(*tt);

  if(xyzTracks)
    for(COMET::TOATrackContainer::iterator tt = xyzTracks->begin();
    tt != xyzTracks->end(); tt++ )
  allTracks.push_back(*tt);

  // Now loop over tracks.
  for(COMET::TOATrackContainer::iterator track = allTracks.begin();
  track != allTracks.end(); track++) {

    ITrackerTrack* trackerTrack =
      new((*fTracks)[fNTracks++]) ITrackerTrack;

    trackerTrack->AlgorithmName = "";
    trackerTrack->Status  = 0;
    trackerTrack->Quality = 0.0;
    trackerTrack->NDOF    = 0;
    trackerTrack->isForwards = true;

    // Find the median time for this track.
    double track_time = MedianTrackTime(*track);

    // Set the track front position.
    TVector3 tmp_pos = (*track)->GetFrontPosition();
    TLorentzVector fpos = TLorentzVector(tmp_pos.X(),tmp_pos.Y(),tmp_pos.Z(),track_time);
    trackerTrack->FrontPosition = fpos;

    TLorentzVector pos_var = TLorentzVector(0,0,0,0);
    trackerTrack->FrontVariance = pos_var;

    // Set the track back position.
    tmp_pos = (*track)->GetBackPosition();
    TLorentzVector bpos = TLorentzVector(tmp_pos.X(),tmp_pos.Y(),tmp_pos.Z(),track_time);
    trackerTrack->BackPosition = bpos;

    trackerTrack->BackVariance = pos_var;

    // Set front direction.
    COMET::IHandle<COMET::TOATrackPoint> pt = (*track)->GetFront();
    double tx = pt->Get("TX");
    double ty = pt->Get("TY");
    trackerTrack->FrontDirection  = TVector3(tx,ty,1.);

    pt = (*track)->GetBack();
    tx = pt->Get("TX");
    ty = pt->Get("TY");
    trackerTrack->BackDirection  = TVector3(tx,ty,1.);

    // Set the momentum, momentum error, charge and dE/dx
    // for each TPC.

    // Set defaults
    for (int i = 0; i < 3; i++) {
      trackerTrack->tpc_dedx[i] = 0.0;
      trackerTrack->tpc_momentum[i] = 0.0;
      trackerTrack->tpc_momentum_error[i] = 0.0;
      trackerTrack->tpc_charge[i] = 0.0;
    }

    // Get momentum and charge for each TPC using RealDatum
    // information.  If no RealDatum information is available
    // then this must be just an FGD track.

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

      char name[10], mom_name[20],  mom_err_name[20], chg_name[20];;
      sprintf(name,"TPC%i",i+1);
      sprintf(mom_name,"TPC%i_momentum",i+1);
      sprintf(mom_err_name,"TPC%i_momentum_error",i+1);
      sprintf(chg_name,"TPC%i_charge",i+1);

      if((*track)->GetStatus().find(name) != std::string::npos) {
        trackerTrack->tpc_charge[i] = (*track)->Get<COMET::IRealDatum>(chg_name)->GetValue();
        trackerTrack->tpc_momentum[i] = (*track)->Get<COMET::IRealDatum>(mom_name)->GetValue();
        trackerTrack->tpc_momentum_error[i] = (*track)->Get<COMET::IRealDatum>(mom_err_name)->GetValue();
      }

    }
  }

  // __________________________________________________________
  // Fill the delayed FGD clusters.

  // Get all FGD hits
  COMET::IHandle<COMET::IHitSelection> fgdHits = ReconTracker->GetHitSelection("fgd");

  // Loop over vertices.
  if (fgdHits) {

    std::vector<COMET::IHitSelection> fgd_clusters;

    // Find the earliest track time.
    double earliest_track_time = 9999.0;
    if(allTracks.size() == 0)                                           // No tracks, so save all FGD clusters.
      earliest_track_time = 0.0;
    else {
      for(COMET::TOATrackContainer::iterator track = allTracks.begin();
      track != allTracks.end(); track++) {
        double track_time = MedianTrackTime(*track);
        if(track_time < earliest_track_time)
          earliest_track_time = track_time;
      }
    }

    // Need to make this parameters somewhere else!!!
    double delay_time = 200*unit::ns;
    double min_distance = 5*unit::cm;
    double min_charge = 5.0;
    size_t min_hits = 2;

    for(COMET::IHitSelection::iterator hit = fgdHits->begin();
    hit != fgdHits->end(); hit++) {

      // Find all delayed hits.
      double dtime = (*hit)->GetTime() - earliest_track_time;

      // Need to make this parameter!!!
      if(dtime > delay_time) {

        // Check if this hit is close to an existing cluster
        // of FGD hits.

        bool found_match = false;
        for(unsigned int i = 0; i < fgd_clusters.size(); i++) {
          for(COMET::IHitSelection::iterator chit = fgd_clusters[i].begin();
          chit != fgd_clusters[i].end(); chit++) {
            if(((*hit)->IsXHit() && (*chit)->IsXHit()) ||
            ((*hit)->IsYHit() && (*chit)->IsYHit())) {
              double pos_diff = ((*hit)->GetPosition() - (*chit)->GetPosition()).Mag();
              if(pos_diff < min_distance
                && (*hit)->GetCharge() > min_charge)
                found_match = true;
            }
          }

          if(found_match) {
            fgd_clusters[i].push_back(*hit);
            break;
          }
        }

        // If we didn't find a match, then start new hit selection.
        if(!found_match) {
          IHitSelection hits;
          hits.push_back(*hit);
          fgd_clusters.push_back(hits);
        }
      }
    }

    // For every delayed FGD cluster with more than one hit,
    // create a new TFgdClusterObject
    for(unsigned int i = 0; i < fgd_clusters.size(); i++) {

      if(fgd_clusters[i].size() > min_hits) {

        // Calculate average position and time.
        TVector3 avg_pos;
        double avg_time = 0.0;
        double total_charge = 0.0;
        TLorentzVector position_variance(1.0e8,1.0e8,0.0,0.0);
        for(COMET::IHitSelection::iterator hit = fgd_clusters[i].begin();
        hit != fgd_clusters[i].end(); hit++) {
          avg_pos += (*hit)->GetPosition();
          avg_time += (*hit)->GetTime();
          total_charge += (*hit)->GetCharge();
          if((*hit)->IsXHit())
            position_variance[0] = 0.0;
          else if((*hit)->IsYHit())
            position_variance[1] = 0.0;
        }

        IFgdCluster* fgdCluster =
          new((*fDelayedClusters)[fNDelayedClusters++]) IFgdCluster;

        avg_pos *= 1.0/((double)fgd_clusters[i].size());
        avg_time *= 1.0/((double)fgd_clusters[i].size());

        fgdCluster->TotalCharge = total_charge;
        fgdCluster->Position = TLorentzVector(avg_pos.X(),avg_pos.Y(),avg_pos.Z(),avg_time);
        fgdCluster->Variance = position_variance;
      }
    }
  }

  return true;

}
#endif // UNDEFINED_UNTIL_OATRACK_REENABLED