/////////////////////////////////////////////////////////////////
// EnergyRSP.cc
// Contact person: John Walker <john.walker@liverpool.ac.uk>
// See EnergyRSP.hh for more details
/////////////////////////////////////////////////////////////////

#include <RAT/EnergyRSP.hh>
#include <RAT/DS/FitResult.hh>
#include <RAT/SeededMethod.hh>
#include <RAT/Method.hh>
#include <RAT/PMTSelector.hh>
#include <RAT/ChannelEfficiency.hh>
#include <RAT/PMTVariation.hh>
#include <RAT/DU/LightPathCalculator.hh>
#include <RAT/PhysicsUtil.hh>

#include <RAT/DB.hh>
#include <RAT/DU/Utility.hh>
#include <RAT/Optics.hh>
#include <RAT/PMTSelectorFactory.hh>
#include <RAT/DS/Run.hh>
#include <RAT/DS/FitVertex.hh>
#include <RAT/DU/PMTInfo.hh>
#include <RAT/FitterPMT.hh>
#include <RAT/DU/ChanHWStatus.hh>
#include <RAT/Log.hh>

#include <G4MaterialPropertyVector.hh>

#include <G4SystemOfUnits.hh>
#include <G4PhysicalConstants.hh>

#include <numeric>
#include <algorithm>
#include <iterator>
#include <functional>
#include <TVector3.h>
#include <TSpline.h>
#include <cmath>

using namespace RAT;
using namespace RAT::Methods;
using namespace RAT::DS;
using std::string;
using std::vector;
using std::greater;
using std::isfinite;
using std::abs;
using CLHEP::nm;

void
EnergyRSP::Initialise( const string& param )
{
  fIndex = param;
  fUseRefracted = false;
  fStoreDiagnostics = false;
  fTimeResidualCut = PMTSelectors::PMTSelectorFactory::Get()->GetPMTSelector( "timeResidualCut" );
  fPMTCalSelector = PMTSelectors::PMTSelectorFactory::Get()->GetPMTSelector( "PMTCalSelector" );
  twLowLimit = -10.0*ns;
  twHighLimit = 8.0*ns;
  fChannelEfficiency = ChannelEfficiency::Get();
  fPMTVariation = PMTVariation::Get();
}


void
EnergyRSP::SetI( const string& param, const int value ) {

  if(param == "useRefracted") {
    if(value == 1)
      fUseRefracted = true;
    else
      fUseRefracted = false;
  }
  else if(param == "storeDiagnostics") {
    if(value == 1)
      fStoreDiagnostics = true;
    else
      fStoreDiagnostics = false;
  }
  else{
    info << "Unkown RSP parameter" << newline;
    throw 1;
  }

}

