' ******************************************************************
' Newton-Raphson solver for system of nonlinear equations
' ******************************************************************

' ------------------------------------------------------------------
' 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 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 NewtEqs (Equations AS SUB(X() AS DOUBLE, F() AS DOUBLE),_
             Jacobian  AS SUB(X() AS DOUBLE, D() AS DOUBLE),_
             X()       AS DOUBLE,_
             MaxIter   AS INTEGER,_ 
             Tol       AS DOUBLE,_ 
             F()       AS DOUBLE)
' ------------------------------------------------------------------
' Solves a system of nonlinear equations by Newton's method
' ------------------------------------------------------------------
' Input parameters  : Equations = subroutine to compute function
'                     Jacobian  = subroutine to compute Jacobian
'                     X         = initial root
'                     MaxIter   = maximum number of iterations
'                     Tol       = required precision
' ------------------------------------------------------------------
' Output parameters : X = refined root
'                     F = function values
' ------------------------------------------------------------------
' Possible results : OptOk      = no error
'                    OptNonConv = non-convergence
'                    OptSing    = singular jacobian 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 DOUBLE  Det   ' Determinant of Jacobian
  DIM AS INTEGER Iter  ' Iteration count

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

  DIM AS DOUBLE OldX(L TO N)       ' Old parameters
  DIM AS DOUBLE D(L TO N, L TO N)  ' Jacobian matrix
  DIM AS DOUBLE DeltaX(L TO N)     ' New search direction
  
  ' Initialize function values
  Equations(X(), F())

  IF MaxIter < 1 THEN ErrCode = OptOk : EXIT SUB
   
  Iter = 0
  
  DO
   ' Compute Jacobian
    Jacobian(X(), D())

    ' Solve linear system
    LinEq(D(), F(), Det)
    IF ErrCode <> 0 THEN ErrCode = OptSing : EXIT SUB
          
    ' Prepare next iteration
    Iter = Iter + 1
    IF Iter > MaxIter THEN ErrCode = OptNonConv : EXIT SUB
    
    ' Save current parameters and initialize the direction search
    FOR I = L TO N
      OldX(I) = X(I)
      DeltaX(I) = -F(I)
    NEXT I

    ' Minimize in the direction specified by DeltaX,
    ' using an initial step of 0.1
    R = 0.1
    LinMinEq(Equations, X(), DeltaX(), R, 10, 0.01, F())
  LOOP UNTIL CompVec(X(), OldX(), Tol)
  
  ErrCode = OptOk
END SUB

