c
c
c     ###################################################
c     ##  COPYRIGHT (C)  1990  by  Jay William Ponder  ##
c     ##              All Rights Reserved              ##
c     ###################################################
c
c     #############################################################
c     ##                                                         ##
c     ##  subroutine lmstep  --  finds Levenberg-Marquardt step  ##
c     ##                                                         ##
c     #############################################################
c
c
c     "lmstep" computes the Levenberg-Marquardt step during a
c     nonlinear least squares calculation; this version is based
c     upon ideas from the Minpack routine LMPAR together with
c     with the internal doubling strategy of Dennis and Schnabel
c
c     arguments and variables :
c
c     n        dimension of the problem
c     ga       real vector of length n containing the gradient
c                of the residual vector
c     a        real n by n array which on input contains in the full
c                upper triangle the upper triangle of the matrix r
c                resulting from the QR factorization of the Jacobian;
c                on output the full upper triangle is unaltered, and
c                the strict lower triangle contains the strict lower
c                triangle of the lower triangular matrix l which is
c                equal to the Cholesky factor of (j**t)*j + amu*xscale
c     lda      leading dimension of a exactly as specified in the
c                dimension statement of the calling program
c     ipvt     integer array of length n containing the pivoting
c                infomation from the QR factorization routine
c     xscale   real vector of length n containing the diagonal
c                scaling matrix for the variables
c     qtf      real vector containing the first n elements of
c                Q(transpose) * (the scaled residual vector)
c     amu      scalar containing an initial estimate of the
c                Levenberg-Marquardt parameter on input; on
c                output, amu contains the final estimate of
c                the Levenberg-Marquardt parameter
c     first    logical variable set true only if this is the first
c                call to this routine in this iteration
c     sa       real vector of length n containing the
c                Levenberg-Marquardt step
c     gnstep   real vector of length n containing the
c                Gauss-Newton step
c     gauss    logical variable which is true if the Gauss-Newton
c                step is acceptable, and is false otherwise
c     diag     vector of length n containing the diagonal elements
c                of the Cholesky factor of (j**t)*j + amu*xscale
c
c
      subroutine lmstep (n,ga,a,lda,ipvt,xscale,qtf,stpmax,
     &                       delta,amu,first,sa,gauss)
      implicit none
      integer maxlsq,maxrsd
      parameter (maxlsq=50)
      parameter (maxrsd=100)
      integer i,j,k,n,lda,ipvt(maxlsq),nsing
      real*8 stpmax,delta,amu
      real*8 ga(maxlsq),a(lda,maxlsq),xscale(maxlsq)
      real*8 qtf(maxlsq),sa(maxlsq),gnstep(maxlsq)
      real*8 diag(maxlsq),work1(maxlsq),work2(maxlsq)
      real*8 alow,alpha,amulow,amuhi,beta,small,sum
      real*8 deltap,gnleng,phi,phip,phipi,sgnorm
      real*8 high,tiny,stplen,temp,precise
      logical first,gauss,done
      save deltap,phi,phip,nsing
      external precise
c
c
c     set smallest floating point magnitude and spacing
c
      tiny = precise (1)
      small = precise (2)
c
c     if initial trust region is not provided by the user,
c     compute and use the length of the Cauchy step given
c     by beta = norm2(r*trans(p)*d**(-2)*g)**2
c
      if (delta .eq. 0.0d0) then
         amu = 0.0d0
         do i = 1, n
            work1(i) = ga(i) / xscale(i)
         end do
         alpha = 0.0d0
         do i = 1, n
            alpha = alpha + work1(i)**2
         end do
         beta = 0.0d0
         do i = 1, n
            temp = 0.0d0
            do j = i, n
               k = ipvt(j)
               temp = temp + a(i,j)*ga(k)/xscale(k)**2
            end do
            beta = beta + temp**2
         end do
         if (beta .le. tiny) then
            delta = alpha * sqrt(alpha)
         else
            delta = alpha * sqrt(alpha)/beta
         end if
         delta = min(delta,stpmax)
      end if
c
c     the following is done only on the first time through
c     this iteration:  (1) compute the Gauss-Newton step;
c     if the Jacobian is rank-deficient, obtain a least
c     squares solution; (2) compute the length of the scaled
c     Gauss-Newton step; (3) compute the norm of the scaled
c     gradient used in computing an upper bound for "amu"
c
      if (first) then
         nsing = n
         do j = 1, n
            if (a(j,j).eq.0.0d0 .and. nsing.eq.n)  nsing = j - 1
            if (nsing .lt. n)  work1(j) = 0.0d0
         end do
         work1(nsing) = qtf(nsing) / a(nsing,nsing)
         do j = nsing-1, 1, -1
            sum = 0.0d0
            do i = j+1, nsing
               sum = sum + a(j,i)*work1(i)
            end do
            work1(j) = (qtf(j)-sum) / a(j,j)
         end do
         do j = 1, n
            gnstep(ipvt(j)) = -work1(j)
         end do