void
EnergyRSP::BeginOfRun( DS::Run& run )
{
  DB *db = DB::Get();
  string index = fIndex;
  if( fIndex.empty() )
    {
      try{ index = db->GetLink("GEO","inner_av")->GetS("material"); }
      catch( DBNotFoundError& e ) { /* Will use defaults instead */ }
    }
  // Get default parameters
  DBLinkPtr dbRSPLink = db->GetLink("FIT_ENERGY_RSP");
  DBLinkGroup grp=db->GetLinkGroup("FIT_ENERGY_RSP");
  DBLinkGroup::iterator it;
  // Check for material specific parameters
  // If not, keep default values
  for(it=grp.begin(); it != grp.end(); ++it){
    if(index == it->first){
      dbRSPLink = db->GetLink("FIT_ENERGY_RSP", index);
      break;
    }
  }

  fLightPath = DU::Utility::Get()->GetLightPathCalculator();

  fTimeResidualCut->BeginOfRun( run );
  fChannelEfficiency->BeginOfRun();
  fPMTVariation->BeginOfRun();

  // Get data ratdb
  fMaxIterations = dbRSPLink->GetI( "max_iterations" );
  fTolerance = dbRSPLink->GetD( "tolerance" );
  fRMax = dbRSPLink->GetD( "r_max" );
  fMeVValues = dbRSPLink->GetDArray( "energies" );
  fPredictedMeVValues = dbRSPLink->GetDArray( "predicted_energies" );
  fEventDirDotInitialLightVecValues = dbRSPLink->GetDArray( "event_dir_dot_initial_light_vec" );
  fDirectionDotVecSize = fEventDirDotInitialLightVecValues.size();
  fDirectionDotVecWidth = fEventDirDotInitialLightVecValues[1] - fEventDirDotInitialLightVecValues[0];
  fDirectionDotVecMin = fEventDirDotInitialLightVecValues[0];
  fDirectionDotVecMax = fEventDirDotInitialLightVecValues[fDirectionDotVecSize-1];
  fRadialValues = dbRSPLink->GetDArray( "radii" );
  fRadialSize = fRadialValues.size();
  fRadialWidth = fRadialValues[1] - fRadialValues[0];
  fRadialMin = fRadialValues[0];
  fRadialMax = fRadialValues[fRadialSize-1];
  fAngularValues = dbRSPLink->GetDArray( "angularValues" );
  fNphotonValues = dbRSPLink->GetDArray( "nphotons_energy" );
  fCerenkovAngularDist = dbRSPLink->GetDArray( "cerenkov_angular_dist" );
  fRayleighLateProb = dbRSPLink->GetDArray( "rayleigh_late_prob" );
  fAngularResponse = dbRSPLink->GetDArray( "pmt_angular_response" );
  fAngularResponseSpline = new TSpline3("f_ang_spline",&fAngularValues[0],&fAngularResponse[0],fAngularValues.size());
  fPhotonScalingFactor = 1;
  fPredictedTableExists = true;
  try{ fPredictedNphotonValues = dbRSPLink->GetDArray( "predicted_nphotons_energy" );
     fPhotonScalingFactor = fNphotonValues[4] / fPredictedNphotonValues[18]; // Set scaling factor to value at 5 MeV
     for( size_t i=0; i<fPredictedNphotonValues.size(); i++){
        fPredictedNphotonValues[i] *= fPhotonScalingFactor;
     }
  }
  catch( DBNotFoundError& e ){
    fPredictedTableExists = false;
    warn << "EnergyRSP::BeginOfRun: predicted_nphotons_energy table not in FIT_ENERGY_RSP.ratdb" << endl;
  } // Set flag false if predicted photons table does not exist. Needs to be created by the fit coordinator (part 2) and copied to FIT_ENERGY_RSP.ratdb.

  fNumChannels = db->GetLink( "PMT_DQXX" )->GetIArray( "dqch" ).size(); // Number of channels
  std::vector<double> ppmt_noise_rate;
  bool table_found = false;
  DBLinkPtr noiseDB;
  try {
    noiseDB = db->GetLink( "NOISE_RUN_INTEGRATED" );
    table_found = true;
  }
  catch( DBNotFoundError& e ){
    warn << "EnergyRSP::BeginOfRun: Integrated noise table not found for this run. Attempting to use less precise low statistics NOISE_RUN_LEVEL" << endl;
  }
  if (!table_found) {
    try {
      noiseDB = db->GetLink( "NOISE_RUN_LEVEL" );
    }
    catch( DBNotFoundError& e ){
      Log::Die("EnergyRSP::BeginOfRun: Failed to find noise tables for this run. Can't estimate the noise level.");
    }
  }

  // Grab the per-pmt noise level
  ppmt_noise_rate = noiseDB->GetDArray("perpmt_noiserate");
  fNoiseRate = 0.0;
  const DU::PMTCalStatus& PMTCalStatus = DU::Utility::Get()->GetPMTCalStatus();
  const DU::PMTInfo& pmtInfo = DU::Utility::Get()->GetPMTInfo();

  for( int lcn = 0; lcn < fNumChannels; lcn++ ) {
    if( pmtInfo.GetType( lcn ) != DU::PMTInfo::NORMAL && pmtInfo.GetType( lcn ) != DU::PMTInfo::HQE)
      continue;
    fNoiseRate += (ppmt_noise_rate[lcn]/s) *PMTCalStatus.GetChannelStatus(lcn)* ( twHighLimit - twLowLimit);
  }
  info << "EnergyRSP::BeginOfRun : Noise rate estimated to be " << fNoiseRate << " in prompt time window [" << twLowLimit << "," << twHighLimit << "]" << newline;
#ifdef RSP_DEBUG
  size_t totalActiveChannels = 0;
  for( int lcn = 0; lcn < fNumChannels; lcn++ ) {
    if( pmtInfo.GetType( lcn ) != DU::PMTInfo::NORMAL && pmtInfo.GetType( lcn ) != DU::PMTInfo::HQE)
      continue;
    if (PMTCalStatus.GetChannelStatus( static_cast<int>(lcn) ) > 0.0) {
      totalActiveChannels++;
    }
  }
  double fNoiseRateOld = db->GetLink( "NOISE_RUN_LEVEL" )->GetD( "average_noiserate" ) / s; // Noise rate
  fNoiseRateOld = fNoiseRateOld * ( twHighLimit - twLowLimit ) * ns * static_cast<double>(totalActiveChannels);
  info << "EnergyRSP::BeginOfRun : Old noise rate estimated to be " << fNoiseRateOld << " in prompt time window [" << twLowLimit << "," << twHighLimit << "]" << newline;
#endif
  // PMT model parameters
  DBLinkPtr pmtModelTable = DB::Get()->GetLink( "PMT_OPTICAL_PARAMETERS", "PMTOptics0_0" );
  fCollectionEfficiency = pmtModelTable->GetD( "collection_efficiency" );
  fPEEscapeProbability = Optics::LoadWavelengthPropertyVector( "pe_escape_probability", pmtModelTable );

  // Absorption and Rayleigh scattering lengths
  string innerAVMaterial = db->GetLink("GEO","inner_av")->GetS("material");
  string avMaterial = db->GetLink("GEO","av")->GetS("material");
  string cavityMaterial = db->GetLink("GEO","cavity")->GetS("material");
  LoadOpticsVectors(innerAVMaterial, &fInnerAV_ABSLENGTH, &fInnerAV_RSLENGTH);
  LoadOpticsVectors(avMaterial, &fAV_ABSLENGTH, &fAV_RSLENGTH);
  LoadOpticsVectors(cavityMaterial, &fCavity_ABSLENGTH, &fCavity_RSLENGTH);

  fInnerAVRadius = db->GetLink( "SOLID", "acrylic_vessel_inner" )->GetD( "r_sphere" );
}


