#include <RAT/MultiPathWaterPosition.hh>
#include <CLHEP/Random/RandFlat.h>
#include <CLHEP/Units/SystemOfUnits.h>
#include <CLHEP/Units/PhysicalConstants.h>
#include <RAT/DB.hh>
#include <RAT/DU/Utility.hh>
#include "TFile.h"
#include "TH1F.h"
#include "TMath.h"

//#define DEBUG

using namespace RAT;
using namespace ROOT;

MultiPathWaterPosition::MultiPathWaterPosition() {
  // parameter definitions
  fPars.push_back(Parameter("x",2000,-10000.0,10000.0));
  fPars.push_back(Parameter("y",2000,-10000.0,10000.0));
  fPars.push_back(Parameter("z",2000,-10000.0,10000.0));
  fPars.push_back(Parameter("t",400,-100.0,300.0));
}

void MultiPathWaterPosition::BeginOfRun(DS::Run&) {
#ifdef MULTIPATH_FIXED_SEED
  //step = 0.0001;
  step = 0.5001;
#endif
  DB* db = DB::Get();
  DBLinkPtr dbLink = db->GetLink( "FIT_MULTIPATH" );
  fPDFDataCh = dbLink->GetDArray( "sPDF_multipathfit" );
  double fWaterRI = dbLink->GetD( "water_RI" );
  double fChBinWidth = dbLink->GetD( "time_bin_width" );
  double fChOffset = dbLink->GetD( "time_offset" );
  fMaxPMTtRes = dbLink->GetD( "max_PMT_tRes" );
  double r = dbLink->GetD( "psup_rad" );
  fPSUPRadius2 = r * r;

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

  fEntriesTimeCh = fPDFDataCh.size();
  double xtemp = fPDFDataCh[0];
  int j;
  for( j = fEntriesTimeCh-1; j >= 0; j-- ) {
    if( fPDFDataCh[j] == fPDFDataCh[0] ) {
      fPDFDataCh[j] = xtemp;
      xtemp -= 0.001;
    }
  }

  xtemp = fPDFDataCh[ fEntriesTimeCh-1 ];

  for( j = fEntriesTimeCh-1; fPDFDataCh[j] == xtemp; j-- );

  for( j++; j < fEntriesTimeCh; j++ ) {
    fPDFDataCh[j] = xtemp;
    xtemp -= 0.001;
  }

  for( j = 0; j < fEntriesTimeCh; j++ ) {
    fPDFxCh.push_back(fChBinWidth * static_cast<double>(j + 1) - fChOffset);
    fPDFCh.push_back(fPDFDataCh[j]);
  }

  for( j = 0; j < fEntriesTimeCh - 1; j++ )
    fDerivativeCh.push_back( ( fPDFCh[j + 1] - fPDFCh[j] ) / fChBinWidth );

#ifdef DEBUG
  //Check the fitting pdfs and their derivatives for any event
  TFile f("pdfDerivatives_MultiPathWaterPosition.root","RECREATE");
  TH1F h1("h1","pdfCherenkov",1600,1.0,1600.0);
  TH1F h1x("h1x","pdfXCh",1600,1.0,1600.0);
  TH1F dh1("dh1","DerivativeCherenkov",1600,1.0,1600.0);

  for( j = 0; j < fEntriesTimeCh - 1 ; j++ ) {
    h1.SetBinContent(j+1, fPDFCh[j]);
    h1x.SetBinContent(j+1, fPDFxCh[j]);
    dh1.SetBinContent(j+1, fDerivativeCh[j]);
  }

  f.cd();
  h1.Write();
  h1x.Write();
  dh1.Write();
  f.Close();
#endif // DEBUG
}

void MultiPathWaterPosition::Initialise(const std::string&) {
}

void MultiPathWaterPosition::SetSeed(const DS::FitResult&) {
}

DS::FitResult MultiPathWaterPosition::MakeResult(const std::vector<double>& pars) {
  TVector3 fittedVertex(pars[0], pars[1], pars[2]);
  DS::FitVertex vertex;
  vertex.SetPosition(fittedVertex);
  vertex.SetTime(pars[3]);
  DS::FitResult result;
  result.SetVertex(0, vertex);
  return result;
}

