#include <string>
#include <vector>
#include <CLHEP/Units/PhysicalConstants.h>
#include <RAT/StraightTimeResidualCut.hh>
#include <RAT/Processor.hh>
#include <RAT/FitterPMT.hh>
#include <RAT/DB.hh>
#include <RAT/DU/Utility.hh>
#include <RAT/DS/FitVertex.hh>
#include "TVector3.h"

using namespace RAT;
using namespace RAT::PMTSelectors;

void StraightTimeResidualCut::Initialise(const std::string&) {
  fLowCut = -50.0;
  fHighCut = 50.0;
}

void StraightTimeResidualCut::BeginOfRun(DS::Run&) {
  DB* db = DB::Get();
  DBLinkPtr dbLink = db->GetLink( "FIT_MULTIPATH" );
  double fWaterRI = dbLink->GetD( "water_RI" );

  double fSpeedOfLight = CLHEP::c_light;
  fSpeedOfLightWater = fSpeedOfLight / fWaterRI;
}

void StraightTimeResidualCut::EndOfRun(DS::Run&) {
}

void StraightTimeResidualCut::SetD(const std::string& param, double value) {
  if (param == "lowLimit") {
    fLowCut = value;
  } else if (param == "highLimit") {
    fHighCut = value;
  } else {
    throw Processor::ParamUnknown(param);
  }
}

std::vector<FitterPMT>
StraightTimeResidualCut::GetSelectedPMTs(const std::vector<FitterPMT>& data,
                                         const DS::FitVertex& vertex) {

  std::vector<FitterPMT> selectedPMTs;
  for (size_t i = 0; i < data.size(); i++) {
    double time = data[i].GetTime();
    size_t id = data[i].GetID();
    const TVector3 pos = DU::Utility::Get()->GetPMTInfo().GetPosition(id);
    const TVector3 vtx = vertex.GetPosition();
    TVector3 diff = pos - vtx;
    double transitTime = diff.Mag() / fSpeedOfLightWater;
    double fittedTime = vertex.GetTime();

    double res = time - fittedTime - transitTime;
    if (res < fLowCut || res > fHighCut) {
      continue;
    }

    selectedPMTs.push_back(data[i]);
  }
  return selectedPMTs;
}