#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/MuonScintillator.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;

double MuonScintillator::FitFuncEPphi(double* x,double* p){
      if(x[0]<p[2]){
          return p[5]+(p[0]*sin(p[1]*(p[2]-x[0])) );
      }
      else {
          return p[5]+(p[3]*sin(p[4]*(x[0]-p[2])));
      }
  }

double MuonScintillator::FitFuncEPtheta(double* x,double* p){
      if(x[0]<p[2]){
          return p[5]+(p[0]*sin(p[1]*(p[2]-x[0])));
      }
      else {
          return p[5]+(p[3]*sin(p[4]*(x[0]-p[2])));
      }
  }

double MuonScintillator::FitFuncEXOplane(double* x,double* p){
      return (sqrt(pow(p[1]*6.,2.)-2*(p[1]*6.)*(cos(x[0]-p[2])))/(p[0]));
  }

double MuonScintillator::FitFuncDoubleGauss(double* x,double* p){
    return (p[2]/p[0]*exp(-0.5*(pow((x[0]-p[1])/p[0],2.))) + p[5]/p[3]*exp(-0.5*(pow((x[0]-p[4])/p[3],2.))) );
}

TVector3 MuonScintillator::CenterOfMass(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 centerMass;

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

  return centerMass;
}

DS::FitResult MuonScintillator::GetBestFit(){

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

    const int iNHits = fPMTData.size();
    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 tmpCoMPmts < (cos(theta)>0.9)
    TVector3 pmtCharge;

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

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

        int calPmtID = iPmt->GetID();
        double calTime = iPmt->GetTime();

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

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

    /// Make sure times are always perfectly sorted
    sort (pmtCalTimes.begin(), pmtCalTimes.end());

    /// Time window for the hits used for the EP first guess
    double timeWindow = (pmtCalTimes.at(5.)+5.);

    // taking the PMT IDs for the PMTs in the first ns in timeWindow from map, with sorted times
    while ( static_cast<int>(earlyPmtIDs.size()) < 10. ){
        for(int k=0; k<iNHits; k++){
            if(pmtCalTimes.at(k) > 0. && pmtCalTimes.at(k) < timeWindow){
                for(int z=0; z < static_cast<int>(mapPmtTimeID[pmtCalTimes.at(k)].size()); z++){
                    earlyPmtIDs.push_back( mapPmtTimeID[pmtCalTimes.at(k)].at(z) );
                }
            }
        }

        timeWindow += 5.;
    }

    if( static_cast<int>(earlyPmtIDs.size()) < 10. ){
        throw MethodFailError("Error: Less than 10 Events in 5 ns time window !");
    }

    // time-sorted PMT IDs for all hit PMTs
    for(int k=1; k<iNHits; k++){
        if( pmtCalTimes.at(k) > 0. && (pmtCalTimes.at(k-1) != pmtCalTimes.at(k)) ){
            for(size_t y=0; y < mapPmtTimeID[pmtCalTimes.at(k)].size(); y++){
                pmtIDs.push_back( mapPmtTimeID[pmtCalTimes.at(k)].at(y) );
            }
        }
    }


    for(size_t k=0; k< earlyPmtIDs.size(); 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());
        earlyPmts50.push_back(pmt);
    }

    // Getting Center of Mass of first hit PMTs (first 5ns) - First Guess
    TVector3 firstGuessEP = CenterOfMass(earlyPmts50);

    //-- Rotating Coordinates for EP reconstruction ------------------------------------------------------

    // Angle for rotation towards phi=0
    double rotationAnglePhi0 = -(firstGuessEP.Phi());

    // Theta Rotation Axis
    TVector3 thetaRotationAxis;
    thetaRotationAxis.SetMagThetaPhi(1.,0.,0.);

    // Defining Rotation to azimuthal plane
    TRotation rotAzimuthPlane;
    rotAzimuthPlane.Rotate(rotationAnglePhi0,thetaRotationAxis);

    TVector3 firstGuessEPAzimuth = (firstGuessEP).Transform(rotAzimuthPlane); // !!! firstGuessEP is changed here !!!

    // Angle for rotation towards theta=pi/2
    double rotationAngleThetaPi2 = (halfpi-(firstGuessEPAzimuth.Theta()));

    // Phi Rotation Axis
    TVector3 phiRotation;
    phiRotation.SetMagThetaPhi(1.,halfpi,0.);
    TVector3 phiRotationAxis = firstGuessEPAzimuth.Cross(phiRotation);

    // Defining Rotation to equatorial plane
    TRotation rotEquatorPlane;
    rotEquatorPlane.Rotate(rotationAngleThetaPi2,phiRotationAxis);

    TVector3 firstGuessEPEquator = (firstGuessEPAzimuth).Transform(rotEquatorPlane);

    //-- Rotating all the other vectors with same transformations ----------------------------------------

    vector<TVector3> earlyPmts, earlyPmtsRot1, earlyPmtsRot2;

    for(size_t k=0; k< pmtIDs.size(); k++){
        TVector3  pmt;
        pmt.SetX(pmtInfo.GetPosition(pmtIDs.at(k)).X());
        pmt.SetY(pmtInfo.GetPosition(pmtIDs.at(k)).Y());
        pmt.SetZ(pmtInfo.GetPosition(pmtIDs.at(k)).Z());
        earlyPmts.push_back(pmt); // put the vectors in vector
    }

    for(size_t k=0; k< pmtIDs.size(); ++k){
        TVector3 pmtRot1;
        pmtRot1 = (earlyPmts.at(k)).Transform(rotAzimuthPlane);
        earlyPmtsRot1.push_back(pmtRot1);
    }

    for(size_t k=0; k< pmtIDs.size(); ++k){
        TVector3 pmtRot2;
        pmtRot2 = (earlyPmtsRot1.at(k)).Transform(rotEquatorPlane);
        earlyPmtsRot2.push_back(pmtRot2);
    }

    //-- Selecting vectors in phi and theta planes ---------------------------------------------------
    vector<TVector3> phiPlane, thetaPlane;
    vector<double> phiTimes, thetaTimes, phiPlaneThetas;

    for(size_t k=0; k< earlyPmtsRot2.size(); ++k){
        //-- Phi PLane ----------
        if( (halfpi-0.05) < ((earlyPmtsRot2.at(k)).Theta()) && ((earlyPmtsRot2.at(k)).Theta()) < (halfpi+0.05) ){
            phiPlane.push_back(earlyPmtsRot2.at(k));
            phiTimes.push_back(pmtCalTimes.at(k));
        }
        //-- Theta Plane --------
        if( (-0.05 < (earlyPmtsRot2.at(k)).Phi()) && ((earlyPmtsRot2.at(k)).Phi()) < 0.05 ){
            thetaPlane.push_back(earlyPmtsRot2.at(k));
            thetaTimes.push_back(pmtCalTimes.at(k));
        }
    }

    //-- Graphs / Fits -------------------------------------------------------------------------

    TGraph* graphRotPhiPlane = new TGraph();
    TGraph* graphRotThetaPlane = new TGraph();
    int idx1 = 0;
    int idx2 = 0;

    if( (static_cast<int>(thetaPlane.size())) >=100 && (static_cast<int>(phiPlane.size())) >=100 ){
        for(size_t k=0; k< thetaPlane.size(); ++k){
            if( k< thetaTimes.size() && thetaTimes.at(k)>0. ){
                graphRotThetaPlane->SetPoint(idx1,(thetaPlane.at(k)).Theta(),thetaTimes.at(k));
                idx1++;
            }
        }

        for(size_t k=0; k< phiPlane.size(); ++k){
            if( k< thetaTimes.size() && thetaTimes.at(k)>0. ){
                graphRotPhiPlane->SetPoint(idx2,(phiPlane.at(k)).Phi(),phiTimes.at(k));
                idx2++;
            }
        }
    }
    else{
        throw MethodFailError("MuonAnalytical Error: Not enough Entries in thetaPlane or phiPlane !!! (<100)");
    }

    //-- Fits ----------------------------------------------------------------------------------------

    TF1* fitEPtheta = new TF1("fitEPtheta",&MuonScintillator::FitFuncEPtheta,0.,pi,6);
    TF1* fitEPphi = new TF1("fitEPphi",&MuonScintillator::FitFuncEPphi,-pi,pi,6);

    // Parameters -------------------------------------------------
    // Help the fit by performing an estimate of the minimum location

        // ***********
        // Theta Plane
        // ***********

    int npxTheta = graphRotThetaPlane->GetN();
    double *xxTheta = graphRotThetaPlane->GetX();
    double *yyTheta = graphRotThetaPlane->GetY();

    double centroidTheta = 0.0;
    double numTheta = 0.0;
    for (int ii = 0; ii < npxTheta; ++ii) {
      centroidTheta += yyTheta[ii]*xxTheta[ii];
      numTheta += yyTheta[ii];
    }
    centroidTheta = centroidTheta/numTheta;
    double offsetTheta = graphRotThetaPlane->Eval(centroidTheta,0,"");

    // Use these parameters to perform linear fits on subranges in the data points
    graphRotThetaPlane->Fit("pol1","E,M,Q,S","",0.,centroidTheta);
    TF1* fitThetaLeft = (TF1*)graphRotThetaPlane->GetFunction("pol1");
    // Get the slope and error
    double p0 = fitThetaLeft->GetParameter(1);
    // other side
    graphRotThetaPlane->Fit("pol1","E,M,Q,S","",centroidTheta,3.1);
    TF1* fitThetaRight = (TF1*)graphRotThetaPlane->GetFunction("pol1");
    double p3 = fitThetaRight->GetParameter(1);

    fitEPtheta->SetParameters(p0,-1.,centroidTheta,p3,1.,offsetTheta);
    /* scale left   */ fitEPtheta->SetParLimits(0,p0+0.5*p0,p0-0.5*p0);
    /* centroid     */ fitEPtheta->SetParLimits(2,1.,2.);
    /* scale right  */ fitEPtheta->SetParLimits(3,p3-0.5*p3,p3+0.5*p3);
    /* offset       */ fitEPtheta->SetParLimits(5,offsetTheta-0.3*offsetTheta,offsetTheta+0.3*offsetTheta);

    // Fit + Range ------------------------------------------------
    fitEPtheta->SetLineColor(2);
    graphRotThetaPlane->Fit("fitEPtheta","E,M,Q","",0.5,2.5);

    double thetaEP = fitEPtheta->GetParameter(2);

        // ***********
        // Phi Plane
        // ***********

    int npxPhi = graphRotPhiPlane->GetN();
    double *xxPhi = graphRotPhiPlane->GetX();
    double *yyPhi = graphRotPhiPlane->GetY();

    double centroidPhi = 0.0;
    double numPhi = 0.0;
    for (int ii = 0; ii < npxPhi; ++ii) {
      centroidPhi += yyPhi[ii]*xxPhi[ii];
      numPhi += yyPhi[ii];
    }
    centroidPhi = centroidPhi/numPhi;
    double offsetPhi = graphRotPhiPlane->Eval(centroidPhi,0,"");

    // Use these parameters to perform linear fits on subranges in the data points
    graphRotPhiPlane->Fit("pol1","E,M,Q,S","",-1.5,centroidPhi);
    TF1* fitPhiLeft = (TF1*)graphRotPhiPlane->GetFunction("pol1");
    // Get the slope and error
    double phiPar0 = fitPhiLeft->GetParameter(1); //slope
    // other side
    graphRotPhiPlane->Fit("pol1","E,M,Q,S","",centroidPhi,1.5);
    TF1* fitPhiRight = (TF1*)graphRotPhiPlane->GetFunction("pol1");
    double phiPar3 = fitPhiRight->GetParameter(1);

    fitEPphi->SetParameters(phiPar0,-1.,centroidPhi,phiPar3,1.,offsetPhi);
    /* scale left   */ fitEPphi->SetParLimits(0,phiPar0+0.5*phiPar0,phiPar0-0.5*phiPar0);
    /* centroid     */ fitEPphi->SetParLimits(2,-0.5,0.5);
    /* scale right  */ fitEPphi->SetParLimits(3,phiPar3-0.5*phiPar3,phiPar3+0.5*phiPar3);
    /* offset       */ fitEPphi->SetParLimits(5,offsetPhi-0.3*offsetPhi,offsetPhi+0.3*offsetPhi);

    // Fit + Range ------------------------------------------------
    graphRotPhiPlane->Fit("fitEPphi","E,M,Q","",-1.0,1.0);
    double phiEP = fitEPphi->GetParameter(2);

    // return Result
    TVector3 fittedEP;
    fittedEP.SetMagThetaPhi(1.,thetaEP,phiEP);

    //-- Rotating back -------------------------------------------------------------------------------

    TVector3 recoEP;
    TVector3 invRot1;

    // Inverse Rotation back from Equatorial Plane
    TRotation invRotEquatorPlane;
    invRotEquatorPlane = rotEquatorPlane.Inverse();

    // Inverse Rotation back from Azimuthal Plane to original Frame
    TRotation invRotAzPlane;
    invRotAzPlane = rotAzimuthPlane.Inverse();

    invRot1 = fittedEP.Transform(invRotEquatorPlane);
    recoEP = invRot1.Transform(invRotAzPlane);


    // Rotation all the other vectors back
    vector<TVector3> earlyPmtsInvRot1;
    vector<TVector3> earlyPmtsInvRot2;

    for(size_t k=0; k< earlyPmtsRot2.size(); ++k){
        TVector3 invRot1Pmt;
        invRot1Pmt = (earlyPmtsRot2.at(k)).Transform(invRotEquatorPlane);
        earlyPmtsInvRot1.push_back(invRot1Pmt);
    }

    for(size_t k=0; k< earlyPmtsInvRot1.size(); ++k){
        TVector3 invRot2Pmt;
        invRot2Pmt = (earlyPmtsInvRot1.at(k)).Transform(invRotAzPlane);
        earlyPmtsInvRot2.push_back(invRot2Pmt);
    }


