#include <CLHEP/Units/PhysicalConstants.h>
using CLHEP::pi; using CLHEP::twopi; using CLHEP::halfpi;

#include <RAT/DU/Segmentor.hh>
#include <RAT/DU/Utility.hh>
#include <RAT/DU/PMTInfo.hh>
#include <RAT/DU/ChanHWStatus.hh>
using namespace RAT;
using namespace RAT::DU;


#include <TVector3.h>
#include <TMath.h>

#include <vector>
using namespace std;

ClassImp( RAT::DU::Segmentor )

void
Segmentor::Calculate()
{
  const PMTInfo& pmtInfo = Utility::Get()->GetPMTInfo();
  const RAT::DU::ChanHWStatus& chanHWStatus = Utility::Get()->GetChanHWStatus();
  fill( fSegmentIDs.begin(), fSegmentIDs.end(), 0);
  fill( fPopulations.begin(), fPopulations.end(), 0);
  fSegmentIDs.resize( pmtInfo.GetCount(), 0 );
  fPopulations.resize( fNDivisions * fNDivisions, 0 );

  // Equal area requires equal intervals in cos(theta) and phi
  vector<double> thetas( fNDivisions, 0 );
  vector<double> phis( fNDivisions, 0 );
  const double deltaCos  = 2.0 / fNDivisions;
  const double deltaPhi  = twopi / fNDivisions;

  for( unsigned int iDivision = 0; iDivision < fNDivisions; iDivision++ )
    phis[iDivision] = iDivision * deltaPhi - pi;
  for( unsigned int iDivision = 0; iDivision < fNDivisions; iDivision++ )
    thetas[iDivision] = TMath::ACos(1.0 - iDivision * deltaCos);

  // For each PMT, loop through thetas, phis and find the first time the
  // boundary overshoots its position. It then belongs to the interval before
  for( size_t ipmt=0; ipmt < pmtInfo.GetCount(); ipmt++ )
    {
      TVector3 pmtpos = pmtInfo.GetPosition( ipmt );
      if( pmtpos.Mag2() == 0.0 ) // PMT is at origin, not in a segment
        continue;

      // vector from pattern origin
      pmtpos -= fPatternOrigin;

      //Grab angles in pattern coords, project onto pattern x,y to get phi
      double theta = pmtpos.Angle(fPatternZaxis);
      double phi   = TMath::ATan2(pmtpos.Dot(fPatternYaxis),pmtpos.Dot(fPatternXaxis));

      int thetaDiv = -1;
      int phiDiv   = -1;

      // Test against boundaries
      for( unsigned int iDivision = 0; iDivision < fNDivisions; iDivision++)
        {
          if( theta > thetas[iDivision] )
            thetaDiv++;
          if( phi > phis  [iDivision] )
            phiDiv++;
        }
      // Each segment is given a unique integer that increases by 1 moving around phi
      // and fNDivisions moving around theta
      const unsigned int segmentID = thetaDiv * fNDivisions + phiDiv;
      fSegmentIDs[ipmt] = segmentID;
      // Add online PMTs to the populations, only these are hit
      if (chanHWStatus.IsEnabled()){
        bool chanOnline = chanHWStatus.IsChannelOnline(ipmt);
        bool pmtOnline  = chanHWStatus.IsTubeOnline(ipmt);
        if(chanOnline && pmtOnline)
          fPopulations[segmentID]++;
      }
      else
        fPopulations[segmentID]++;
    }
}

void Segmentor::SetPatternZaxis(const TVector3& patternZaxis, const int numberOfDivisions){
    fNDivisions = numberOfDivisions;
    SetPatternZaxis(patternZaxis);
}

void Segmentor::SetPatternZaxis(const TVector3& patternZaxis){
    fPatternZaxis = patternZaxis.Unit();
    // cross product fails if chosen axis || z
    if (TMath::Abs(fPatternZaxis.CosTheta()) == 1){
        fPatternXaxis = TVector3(1,0,0);
        fPatternYaxis = TVector3(0,1,0);
    }
    else {
        fPatternXaxis = TVector3(0,0,1).Cross(fPatternZaxis);
        fPatternYaxis = fPatternZaxis.Cross(fPatternXaxis);
    }
    Calculate();
}

void Segmentor::SetPatternOrigin(const TVector3& patternOrigin, const TVector3& patternZaxis,
                                 const int numberOfDivisions){
    fNDivisions = numberOfDivisions;
    fPatternOrigin = patternOrigin;
    SetPatternZaxis(patternZaxis);
}