' ******************************************************************
' Broyden method for system of nonlinear equations
' ******************************************************************

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

#DEFINE OptOk 0
' No error

#DEFINE OptNonConv -1
' Non-convergence

#DEFINE MatErrDim -3
' Non-compatible dimensions

' ------------------------------------------------------------------
' 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 LinMinEq(      Equations AS SUB(X() AS DOUBLE, F() AS DOUBLE),_
                           X()       AS DOUBLE,_ 
                           DeltaX()  AS DOUBLE,_ 
                     BYREF R         AS DOUBLE,_ 
                           MaxIter   AS INTEGER,_ 
                           Tol       AS DOUBLE,_ 
                           F()       AS DOUBLE)
' Minimization of a sum of squared functions along a line

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

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

SUB Broyden (Equations AS SUB(X() AS DOUBLE, F() AS DOUBLE),_
             X()       AS DOUBLE,_
             MaxIter   AS INTEGER,_ 
             Tol       AS DOUBLE,_ 
             F()       AS DOUBLE)
' ------------------------------------------------------------------
' Solves a system of nonlinear equations by Broyden's method
' ------------------------------------------------------------------
' Input parameters  : Equations = subroutine to compute functions
'                     X         = initial roots
'                     MaxIter   = maximum number of iterations
'                     Tol       = required precision
' ------------------------------------------------------------------
' Output parameters : X = refined roots
'                     F = function values
' ------------------------------------------------------------------
' Possible results : OptOk      = no error
'                    OptNonConv = non-convergence
' ------------------------------------------------------------------

  DIM AS INTEGER I, J, K, L, N, Iter
  DIM AS DOUBLE  A, DeltaXmax, Fmax, P, Q, R, S
  
  L = LBOUND(X)
  N = UBOUND(X)

  IF LBOUND(F) <> L OR UBOUND(F) <> N THEN
    ErrCode = MatErrDim
    EXIT SUB
  END IF
  
  DIM AS DOUBLE OldX(L TO N)
  DIM AS DOUBLE DeltaX(L TO N)
  DIM AS DOUBLE dX(L TO N)
  DIM AS DOUBLE OldF(L TO N)
  DIM AS DOUBLE dF(L TO N)
  DIM AS DOUBLE Dinv(L TO N, L TO N)
  DIM AS DOUBLE DdF(L TO N)

  ErrCode = OptOk

  ' Initialize function vector
  Equations(X(), F())

  ' Quit if no iteration required
  IF MaxIter < 1 THEN EXIT SUB

  ' Initialize inverse jacobian to unit matrix
  FOR I = L TO N
    Dinv(I, I) = 1
    FOR J = I + 1 TO N
      Dinv(I, J) = 0
      Dinv(J, I) = 0
    NEXT J
  NEXT I

  Iter = 0

  ' Compute max. function component
  Fmax = ABS(F(L))
  FOR I = L + 1 TO N
    A = ABS(F(I))
    IF A > Fmax THEN Fmax = A
  NEXT I

  ' Quit if function vector is already small
  IF Fmax < MachEp THEN EXIT SUB

  ' Initialize search direction
  FOR I = L TO N
    DeltaX(I) = -F(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 functions
    FOR I = L TO N
      OldX(I) = X(I)
      OldF(I) = F(I)
    NEXT I

    ' Minimize along the direction specified by DeltaX
    ' with initial step R = 1
    R = 1
    LinMinEq(Equations, X(), DeltaX(), R, 10, 0.01, F())

    ' Compute new functions
    Equations(X(), F())

    ' Compute differences between two successive
    ' estimations of parameter vector and function vector
    FOR I = L TO N
      dX(I) = X(I) - OldX(I)
      dF(I) = F(I) - OldF(I)
    NEXT I

    ' Multiply by inverse jacobian
    FOR I = L TO N
      DdF(I) = 0
      FOR J = L TO N
        DdF(I) = DdF(I) + Dinv(I, J) * dF(J)
      NEXT J
    NEXT I

    ' Scalar product in denominator of Broyden formula
    P = 0
    FOR I = L TO N
      P = P + dX(I) * DdF(I)
    NEXT I

    IF P = 0 THEN EXIT SUB

    ' Inverse of scalar product
    Q = 1 / P

    ' Update inverse jacobian
    FOR I = L TO N
      A = (dX(I) - DdF(I)) * Q
      FOR J = L TO N
        S = 0
        FOR K = L TO N
          S = S + dX(K) * Dinv(K, J)
        NEXT K
        Dinv(I, J) = Dinv(I, J) + A * S
      NEXT J
    NEXT I

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

