//
// ********************************************************************
// * License and Disclaimer                                           *
// *                                                                  *
// * The  Geant4 software  is  copyright of the Copyright Holders  of *
// * the Geant4 Collaboration.  It is provided  under  the terms  and *
// * conditions of the Geant4 Software License,  included in the file *
// * LICENSE and available at  http://cern.ch/geant4/license .  These *
// * include a list of copyright holders.                             *
// *                                                                  *
// * Neither the authors of this software system, nor their employing *
// * institutes,nor the agencies providing financial support for this *
// * work  make  any representation or  warranty, express or implied, *
// * regarding  this  software system or assume any liability for its *
// * use.  Please see the license in the file  LICENSE  and URL above *
// * for the full disclaimer and the limitation of liability.         *
// *                                                                  *
// * This  code  implementation is the result of  the  scientific and *
// * technical work of the GEANT4 collaboration.                      *
// * By using,  copying,  modifying or  distributing the software (or *
// * any work based  on the software)  you  agree  to acknowledge its *
// * use  in  resulting  scientific  publications,  and indicate your *
// * acceptance of all terms of the Geant4 Software license.          *
// ********************************************************************
//
// G4PolynomialSolver inline methods implementation
//
// Author: E.Medernach, 19.12.2000 - First implementation
// --------------------------------------------------------------------

#define POLEPSILON 1e-12
#define POLINFINITY 9.0E99
#define ITERATION 12  // 20 But 8 is really enough for Newton with a good guess

template <class T, class F>
G4PolynomialSolver<T, F>::G4PolynomialSolver(T* typeF, F func, F deriv,
                                             G4double precision)
{
  Precision     = precision;
  FunctionClass = typeF;
  Function      = func;
  Derivative    = deriv;
}

template <class T, class F>
G4PolynomialSolver<T, F>::~G4PolynomialSolver()
{}

template <class T, class F>
G4double G4PolynomialSolver<T, F>::solve(G4double IntervalMin,
                                         G4double IntervalMax)
{
  return Newton(IntervalMin, IntervalMax);
}

/* If we want to be general this could work for any
   polynomial of order more that 4 if we find the (ORDER + 1)
   control points
*/
#define NBBEZIER 5

template <class T, class F>
G4int G4PolynomialSolver<T, F>::BezierClipping(/*T* typeF,F func,F deriv,*/
                                               G4double* IntervalMin,
                                               G4double* IntervalMax)
{
  /** BezierClipping is a clipping interval Newton method **/
  /** It works by clipping the area where the polynomial is **/

  G4double P[NBBEZIER][2], D[2];
  G4double NewMin, NewMax;

  G4int IntervalIsVoid = 1;

  /*** Calculating Control Points  ***/
  /* We see the polynomial as a Bezier curve for some control points to find */

  /*
    For 5 control points (polynomial of degree 4) this is:

    0     p0 = F((*IntervalMin))
    1/4   p1 = F((*IntervalMin)) + ((*IntervalMax) - (*IntervalMin))/4
                 * F'((*IntervalMin))
    2/4   p2 = 1/6 * (16*F(((*IntervalMax) + (*IntervalMin))/2)
                      - (p0 + 4*p1 + 4*p3 + p4))
    3/4   p3 = F((*IntervalMax)) - ((*IntervalMax) - (*IntervalMin))/4
                 * F'((*IntervalMax))
    1     p4 = F((*IntervalMax))
  */

  /* x,y,z,dx,dy,dz are constant during searching */

  D[0] = (FunctionClass->*Derivative)(*IntervalMin);

  P[0][0] = (*IntervalMin);
  P[0][1] = (FunctionClass->*Function)(*IntervalMin);

  if(std::fabs(P[0][1]) < Precision)
  {
    return 1;
  }

  if(((*IntervalMax) - (*IntervalMin)) < POLEPSILON)
  {
    return 1;
  }

  P[1][0] = (*IntervalMin) + ((*IntervalMax) - (*IntervalMin)) / 4;
  P[1][1] = P[0][1] + (((*IntervalMax) - (*IntervalMin)) / 4.0) * D[0];

  D[1] = (FunctionClass->*Derivative)(*IntervalMax);

  P[4][0] = (*IntervalMax);
  P[4][1] = (FunctionClass->*Function)(*IntervalMax);

  P[3][0] = (*IntervalMax) - ((*IntervalMax) - (*IntervalMin)) / 4;
  P[3][1] = P[4][1] - ((*IntervalMax) - (*IntervalMin)) / 4 * D[1];

  P[2][0] = ((*IntervalMax) + (*IntervalMin)) / 2;
  P[2][1] =
    (16 * (FunctionClass->*Function)(((*IntervalMax) + (*IntervalMin)) / 2) -
     (P[0][1] + 4 * P[1][1] + 4 * P[3][1] + P[4][1])) /
    6;

  {
    G4double Intersection;
    G4int i, j;

    NewMin = (*IntervalMax);
    NewMax = (*IntervalMin);

    for(i = 0; i < 5; ++i)
      for(j = i + 1; j < 5; ++j)
      {
        /* there is an intersection only if each have different signs */
        if(((P[j][1] > -Precision) && (P[i][1] < Precision)) ||
           ((P[j][1] < Precision) && (P[i][1] > -Precision)))
        {
          IntervalIsVoid = 0;
          Intersection =
            P[j][0] - P[j][1] * ((P[i][0] - P[j][0]) / (P[i][1] - P[j][1]));
          if(Intersection < NewMin)
          {
            NewMin = Intersection;
          }
          if(Intersection > NewMax)
          {
            NewMax = Intersection;
          }
        }
      }

    if(IntervalIsVoid != 1)
    {
      (*IntervalMax) = NewMax;
      (*IntervalMin) = NewMin;
    }
  }

  if(IntervalIsVoid == 1)
  {
    return -1;
  }

  return 0;
}

template <class T, class F>
G4double G4PolynomialSolver<T, F>::Newton(G4double IntervalMin,
                                          G4double IntervalMax)
{
  /* So now we have a good guess and an interval where
     if there are an intersection the root must be */

  G4double Value    = 0;
  G4double Gradient = 0;
  G4double Lambda;

  G4int i = 0;
  G4int j = 0;

  /* Reduce interval before applying Newton Method */
  {
    G4int NewtonIsSafe;

    while((NewtonIsSafe = BezierClipping(&IntervalMin, &IntervalMax)) == 0)
      ;

    if(NewtonIsSafe == -1)
    {
      return POLINFINITY;
    }
  }

  Lambda = IntervalMin;
  Value  = (FunctionClass->*Function)(Lambda);

  //  while ((std::fabs(Value) > Precision)) {
  while(j != -1)
  {
    Value = (FunctionClass->*Function)(Lambda);

    Gradient = (FunctionClass->*Derivative)(Lambda);

    Lambda = Lambda - Value / Gradient;

    if(std::fabs(Value) <= Precision)
    {
      ++j;
      if(j == 2)
      {
        j = -1;
      }
    }
    else
    {
      ++i;

      if(i > ITERATION)
        return POLINFINITY;
    }
  }
  return Lambda;
}