//------ XP ------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------

    // Angle for Rotation to "North" Pole
    double angleNP = -(recoEP.Theta());

    // Axis for Rotation to NP
    TVector3 recoEPequator;
    recoEPequator.SetMagThetaPhi(1.,halfpi,(recoEP.Phi()));
    TVector3 rotationAxisNP = recoEP.Cross(recoEPequator);

    // Rotation to North Pole
    TRotation rotationNP;
    rotationNP.Rotate(angleNP,rotationAxisNP);

    // Rotating
    TVector3 recoEP2 = recoEP;
    TVector3 rotatedEPNP = (recoEP2).Transform(rotationNP);

    // Rotation All the other vectors too
    vector<TVector3> pmtsRotNP;

    for(size_t k=0; k< earlyPmtsInvRot2.size(); ++k){
        TVector3 Pmt_RotNP;
        Pmt_RotNP = (earlyPmtsInvRot2.at(k)).Transform(rotationNP);
        pmtsRotNP.push_back(Pmt_RotNP);
    }

  //-- Select vectors in 9 azimuthal planes---------------------------------------------------------

    int numPlanes = 9;
    bool piTest = false;
    bool piTest2 = false;
    char fitName[100];
    double phi = 0;
    double phiW = 0;
    double phiX = 0;
    TF1* fitsExoPlane[30];
    TF1* fitsExoPlaneRot[30];
    TGraph* graphAzPlane[20];
    TGraph* graphAzPlaneRot[20];

    for(int l=0; l<numPlanes; ++l){
        graphAzPlane[l] = new TGraph();
        int idx = 0;
        for(size_t k=0; k< pmtsRotNP.size(); ++k){
            if( (0.3*(l+1)) < ((pmtsRotNP.at(k)).Theta()) && ((pmtsRotNP.at(k)).Theta()) < ((0.3*(l+1))+0.05) ){
                if(k< pmtCalTimes.size() && (pmtCalTimes.at(k)) >5.){
                    graphAzPlane[l]->SetPoint(idx,(pmtsRotNP.at(k)).Phi(),pmtCalTimes.at(k));
                    idx++;
                }
            }
        }
    }

  //-- Fit the 9 azimuthal planes --------------------------------------------------------------

    double* xx[10000];
    double* yy[10000];
    double centroid[100];
    double num[100];
    double offset[100];
    double rs[100];
    int fitStatus[100];

    double* xxRot[10000];
    double* yyRot[10000];
    double centroidRot[100];
    double numRot[100];
    double offsetRot[100];
    double rsRot[100];
    int fitStatusRot[100];

    for(int N=1; N<numPlanes; ++N){

        xx[N] = graphAzPlane[N]->GetX();
        yy[N] = graphAzPlane[N]->GetY();

        centroid[N] = 0.0;
        num[N] = 0.0;
        for (int ii = 0; ii < 31; ++ii) {
          centroid[N] += yy[N][ii]*xx[N][ii];
          num[N] += yy[N][ii];
        }
        centroid[N] = centroid[N]/num[N];
        offset[N] = graphAzPlane[N]->Eval(centroid[N],0,"");

        // If phi_x is ~Pi -> rotate coordinates
        if(centroid[N] >= 2.3 || centroid[N] <= (-2.3)){
            piTest = true;
        }

        sprintf(fitName,"fitExoPlane_%d",N);
        fitsExoPlane[N] = new TF1(fitName,&MuonScintillator::FitFuncEXOplane,-pi,pi,3);

        rs[N] = (-0.5+sqrt(0.25+pow(offset[N],2.)))/10.;
        fitsExoPlane[N]->SetParameters(0.1,rs[N],centroid[N]);
        fitsExoPlane[N]->SetParLimits(2,centroid[N]-0.5,centroid[N]+0.5);

        fitStatus[N] = graphAzPlane[N]->Fit(fitName,"E,Q","",-3.,3.);

        // Average phiX
        if(fitStatus[N] == 0){
            phi += (fitsExoPlane[N]->GetParameter(2))*(1/(fitsExoPlane[N]->GetChisquare() / fitsExoPlane[N]->GetNDF()));
            phiW += (1/(fitsExoPlane[N]->GetChisquare() / fitsExoPlane[N]->GetNDF()));
            phiX = phi/phiW;
        }

        //Check again for phiX ~ Pi
        if(fitsExoPlane[N]->GetParameter(2) <= -2. || fitsExoPlane[N]->GetParameter(2) >= 2.){
            piTest2 = true;
            phiX = 0.;
            phiW = 0.;
        }

    }

    if(piTest || piTest2){

        // Rotating All Coordinates by Pi ... ------------------------------------------------

        const double rotationAngleZ = pi;
        TVector3 rotationAxisZ;
        rotationAxisZ.SetMagThetaPhi(1.,0.,0.);

        TRotation rotationZAxis;
        rotationZAxis.Rotate(rotationAngleZ,rotationAxisZ);

        vector<TVector3> copyPmtRotNP(pmtsRotNP);
        vector<TVector3> copyPmtRotNP2;

        for(size_t m=0; m< copyPmtRotNP.size(); ++m){
            TVector3 CopyPmtRotNP(copyPmtRotNP.at(m));
            CopyPmtRotNP.Transform(rotationZAxis);
            copyPmtRotNP2.push_back(CopyPmtRotNP);
        }

        // ... and selecting planes again ----------------------------------------------------

        for(int P=1; P<numPlanes; ++P){
            graphAzPlaneRot[P] = new TGraph();
            int idx3 = 0;

            for(size_t k=0; k< copyPmtRotNP2.size(); ++k){
                if( (0.3*(P+1)) < ((copyPmtRotNP2.at(k)).Theta()) && ((copyPmtRotNP2.at(k)).Theta()) < ((0.3*(P+1))+0.05) ){
                    if(k< pmtCalTimes.size() && (pmtCalTimes.at(k))>5.){
                        graphAzPlaneRot[P]->SetPoint(idx3,(copyPmtRotNP2.at(k)).Phi(),pmtCalTimes.at(k));
                        idx3++;
                    }
                }
            }

            // Now all the fitting again with rotated planes ---------------------------------

            xxRot[P] = graphAzPlaneRot[P]->GetX();
            yyRot[P] = graphAzPlaneRot[P]->GetY();

            centroidRot[P] = 0.0;
            numRot[P] = 0.0;
            for (int ii = 0; ii < 31; ++ii) {
              centroidRot[P] += yyRot[P][ii]*xxRot[P][ii];
              numRot[P] += yyRot[P][ii];
            }
            centroidRot[P] = centroidRot[P]/numRot[P];
            offsetRot[P] = graphAzPlaneRot[P]->Eval(centroidRot[P],0,"");

            rsRot[P] = (-0.5+sqrt(0.25+pow(offsetRot[P],2.)))/10.;

            sprintf(fitName,"fitExoPlane_%d",P);
            fitsExoPlaneRot[P] = new TF1(fitName,&MuonScintillator::FitFuncEXOplane,-pi,pi,3);
            fitsExoPlaneRot[P]->SetParameters(0.1,rsRot[P],centroidRot[P]);
            fitsExoPlaneRot[P]->SetParLimits(2,centroidRot[P]-0.5,centroidRot[P]+0.5);

            fitStatusRot[P] = graphAzPlaneRot[P]->Fit(fitName,"E,Q","",-3.,3.);

            // Average phiX
            if(fitStatusRot[P] == 0){
                phi += (fitsExoPlaneRot[P]->GetParameter(2))*(1/(fitsExoPlaneRot[P]->GetChisquare() / fitsExoPlaneRot[P]->GetNDF()));
                phiW += (1/(fitsExoPlaneRot[P]->GetChisquare() / fitsExoPlaneRot[P]->GetNDF()));
                phiX = phi/phiW;
            }

            // Rotating phiX back
            if(phiX > 0.){
                phiX = phiX-pi;
            }
            else{
                phiX = phiX+pi;
            }
        }

    }

    // The EXO Plane ----------------------------------------------------------------------------------------

    TGraph* graphExoPlane = new TGraph();
    int idx4 = 0;
    double widthEXO = 0.06; // P_EXO Width
    double sign = 1.;

    if(phiX > 0.){
        sign = 1.;
    }
    else{
        sign =-1.;
    }

    for(size_t k=0; k< pmtsRotNP.size(); ++k){
        if( (phiX-widthEXO < ((pmtsRotNP.at(k)).Phi()) && ((pmtsRotNP.at(k)).Phi()) < phiX+widthEXO) || (phiX-sign*pi-widthEXO < ((pmtsRotNP.at(k)).Phi()) && ((pmtsRotNP.at(k)).Phi()) < phiX-sign*pi+widthEXO) ){
            if(k< pmtCalTimes.size() && (pmtCalTimes.at(k))>2.){
                if(phiX-widthEXO < ((pmtsRotNP.at(k)).Phi()) && ((pmtsRotNP.at(k)).Phi()) < phiX+widthEXO){
                    graphExoPlane->SetPoint(idx4,(pmtsRotNP.at(k)).Theta(),pmtCalTimes.at(k));
                }
                else{
                    graphExoPlane->SetPoint(idx4,(twopi-((pmtsRotNP.at(k)).Theta())),pmtCalTimes.at(k));
                }
                idx4++;
            }
        }
    }

    TF1* rightGauss = new TF1("rightGauss","gaus",0.,10.);
    rightGauss->SetParameters(100.,4.,2.);
    rightGauss->SetParLimits(0,60.,130.);
    rightGauss->SetParLimits(1,3.,4.5);
    graphExoPlane->Fit("rightGauss","E,Q","",2.6,6.);
    double rgStrength = rightGauss->GetParameter(0);
    double rgMean = rightGauss->GetParameter(1);
    double rgSigma = rightGauss->GetParameter(2);

    // Fit to find Range -------------------------------------------------------------------

    TF1* fitExoPlaneRight = new TF1("fitExo",&MuonScintillator::FitFuncDoubleGauss,0.,twopi,6);
    fitExoPlaneRight->SetParameters(0.5,1.5,(150.-rgStrength),rgSigma,rgMean,rgStrength);

    fitExoPlaneRight->SetParLimits(0,0.2,1.2); // Sigma_1
    fitExoPlaneRight->SetParLimits(1,1.,1.6); // Mu_1
    fitExoPlaneRight->SetParLimits(2,40.,100.); // Strength_1
    fitExoPlaneRight->SetParLimits(5,rgStrength-0.05*rgStrength,rgStrength+0.05*rgStrength); // Strength 2

    fitExoPlaneRight->SetLineColor(2);
    graphExoPlane->Fit("fitExo","Q","",0.2,6.);

    double fitRangeLeft = (fitExoPlaneRight->GetParameter(1)-0.4);
    double fitRangeRight = fitExoPlaneRight->GetParameter(4);
    double fitRangeCenter = (fitRangeLeft + fitRangeRight)/2.;

    graphExoPlane->Fit("pol1","E,M,Q,S","",rgMean-1.,rgMean-0.2);
    TF1* fitRG = (TF1*)graphExoPlane->GetFunction("pol1");
    double par0rg = fitRG->GetParameter(1); // Get the slope

    // Fitting P_EXO ----------------------------------------------------------------------
    TF1* fitExoPlane = new TF1("fitExoPlane",&MuonScintillator::FitFuncEPtheta,fitRangeLeft,fitRangeRight,6);

    fitExoPlane->SetParameters(par0rg,-1.,fitRangeCenter,10.,0.5,50.);
    fitExoPlane->SetParLimits(2,fitRangeCenter-1.,fitRangeCenter+0.2); // theta_prime_x
    fitExoPlane->SetParLimits(0,par0rg-0.5*par0rg,par0rg+0.5*par0rg);

    fitExoPlane->SetLineColor(3);
    graphExoPlane->Fit("fitExoPlane","E","SAME",fitRangeLeft,fitRangeRight);

    //-- Theta Prime X
    double thetaPrimeX = fitExoPlane->GetParameter(2);

    if(thetaPrimeX > pi){
        thetaPrimeX = twopi-thetaPrimeX;
    }

    //-- Fitting phi_x again in a plane around theta_x ----------------------------------------------

    TGraph* finalPlane = new TGraph();
    int idx5 = 0;

    for(size_t k=0; k< pmtsRotNP.size(); ++k){
        if( thetaPrimeX-0.1 < ((pmtsRotNP.at(k)).Theta()) && ((pmtsRotNP.at(k)).Theta()) < thetaPrimeX+0.1){
            if(k< pmtCalTimes.size() && (pmtCalTimes.at(k))>5.){
                finalPlane->SetPoint(idx5,(pmtsRotNP.at(k)).Phi(),pmtCalTimes.at(k));
                idx5++;
            }
        }
    }

    if(finalPlane->GetN() < 100){
        delete finalPlane;
        finalPlane = new TGraph();
        int idx6 = 0;

        for(size_t k=0; k< pmtsRotNP.size(); ++k){
            if( thetaPrimeX-0.3 < ((pmtsRotNP.at(k)).Theta()) && ((pmtsRotNP.at(k)).Theta()) < thetaPrimeX+0.3){
                if(k< pmtCalTimes.size() && (pmtCalTimes.at(k))>5.){
                    finalPlane->SetPoint(idx6,(pmtsRotNP.at(k)).Phi(),pmtCalTimes.at(k));
                    idx6++;
                }
            }
        }
    }

    //-- Range Fit ----------------------------------------------------------------------------------
    TF1* fitRangePhi = new TF1("fitRangePhi",&MuonScintillator::FitFuncEXOplane,-pi,pi,3);

    fitRangePhi->SetParameters(0.1,30.,0.);
    fitRangePhi->SetParLimits(2,-pi,pi);

    finalPlane->Fit(fitRangePhi,"E,M,N,Q","",-pi,pi);
    double firstGuessPhi = fitRangePhi->GetParameter(2);
    double finalOffset = finalPlane->Eval(firstGuessPhi,0,"");

    double phiXP = 0.;

    // If firstGuessPhi ~Pi -> Rotate coordinates
    if(firstGuessPhi < -2.4 || firstGuessPhi > 2.4){

        // Rotating All Coordinates by Pi ... ------------------------------------------------

        const double rotationAngleZ = pi;
        TVector3 rotationAxisZ;
        rotationAxisZ.SetMagThetaPhi(1.,0.,0.);

        TRotation rotationZAxis;
        rotationZAxis.Rotate(rotationAngleZ,rotationAxisZ);
        vector<TVector3> lastCopyRotNP(pmtsRotNP);
        vector<TVector3> lastCopyRotNP2;

        for(size_t m=0; m< lastCopyRotNP.size(); ++m){
            TVector3 LastCopyRotNP(lastCopyRotNP.at(m));
            LastCopyRotNP.Transform(rotationZAxis);
            lastCopyRotNP2.push_back(LastCopyRotNP);
        }

        // ... and selecting plane again -----------------------------------------------------

        delete finalPlane;
        finalPlane = new TGraph();
        int idx7 = 0;

        for(size_t k=0; k< lastCopyRotNP2.size(); ++k){
            if( thetaPrimeX-0.1 < ((lastCopyRotNP2.at(k)).Theta()) && ((lastCopyRotNP2.at(k)).Theta()) < thetaPrimeX+0.1){
                if(k< pmtCalTimes.size() && (pmtCalTimes.at(k))>5.){
                    finalPlane->SetPoint(idx7,(lastCopyRotNP2.at(k)).Phi(),pmtCalTimes.at(k));
                    idx7++;
                }
            }
        }

        if(finalPlane->GetN() < 100){
            delete finalPlane;
            finalPlane = new TGraph();
            int idx8 = 0;

            for(size_t k=0; k< lastCopyRotNP2.size(); ++k){
                if( thetaPrimeX-0.2 < ((lastCopyRotNP2.at(k)).Theta()) && ((lastCopyRotNP2.at(k)).Theta()) < thetaPrimeX+0.2){
                    if(k< pmtCalTimes.size() && (pmtCalTimes.at(k))>5.){
                        finalPlane->SetPoint(idx8,(lastCopyRotNP2.at(k)).Phi(),pmtCalTimes.at(k));
                        idx8++;
                    }
                }
            }
        }

        // ... and final fit ------------------------------------------------------------------

        if(firstGuessPhi < 0.){
            firstGuessPhi += pi;
        }
        else{
            firstGuessPhi -= pi;
        }

        // perform linear fits on subranges in the data points
        finalPlane->Fit("pol1","E,M,Q,S","",firstGuessPhi-1.,firstGuessPhi);
        TF1* fitPhiFinalLeft = (TF1*)finalPlane->GetFunction("pol1");
        // Get the slope
        double phiPar0 = fitPhiFinalLeft->GetParameter(1);
        // other side
        finalPlane->Fit("pol1","E,M,Q,S","",firstGuessPhi,firstGuessPhi+1.);
        TF1* fitPhiFinalRight = (TF1*)finalPlane->GetFunction("pol1");
        double phiPar3 = fitPhiFinalRight->GetParameter(1);

        TF1* fitPhiXP = new TF1("f_Phi_XP",&MuonScintillator::FitFuncEPphi,-pi,pi,6);
        fitPhiXP->SetParameters(phiPar0,-1.,firstGuessPhi,phiPar3,1.,finalOffset);
        /* scale left   */ fitPhiXP->SetParLimits(0,phiPar0+0.5*phiPar0,phiPar0-0.5*phiPar0);
        /* centroid     */ fitPhiXP->SetParLimits(2,firstGuessPhi-0.6,firstGuessPhi+0.6);
        /* scale right  */ fitPhiXP->SetParLimits(3,phiPar3-0.5*phiPar3,phiPar3+0.5*phiPar3);
        /* offset       */ fitPhiXP->SetParLimits(5,finalOffset-0.3*finalOffset,finalOffset+0.3*finalOffset);

        finalPlane->Fit("f_Phi_XP","E,M,Q","",-2.,2.);

        // Rotate back phiXP
        if(firstGuessPhi < -2.4){
            phiXP = (fitPhiXP->GetParameter(2))-pi;
        }
        else{
            phiXP = (fitPhiXP->GetParameter(2))+pi;
        }
    }
    else{
        //-- Final Fit -------------------------------------------------------------

        // perform linear fits on subranges in the data points
        finalPlane->Fit("pol1","E,M,Q,S","",firstGuessPhi-1.,firstGuessPhi);
        TF1* fitPhiFinalLeft = (TF1*)finalPlane->GetFunction("pol1");
        // Get the slope
        double phiPar0 = fitPhiFinalLeft->GetParameter(1);
        // other side
        finalPlane->Fit("pol1","E,M,Q,S","",firstGuessPhi,firstGuessPhi+1.);
        TF1* fitPhiFinalRight = (TF1*)finalPlane->GetFunction("pol1");
        double phiPar3 = fitPhiFinalRight->GetParameter(1);

        TF1* fitPhiXP = new TF1("fitPhiXP",&MuonScintillator::FitFuncEPphi,-pi,pi,6);
        fitPhiXP->SetParameters(phiPar0,-1.,firstGuessPhi,phiPar3,1.,finalOffset);
        /* scale left   */ fitPhiXP->SetParLimits(0,phiPar0+0.5*phiPar0,phiPar0-0.5*phiPar0);
        /* centroid     */ fitPhiXP->SetParLimits(2,firstGuessPhi-0.6,firstGuessPhi+0.6);
        /* scale right  */ fitPhiXP->SetParLimits(3,phiPar3-0.5*phiPar3,phiPar3+0.5*phiPar3);
        /* offset       */ fitPhiXP->SetParLimits(5,finalOffset-0.3*finalOffset,finalOffset+0.3*finalOffset);

        finalPlane->Fit("f_Phi_XP","E,M,Q","",firstGuessPhi-2.,firstGuessPhi+2.);
        phiXP = fitPhiXP->GetParameter(2);
    }

    //-- XP Vector in North-Pole-Coordinates before rotating back ------------------------------------
    TVector3 northPoleCoorXP;
    northPoleCoorXP.SetMagThetaPhi(1.,thetaPrimeX,phiXP);

    //-- Rotating back from NP coordinates to normal coordinates -------------------------------------------------

    double invRotAngleNP = -angleNP;

    // Inverse Rotation from NP to Normal
    TRotation invRotationNP;
    invRotationNP.Rotate(invRotAngleNP,rotationAxisNP);

    // Rotating
    TVector3 recoXP = (northPoleCoorXP).Transform(invRotationNP);

    DS::FitVertex muonTrack;
    muonTrack.SetPosition( recoXP );
    muonTrack.SetDirection( recoXP - recoEP );
    fFitResult.SetVertex( 0, muonTrack );
    return fFitResult;

}