#include <iostream>
#include <vector>
#include "TMath.h"
#include "TGraph.h"
#include "TF1.h"
#include "TRandom.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 FindAllRoots2.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
   /*
    * Extended check including boundaries; and using IsEqual
    */ 
   if ( status == 0 && (s <= r || IsEqual(r, s, tol) ) && ( r <= e || IsEqual(r, e, tol)) ) { 
      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; 
   }
}

class MyFunctionObject {
public:
  MyFunctionObject() {
    //fN = gRandom->Integer(10) + 1;
    fN = gRandom ->Poisson(8);
    fSigmas = new double[fN];
    fMus    = new double[fN];
    fAmps   = new double[fN];
    cout << "We have " << fN << " pulses:" << endl;
    for(int i = 0; i < fN; ++i) {
      fSigmas[i] = TMath::Abs(gRandom->Gaus(3.12, 0.312));
      fMus   [i] = TMath::Abs(gRandom->Exp(3.7) + gRandom->Exp(8.8));
      fAmps  [i] = TMath::Abs(gRandom->Gaus(0.3, 0.14));
      cout << fAmps[i] << " " << fMus[i] << " " << fSigmas[i] << endl;
    }
  }
  double operator()(double *x, double* par) {
    double r = 0;
    for(int i = 0; i < fN; ++i) {
      r += fAmps[i] * TMath::Gaus(x[0], fMus[i], fSigmas[i], kFALSE);
    }
    // Subtract in order to cross the y axis.
    double s = 0.1 * fN; // one third of the mean times the number of pulses.
    return r - s;
  }
private:
  int fN;
  double* fSigmas;
  double* fMus;
  double* fAmps;
};

void FindAllRoots2()
{
  cout << "seed=" << gRandom->GetSeed() << endl;
  double s = -50;
  double e = 50;
  cout << "start =" << s << endl;
  cout << "end   =" << e << endl;
  // Create the function and wrap it
  MyFunctionObject* fobj = new MyFunctionObject();
  TF1* f = new TF1("MyFunctionObject", fobj, s, e, 1, "MyFunctionObject");
  f->Draw();
  vector<double> roots;
  ROOT::Math::RootFinder rf(ROOT::Math::RootFinder::kGSL_STEFFENSON);
  ROOT::Math::GradFunctor1D wf(*f);
  // Divide the interval in smaller regions.
  int N = 80;
  double gap = (e-s) / N;
  double currStart = s;
  double currEnd   = currStart + gap;
  for (int i = 0; i < N; ++i, currStart += gap, currEnd += gap) {
    cout << "[" << currStart << "," << currEnd << "]" << endl;
    // Create a function for the smaller interval.
    TF1 g("g", fobj, currStart, currEnd, 1, "MyFunctionObject");
    // Check extrema
    double min = g.GetMinimum();
    double max = g.GetMaximum();
    cout << "min=" << min << " max=" << max << endl;
    if (TMath::Sign(1.0, min) != TMath::Sign(1.0, max)){
      // Solve for roots if the extrema have different sign.
      Solve(rf, roots, 0, wf, currStart, currEnd);
    }
  }
  sort(roots.begin(), roots.end());
  double* xx = new double[roots.size()];
  double* yy = new double[roots.size()];
  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;
  if (roots.size()){
    TGraph* gr = new TGraph(roots.size(), xx, yy);
    gr->SetMarkerColor(kRed);
    gr->SetMarkerStyle(21);
    gr->Draw("P");
  }
  cout.flush();
}