#include <RAT/HoughDirection.hh>
#include <RAT/Processor.hh>
#include <RAT/Log.hh>

#include <RAT/DS/FitResult.hh>
#include <RAT/DS/FitVertex.hh>
#include <RAT/DU/Utility.hh>
#include <RAT/DU/PMTInfo.hh>
#include <RAT/DU/LightPathCalculator.hh>

#include <TH2F.h>
#include <TVector3.h>

#include <G4PhysicsOrderedFreeVector.hh>

#include <CLHEP/Units/PhysicalConstants.h>

using namespace CLHEP;
using namespace RAT;
using namespace RAT::Methods;

void HoughDirection::Initialise(const std::string&)
{
  // NLoop close to or greater than nbins will ensure
  // each bin through which a ring passes is filled
  fNLoop = 100;
  // NBins value around a few degrees, using Gene's original values,
  // is reasonable (given angular resolution we can hope to achieve).
  fNBinsPhi = 45;
  fNBinsTheta = 45;
  fPhotonWavelength = 400.0; // 400nm - could change for LAB vs H2O
  fPathAccuracy = 10.0; // 10mm, lightpathcalculator fine with this
}

void HoughDirection::SetI(const std::string& param, const int value)
{
  if(param == "nLoop")
    {
      if(value < 1)
        throw Processor::ParamInvalid(param, "must be >=1");
      fNLoop = value;
    }
  else if(param == "nPhi")
    {
      if(value < 1)
        throw Processor::ParamInvalid(param, "must be >=1");
      fNBinsPhi = value;
    }
  else if(param == "nBinsTheta")
    {
      if(value < 1)
        throw Processor::ParamInvalid(param, "must be >=1");;
      fNBinsTheta = value;
    }
  else
    throw Processor::ParamUnknown(param);
}

void HoughDirection::SetD(const std::string& param, const double value)
{
  if(param == "wavelength")
    {
      if(value <= 0)
        throw Processor::ParamInvalid(param, "must be >0");
      fPhotonWavelength = value;
    }
  else if(param == "pathAccuracy")
    {
      if(value <= 0)
        throw Processor::ParamInvalid(param, "must be >0");
      fPathAccuracy = value;
    }
  else
    throw Processor::ParamUnknown(param);
}

void HoughDirection::BeginOfRun(DS::Run&)
{
  fLightPathCalculator = DU::Utility::Get()->GetLightPathCalculator();
}

void HoughDirection::DefaultSeed()
{
  DS::FitVertex seedVertex;
  seedVertex.SetPosition(TVector3(0, 0, 0), false, true);
  // Use 2.5 MeV - assumes the default use case is for B8 rejection in ROI
  seedVertex.SetEnergy(2.5, false, true);
  fSeedResult.SetVertex(0, seedVertex);
}

DS::FitResult HoughDirection::GetBestFit()
{
  fFitResult.Reset();
  if(fPMTData.empty())
    throw MethodFailError("HoughDirection: no hits to fit");
  CopySeedToResult();

  SelectPMTData(fFitResult.GetVertex(0));
  fFitResult.SetFOM("SelectedNHit", static_cast<double>(fSelectedPMTData.size()));
  if(fSelectedPMTData.empty())
    throw MethodFailError("HoughDirection: no hits in selected PMT data");

  TVector3 seedPosition = fFitResult.GetVertex(0).GetPosition();
  double seedEnergy = fFitResult.GetVertex(0).GetEnergy();

  const DU::PMTInfo& pmtInfo = DU::Utility::Get()->GetPMTInfo();
  // CLHEP uses h_Planck as 4.14e-12 - in MeV.ns
  // CLHEP uses c_light as 299.792 - in mm / ns
  // Keep MeV, ns; convert mm (c_light) and nm (fPhotonWavelength) to m
  double photonEnergyMeV = h_Planck * 1e-3 * c_light / (fPhotonWavelength * 1e-9);
  double nRefraction = fLightPathCalculator.GetInnerAVRI(photonEnergyMeV);
  // Assumes we're looking at a single beta for the cherenkov angle
  double beta = sqrt(1.0 - (electron_mass_c2 / ( electron_mass_c2 + seedEnergy ) ) *
                     (electron_mass_c2 / (electron_mass_c2 + seedEnergy) ) );
  double cosThetaCherenkov = 1.0 / (nRefraction * beta);
  if(cosThetaCherenkov >= 1.0)
    throw MethodFailError("HoughDirection: energy below Cherenkov threshold");
  double thetaCherenkov = acos(cosThetaCherenkov);

  // Histogram filled with hough rings, from which direction is taken
  TH2F houghHist("houghHist", "", fNBinsPhi, -pi, pi, fNBinsTheta, 0, pi);

  // Loop through each selected PMT, draw a circle at the Cherenkov angle
  for(std::vector<FitterPMT>::const_iterator iPMT = fSelectedPMTData.begin();
      iPMT != fSelectedPMTData.end(); ++iPMT)
    {

      fLightPathCalculator.CalcByPosition(seedPosition, pmtInfo.GetPosition(iPMT->GetID()),
                                          photonEnergyMeV, fPathAccuracy);
      TVector3 emission = fLightPathCalculator.GetInitialLightVec();

      // Fill a ring for each hit PMT in the Hough histogram
      // with an opening angle at the cherenkov angle
      TVector3 testDirection(0, 0, 0);
      testDirection.SetMagThetaPhi(1, emission.Theta() + thetaCherenkov, emission.Phi());

      for(int iLoop = 0; iLoop < fNLoop; iLoop++)
        {

          testDirection.Rotate(2*pi/fNLoop, emission);
          houghHist.Fill(testDirection.Phi() * sin(testDirection.Theta()), testDirection.Theta());

        }

    }

  // Just take the position of the maximal bin (phi, theta) as the fitted direction
  int binx;
  int biny;
  int binz;
  houghHist.GetBinXYZ(houghHist.GetMaximumBin(), binx, biny, binz);
  // Y is theta, X is phi
  TVector3 fitDirection(0, 0, 0);
  fitDirection.SetMagThetaPhi(1, houghHist.GetYaxis()->GetBinCenter(biny),
                              houghHist.GetXaxis()->GetBinCenter(binx));

  DS::FitVertex vertex = fFitResult.GetVertex(0);
  vertex.SetDirection(fitDirection);
  vertex.SetPositiveDirectionError(TVector3(1, 1, 1)); // FIXME: set useful error
  vertex.SetNegativeDirectionError(TVector3(1, 1, 1)); // FIXME: set useful errors
  fFitResult.SetVertex(0, vertex);

  return fFitResult;

}