void
EnergyRSP::EndOfRun( DS::Run& )
{
  fMeVValues.clear();
  fPredictedMeVValues.clear();
  fEventDirDotInitialLightVecValues.clear();
  fRadialValues.clear();
  fAngularValues.clear();
  fNphotonValues.clear();
  fCerenkovAngularDist.clear();
  fRayleighLateProb.clear();
  fAngularResponse.clear();
  fPredictedNphotonValues.clear();

  fDistInInnerAV.clear();
  fDistInAV.clear();
  fDistInWater.clear();
  fCosThetan.clear();
  fInitialLightVec.clear();
  fCerenkovSpline.clear();
  if(fAngularResponseSpline)
    delete fAngularResponseSpline;
}

void
EnergyRSP::DefaultSeed()
{
  // Radius = 0.0, direction = -1z, time = 0.0
  DS::FitVertex vertex;
  vertex.SetPosition( TVector3( 0.0, 0.0, 0.0 ), false, true );
  vertex.SetDirection( TVector3( 0.0, 0.0, -1.0 ), false, true );
  vertex.SetEnergy( 1.0 , false, true );
  vertex.SetEnergyErrors( 1.0, false, true );
  vertex.SetTime( 0.0, false, true );
  fSeedResult.SetVertex( 0, vertex );
}

