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

#include <TROOT.h>
#include <TH1F.h>
#include <TH2F.h>
#include <TF1.h>
#include <TMath.h>
#include <TString.h>
#include <TVector3.h>
#include <TRotation.h>
#include <TGraph.h>
#include <TMinuit.h>
#include <TFitResult.h>
using namespace ROOT;

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

#include <vector>
#include <algorithm>
#include <map>
#include <exception>
using namespace std;


TVector3 MuonWater::CenterOfMass_MuonWater(const std::vector<TVector3>& list){

  double x = 0;
  double y = 0;
  double z = 0;

  for(size_t i=0; i<list.size(); i++){
    x+=list.at(i).X();
    y+=list.at(i).Y();
    z+=list.at(i).Z();
  }

  x/=list.size();
  y/=list.size();
  z/=list.size();

  TVector3 centerOfMass;

  centerOfMass.SetX(x);
  centerOfMass.SetY(y);
  centerOfMass.SetZ(z);

  return centerOfMass;
}

DS::FitResult MuonWater::GetBestFit(){

    fFitResult.Reset();
    if( fPMTData.empty() )
      return fFitResult;

    double radDeg = 180/pi; ///< conversion from [rad] to [degree]
    map< double,vector<int> > mapPmtTimeID;
    vector<double> pmtCalTimes;
    vector<int> earlyPmtIDs, pmtIDs;
    vector<TVector3> earlyPmts20, earlyPmts50, earlyPmts80, tmpCoMPmts;
    /// Vector to hold all vectors with angle to centerOfMass80 < (cos(theta)>0.9)
    TVector3 pmtCharge, recoEP, recoXP;

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

    for( vector<FitterPMT>::const_iterator iPmt = fPMTData.begin(); iPmt != fPMTData.end(); iPmt++ ){

        int iPmtID_Cal = iPmt->GetID();
        double dCalTime = iPmt->GetTime();

        mapPmtTimeID[dCalTime].push_back(iPmtID_Cal); ///< map with times and PMT ID
        pmtCalTimes.push_back(dCalTime); ///< vector with times

        pmtCharge += (pmtInfo.GetPosition(iPmtID_Cal))*(iPmt->GetQHL());
    }

    // to be sure times are sorted
    sort (pmtCalTimes.begin(), pmtCalTimes.end());

    /// taking the 80 earliest PMT IDs from  map, with sorted times
    if( static_cast<size_t>(pmtCalTimes.size()) > 80){
        for(int k=0; k<80; k++){
            for(size_t z=0; z < mapPmtTimeID[pmtCalTimes.at(k)].size(); z++){
                earlyPmtIDs.push_back( mapPmtTimeID[pmtCalTimes.at(k)].at(z) );
            }
        }
    }
    else {
        throw MethodFailError("MuonAnalytical ERROR: Not enough ( < 80 ) PMT Hits ...");
    }

    for(int k=0; k<80; k++){
        TVector3  Pmt;
        Pmt.SetX(pmtInfo.GetPosition(earlyPmtIDs.at(k)).X());
        Pmt.SetY(pmtInfo.GetPosition(earlyPmtIDs.at(k)).Y());
        Pmt.SetZ(pmtInfo.GetPosition(earlyPmtIDs.at(k)).Z());
        earlyPmts80.push_back(Pmt);
    }

    // Getting Center of Mass of first 80 PMTs
    TVector3 centerOfMass80 = CenterOfMass_MuonWater(earlyPmts80);

    for(int k=0; k<80; k++){
        double angleCoM = centerOfMass80.Angle(earlyPmts80.at(k));
        if(angleCoM < (26/radDeg)){
            tmpCoMPmts.push_back(earlyPmts80.at(k));
        }
    }

    if( static_cast<size_t>(tmpCoMPmts.size() < 19 )){
        if(tmpCoMPmts.size()<=2){
            throw MethodFailError("MuonAnalytical (FTR) ERROR: Empty vector-list");
        }
        else { earlyPmts20 = tmpCoMPmts;}
    }
    else{
        for(int k=0; k<20; k++){
            earlyPmts20.push_back(tmpCoMPmts.at(k));
        }
    }

    recoEP = CenterOfMass_MuonWater(earlyPmts20); /// final vector to rc entry point from 20 earliest PMTs


    /// Exit point reconstruction

    if (pmtCharge.Mag()>0){
        recoXP = pmtCharge * (1./(pmtCharge.Mag()));
    }
    else {
        throw MethodFailError ("MuonAnalytical ERROR: no PMT charge recorded");
    }

    DS::FitVertex muonTrack;
    muonTrack.SetPosition( recoXP );
    muonTrack.SetDirection( recoXP - recoEP );

    fFitResult.SetVertex( 0, muonTrack );
    return fFitResult;
}