#include "TCanvas.h"
#include "TRandom3.h"
#include "TGraph.h"
#include "TMath.h"
#include "TArc.h"
#include "Fit/Fitter.h"
#include <Math/Functor.h>

void fitCircle(Int_t n=10000) {
   //generates n points around a circle and fit them
   TCanvas *c1 = new TCanvas("c1","c1",600,600);
   c1->SetGrid();
   TGraph* gr = new TGraph(n);
   if (n> 999) gr->SetMarkerStyle(1);
   else        gr->SetMarkerStyle(3);
   TRandom3 r;
   Double_t x,y;
   for (Int_t i=0;i<n;i++) {
      r.Circle(x,y,r.Gaus(4,0.3));
      gr->SetPoint(i,x,y);
   }
   c1->DrawFrame(-5,-5,5,5);
   gr->Draw("p");



   auto chi2Function2 = [&](const Double_t *par) {
      //minimisation function computing the sum of squares of residuals
      // looping at the graph points
      Int_t np = gr->GetN();
      Double_t f = 0;
      Double_t *x = gr->GetX();
      Double_t *y = gr->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);
         f += dr*dr;
      }
      return f;
   };

   // wrap chi2 function in a function object for the fit
   // 3 is the number of fit parameters (size of array par)
   ROOT::Math::Functor fcn(chi2Function2,3);
   ROOT::Fit::Fitter  fitter;

   ROOT::Fit::FitConfig& fitConfig = fitter.Config();

   double pStart[3] = {0,0,1};
   fitter.SetFCN(fcn, pStart);
   fitter.Config().ParSettings(0).SetName("x0");
   fitter.Config().ParSettings(1).SetName("y0");
   fitter.Config().ParSettings(2).SetName("R");

   ROOT::Math::MinimizerOptions minOpt;
   minOpt.SetMinimizerType("Minuit2");
   minOpt.SetMinimizerAlgorithm("MIGRAD");
   fitConfig.SetMinimizerOptions(minOpt);

   // do the fit
//   bool ok = fitter.FitFCN();
   bool ok;
   ok = fitter.FitFCN();
   if (!ok) {
      Error("line3Dfit","Line3D Fit failed");
   }

//   minOpt.SetMinimizerAlgorithm("MIGRAD");
//   fitConfig.SetMinimizerOptions(minOpt);
//   ok = fitter.FitFCN();


   const ROOT::Fit::FitResult & result = fitter.Result();
   result.Print(std::cout);

   TFitResult t_result(result);
   TMatrixDSym correlationM =  t_result.GetCorrelationMatrix();
   correlationM.Print();

   //Draw the circle on top of the points
   TArc *arc = new TArc(result.Parameter(0),result.Parameter(1),result.Parameter(2));
   arc->SetLineColor(kRed);
   arc->SetLineWidth(4);
   arc->SetFillStyle(0);
   arc->Draw();

}