//https://root.cern.ch/root/html/ROOT__Math__GSLMultiRootFinder.html:
//solves a non-linear system of equations...
//x[0]=t0 / x[1]=x / x[2]=y / x[3]=z: 0=c^2*x[0]^2 - (Rx.x-x[1])^2 - (Rx.y-x[2])^2 - (Rx.z-x[3])^2
// example of using multiroot finder 
// based on GSL algorithm 
//
// The MultiRootFinder is based on GSL and it requires the MathMore library 
// installed 
//
// Usage: 
//  >.x exampleMultiRoot.C()
// or 
// >.x exampleMultiRoot(algoname,printlevel)
//
// where algoname is for an algorithm not using the derivatives:
//  hybridS (default) , hybrid, dnewton, broyden
// 
#include "RConfigure.h"
#ifdef R__HAS_MATHMORE
#include "Math/MultiRootFinder.h"
#endif
#include "Math/WrappedMultiTF1.h"
//#include "TF2.h"
#include "TError.h"
// example of using multi root finder based on GSL 
// need to use an algorithm not requiring the derivative
//like hybrids (default), hybrid, dnewton, broyden
const Float_t cmpernano=TMath::C()*1e-9;
double ctsqr0 (double *x, double *p0 ) {
  return cmpernano*(p0[0]-x[0])*cmpernano*(p0[0]-x[0]) - (p0[1]-x[1])*(p0[1]-x[1]) - (p0[2]-x[2])*(p0[2]-x[2]) - (p0[3]-x[3])*(p0[3]-x[3]);}

double ctsqr1 (double *x, double *p1 ) {
  return cmpernano*(p1[0]-x[0])*cmpernano*(p1[0]-x[0]) - (p1[1]-x[1])*(p1[1]-x[1]) - (p1[2]-x[2])*(p1[2]-x[2]) - (p1[3]-x[3])*(p1[3]-x[3]);}

double ctsqr2 (double *x, double *p2 ) {
  return cmpernano*(p2[0]-x[0])*cmpernano*(p2[0]-x[0]) - (p2[1]-x[1])*(p2[1]-x[1]) - (p2[2]-x[2])*(p2[2]-x[2]) - (p2[3]-x[3])*(p2[3]-x[3]);}

double ctsqr3 (double *x, double *p3 ) {
  return cmpernano*(p3[0]-x[0])*cmpernano*(p3[0]-x[0]) - (p3[1]-x[1])*(p3[1]-x[1]) - (p3[2]-x[2])*(p3[2]-x[2]) - (p3[3]-x[3])*(p3[3]-x[3]);}

void exampleMultiRoot(const char * algo=0, int printlevel=3) {
#ifndef R__HAS_MATHMORE
  Error("exampleMultiRoot","libMathMore is not available - cannot run this tutorial");
#else 
  ROOT::Math::MultiRootFinder r(algo);
  //defining the function
  TF1 *ctsqr0 = new TF1("ctsqr0", ctsqr0, 0, 0, 4);
  TF1 *ctsqr1 = new TF1("ctsqr1", ctsqr1, 0, 0, 4);
  TF1 *ctsqr2 = new TF1("ctsqr2", ctsqr2, 0, 0, 4);
  TF1 *ctsqr3 = new TF1("ctsqr3", ctsqr3, 0, 0, 4);

  //fake parameters - source at 0 0 0 0
  //parameters are: hittime / x / y / z
  ctsqr0->SetParameters(0,0,0,3.333);
  ctsqr0->SetParameters(0,0,3.333,0);
  ctsqr0->SetParameters(0,3.3333,0,0);
  ctsqr0->SetParameters(10,0,0,6.666);

  // wrap the functions
  ROOT::Math::WrappedMultiTF1 g0(*ctsqr0, 4);
  ROOT::Math::WrappedMultiTF1 g1(*ctsqr1, 4);
  ROOT::Math::WrappedMultiTF1 g2(*ctsqr2, 4);
  ROOT::Math::WrappedMultiTF1 g3(*ctsqr3, 4);

  r.AddFunction(g0);
  r.AddFunction(g1);
  r.AddFunction(g2);
  r.AddFunction(g3);

  r.SetPrintLevel(printlevel);
  // starting point - This starting point is the solution and when used here, program converges.
  double x0[4]={-10,-1,-2,-1}; 
  /*
 root [0] .x exampleMultiRoot.C("kDNewton",1)
 GSLMultiRootFinder::Solve:dnewton max iterations 100 and  tolera1e-06
 Info in <ROOT::Math::GSLMultiRootFinder::Solve>: The iteration converged
 GSL Algorithm used is :  dnewton
 Number of iterations  =  4
 Root values     = x[0] =     0.249173   x[1] =    -0.557852   x[2] =     0.926722   x[3] =     0.370827   x[4] =     0.130852   x[5] =     0.833278   x[6] =    -0.714794   x[7] =     0.475908
 Function values = f[0] = -4.58058e-10   f[1] =  4.58043e-10   f[2] = -5.12292e-10   f[3] =  5.12289e-10   f[4] =   2.1217e-10   f[5] = -2.12151e-10   f[6] =  8.80505e-11   f[7] =  1.00293e-10
  */
  // With this starting value, program does not converge.
  //    double x0[8]={ 0.01, -0.02, 0.06, 0.01, 0., 0.01, -0.09, 0.02};
  /*
 root [0] .x exampleMultiRoot.C("kDNewton",1)
 GSLMultiRootFinder::Solve:dnewton max iterations 100 and  tolerance 1e-06
 Info in <ROOT::Math::GSLMultiRootFinder::Solve>: exceeded max iterations, reached tolerance is not sufficient; absTol = 1e-06
  */
  r.Solve(x0);
#endif
}