' ******************************************************************
' Function minimization by Marquardt's method
' ******************************************************************

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

#DEFINE OptOk 0  
' No error

#DEFINE OptNonConv -1  
' Non-convergence

#DEFINE OptSing -2  
' Quasi-singular hessian matrix

#DEFINE OptBigLambda -3  
' Too high Marquardt parameter

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

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

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

DECLARE SUB GaussJordan(      A() AS DOUBLE,_
                        BYREF Det AS DOUBLE)
' Solves a system of linear equations by the Gauss-Jordan method

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 Marquardt(      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 Marquardt'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
'   OptBigLambda = too high Marquardt parameter Lambda
' ------------------------------------------------------------------

  CONST Lambda0   = 1D-2   ' Initial lambda value
  CONST LambdaMax = 1D+3   ' Highest lambda value
  CONST FTol      = 1D-10  ' Tolerance on function decrease

  DIM AS INTEGER L, N, M          ' Array bounds
  DIM AS INTEGER I, J             ' Loop variables
  DIM AS DOUBLE  Lambda, Lambda1  ' Marquardt's lambda
  DIM AS DOUBLE  F1               ' New minimum
  DIM AS DOUBLE  R                ' Step for line minimization
  DIM AS INTEGER LambdaOk         ' Successful Lambda decrease
  DIM AS INTEGER Iter             ' Iteration count

  L = LBOUND(X)
  N = UBOUND(X) 
  M = N + 1

  DIM AS DOUBLE OldX(L TO N)       ' Old parameters
  DIM AS DOUBLE H(L TO N, L TO N)  ' Hessian matrix
  DIM AS DOUBLE A(L TO N, L TO M)  ' System matrix
  DIM AS DOUBLE DeltaX(L TO N)     ' New search direction

  Lambda = Lambda0
  Iter = 0

  ' Initialize function
  Fmin = Func(X())

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

    ' Compute Gradient and Hessian
    HessGrad(X(), G(), H())
    FOR I = L TO N
      FOR J = L TO N
        A(I, J) = H(I, J)
      NEXT J
      A(I, M) = -G(I)
    NEXT I

    IF MaxIter < 1 THEN
      ' Solve linear system
      GaussJordan A(), Det
      IF ErrCode <> 0 THEN ErrCode = OptSing : EXIT SUB
   
      ' Retrieve inverse hessian
      FOR I = L TO N
        FOR J = L TO N
          Hinv(I, J) = A(I, J)
        NEXT J
      NEXT I
   
      EXIT SUB
    END IF

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

    DO
      ' Multiply each diagonal term of H by (1 + Lambda)
      Lambda1 = 1 + Lambda
      FOR I = L TO N
        A(I, I) = Lambda1 * H(I, I)
      NEXT I
     
      ' Solve linear system
      GaussJordan A(), Det
      IF ErrCode <> 0 THEN ErrCode = OptSing : EXIT SUB

      ' Retrieve inverse hessian
      FOR I = L TO N
        FOR J = L TO N
          Hinv(I, J) = A(I, J)
        NEXT J
      NEXT I

      ' Initialize parameters and direction search
      FOR I = L TO N
        X(I) = OldX(I)
        DeltaX(I) = A(I, M)
      NEXT I

      ' Minimize in the direction specified by DeltaX,
      ' using an initial step of 0.1
      R = 0.1
      LinMin(Func, X(), DeltaX(), R, 10, 0.01, F1)

      ' Check that the function has decreased,
      ' otherwise increase Lambda, without exceeding LambdaMax
      LambdaOk = (F1 - Fmin) < Fmin * FTol
      IF NOT LambdaOk THEN Lambda = 10 * Lambda
       
      IF Lambda > LambdaMax THEN ErrCode = OptBigLambda : EXIT SUB
    LOOP UNTIL LambdaOk

    Lambda = 0.1 * Lambda
    Fmin = F1
  LOOP UNTIL CompVec(X(), OldX(), Tol)
  
  ErrCode = OptOk
END SUB