DS::FitResult
EnergyRSP::GetBestFit()
{

  fFitResult.Reset();
  if( fPMTData.empty() )
    return fFitResult;
  CopySeedToResult();

  DS::FitVertex fitVertex = fFitResult.GetVertex( 0 );

  // Get reconstructed event information from seed
  // True values used by fit coordinator (part 2) to generate table of predicted Cerenkov photons
  if( fPredictedTableExists ){
    fSeedEnergy = fitVertex.GetEnergy();
    fEventPosition = fitVertex.GetPosition();
    fEventDirection = fitVertex.GetDirection();
  }
  else{
    fSeedEnergy = fDS->GetMC().GetMCParticle(0).GetKineticEnergy();
    fEventPosition = fDS->GetMC().GetMCParticle(0).GetPosition();
    fEventDirection = fDS->GetMC().GetMCParticle(0).GetMomentum().Unit();
  }

  // Validity flag
  // If the seed is bad reset it
  bool valid = true;

  if ( fEventPosition.Mag() > fRMax ) {
    valid = false; // Position outside maximum radius
    fitVertex.SetEnergyValid( valid ); // Leave energy as seed but set validity 'false'
  }

  if (!fitVertex.ValidEnergy()) {
    fitVertex.SetEnergy(1.0, true);
    fitVertex.SetEnergyErrors(0.0);
    fitVertex.SetEnergyValid(false);
  }

  if( valid ){ // Only perform EnergyRSP if seed position is inside max radius

    const DU::PMTInfo& pmtInfo = DU::Utility::Get()->GetPMTInfo();
    // Check number of channels match
    if( fNumChannels != pmtInfo.GetCount() )
      warn << "EnergyRSP::GetBestFit: Channel numbers do not match!!!" << newline;

    // Get prompt PMTs
    fTimeResidualCut->SetD( "lowLimit", twLowLimit );
    fTimeResidualCut->SetD( "highLimit", twHighLimit );
    fTimeResidualCut->SetI( "useGroup", 1 );
    if(fUseRefracted)
      fTimeResidualCut->SetI( "useRefracted", 1 );
    const vector<FitterPMT> promptPMTs = fTimeResidualCut->GetSelectedPMTs( fPMTData, fitVertex );
    const vector<FitterPMT> promptCalPMTs = fPMTCalSelector->GetSelectedPMTs( promptPMTs, fitVertex );
    unsigned int promptNhits = 0;
    for( unsigned int pmt = 0; pmt < promptCalPMTs.size(); pmt++ ){
      unsigned int lcn = promptCalPMTs[pmt].GetID();
      if( pmtInfo.GetType( lcn ) != DU::PMTInfo::NORMAL &&
          pmtInfo.GetType( lcn ) != DU::PMTInfo::HQE)
        continue;
      promptNhits++;
    }

    if (promptNhits == 0) {
      // -- Return a bad result of there are no promptNhits
      fitVertex.SetEnergy(0.0, false);
      fitVertex.SetEnergyErrors(0.0);
      fitVertex.SetEnergyValid(false);
      fFitResult.SetVertex( 0, fitVertex );
      return fFitResult;
    }
    // Get initial estimate for Cerenkov photons
    // If fit coordinator (part 2) has not generated table of predicted Cerenkov photons, use table of true Cerenkov photons to seed method.
    double nPhotons = 0.0;
    if( fPredictedTableExists )
      nPhotons = Interpolate1D( fSeedEnergy, fPredictedMeVValues, fPredictedNphotonValues );
    else
      nPhotons = Interpolate1D( fSeedEnergy, fMeVValues, fNphotonValues );

    // Find active channels and calculate wavelength independent factors of PMT response
    const DU::PMTCalStatus& PMTCalStatus = DU::Utility::Get()->GetPMTCalStatus();
    unsigned int totalActiveChannels = 0;
    vector<bool> activeChannels;
    vector<double> wavelengthIndependentPMTResponse;
    activeChannels.assign( fNumChannels, false );
    wavelengthIndependentPMTResponse.assign( fNumChannels, 0.0 );
    fDistInInnerAV.assign( fNumChannels, 0.0 );
    fDistInAV.assign( fNumChannels, 0.0 );
    fDistInWater.assign( fNumChannels, 0.0 );
    fCosThetan.assign( fNumChannels, 0.0 );
    fInitialLightVec.assign( fNumChannels, TVector3( 0.0, 0.0, 0.0 ) );

    for( size_t lcn = 0; lcn < fNumChannels; lcn++ ){
      if( pmtInfo.GetType( lcn ) != DU::PMTInfo::NORMAL &&
          pmtInfo.GetType( lcn ) != DU::PMTInfo::HQE)
        continue;
      double fractionGoodCells = PMTCalStatus.GetChannelStatus( static_cast<int>(lcn) );
      if( fractionGoodCells ){
          totalActiveChannels++;
          activeChannels[ lcn ] = true;
          const TVector3 pmtPosition = pmtInfo.GetPosition( lcn );
          const TVector3 pmtDirection = pmtInfo.GetDirection( lcn );
          double wiresponse = wiPMTResponse(pmtPosition,pmtDirection,lcn);
          if( wiresponse <= 0.0 )
            wavelengthIndependentPMTResponse[ lcn ] = 0.0;
          else
            wavelengthIndependentPMTResponse[ lcn ] = wiresponse * fractionGoodCells;
        }
      }

    // Calculate effective nhits
    double nEff = 1.0e-6;

    if( static_cast<double>(promptNhits) > fNoiseRate) {
      nEff = static_cast<double>(promptNhits) - fNoiseRate;
    } else {
      debug << "EnergyRSP::GetBestFit:: Effective hits " << promptNhits << " less than noise rate " << fNoiseRate << "! Setting result invalid." << newline;
      fitVertex.SetEnergy(0.0, false);
      fitVertex.SetEnergyErrors(0.0);
      fitVertex.SetEnergyValid(false);
      fFitResult.SetVertex( 0, fitVertex );

      return fFitResult;
    }

    // Modify estimated Cerenkov photons until efective prompt hits and predicted hits match
    double nPredicted = 1e-6;
    bool first = true;
    unsigned int it = 0;
    double eventEnergy = fSeedEnergy;

    // To match RSP in SNO save 6 FOMs to try to check how the different scalings contribute
    // Also add in the rayleigh scattering coeff, and the final response
    double finalResponse = 0.0;
    double preMPEResponse = 0.0;
    double attnInnerAV = 0;
    double attnAV = 0;
    double attnCavity = 0;
    double mpeEffect = 0;
    double rayleighCoeff = 0;
    double pmtAngularResponse = 0;

    // Calculate response for each active PMT
    vector<double> responsePerPMT(fNumChannels, 0.0);

    while( (first || abs(100*(nEff*fPhotonScalingFactor/nPredicted)-100.0) > fTolerance) && it < fMaxIterations ){
      first = false;
      double responseNoAttn = 0.0;
      double responseNoInnerAV = 0.0;
      double responseNoCavity = 0.0;
      double responseNoAV = 0.0;
      double responseNoRayleigh = 0.0;

      // Generate cubic spline of Cerenkov angular distribution
      fCerenkovSpline = GenerateCerenkovSpline( eventEnergy );

      for( size_t lcn = 0; lcn < fNumChannels; lcn++ ){
        if( !activeChannels[ lcn ] )
          continue;
        double cherPMT = CerenkovAngularDist(lcn);
        double response = 0.0;
        double pmtResponseNoAttn = 0.0;
        double pmtResponseNoInnerAV = 0.0;
        double pmtResponseNoCavity = 0.0;
        double pmtResponseNoAV = 0.0;
        double pmtResponseNoRayleigh = 0.0;
        double oneOverWl2 = 0.0;

        for( double wl = 220.0; wl <= 710.0; wl = wl+10.0 ){ // wavelength in nm
          double absInnerAV = 0, absAV = 0, absCavity = 0, rayleigh = 0;
          double energy = RAT::util::WavelengthToEnergy(wl*nm);
          double pmtResp = PMTEfficiency(energy, fCosThetan[lcn], lcn);
          double wl2 = wl*wl;
          double attenuation = Attenuation(energy, lcn, absInnerAV, absAV, absCavity, rayleigh);

          response += pmtResp * attenuation / wl2;
          pmtResponseNoAttn += (pmtResp / wl2);
          pmtResponseNoInnerAV += (pmtResp * absAV * absCavity * rayleigh) / wl2;
          pmtResponseNoAV += (pmtResp * absInnerAV * absCavity * rayleigh) / wl2;
          pmtResponseNoCavity += (pmtResp * absInnerAV * absAV * rayleigh) / wl2;
          pmtResponseNoRayleigh += (pmtResp * absInnerAV * absAV * absCavity) / wl2;

          // To get the individual components need to back out the pmtresponse here
          // and then the angular response below (wavelength independent)
          oneOverWl2 += 1.0 /(wl2);
        } // wl
        double wicher = wavelengthIndependentPMTResponse[ lcn ] * cherPMT;
        response *= wicher;
        response /= oneOverWl2;
        pmtResponseNoAttn *= wicher / oneOverWl2;
        pmtResponseNoInnerAV *= wicher / oneOverWl2;
        pmtResponseNoAV *= wicher / oneOverWl2;
        pmtResponseNoCavity *= wicher / oneOverWl2;
        pmtResponseNoRayleigh *= wicher / oneOverWl2;
        if( response <= 0.0 || std::isnan(response))
          responsePerPMT[lcn] = 0.0;
        else {
          responsePerPMT[lcn] = response;
          responseNoAttn += pmtResponseNoAttn;
          responseNoInnerAV += pmtResponseNoInnerAV;
          responseNoAV += pmtResponseNoAV;
          responseNoCavity += pmtResponseNoCavity;
          responseNoRayleigh += pmtResponseNoRayleigh;
        }

      } // lcn

      nPredicted = 0.0;
      finalResponse = 0.0;
      preMPEResponse = 0.0;
      // Sum estimated photons registering hits for each PMT (corrected for multi-photons)
      mpeEffect = 0.0;
      for( unsigned int pmt = 0; pmt < responsePerPMT.size(); pmt++ ){
        double pmtMPEEffect = MultiPhotonCorrector(nPhotons * responsePerPMT[pmt]);
        preMPEResponse += responsePerPMT[pmt];
        responsePerPMT[pmt] = responsePerPMT[pmt] * pmtMPEEffect;
        finalResponse += responsePerPMT[pmt];
        mpeEffect += responsePerPMT[pmt];
        nPredicted += nPhotons * responsePerPMT[pmt];
      }
      mpeEffect /= preMPEResponse;

      attnInnerAV = preMPEResponse / responseNoInnerAV;
      attnAV = preMPEResponse /responseNoAV;
      attnCavity = preMPEResponse / responseNoCavity;
      rayleighCoeff = preMPEResponse / responseNoRayleigh;
      pmtAngularResponse = responseNoAttn;

      // Modify number of photons
      nPhotons *= nEff * fPhotonScalingFactor / nPredicted;
      // Re-calculate event energy, except when generating table of predicted Cerenkov photons in fit coordinator (part 2)
      if( fPredictedTableExists )
        eventEnergy = Interpolate1D( nPhotons, fPredictedNphotonValues , fPredictedMeVValues);
      it++;
    } // while
    // Clear vectors for next event
    fDistInInnerAV.clear();
    fDistInAV.clear();
    fDistInWater.clear();
    fCosThetan.clear();
    fInitialLightVec.clear();


    // Calculate additional figures of merit

    // Loops over all active channels

    // Determine median probability of all active PMTs
    const unsigned int numChannels = totalActiveChannels;
    float probPMT[numChannels];
    double sumProb = 0;
    unsigned int i = 0;
    for( size_t lcn = 0; lcn < fNumChannels; lcn++ ){
      if( !activeChannels[ lcn ] )
        continue;
      probPMT[i] = responsePerPMT[lcn];
      sumProb += responsePerPMT[lcn];
      i++;
    }
    qsort( probPMT, numChannels, sizeof(float), Compare );
    float medianProb = probPMT[numChannels/2];

    // Determine median absolute deviation and rank of all active PMTs
    vector<float> absDevPMT(numChannels, 0.0);
    vector<float> rankPMT(numChannels, 1.0);
    unsigned int firstPMT = 0, lastPMT = 0;
    float mean = 1.0;
    for( unsigned int pmt = 0; pmt < numChannels-1; pmt++ ){

      // save absolute deviation
      absDevPMT[pmt] = abs( probPMT[pmt] - medianProb );

      // rank PMT probabilities
      rankPMT[pmt] = (float)pmt + 1.0;

      // equivalent probabilities are assigned a mean rank
      if( probPMT[pmt+1]==probPMT[pmt] ) {
        lastPMT = pmt+1;
        mean += (float)lastPMT + 1.0;
      }
      else {
        if( firstPMT < lastPMT ) {
          mean /= (lastPMT - firstPMT + 1);
          for( unsigned int n = firstPMT; n <= lastPMT; n++ )
            rankPMT[n] = mean;
        }
        firstPMT = pmt+1;
        mean = (float)firstPMT + 1.0;
      }

    }
    // assign last elements
    absDevPMT[numChannels-1] = abs( probPMT[numChannels-1] - medianProb );
    rankPMT[numChannels-1] = (float)numChannels;
    if( firstPMT < lastPMT ) {
      mean /= (lastPMT - firstPMT + 1);
      for( unsigned int n = firstPMT; n <= lastPMT; n++ )
        rankPMT[n] = mean;
    }
    std::nth_element(absDevPMT.begin(), absDevPMT.begin() + absDevPMT.size()/2, absDevPMT.end());
    float medianDev = absDevPMT[absDevPMT.size()/2];

    // Loops over hit channels

    float medianProbHit = 0.0;
    float medianDevHit = 0.0;
    double sumGtest = 0.0, sumRank = 0.0;
    const unsigned int numHits = promptNhits;
    if( numHits > 0 ) {

      // Determine median probability of hit PMTs
      vector<float> probPMThit(numHits, 0.0);
      i=0;
      for( unsigned int pmt = 0; pmt < promptCalPMTs.size(); pmt++ ){
        unsigned int lcn = promptCalPMTs[pmt].GetID();
        if( pmtInfo.GetType( lcn ) != DU::PMTInfo::NORMAL &&
            pmtInfo.GetType( lcn ) != DU::PMTInfo::HQE )
          continue;
        probPMThit[i] = responsePerPMT[lcn];
        i++;
      }
      std::nth_element(probPMThit.begin(), probPMThit.begin() + probPMThit.size()/2, probPMThit.end());
      medianProbHit = probPMThit[ probPMThit.size()/2 ];

      // Determine median absolute deviation of hit PMTs (and calculate G-test and U-test sums)
      vector<float> absDevPMThit(probPMThit.size(), 0.0);
      float prob = 0.0;
      double norm = nEff/sumProb;  // to normalize channel probabilities to the number of hits
      for( unsigned int pmt = 0; pmt < numHits; pmt++ ){
        prob = probPMThit[pmt];
        absDevPMThit[pmt] = abs( prob - medianProbHit );
        if( prob>0 )
          sumGtest += log( 1./(norm*prob) );
        for( int n = numChannels-1; n>=0; n-- ){
          if( probPMT[n]==prob ){
            sumRank += rankPMT[n];
            break;
          }
        }
      }
      std::nth_element(absDevPMThit.begin(), absDevPMThit.begin() + absDevPMThit.size()/2, absDevPMThit.end());
      medianDevHit = absDevPMThit[ absDevPMThit.size()/2 ];
    }


    // Store Figures Of Merit
    fFitResult.SetFOM( "promptHits", promptNhits );
    fFitResult.SetFOM( "nCerPhotons", nPhotons );
    fFitResult.SetFOM( "MedianProb", medianProb );
    fFitResult.SetFOM( "MedianProbHit", medianProbHit );
    fFitResult.SetFOM( "MedianDev", medianDev );
    fFitResult.SetFOM( "MedianDevHit", medianDevHit );
    if( numHits > 0 ) {
      fFitResult.SetFOM( "Gtest", sumGtest/numHits );
      fFitResult.SetFOM( "Utest", (sumRank-numHits*(numHits-1)/2.)/numHits/(numChannels-numHits) );
    } else {
      fFitResult.SetFOM( "Gtest", 12.0 );  // "impossible" large value
      fFitResult.SetFOM( "Utest", 1.1 );  // impossible value
    }
    if(fStoreDiagnostics) {
      fFitResult.SetFOM( "finalResponse", finalResponse );
      fFitResult.SetFOM( "mpeEffect", mpeEffect );
      fFitResult.SetFOM( "attnInnerAV", attnInnerAV );
      fFitResult.SetFOM( "attnAV", attnAV );
      fFitResult.SetFOM( "attnCavity", attnCavity );
      fFitResult.SetFOM( "rayleighCoeff", rayleighCoeff );
      fFitResult.SetFOM( "pmtAngularResponse", pmtAngularResponse );
    }

    // Check if result is valid
    if( eventEnergy < 0.26 ){
      debug << "EnergyRSP::GetBestFit: Energy below Cerenkov threshold." << newline;
      // Keep result because of large resolution at low energies
    }
    if( eventEnergy < 0.0 || !isfinite(eventEnergy) ){
      debug << "EnergyRSP::GetBestFit: Fit energy negative, infinite, or nan. Setting result invalid." << newline;
      valid = false;
    }
    if( it == fMaxIterations ){
      debug << "EnergyRSP::GetBestFit: Reached maximum iterations." << newline;
      valid = false;
    }
    if( !fPredictedTableExists ){
      debug << "EnergyRSP::GetBestFit: Predicted Cerenkov photons per MeV table does not exist." << newline;
      valid = false;
    }

    // Set fit result
    fitVertex.SetEnergy( eventEnergy, valid );
    fitVertex.SetEnergyErrors( 1.0 );

  } // if( valid )
  fFitResult.SetVertex( 0, fitVertex );
  return fFitResult;
}

