' ******************************************************************
' Function minimization by Newton-Raphson method
' ******************************************************************

' ------------------------------------------------------------------
' Error codes
' ------------------------------------------------------------------

#DEFINE OptOk 0
' No error

#DEFINE OptNonConv -1
' Non-convergence

#DEFINE OptSing -2
' Quasi-singular hessian matrix

' ------------------------------------------------------------------
' Global variable
' ------------------------------------------------------------------

COMMON SHARED ErrCode AS INTEGER
' Error code from the last function evaluation

' ------------------------------------------------------------------
' External functions
' ------------------------------------------------------------------

DECLARE SUB LinEq(      A() AS DOUBLE,_
                        B() AS DOUBLE,_
                  BYREF Det AS DOUBLE)
' Solves the linear system A * X = B

DECLARE SUB LinMin(      Func     AS FUNCTION(X() AS DOUBLE) AS DOUBLE,_
                         X()      AS DOUBLE,_ 
                         DeltaX() AS DOUBLE,_ 
                   BYREF R        AS DOUBLE,_ 
                         MaxIter  AS INTEGER,_ 
                         Tol      AS DOUBLE,_ 
                   BYREF Fmin     AS DOUBLE)
' Minimizes function Func from X() in the direction specified by DeltaX()

DECLARE FUNCTION CompVec(X()    AS DOUBLE,_ 
                         Xref() AS DOUBLE,_ 
                         Tol    AS DOUBLE) AS INTEGER
' Compares two vectors

' ******************************************************************

SUB Init(      Func     AS FUNCTION(X() AS DOUBLE) AS DOUBLE,_
               HessGrad AS SUB(X() AS DOUBLE, G() AS DOUBLE, H() AS DOUBLE),_
               X()      AS DOUBLE,_ 
               DeltaX() AS DOUBLE,_
         BYREF Fmin     AS DOUBLE,_ 
               G()      AS DOUBLE,_ 
               Hinv()   AS DOUBLE,_ 
         BYREF Det      AS DOUBLE)
' ------------------------------------------------------------------
' Initializes variables
' ------------------------------------------------------------------

  DIM AS INTEGER I  ' Loop variable

  ' Initialize function
  Fmin = Func(X())

  ' Initialize Gradient and Hessian (Stored in Hinv)
  HessGrad(X(), G(), Hinv())

  ' Initialize search direction
  FOR I = LBOUND(X) TO UBOUND(X)
    DeltaX(I) = - G(I)
  NEXT I

  ' Solve system
  LinEq(Hinv(), DeltaX(), Det)
END SUB

SUB Newton (      Func     AS FUNCTION(X() AS DOUBLE) AS DOUBLE,_
                  HessGrad AS SUB(X() AS DOUBLE, G() AS DOUBLE, H() AS DOUBLE),_
                  X()      AS DOUBLE,_ 
                  MaxIter  AS INTEGER,_ 
                  Tol      AS DOUBLE,_ 
            BYREF Fmin     AS DOUBLE,_ 
                  G()      AS DOUBLE,_ 
                  Hinv()   AS DOUBLE,_ 
            BYREF Det      AS DOUBLE)
' ------------------------------------------------------------------
' Minimizes a function of several variables by Newton's method.
' ------------------------------------------------------------------
' Input parameters  : Func     = function to be minimized
'                     HessGrad = subroutine to compute hessian
'                                and gradient
'                     X        = initial minimum coordinates
'                     MaxIter  = maximum number of iterations
'                     Tol      = required precision
' ------------------------------------------------------------------
' Output parameters : X    = refined minimum coordinates
'                     Fmin = function value at minimum
'                     G    = gradient vector
'                     Hinv = inverse hessian matrix
'                     Det  = determinant of hessian
' ------------------------------------------------------------------
' The function MathErr() returns one of the following codes:
'   OptOk      = no error
'   OptNonConv = non-convergence
'   OptSing    = singular hessian matrix
' ------------------------------------------------------------------

  DIM AS INTEGER L, N  ' Array bounds
  DIM AS INTEGER I, J  ' Loop variables
  DIM AS DOUBLE  R     ' Step for line minimization
  DIM AS INTEGER Iter  ' Iteration count

  L = LBOUND(X)
  N = UBOUND(X)

  DIM AS DOUBLE OldX(L TO N)    ' Old parameters
  DIM AS DOUBLE DeltaX(L TO N)  ' New search direction

  ' Initialize
  Init(Func, HessGrad, X(), DeltaX(), Fmin, G(), Hinv(), Det)
  if ErrCode <> 0 THEN ErrCode = OptSing : EXIT SUB

  ' Quit if no iteration required
  IF MaxIter < 1 THEN ErrCode = OptOk : EXIT SUB

  Iter = 0

  DO
    ' Prepare next iteration
    Iter = Iter + 1
    IF Iter > MaxIter THEN ErrCode = OptNonConv : EXIT SUB

    ' Save current parameters
    FOR I = L TO N
      OldX(I) = X(I)
    NEXT I

    ' Compute new parameter values by minimizing in the direction
    ' specified by DeltaX, using an initial step of 0.1
    R = 0.1
    LinMin(Func, X(), DeltaX(), R, 10, 0.01, Fmin)
    
    ' Compute new Hessian and Gradient
    HessGrad(X(), G(), Hinv())
    
    ' Initialize search direction
    FOR I = L TO N
      DeltaX(I) = - G(I)
    NEXT I

    ' Solve linear system
    LinEq(Hinv(), DeltaX(), Det)
    IF ErrCode <> 0 THEN ErrCode = OptSing : EXIT SUB
  LOOP UNTIL CompVec(X(), OldX(), Tol)
  
  ErrCode = OptOk
END SUB

