#include <RAT/Isotropy.hh>
#include <RAT/DB.hh>
#include <RAT/PMTSelector.hh>
#include <RAT/Factory.hh>
#include <RAT/ClassifierFactory.hh>
#include <RAT/TimeResidualCut.hh>
#include <RAT/DS/FitVertex.hh>
#include <RAT/DU/Utility.hh>
#include <RAT/PMTSelectorFactory.hh>
#include <TVector3.h>
#include <math.h>
using namespace std;

using namespace RAT;
using namespace RAT::Classifiers;
using namespace RAT::DS;


void
Isotropy::DefaultSeed()
{

  fEventPos = TVector3( 0.0, 0.0, 0.0 );
  fSeedVertex.SetPosition( fEventPos, false, true );
}

void
Isotropy::SetSeed( const DS::FitResult& seed )
{
  try
    {
      fSeedVertex = seed.GetVertex( 0 );
    }
  catch( FitResult::NoVertexError& error )
    {
      // Nothing to seed from, strange request
      return;
    }
  try { fEventPos = fSeedVertex.GetPosition(); }
  catch( FitVertex::NoValueError& error ) { /* No data to seed from */ }
}

DS::ClassifierResult
Isotropy::GetClassification()
{
  fClassifierResult.Reset();
  if( fPMTData.empty() )
    return fClassifierResult;

  SelectPMTData( fSeedVertex );

  const DU::PMTInfo& pmtInfo = DU::Utility::Get()->GetPMTInfo();

  double p1 = 0.0;
  double p2 = 0.0;
  double p3 = 0.0;
  double p4 = 0.0;
  double thetaij = 0.0; // One angle
  double tij = 0.0; // Summed angles

  vector<TVector3> pmtDir;
  for( vector<FitterPMT>::const_iterator iPMT = fSelectedPMTData.begin(); iPMT != fSelectedPMTData.end(); ++iPMT )
    {
      const TVector3 pmtPos = pmtInfo.GetPosition( iPMT->GetID() );

      pmtDir.push_back( (pmtPos - fEventPos).Unit() );

    }
  if( pmtDir.size() <= 1. ) return fClassifierResult;

  for( size_t iPMT = 0; iPMT < pmtDir.size(); ++iPMT )
    {
      const TVector3 pmtDir1 = pmtDir[iPMT];
      for( size_t iPMT2 = iPMT + 1; iPMT2 < pmtDir.size(); ++iPMT2 )
        {
          const TVector3 pmtDir2 = pmtDir[iPMT2];
          thetaij = pmtDir[iPMT].Angle(pmtDir[iPMT2]);
          tij += thetaij;
          const double cosTheta12 = pmtDir1.Dot( pmtDir2 );
          p1 += cosTheta12; // px is Legendre Polynomial order x
          const double cosThetaSquared = cosTheta12 * cosTheta12;
          p2 += ( 3.0 * cosThetaSquared - 1.0 ) / 2.0;
          p3 += ( 5.0 * cosThetaSquared - 3.0 ) * cosTheta12 / 2.0;
          p4 += ( 35.0 * cosThetaSquared * cosThetaSquared - 30.0 * cosThetaSquared + 3.0 ) / 8.0;
        }
    }
  p1 = 2.0 * p1 / static_cast<double>( pmtDir.size() * ( pmtDir.size() - 1 ) );
  p2 = 2.0 * p2 / static_cast<double>( pmtDir.size() * ( pmtDir.size() - 1 ) );
  p3 = 2.0 * p3 / static_cast<double>( pmtDir.size() * ( pmtDir.size() - 1 ) );
  p4 = 2.0 * p4 / static_cast<double>( pmtDir.size() * ( pmtDir.size() - 1 ) );
  tij = 2.0 * tij / static_cast<double>( pmtDir.size() * ( pmtDir.size() - 1 ) );

  const double beta14 = p1 + 4 * p4;

  fClassifierResult.SetClassification( "snobeta14", beta14 );
  fClassifierResult.SetClassification( "beta1", p1 );
  fClassifierResult.SetClassification( "beta2", p2 );
  fClassifierResult.SetClassification( "beta3", p3 );
  fClassifierResult.SetClassification( "beta4", p4 );
  fClassifierResult.SetClassification( "thetaij", tij );
  fClassifierResult.SetValid( true );
  return fClassifierResult;
}