MODULE Levenberg_Marquardt
! MINPACK routines which are used by both LMDIF & LMDER
! 25 October 2001:
!    Changed INTENT of iflag in several places to IN OUT.
!    Changed INTENT of fvec to IN OUT in user routine FCN.
!    Removed arguments diag and qtv from LMDIF & LMDER.
!    Replaced several DO loops with array operations.
! amiller @ bigpond.net.au

IMPLICIT NONE
INTEGER, PARAMETER :: dp = SELECTED_REAL_KIND(12, 60)

PRIVATE
PUBLIC :: dp, lmdif1, lmdif, lmder1, lmder, enorm

CONTAINS


SUBROUTINE lmdif1(fcn, m, n, x, fvec, tol, info, iwa)
 
! Code converted using TO_F90 by Alan Miller
! Date: 1999-12-11  Time: 00:51:44

! N.B. Arguments WA & LWA have been removed.

INTEGER, INTENT(IN)        :: m
INTEGER, INTENT(IN)        :: n
REAL (dp), INTENT(IN OUT)  :: x(:)
REAL (dp), INTENT(OUT)     :: fvec(:)
REAL (dp), INTENT(IN)      :: tol
INTEGER, INTENT(OUT)       :: info
INTEGER, INTENT(OUT)       :: iwa(:)

! EXTERNAL fcn

INTERFACE
  SUBROUTINE fcn(m, n, x, fvec, iflag)
    IMPLICIT NONE
    INTEGER, PARAMETER         :: dp = SELECTED_REAL_KIND(12, 60)
    INTEGER, INTENT(IN)        :: m, n
    REAL (dp), INTENT(IN)      :: x(:)
    REAL (dp), INTENT(IN OUT)  :: fvec(:)
    INTEGER, INTENT(IN OUT)    :: iflag
  END SUBROUTINE fcn
END INTERFACE

!  **********

!  subroutine lmdif1

!  The purpose of lmdif1 is to minimize the sum of the squares of m nonlinear
!  functions in n variables by a modification of the Levenberg-Marquardt
!  algorithm.  This is done by using the more general least-squares
!  solver lmdif.  The user must provide a subroutine which calculates the
!  functions.  The jacobian is then calculated by a forward-difference
!  approximation.

!  the subroutine statement is

!    subroutine lmdif1(fcn, m, n, x, fvec, tol, info, iwa)

!  where

!    fcn is the name of the user-supplied subroutine which calculates
!      the functions.  fcn must be declared in an external statement in the
!      user calling program, and should be written as follows.

!      subroutine fcn(m, n, x, fvec, iflag)
!      integer m, n, iflag
!      REAL (dp) x(n), fvec(m)
!      ----------
!      calculate the functions at x and return this vector in fvec.
!      ----------
!      return
!      end

!      the value of iflag should not be changed by fcn unless
!      the user wants to terminate execution of lmdif1.
!      In this case set iflag to a negative integer.

!    m is a positive integer input variable set to the number of functions.

!    n is a positive integer input variable set to the number of variables.
!      n must not exceed m.

!    x is an array of length n.  On input x must contain an initial estimate
!      of the solution vector.  On output x contains the final estimate of
!      the solution vector.

!    fvec is an output array of length m which contains
!      the functions evaluated at the output x.

!    tol is a nonnegative input variable.  Termination occurs when the
!      algorithm estimates either that the relative error in the sum of
!      squares is at most tol or that the relative error between x and the
!      solution is at most tol.

!    info is an integer output variable.  If the user has terminated execution,
!      info is set to the (negative) value of iflag.  See description of fcn.
!      Otherwise, info is set as follows.

!      info = 0  improper input parameters.

!      info = 1  algorithm estimates that the relative error
!                in the sum of squares is at most tol.

!      info = 2  algorithm estimates that the relative error
!                between x and the solution is at most tol.

!      info = 3  conditions for info = 1 and info = 2 both hold.

!      info = 4  fvec is orthogonal to the columns of the
!                jacobian to machine precision.

!      info = 5  number of calls to fcn has reached or exceeded 200*(n+1).

!      info = 6  tol is too small. no further reduction in
!                the sum of squares is possible.

!      info = 7  tol is too small.  No further improvement in
!                the approximate solution x is possible.

!    iwa is an integer work array of length n.

!    wa is a work array of length lwa.

!    lwa is a positive integer input variable not less than m*n+5*n+m.

!  subprograms called

!    user-supplied ...... fcn

!    minpack-supplied ... lmdif

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!  **********
INTEGER   :: maxfev, mode, nfev, nprint
REAL (dp) :: epsfcn, ftol, gtol, xtol, fjac(m,n)
REAL (dp), PARAMETER :: factor = 100._dp, zero = 0.0_dp

info = 0

!     check the input parameters for errors.

IF (n <= 0 .OR. m < n .OR. tol < zero) GO TO 10