double
EnergyRSP::wiPMTResponse( const TVector3& pmtPos,
                          const TVector3& pmtDir,
                          const int pmtID )
{
  // Calculate light path
  if(!fUseRefracted)
    fLightPath.CalcByPosition( fEventPosition, pmtPos );
  else
    fLightPath.CalcByPosition( fEventPosition, pmtPos, 3.103125 * 1e-6, 10.0 );

  TVector3 initialLightVec = fLightPath.GetInitialLightVec();
  double distInInnerAV = fLightPath.GetDistInInnerAV();
  double distInAV = fLightPath.GetDistInAV();
  double distInWater = fLightPath.GetDistInWater();

  fLightPath.CalculateSolidAngle( pmtDir, 0 );
  double solidAngle = fLightPath.GetSolidAngle();
  double cosThetaPMT = fLightPath.GetCosThetaAvg();

  fLightPath.CalculateFresnelTRCoeff();
  double fresnelTCoeff = fLightPath.GetFresnelTCoeff();

  // Store values needed for later
  fDistInInnerAV[ pmtID ] = distInInnerAV;
  fDistInAV[ pmtID ] = distInAV;
  fDistInWater[ pmtID ] = distInWater;
  fCosThetan[ pmtID ] = cosThetaPMT;

  fInitialLightVec[ pmtID ] = initialLightVec;

  // Get Channel Efficiency
  double chanEff = fChannelEfficiency->GetChannelEfficiency( pmtID );

  // Wavelength independent factors for PMT response:
  // PMT solid angle
  // Cerenkov angular distribution
  // Fresnel transmission probability
  // Channel Efficiency
  double response = ( solidAngle / (4*pi) ) * fresnelTCoeff * chanEff;

  return response;
}