bool MultiPathWaterPosition::ValidResult(const std::vector<double>& pars) {
  double r2 = pars[0]*pars[0] + pars[1]*pars[1] + pars[2]*pars[2];
  return r2 < fPSUPRadius2;
}

std::vector<double> MultiPathWaterPosition::GetStart() {
  // Could set up a seed here, but I'm not using it for now (from aTime and aVertex)
#ifdef MULTIPATH_FIXED_SEED
  double ran0 = step;
  double ran1 = 2.0*step - 1.0;
  double ran2Pi = step*2.0*CLHEP::pi;
  double t = 100.0 + 200.0*step;
  step += 0.0001;
  if (step > 1.0) step = 0.0001;
#else
  double ran0 = CLHEP::RandFlat::shoot( 0.0, 1.0 );
  double ran1 = CLHEP::RandFlat::shoot( -1.0, 1.0 );
  double ran2Pi = CLHEP::RandFlat::shoot( 0.0, 2.0*CLHEP::pi );
  double t = CLHEP::RandFlat::shoot(100.0,300.0);
#endif

  double r = pow(ran0, 1.0/3.0) * 10000.0; // mm
  double costheta = ran1;
  double sintheta = sqrt(1.0 - costheta*costheta);
  std::vector<double> v;
  v.push_back(r*sintheta*cos(ran2Pi));
  v.push_back(r*sintheta*sin(ran2Pi));
  v.push_back(r*costheta);
  // in principle, MC and data could have different PMT hit time offset,
  // but in latest incarnation of MultiPathFitter code, this doesn't seem to be the case
  v.push_back(t);
  return v;
}

void MultiPathWaterPosition::LAnddLdt_Ch(double& L_Ch, double& dLdt_Ch) {
  if (fRes > fMaxPMTtRes) fRes = fMaxPMTtRes; //max_PMT_tRes
  else if (fRes < -fMaxPMTtRes) fRes = -fMaxPMTtRes;

  int ibin = (int) floor( ( fRes + 100.0 ) * 4.0 );
  if(ibin > 0 && ibin < fEntriesTimeCh - 2) {
    double dt = fRes-fPDFxCh[ibin];
    L_Ch = (fPDFCh[ibin] + fDerivativeCh[ibin] * dt);
    if (dt < 0.125) {
      double u = 0.5 + dt * 4.0;
      dLdt_Ch = -u*fDerivativeCh[ibin] - (1.0-u)*fDerivativeCh[ibin-1];
    } else {
      double u = 4.0 * dt - 0.5;
      dLdt_Ch = -(1.0-u)*fDerivativeCh[ibin] - u*fDerivativeCh[ibin+1];
    }// end of the if statement if dt<0.125
  } else if (ibin <= 0) {
    L_Ch = fPDFCh[0];
    dLdt_Ch = -fDerivativeCh[0];
  } else {
    L_Ch = fPDFCh[fEntriesTimeCh - 2];
    dLdt_Ch = -fDerivativeCh[fEntriesTimeCh - 2];
  }
}

double MultiPathWaterPosition::Calculate(const std::vector<double>& pmt,
                                         const std::vector<double>& par,
                                         std::vector<double>& diff) {

  double dx = pmt[0] - par[0];
  double dy = pmt[1] - par[1];
  double dz = pmt[2] - par[2];
  double dr = sqrt(dx*dx + dy*dy + dz*dz);

  double tCh = dr / fSpeedOfLightWater;
  fRes = pmt[3] - tCh - par[3]; // residual:  (PMT time - distance) - trial time

  double L = 0.0;
  double dLdtCh = 0.0;
  LAnddLdt_Ch(L, dLdtCh); // uses fRes to get L and dLdtCh

  double dLdt0 = dLdtCh / L;
  double factor = -dLdt0 / (dr * fSpeedOfLightWater);
  diff.assign(4, 0.0);
  diff[0] = factor * dx;
  diff[1] = factor * dy;
  diff[2] = factor * dz;
  diff[3] = dLdt0;

  L = log(L);

#ifdef DEBUG
  debug << "AWP::CL pmt= (" << pmt[0] << "," << pmt[1] << "," << pmt[2] << ")\n";
  debug << "        fRes=" << fRes << " dL=" << dLdtCh << "\n";
  debug << "        L=" << L << " diff=(" << diff[0] << "," << diff[1] << "," << diff[2] << "," << diff[3] << ")\n";
#endif

  return L;
}