!     call lmdif.

maxfev = 200*(n + 1)
ftol = tol
xtol = tol
gtol = zero
epsfcn = zero
mode = 1
nprint = 0
CALL lmdif(fcn, m, n, x, fvec, ftol, xtol, gtol, maxfev, epsfcn,   &
           mode, factor, nprint, info, nfev, fjac, iwa)
IF (info == 8) info = 4

10 RETURN

!     last card of subroutine lmdif1.

END SUBROUTINE lmdif1



SUBROUTINE lmdif(fcn, m, n, x, fvec, ftol, xtol, gtol, maxfev, epsfcn,  &
                 mode, factor, nprint, info, nfev, fjac, ipvt)
 
! N.B. Arguments LDFJAC, DIAG, QTF, WA1, WA2, WA3 & WA4 have been removed.

INTEGER, INTENT(IN)        :: m
INTEGER, INTENT(IN)        :: n
REAL (dp), INTENT(IN OUT)  :: x(:)
REAL (dp), INTENT(OUT)     :: fvec(:)
REAL (dp), INTENT(IN)      :: ftol
REAL (dp), INTENT(IN)      :: xtol
REAL (dp), INTENT(IN OUT)  :: gtol
INTEGER, INTENT(IN OUT)    :: maxfev
REAL (dp), INTENT(IN OUT)  :: epsfcn
INTEGER, INTENT(IN)        :: mode
REAL (dp), INTENT(IN)      :: factor
INTEGER, INTENT(IN)        :: nprint
INTEGER, INTENT(OUT)       :: info
INTEGER, INTENT(OUT)       :: nfev
REAL (dp), INTENT(OUT)     :: fjac(:,:)    ! fjac(ldfjac,n)
INTEGER, INTENT(OUT)       :: ipvt(:)

! EXTERNAL fcn

INTERFACE
  SUBROUTINE fcn(m, n, x, fvec, iflag)
    IMPLICIT NONE
    INTEGER, PARAMETER         :: dp = SELECTED_REAL_KIND(12, 60)
    INTEGER, INTENT(IN)        :: m, n
    REAL (dp), INTENT(IN)      :: x(:)
    REAL (dp), INTENT(IN OUT)  :: fvec(:)
    INTEGER, INTENT(IN OUT)    :: iflag
  END SUBROUTINE fcn
END INTERFACE

!  **********

!  subroutine lmdif

!  The purpose of lmdif is to minimize the sum of the squares of m nonlinear
!  functions in n variables by a modification of the Levenberg-Marquardt
!  algorithm.  The user must provide a subroutine which calculates the
!  functions.  The jacobian is then calculated by a forward-difference
!  approximation.

!  the subroutine statement is

!    subroutine lmdif(fcn, m, n, x, fvec, ftol, xtol, gtol, maxfev, epsfcn,
!                     diag, mode, factor, nprint, info, nfev, fjac,
!                     ldfjac, ipvt, qtf, wa1, wa2, wa3, wa4)

! N.B. 7 of these arguments have been removed in this version.

!  where

!    fcn is the name of the user-supplied subroutine which calculates the
!      functions.  fcn must be declared in an external statement in the user
!      calling program, and should be written as follows.

!      subroutine fcn(m, n, x, fvec, iflag)
!      integer m, n, iflag
!      REAL (dp) x(:), fvec(m)
!      ----------
!      calculate the functions at x and return this vector in fvec.
!      ----------
!      return
!      end

!      the value of iflag should not be changed by fcn unless
!      the user wants to terminate execution of lmdif.
!      in this case set iflag to a negative integer.

!    m is a positive integer input variable set to the number of functions.

!    n is a positive integer input variable set to the number of variables.
!      n must not exceed m.

!    x is an array of length n.  On input x must contain an initial estimate
!      of the solution vector.  On output x contains the final estimate of the
!      solution vector.

!    fvec is an output array of length m which contains
!      the functions evaluated at the output x.

!    ftol is a nonnegative input variable.  Termination occurs when both the
!      actual and predicted relative reductions in the sum of squares are at
!      most ftol.  Therefore, ftol measures the relative error desired
!      in the sum of squares.

!    xtol is a nonnegative input variable.  Termination occurs when the
!      relative error between two consecutive iterates is at most xtol.
!      Therefore, xtol measures the relative error desired in the approximate
!      solution.

!    gtol is a nonnegative input variable.  Termination occurs when the cosine
!      of the angle between fvec and any column of the jacobian is at most
!      gtol in absolute value.  Therefore, gtol measures the orthogonality
!      desired between the function vector and the columns of the jacobian.

!    maxfev is a positive integer input variable.  Termination occurs when the
!      number of calls to fcn is at least maxfev by the end of an iteration.