double
EnergyRSP::CerenkovAngularDist( const int pmtID )
{
  double eventDirDotInitialLightVec = fEventDirection.Unit().Dot(fInitialLightVec[ pmtID ].Unit());
  double scalefactor = Interpolate1DSpline( eventDirDotInitialLightVec, fEventDirDotInitialLightVecValues, fCerenkovSpline );
  if( scalefactor > 0.0 )
    return scalefactor;
  else
    return 0.0;
}

double
EnergyRSP::PMTEfficiency( const double energy,
                          const double CosThetan,
                          const int pmtID )
{
  double angle = acos( -1.0 * CosThetan ) / CLHEP::degree;
  double angularResponse = 0;
  angularResponse = fAngularResponseSpline->Eval(angle);

  // PMT efficiency:
  // Photoelectron escape probability
  // PMT collection efficiency
  // PMT angular response
  // Relative PMT efficiency
  double pmtEfficiency = 0;
  if( angularResponse > 0.0 )
    pmtEfficiency = fPEEscapeProbability->Value( energy ) * fCollectionEfficiency * angularResponse * fPMTVariation->GetVariation( pmtID );
  return pmtEfficiency;
}

double
EnergyRSP::Attenuation( const double energy,
                        const int pmtID,
                        double& absInnerAV,
                        double& absAV,
                        double& absCavity,
                        double& rayleigh )
{
  // Get absorption and Rayleigh scattering lengths
  double innerAVAbsLength = fInnerAV_ABSLENGTH->Value( energy );
  double innerAVRSLength = fInnerAV_RSLENGTH->Value( energy );
  double avAbsLength = fAV_ABSLENGTH->Value( energy );
  double avRSLength = fAV_RSLENGTH->Value( energy );
  double cavityAbsLength = fCavity_ABSLENGTH->Value( energy );
  double cavityRSLength = fCavity_RSLENGTH->Value( energy );

  // Get distances travelled in each medium
  double distInInnerAV = fDistInInnerAV[ pmtID ];
  double distInAV = fDistInAV[ pmtID ];
  double distInWater = fDistInWater[ pmtID ];

  // Calculate absorption
  absInnerAV = exp(-distInInnerAV / innerAVAbsLength);
  absAV = exp(-distInAV / avAbsLength);
  absCavity = exp(-distInWater / cavityAbsLength);
  double absorption = absInnerAV * absAV * absCavity;

  // Get probability that a Rayleigh scattered photon will be late
  double initialLightVecDocEventPos = fInitialLightVec[ pmtID ].Unit().Dot(fEventPosition.Unit());
  double RayleighLateProb = RayleighLookup(pow(fEventPosition.Mag()/fInnerAVRadius, 3), initialLightVecDocEventPos);

  // Calculate Rayleigh scattering
  rayleigh = 0;
  if( RayleighLateProb >= 0.0 && RayleighLateProb <= 1.0 && fEventPosition.Mag() < fRMax) {
    rayleigh = 1 - ((1 - exp( - distInInnerAV/innerAVRSLength - distInAV/avRSLength - distInWater/cavityRSLength )) * RayleighLateProb);
  }
  else if( RayleighLateProb > 1.0 ){
    rayleigh = 1 - ((1 - exp( - distInInnerAV/innerAVRSLength - distInAV/avRSLength - distInWater/cavityRSLength )));
  }
  else if( RayleighLateProb < 0.0 ){
    rayleigh = 1;
  }

  // Calculate attenuation:
  // Absorption and Rayleigh scattering
  double attenuation = absorption * rayleigh;

  return attenuation;
}

