#include "TCanvas.h"
 #include "TRandom3.h"
 #include "TGraph.h"
 #include "TMath.h"
 #include "TArc.h"
 #include "Fit/Fitter.h"

void circleFit(Float_t *x,Float_t* y);
Float_t  x[4],y[4];

void targetPosition() {

   gStyle->SetOptStat(0);

   TChain *ch_be = new TChain("tree");
   ch_be->Add("/media/ivan/data/exp1906/be10/analysed/beamDiagnostics/eqTriggers/be10*");
   // ch_be->Add("/mnt/data/exp1904/analysed/selected/beamDiagnostics/h7*");


   TF2 *fitf = new TF2("fitf","bigaus");
   
   TCanvas *c1 = new TCanvas("c1","title",1000,1000);
   c1->Divide(2,2);

   c1->cd(1);
   ch_be->Draw("fYt:fXt>>p2(64,-20,20,64,-20,20)","trigger==2","col");
   TH2F *htemp = (TH2F*)gPad->GetPrimitive("p2");
   fitf->SetRange(-14.38,-6.25,-8.13,6.88);
   htemp->Fit(fitf,"R");

   x[0] = fitf->GetParameter("MeanX");
   y[0] = fitf->GetParameter("MeanY");

   c1->cd(2);
   ch_be->Draw("fYt:fXt>>p3(64,-20,20,64,-20,20)","trigger==3","col");
   htemp = (TH2F*)gPad->GetPrimitive("p3");
   fitf->SetRange(-8.13,-14.38,6.88,-9.38);
   htemp->Fit(fitf,"R+");

   x[1] = fitf->GetParameter("MeanX");
   y[1] = fitf->GetParameter("MeanY");   

   c1->cd(3);
   ch_be->Draw("fYt:fXt>>p4(64,-20,20,64,-20,20)","trigger==4","col");
   htemp = (TH2F*)gPad->GetPrimitive("p4");
   fitf->SetRange(8.75,-6.88,13.75,5.63);
   htemp->Fit(fitf,"R+");

   x[2] = fitf->GetParameter("MeanX");
   y[2] = fitf->GetParameter("MeanY"); 

   c1->cd(4);
   ch_be->Draw("fYt:fXt>>p5(64,-20,20,64,-20,20)","trigger==5","col");
   htemp = (TH2F*)gPad->GetPrimitive("p5");
   fitf->SetRange(-6.25,8.75,8.13,14.38);
   htemp->Fit(fitf,"R+");

   x[3] = fitf->GetParameter("MeanX");
   y[3] = fitf->GetParameter("MeanY");    

   c1->Update();
   c1->Print("/home/ivan/Desktop/sent/triggerPositions.png");

   for (Int_t i=0;i<4;i++) {
      cout << x[i] << " " << y[i] << endl;
   }

   circleFit(x,y);

}


void circleFit(Float_t *x,Float_t* y) {

  TGraph *g = new TGraph(4,x,y);
  g->SetMarkerStyle(20);

  auto chi2Function = [&](const Double_t *par) {
       //minimisation function computing the sum of squares of residuals
       // looping at the graph points
       Int_t np = g->GetN();
       Double_t f = 0;
       Double_t *x = g->GetX();
       Double_t *y = g->GetY();
       for (Int_t i=0;i<np;i++) {
          Double_t u = x[i] - par[0];
          Double_t v = y[i] - par[1];
          Double_t dr = par[2] - std::sqrt(u*u+v*v);
          // Double_t dr = 12.5*12.5 - std::sqrt(u*u+v*v);
          f += dr*dr;
       }
       return f;
    };

    // wrap chi2 funciton in a function object for the fit
    // 3 is the number of fit parameters (size of array par)
    ROOT::Math::Functor fcn(chi2Function,3);
    ROOT::Fit::Fitter  fitter;
 
 
    double pStart[3] = {0,0,12};
    fitter.SetFCN(fcn, pStart);
    fitter.Config().ParSettings(0).SetName("x0");
    fitter.Config().ParSettings(0).SetLimits(-5,5);

    fitter.Config().ParSettings(1).SetName("y0");
    fitter.Config().ParSettings(1).SetLimits(-5,5);

    fitter.Config().ParSettings(2).SetName("R");
    fitter.Config().ParSettings(2).SetLimits(5,20);
 
    // do the fit 
    bool ok = fitter.FitFCN();
    if (!ok) {
       Error("line3Dfit","Line3D Fit failed");
    }   
 
    const ROOT::Fit::FitResult & result = fitter.Result();
    result.Print(std::cout);
 
    //Draw the circle on top of the points

    TCanvas *c1 = new TCanvas("c1","fit",1000,1000);
    c1->cd();

    TArc *arc = new TArc(result.Parameter(0),result.Parameter(1),result.Parameter(2));
    arc->SetFillStyle(0); 
    arc->SetLineColor(kRed);
    arc->SetLineWidth(2);

    g->Draw();
    arc->Draw("same");
    c1->Print("/home/ivan/Desktop/sent/targetFit.png");
}