//
// ********************************************************************
// * 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.          *
// ********************************************************************
//
//
// $Id: G4PolynomialSolver.icc,v 1.8 2006/06/29 18:59:54 gunter Exp $
// GEANT4 tag $Name: geant4-09-02-ref-02 $
// 
// class G4PolynomialSolver
//
// 19.12.00 E.Medernach, 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 ;
}