double
EnergyRSP::MultiPhotonCorrector( const double photons )
{
  // Correct for multi-photons using a Poisson estimate
  double corrector = 0.0;
  if(photons>0){
    corrector = (1-exp(-photons))/photons;
  }
  else if(photons==0){
    corrector = 1;
  }
  else{
    debug << "EnergyRSP::MultiPhotonCorrector: Negative number of photons" << newline;
  }
  return corrector;
}

// 1D interpolation using TSpline3 method for selected values
double
EnergyRSP::Interpolate1DSpline( const double inValue,
                                const vector<double>& inTable,
                                const vector<double>& outTable )
{
  if( inTable.size() != outTable.size() ){
    debug << "EnergyRSP::Interpolate tables do not match in size" << newline;
  }

  // Find lowest inTable value greater than inValue
  int inTableEntry = distance( inTable.begin(), find_if( inTable.begin(), inTable.end(), bind2nd(greater<double>(),inValue) ) );

  // Set points to use
  vector<double> x(6, 0);
  vector<double> y(6, 0);
  int startEntry = inTableEntry - 3;
  int endEntry = inTableEntry + 2;
  int idx = 0;
  for(int i = startEntry; i <= endEntry; i++) {
    if((i >= 0) && (i < static_cast<int>(inTable.size()))) {
      x[idx] = inTable[i];
      y[idx] = outTable[i];
      idx++;
    }
  }

  if(idx!=6) {
    x.resize(idx);
    y.resize(idx);
  }

  TSpline3 Spline("spline",&x[0],&y[0],x.size());
  return Spline.Eval(inValue);
}

