#include <TMath.h>
#include <TGraph2D.h>
#include <TRandom2.h>
#include <TStyle.h>
#include <TCanvas.h>
#include <TF2.h>
#include <TH1.h>
#include <Math/Functor.h>
#include <TPolyLine3D.h>
#include <Math/Vector3D.h>
#include <Fit/Fitter.h>

#include <cassert>
 
using namespace ROOT::Math;

// define the parametric line equation
void line(double t, const double *p, double &x, double &y, double &z) {
   // a parametric line is define from 6 parameters but 4 are independent
   // x0,y0,z0,z1,y1,z1 which are the coordinates of two points on the line
   // can choose z0 = 0 if line not parallel to x-y plane and z1 = 1;
   x = p[0] + p[1]*t;
   y = p[2] + p[3]*t;
   z = t;
}

bool first = true;
 
// function Object to be minimized
struct SumDistance2 {
   // the TGraph is a data member of the object
   TGraph2D *fGraph;
 
   SumDistance2(TGraph2D *g) : fGraph(g) {}
 
   // calculate distance line-point
   double distance2(double x,double y,double z, const double *p) {
      // distance line point is D= | (xp-x0) cross  ux |
      // where ux is direction of line and x0 is a point in the line (like t = 0)
      XYZVector xp(x,y,z);
      XYZVector x0(p[0], p[2], 0. );
      XYZVector x1(p[0] + p[1], p[2] + p[3], 1. );
      XYZVector u = (x1-x0).Unit();
      double d2 = ((xp-x0).Cross(u)).Mag2();
      return d2;
   }
 
   // implementation of the function to be minimized
   double operator() (const double *par) {
      assert(fGraph != nullptr);
      double * x = fGraph->GetX();
      double * y = fGraph->GetY();
      double * z = fGraph->GetZ();
      int npoints = fGraph->GetN();
      double sum = 0;
      for (int i  = 0; i < npoints; ++i) {
         double d = distance2(x[i],y[i],z[i],par);
         sum += d;
      }
      return sum;
   }
 
};
 
void tracker_minimum_resolution()
{
   gStyle->SetOptStat(0);
   gStyle->SetOptFit();
 
   int nSimPoints = 1000;

   const float z_DUT = 288;
   
   const int nPlanes = 6;
   const float xSigmaPlanes[nPlanes] = {1.98233e-03,2.97789e-03,3.21846e-03,4.32309e-03,3.24355e-03,1.67209e-03};
   const float ySigmaPlanes[nPlanes] = {1.99629e-03,3.01630e-03,3.19757e-03,4.36420e-03,3.25503e-03,1.68557e-03};
   const float zPlanes[nPlanes] = {573.9,490.4,399.7,213.4,135.1,0};
   const float eAlignment = 1;
 
   TGraph2D *gr[nSimPoints+1];
   TPolyLine3D *l[nSimPoints]; 
   auto *hResolutionXY = new TH2D("tracker resolution at zDUT", "tracker resolution at zDUT",100, -50,50,100,-50,50);


   //creating a graph just to set the right ranges on x and y
   gr[0] = new TGraph2D();
   gr[0]->SetPoint(0,-.1,-.1,zPlanes[0]);
   gr[0]->SetPoint(1, .1, .1,zPlanes[nPlanes-1]);
   gr[0]->GetXaxis()->SetTitle("X [um]");
   gr[0]->GetYaxis()->SetTitle("Y [um]");

   gr[0]->Draw("p0");

   for(int i = 0; i < nSimPoints; i++){
      //filling the graphs with random points with sigma equal to the one obtaianed from the alignment output files
      gr[i+1] = new TGraph2D();
      for(int j = 0; j < nPlanes; j++){
         double x,y = 0;
         x = gRandom->Gaus(0.,xSigmaPlanes[j]);
         y = gRandom->Gaus(0.,ySigmaPlanes[j]);
         gr[i+1]->SetPoint(j,x,y,zPlanes[j]);
      }

      ROOT::Fit::Fitter fitter;
      // make the functor objet
      SumDistance2 sdist(gr[i+1]);
      ROOT::Math::Functor fcn(sdist,4);
      // set the function and the initial parameter values
      double pStart[4] = {0.001,0.,-0.001,0.};
      fitter.SetFCN(fcn,pStart);
      // set step sizes different than default ones (0.3 times parameter values)
      for (int i = 0; i < 4; ++i) fitter.Config().ParSettings(i).SetStepSize(0.01);
      bool ok = fitter.FitFCN();
      if (!ok){
         Error("line3Dfit","Line3D Fit failed");
         return 1;
      }
      const ROOT::Fit::FitResult & result = fitter.Result();

      gr[i+1]->Draw("same p0");
      
      // get fit parameters
      const double *parFit = result.GetParams();
      // draw the fitted line(s)
      l[i] = new TPolyLine3D(nPlanes);
      for (int j = 0; j <nPlanes;++j) {
         double t = zPlanes[j];
         double x,y,z;
         line(t,parFit,x,y,z);
         l[i]->SetPoint(i,x,y,z);
      }
      l[i]->SetLineColor(kRed);
      l[i]->Draw("same");
      
      //Filling the histogram for the resolution at the DUT z
      double x,y,z;
      line(z_DUT,parFit,x,y,z);
      hResolutionXY->Fill(x*1000.,y*1000.); 
   }
   
   auto *c2 =new TCanvas();
   hResolutionXY->Draw("colz");

}