' ******************************************************************
' Function minimization by the BFGS (Broyden-Fletcher-Goldfarb-
' Shanno method)
' ******************************************************************

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

#DEFINE OptOk 0
' No error

#DEFINE OptNonConv -1
' Non-convergence

' ------------------------------------------------------------------
' Machine-dependent constant
' ------------------------------------------------------------------

#DEFINE MachEp 2.220446049250313D-16
' Floating point precision: 2^(-52)

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

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

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

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,_
               Gradient AS SUB(X() AS DOUBLE, G() AS DOUBLE),_
               X()      AS DOUBLE,_ 
         BYREF Fmin     AS DOUBLE,_ 
               G()      AS DOUBLE,_ 
               Hinv()   AS DOUBLE)
' ------------------------------------------------------------------
' Initializes Function, Gradient and Inverse Hessian
' ------------------------------------------------------------------

  DIM AS INTEGER L, N  ' Array bounds
  DIM AS INTEGER I, J  ' Loop variables

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

  ' Initialize function
  Fmin = Func(X())

  ' Initialize gradient
  Gradient(X(), G())

  ' Initialize inverse hessian to unit matrix
  FOR I = L TO N
    Hinv(I, I) = 1
    FOR J = I + 1 TO N
      Hinv(I, J) = 0
      Hinv(J, I) = 0
    NEXT J
  NEXT I
END SUB

SUB BFGS(      Func     AS FUNCTION(X() AS DOUBLE) AS DOUBLE,_
               Gradient AS SUB(X() AS DOUBLE, G() AS DOUBLE),_
               X()      AS DOUBLE,_ 
               MaxIter  AS INTEGER,_ 
               Tol      AS DOUBLE,_ 
         BYREF Fmin     AS DOUBLE,_ 
               G()      AS DOUBLE,_ 
               Hinv()   AS DOUBLE)
' ------------------------------------------------------------------
' Minimizes a function of several variables by the Broyden-Fletcher-
' Goldfarb-Shanno (BFGS) method.
' ------------------------------------------------------------------
' Input parameters  : Func     = function to be minimized
'                     Gradient = subroutine to compute 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
' ------------------------------------------------------------------
' The function MathErr() returns one of the following codes:
'   OptOk      = no error
'   OptNonConv = non-convergence
' ------------------------------------------------------------------

  DIM AS INTEGER I, J, L, N, Iter
  DIM AS DOUBLE  A, DeltaXmax, Gmax, P1, P2, R, R1, R2
  
  L = LBOUND(X)
  N = UBOUND(X)

  DIM AS DOUBLE OldX(L TO N)
  DIM AS DOUBLE DeltaX(L TO N)
  DIM AS DOUBLE dX(L TO N)
  DIM AS DOUBLE OldG(L TO N)
  DIM AS DOUBLE dG(L TO N)
  DIM AS DOUBLE HdG(L TO N)
  DIM AS DOUBLE R1dX(L TO N)
  DIM AS DOUBLE R2HdG(L TO N)
  DIM AS DOUBLE U(L TO N)
  DIM AS DOUBLE P2U(L TO N)

  ' Initialize
  Init(Func, Gradient, X(), Fmin, G(), Hinv())
 
  ' Quit if no iteration required
  IF MaxIter < 1 THEN ErrCode = OptOk : EXIT SUB
   
  Iter = 0

  ' Compute max. gradient component
  Gmax = ABS(G(L))
  FOR I = L + 1 TO N
    A = ABS(G(I))
    IF A > Gmax THEN Gmax = A
  NEXT I

  ' Quit if gradient is already small
  IF Gmax < MachEp THEN ErrCode = OptOk : EXIT SUB
    
  ' Initialize search direction
  FOR I = L TO N
    DeltaX(I) = -G(I)
  NEXT I

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

    ' Normalize search direction to avoid excessive displacements
    DeltaXmax = ABS(DeltaX(L))
    FOR I = L + 1 TO N
      A = ABS(DeltaX(I))
      IF A > DeltaXmax THEN DeltaXmax = A
    NEXT I
    IF DeltaXmax > 1 THEN
      FOR I = L TO N
        DeltaX(I) = DeltaX(I) / DeltaXmax
      NEXT I
    END IF

    ' Save old parameters and gradient
    FOR I = L TO N
      OldX(I) = X(I)
      OldG(I) = G(I)
    NEXT I

    ' Minimize along the direction specified by DeltaX
    ' with initial step R = 1
    R = 1
    LinMin(Func, X(), DeltaX(), R, 10, 0.01, Fmin)

    ' Compute new gradient
    Gradient(X(), G())

    ' Compute differences between two successive
    ' estimations of parameter vector and gradient vector
    FOR I = L TO N
      dX(I) = X(I) - OldX(I)
      dG(I) = G(I) - OldG(I)
    NEXT I

    ' Multiply by inverse hessian
    FOR I = L TO N
      HdG(I) = 0
      FOR J = L TO N
        HdG(I) = HdG(I) + Hinv(I, J) * dG(J)
      NEXT J
    NEXT I

    ' Scalar products in denominator of BFGS formula
    P1 = 0: P2 = 0
    FOR I = L TO N
      P1 = P1 + dX(I) * dG(I)
      P2 = P2 + dG(I) * HdG(I)
    NEXT I

    IF P1 = 0 OR P2 = 0 THEN ErrCode = OptOk : EXIT SUB
      
    ' Inverses of scalar products
    R1 = 1 / P1: R2 = 1 / P2

    ' Compute BFGS correction terms
    FOR I = L TO N
      R1dX(I) = R1 * dX(I)
      R2HdG(I) = R2 * HdG(I)
      U(I) = R1dX(I) - R2HdG(I)
      P2U(I) = P2 * U(I)
    NEXT I

    ' Update inverse hessian
    FOR I = L TO N
      FOR J = L TO N
        Hinv(I, J) = Hinv(I, J) + R1dX(I) * dX(J)_
                     - R2HdG(I) * HdG(J) + P2U(I) * U(J)
      NEXT J
    NEXT I

    ' Update search direction
    FOR I = L TO N
      DeltaX(I) = 0
      FOR J = L TO N
        DeltaX(I) = DeltaX(I) - Hinv(I, J) * G(J)
      NEXT J
    NEXT I
  LOOP UNTIL CompVec(X(), OldX(), Tol)
  
  ErrCode = OptOk
END SUB