// 1D linear interpolation
double
EnergyRSP::Interpolate1D( const double inValue,
                          const vector<double>& inTable,
                          const vector<double>& outTable )
{
  if( inTable.size() != outTable.size() ){
    debug << "EnergyRSP::Interpolate tables do not match in size" << newline;
  }

  // Find lowest inTable value greater than inValue
  int inTableEntry = distance( inTable.begin(), find_if( inTable.begin(), inTable.end(), bind2nd(greater<double>(),inValue) ) );

  // If inValue < inTable.begin() use inTableEntry=1 and extrapolate from lowest two points
  if(inTableEntry==0) inTableEntry=1;

  // Return linear interpolation
  return (outTable[inTableEntry-1]*(inTable[inTableEntry]-inValue)+outTable[inTableEntry]*(inValue-inTable[inTableEntry-1]))/(inTable[inTableEntry]-inTable[inTableEntry-1]);
}

double
EnergyRSP::RayleighLookup( const double radius,
                           const double eventDotLightVec )
{
  // For some reason the data file bin values are bin centres!
  int radiusBin = (radius - (fRadialMin - fRadialWidth/2)) / fRadialWidth;
  if(radiusBin < 0) radiusBin = 0;
  else if(radiusBin > static_cast<int>(fRadialSize)-1) radiusBin = fRadialSize-1;

  int dotBin = (eventDotLightVec - (fDirectionDotVecMin - fDirectionDotVecWidth/2)) / fDirectionDotVecWidth;
  if(dotBin < 0) dotBin = 0;
  else if(dotBin > static_cast<int>(fDirectionDotVecSize)-1) dotBin = fDirectionDotVecSize-1;

  return fRayleighLateProb[radiusBin * fDirectionDotVecSize + dotBin];

}


// Generate vector of interpolated Cerenkov angular distribution
vector<double>
EnergyRSP::GenerateCerenkovSpline( const double energy )
{
  if( fEventDirDotInitialLightVecValues.size() * fMeVValues.size() != fCerenkovAngularDist.size() ){
    debug << "EnergyRSP::GenerateCerenkovSpline: Tables do not match in size" << newline;
  }

  // Create vector of interpolated Cerenkov angular distribution for seed energy
  vector<double> fCerenkovAngularDist_interp;
  for( unsigned int i=0; i<fEventDirDotInitialLightVecValues.size(); i++ ){
    vector<double> yValues;
    for( unsigned int j=0; j<fMeVValues.size(); j++ ){
      yValues.push_back( fCerenkovAngularDist[ (j * fEventDirDotInitialLightVecValues.size()) + i ] );
    }
    fCerenkovAngularDist_interp.push_back( Interpolate1DSpline(energy, fMeVValues, yValues) );
  }

  return fCerenkovAngularDist_interp;

}


void
EnergyRSP::LoadOpticsVectors(const string& materialName, G4MaterialPropertyVector** abslength, G4MaterialPropertyVector** rindex)
{
  DB *db = DB::Get();
  vector<string> properties = db->GetLink("OPTICS", materialName)->GetSArray("PROPERTY_LIST");
  for(vector<string>::const_iterator iProperty = properties.begin(); iProperty != properties.end(); ++iProperty)
    {
      if(*iProperty == "ABSLENGTH0" || *iProperty == "ABSLENGTH")
        (*abslength) = Optics::LoadWavelengthPropertyVector( *iProperty, db->GetLink("OPTICS",materialName) );
      if(*iProperty == "RSLENGTH0" || *iProperty == "RSLENGTH")
        (*rindex) = Optics::LoadWavelengthPropertyVector( *iProperty, db->GetLink("OPTICS",materialName) );
    }
}