c
c     find the length of scaled Gauss-Newton step
c
         do j = 1, n
            work1(j) = xscale(j) * gnstep(j)
         end do
         gnleng = 0.0d0
         do j = 1, n
            gnleng = gnleng + work1(j)**2
         end do
         gnleng = sqrt(gnleng)
c
c     find the length of the scaled gradient
c
         do j = 1, n
            work1(j) = ga(j) / xscale(j)
         end do
         sgnorm = 0.0d0
         do j = 1, n
            sgnorm = sgnorm + work1(j)**2
         end do
         sgnorm = sqrt(sgnorm)
      end if
c
c     set the bounds on the computed step
c
      high = 1.5d0
      alow = 0.75d0
c
c     check to see if the Gauss-Newton step is acceptable
c
      if (gnleng .le. high*delta) then
         gauss = .true.
         do j = 1, n
            sa(j) = gnstep(j)
         end do
         amu = 0.0d0
         delta = min(delta,gnleng)
c
c     the Gauss-Newton step is rejected, find a nontrivial step;
c     first compute a starting value of "amu" if previous step
c     was not a Gauss-Newton step
c
      else
         gauss = .false.
         if (amu .gt. 0.0d0)
     &      amu = amu - ((phi+deltap)/delta)*(((deltap-delta)+phi)/phip)
         phi = gnleng - delta
c
c     if the Jacobian is not rank deficient, the Newton step
c     provides a lower bound for "amu"; else set bound to zero
c
         if (nsing .eq. n) then
            if (first) then
               first = .false.
               do j = 1, n
                  k = ipvt(j)
                  work1(j) = gnstep(k) * xscale(k)**2
               end do
c
c     obtain trans(r**-1)*(trans(p)*s) by solving the
c     system of equations trans(r)*work1 = work1
c
               work1(n) = work1(n) / a(n,n)
               do j = n-1, 1, -1
                  sum = 0.0d0
                  do i = j+1, n
                     sum = sum + a(j,i)*work1(i)
                  end do
                  work1(j) = (work1(j)-sum) / a(j,j)
               end do
               phipi = 0.0d0
               do j = 1, n
                  phipi = phipi - work1(j)**2
               end do
               phipi = phipi / gnleng
            end if
            amulow = -phi / phipi
         else
            first = .false.
            amulow = 0.0d0
         end if
         amuhi = sgnorm / delta
c
c     iterate until a satisfactory "amu" is generated
c
         done = .false.
         dowhile (.not. done)
            if (amu.lt.amulow .or. amu.gt.amuhi) then
               amu = max(sqrt(amulow*amuhi),0.001d0*amuhi)
            end if
            temp = sqrt(amu)
            do j = 1, n
               work1(j) = temp * xscale(j)
            end do
c
c     solve the damped least squares system for the value of the
c     Levenberg-Marquardt step using More's Minpack technique
c
            call qrsolv (n,a,lda,ipvt,work1,qtf,sa,diag,work2)
            do j = 1, n
               sa(j) = -sa(j)
            end do
            do j = 1, n
               work2(j) = xscale(j) * sa(j)
            end do
            stplen = 0.0d0
            do j = 1, n
               stplen = stplen + work2(j)**2
            end do
            stplen = sqrt(stplen)
            phi = stplen - delta
            do j = 1, n
               k = ipvt(j)
               work1(j) = xscale(k) * work2(k)
            end do
            do j = 1, n
               if (abs(diag(j)) .ge. tiny) then
                  work1(j) = work1(j) / diag(j)
               end if
               if (j .lt. n) then
                  do i = j+1, n
                     work1(i) = work1(i) - work1(j)*a(i,j)
                  end do
               end if
            end do
            phip = 0.0d0
            do j = 1, n
               phip = phip - work1(j)**2
            end do
            phip = phip / stplen
c
c     check to see if the step is acceptable; if not,
c     update amulow, amuhi and amu for next iteration
c
            if ((stplen.ge.alow*delta.and.stplen.le.high*delta)
     &            .or. (amuhi-amulow).le.small) then
               done = .true.
            else
               amulow = max(amulow,amu-(phi/phip))
               if (phi .lt. 0.0d0)  amuhi = amu
               amu = amu - (stplen/delta)*(phi/phip)
            end if
         end do
      end if
      deltap = delta
      return
      end