!    epsfcn is an input variable used in determining a suitable step length
!      for the forward-difference approximation.  This approximation assumes
!      that the relative errors in the functions are of the order of epsfcn.
!      If epsfcn is less than the machine precision, it is assumed that the
!      relative errors in the functions are of the order of the machine
!      precision.

!    diag is an array of length n.  If mode = 1 (see below), diag is
!      internally set.  If mode = 2, diag must contain positive entries that
!      serve as multiplicative scale factors for the variables.

!    mode is an integer input variable.  If mode = 1, the variables will be
!      scaled internally.  If mode = 2, the scaling is specified by the input
!      diag. other values of mode are equivalent to mode = 1.

!    factor is a positive input variable used in determining the initial step
!      bound.  This bound is set to the product of factor and the euclidean
!      norm of diag*x if nonzero, or else to factor itself.  In most cases
!      factor should lie in the interval (.1,100.). 100. is a generally
!      recommended value.

!    nprint is an integer input variable that enables controlled printing of
!      iterates if it is positive.  In this case, fcn is called with iflag = 0
!      at the beginning of the first iteration and every nprint iterations
!      thereafter and immediately prior to return, with x and fvec available
!      for printing.  If nprint is not positive, no special calls
!      of fcn with iflag = 0 are made.

!    info is an integer output variable.  If the user has terminated
!      execution, info is set to the (negative) value of iflag.
!      See description of fcn.  Otherwise, info is set as follows.

!      info = 0  improper input parameters.

!      info = 1  both actual and predicted relative reductions
!                in the sum of squares are at most ftol.

!      info = 2  relative error between two consecutive iterates <= xtol.

!      info = 3  conditions for info = 1 and info = 2 both hold.

!      info = 4  the cosine of the angle between fvec and any column of
!                the Jacobian is at most gtol in absolute value.

!      info = 5  number of calls to fcn has reached or exceeded maxfev.

!      info = 6  ftol is too small. no further reduction in
!                the sum of squares is possible.

!      info = 7  xtol is too small. no further improvement in
!                the approximate solution x is possible.

!      info = 8  gtol is too small. fvec is orthogonal to the
!                columns of the jacobian to machine precision.

!    nfev is an integer output variable set to the number of calls to fcn.

!    fjac is an output m by n array. the upper n by n submatrix
!      of fjac contains an upper triangular matrix r with
!      diagonal elements of nonincreasing magnitude such that

!             t     t           t
!            p *(jac *jac)*p = r *r,

!      where p is a permutation matrix and jac is the final calculated
!      Jacobian.  Column j of p is column ipvt(j) (see below) of the
!      identity matrix. the lower trapezoidal part of fjac contains
!      information generated during the computation of r.

!    ldfjac is a positive integer input variable not less than m
!      which specifies the leading dimension of the array fjac.

!    ipvt is an integer output array of length n.  ipvt defines a permutation
!      matrix p such that jac*p = q*r, where jac is the final calculated
!      jacobian, q is orthogonal (not stored), and r is upper triangular
!      with diagonal elements of nonincreasing magnitude.
!      Column j of p is column ipvt(j) of the identity matrix.

!    qtf is an output array of length n which contains
!      the first n elements of the vector (q transpose)*fvec.

!    wa1, wa2, and wa3 are work arrays of length n.

!    wa4 is a work array of length m.

!  subprograms called

!    user-supplied ...... fcn

!    minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac

!    fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!  **********
INTEGER   :: i, iflag, iter, j, l
REAL (dp) :: actred, delta, dirder, epsmch, fnorm, fnorm1, gnorm,  &
             par, pnorm, prered, ratio, sum, temp, temp1, temp2, xnorm
REAL (dp) :: diag(n), qtf(n), wa1(n), wa2(n), wa3(n), wa4(m)
REAL (dp), PARAMETER :: one = 1.0_dp, p1 = 0.1_dp, p5 = 0.5_dp,  &
                        p25 = 0.25_dp, p75 = 0.75_dp, p0001 = 0.0001_dp, &
                        zero = 0.0_dp

!     epsmch is the machine precision.

epsmch = EPSILON(zero)

info = 0
iflag = 0
nfev = 0

!     check the input parameters for errors.

IF (n <= 0 .OR. m < n .OR. ftol < zero .OR. xtol < zero .OR. gtol < zero  &
    .OR. maxfev <= 0 .OR. factor <= zero) GO TO 300
IF (mode /= 2) GO TO 20
DO  j = 1, n
  IF (diag(j) <= zero) GO TO 300
END DO

!     evaluate the function at the starting point and calculate its norm.

20 iflag = 1
CALL fcn(m, n, x, fvec, iflag)
nfev = 1
IF (iflag < 0) GO TO 300
fnorm = enorm(m, fvec)

!     initialize levenberg-marquardt parameter and iteration counter.

par = zero
iter = 1

!     beginning of the outer loop.

!        calculate the jacobian matrix.