#include <iostream>
#include <vector>
#include "TMath.h"
#include "TGraph.h"
#include "TF1.h"


#include "Math/RootFinder.h"
#include "Math/Functor.h"

// Find  multiple root of a function
// script must be run in compiled mode for instantiating correctly the GradFunctor1D object
// .x FindRoot.C+

bool IsEqual(double a, double b, double eps) { 
   return TMath::Abs(a-b) <  eps * TMath::Max( TMath::Abs(a), TMath::Abs(b) );
} 

void Solve(ROOT::Math::RootFinder & rf, vector<double>& roots, int nest, const ROOT::Math::IGradFunction & f, double s, double e)
{

   double x0 = (s+e)/2;
   rf.SetFunction(f, x0); 
   double tol = 1.E-6;
   int status = rf.Solve(100,tol,tol);
   double r = rf.Root();

   // is status is ok and root is in the interval
   if ( status == 0 && r > s && r < e) { 
      cout << "(" << nest << ") start=" << s << " end=" << e << " root=" << r << endl;
      roots.push_back(r);
      if ( !IsEqual(r,s,tol) && ! IsEqual(r,e,tol ) ) {
         Solve(rf, roots, nest+1, f, s, r);
         Solve(rf, roots, nest+1, f, r, e);
      }
   }
   else { 
      std::cout << "(" << nest << ") start=" << s << " end=" << e; 
      if (status != 0) 
         std::cout << " Error solving for roots - status " << status << std::endl;
      else 
         std::cout << " No  root found in  interval  -  r=" << r <<   std::endl;      
      return; 
   }
}
void FindRoot()
{
  double s = - TMath::PiOver2();
  double e = 2 * TMath::TwoPi() + TMath::PiOver2();
  cout << "start =" << s << endl;
  cout << "end   =" << e << endl;
  // Create the function and wrap it
  TF1* f = new TF1("Sin Function", "sin(x)", s, e);
  f->Draw();
  vector<double> roots;
  ROOT::Math::RootFinder rf(ROOT::Math::RootFinder::kGSL_STEFFENSON);
  ROOT::Math::GradFunctor1D wf(*f); 
  
  Solve(rf, roots, 0, wf, s, e);
  sort(roots.begin(), roots.end());
  double* xx = new double[roots.size()];
  double* yy = new double[roots.size()];
  // If this is put here; then only the first point
  // is shown in the plot.
  //TGraph* gr = new TGraph(7, xx, yy);
  cout << "Found " << roots.size() << " roots:";
  for (unsigned int i = 0; i < roots.size(); ++i) {
    xx [i] = roots[i];
    yy [i] = 0;
    cout << xx[i] << " "; 
  }
  cout << endl;
  TGraph* gr = new TGraph(7, xx, yy);
  gr->SetMarkerColor(kRed);
  gr->SetMarkerStyle(21);
  gr->Draw("P");
  //c1->Update();
  cout.flush();
}