Compute the Levenberg-Marquardt parameter and solution vector.

Parameters:
r     - IN/OUT n-by-m matrix, m >= n. On input, the full lower triangle is
        the full lower  triangle of R and the strict upper triangle is
        ignored.  On output, the strict upper triangle has been
        obliterated. The value of 'm' here is not relevant so long as it
        is at least n.
pmut  - n-vector, defines permutation of R
ddiag - n-vector, diagonal elements of D
bqt   - n-vector, first elements of B Q^T
delta - positive scalar, specifies scale of enorm(Dx)
par0  - positive scalar, initial estimate of the LM parameter
enorm - norm-computing function
finfo - info about chosen floating-point representation

Returns:
par   - positive scalar, final estimate of LM parameter
x     - n-vector, least-squares solution of LM equation (see below)

This routine computes the Levenberg-Marquardt parameter 'par' and a LM
solution vector 'x'. Given an n-by-n matrix A, an n-by-n nonsingular
diagonal matrix D, an m-vector B, and a positive number delta, the
problem is to determine values such that 'x' is the least-squares
solution to

 A x = B
 sqrt(par) * D x = 0

and either

 (1) par = 0, dxnorm - delta <= 0.1 delta or
 (2) par > 0 and |dxnorm - delta| <= 0.1 delta

where dxnorm = enorm(D x).

This routine is not given A, B, or D directly. If we define the
column-pivoted transposed QR factorization of A such that

 A P = R Q

where P is a permutation matrix, Q has orthogonal rows, and R is a
lower triangular matrix with diagonal elements of nonincreasing
magnitude, this routine is given the full lower triangle of R, a
vector defining P ('pmut'), and the first n components of B Q^T
('bqt'). These values are essentially passed verbatim to _qrd_solve().

This routine iterates to estimate par. Usually only a few iterations
are needed, but no more than 10 are performed.

def _lm_solve(r, pmut, ddiag, bqt, delta, par0, enorm, finfo):
    """Compute the Levenberg-Marquardt parameter and solution vector.

Parameters:
r     - IN/OUT n-by-m matrix, m >= n. On input, the full lower triangle is
        the full lower  triangle of R and the strict upper triangle is
        ignored.  On output, the strict upper triangle has been
        obliterated. The value of 'm' here is not relevant so long as it
        is at least n.
pmut  - n-vector, defines permutation of R
ddiag - n-vector, diagonal elements of D
bqt   - n-vector, first elements of B Q^T
delta - positive scalar, specifies scale of enorm(Dx)
par0  - positive scalar, initial estimate of the LM parameter
enorm - norm-computing function
finfo - info about chosen floating-point representation

Returns:
par   - positive scalar, final estimate of LM parameter
x     - n-vector, least-squares solution of LM equation (see below)

This routine computes the Levenberg-Marquardt parameter 'par' and a LM
solution vector 'x'. Given an n-by-n matrix A, an n-by-n nonsingular
diagonal matrix D, an m-vector B, and a positive number delta, the
problem is to determine values such that 'x' is the least-squares
solution to

 A x = B
 sqrt(par) * D x = 0

and either

 (1) par = 0, dxnorm - delta <= 0.1 delta or
 (2) par > 0 and |dxnorm - delta| <= 0.1 delta

where dxnorm = enorm(D x).

This routine is not given A, B, or D directly. If we define the
column-pivoted transposed QR factorization of A such that

 A P = R Q

where P is a permutation matrix, Q has orthogonal rows, and R is a
lower triangular matrix with diagonal elements of nonincreasing
magnitude, this routine is given the full lower triangle of R, a
vector defining P ('pmut'), and the first n components of B Q^T
('bqt'). These values are essentially passed verbatim to _qrd_solve().

This routine iterates to estimate par. Usually only a few iterations
are needed, but no more than 10 are performed.
"""
    dwarf = finfo.tiny
    n, m = r.shape
    x = np.empty_like(bqt)
    sdiag = np.empty_like(bqt)

    # "Compute and store x in the Gauss-Newton direction. If the
    # Jacobian is rank-deficient, obtain a least-squares solution."

    nnonsingular = n
    wa1 = bqt.copy()

    for i in range(n):
        if r[i,i] == 0:
            nnonsingular = i
            wa1[i:] = 0
            break

    for j in range(nnonsingular - 1, -1, -1):
        wa1[j] /= r[j,j]
        wa1[:j] -= r[j,:j] * wa1[j]

    x[pmut] = wa1

    # Initial function evaluation. Check if the Gauss-Newton direction
    # was good enough.

    wa2 = ddiag * x
    dxnorm = enorm(wa2, finfo)
    normdiff = dxnorm - delta

    if normdiff <= 0.1 * delta:
        return 0, x

    # If the Jacobian is not rank deficient, the Newton step provides
    # a lower bound for the zero of the function.

    par_lower = 0.

    if nnonsingular == n:
        wa1 = ddiag[pmut] * wa2[pmut] / dxnorm
        wa1[0] /= r[0,0] # "Degenerate case"

        for j in range(1, n):
            wa1[j] = (wa1[j] - np.dot(wa1[:j], r[j,:j])) / r[j,j]

        temp = enorm(wa1, finfo)
        par_lower = normdiff / delta / temp**2

    # We can always find an upper bound.

    for j in range(n):
        wa1[j] = np.dot(bqt[:j+1], r[j,:j+1]) / ddiag[pmut[j]]

    gnorm = enorm(wa1, finfo)
    par_upper = gnorm / delta
    if par_upper == 0:
        par_upper = dwarf / min(delta, 0.1)

    # Now iterate our way to victory.

    par = np.clip(par0, par_lower, par_upper)
    if par == 0:
        par = gnorm / dxnorm

    itercount = 0

    while True:
        itercount += 1

        if par == 0:
            par = max(dwarf, par_upper * 0.001)

        temp = np.sqrt(par)
        wa1 = temp * ddiag
        x = _qrd_solve(r[:,:n], pmut, wa1, bqt, sdiag) # sdiag is an output arg here
        wa2 = ddiag * x
        dxnorm = enorm(wa2, finfo)
        olddiff = normdiff
        normdiff = dxnorm - delta

        if abs(normdiff) < 0.1 * delta:
            break # converged
        if par_lower == 0 and normdiff <= olddiff and olddiff < 0:
            break # overshot, I guess?
        if itercount == 10:
            break # this is taking too long

        # Compute and apply the Newton correction

        wa1 = ddiag[pmut] * wa2[pmut] / dxnorm

        for j in range(n - 1):
            wa1[j] /= sdiag[j]
            wa1[j+1:n] -= r[j,j+1:n] * wa1[j]
        wa1[n-1] /= sdiag[n-1] # degenerate case

        par_delta = normdiff / delta / enorm(wa1, finfo)**2

        if normdiff > 0:
            par_lower = max(par_lower, par)
        elif normdiff < 0:
            par_upper = min(par_upper, par)

        par = max(par_lower, par + par_delta)

    return par, x