#include <RAT/Centroid.hh>
#include <RAT/DS/Entry.hh>
#include <RAT/DS/FitVertex.hh>
#include <RAT/DU/Utility.hh>
#include <RAT/DU/PMTInfo.hh>
#include <RAT/DU/GroupVelocity.hh>
using namespace RAT;
using namespace RAT::Methods;

#include <TH1D.h>
using namespace ROOT;

#include <math.h>
using namespace std;

void
Centroid::Initialise( const std::string& )
{
  fPower = 2.0;
}

DS::FitResult
Centroid::GetBestFit()
{
  fFitResult.Reset();
  if( fPMTData.empty() )
    return fFitResult;

  double totalCharge = 0;
  TVector3 centroidPos;

  TH1D histTiming( "times", "times", 500, 0.0, 500.0 );
  histTiming.SetDirectory(0);

  const DU::PMTInfo& pmtInfo = DU::Utility::Get()->GetPMTInfo();
  for( vector<FitterPMT>::const_iterator iPMT = fPMTData.begin(); iPMT != fPMTData.end(); ++iPMT )
	{
	  double chargePower = pow( iPMT->GetQHS(), fPower );
	  centroidPos += chargePower * pmtInfo.GetPosition( iPMT->GetID() );
	  totalCharge += chargePower;

	  histTiming.Fill( iPMT->GetTime() );
	}
  centroidPos *= 1.0 / totalCharge;

  const double firstHitTime = histTiming.GetXaxis()->GetBinCenter( histTiming.FindFirstBinAbove( 3.0 ) );

  DS::FitVertex fitVertex;

  fitVertex.SetPosition( centroidPos );
  fitVertex.SetPositionErrors( ROOT::Math::XYZVectorF(100.0, 100.0, 100.0) );
  const double transitTime = DU::Utility::Get()->GetGroupVelocity().CalcByDistance( 6005.3 - centroidPos.Mag(), 55.0, 2340.0 );
  fitVertex.SetTime( firstHitTime - transitTime ); //Assumed straight line
  fitVertex.SetTimeErrors( 10.0 );
  fFitResult.SetVertex( 0, fitVertex );
  return fFitResult;
}