/*
 * Library:   lmfit (Levenberg-Marquardt least squares fitting)
 *
 * File:      lmmin.c
 *
 * Contents:  Levenberg-Marquardt minimization.
 *
 * Copyright: MINPACK authors, The University of Chikago (1980-1999)
 *            Joachim Wuttke, Forschungszentrum Juelich GmbH (2004-2013)
 *
 * License:   see ../COPYING (FreeBSD)
 *
 * Homepage:  apps.jcns.fz-juelich.de/lmfit
 */

#include <assert.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include "lmmin.h"

#define MIN(a, b) (((a) <= (b)) ? (a) : (b))
#define MAX(a, b) (((a) >= (b)) ? (a) : (b))
#define SQR(x) (x) * (x)

/* Declare functions that do the heavy numerics.
   Implementions are in this source file, below lmmin.
   Dependences: lmmin calls lmpar, which calls qrfac and qrsolv. */
void lm_lmpar(const int n, double* r, const int ldr, const int* Pivot,
              const double* diag, const double* qtb, const double delta,
              double* par, double* x, double* Sdiag, double* aux, double* xdi);
void lm_qrfac(const int m, const int n, double* A, int* Pivot, double* Rdiag,
              double* Acnorm, double* W);
void lm_qrsolv(const int n, double* r, const int ldr, const int* Pivot,
               const double* diag, const double* qtb, double* x,
               double* Sdiag, double* W);

/******************************************************************************/
/*  Numeric constants                                                         */
/******************************************************************************/

/* Set machine-dependent constants to values from float.h. */
#define LM_MACHEP DBL_EPSILON       /* resolution of arithmetic */
#define LM_DWARF DBL_MIN            /* smallest nonzero number */
#define LM_SQRT_DWARF sqrt(DBL_MIN) /* square should not underflow */
#define LM_SQRT_GIANT sqrt(DBL_MAX) /* square should not overflow */
#define LM_USERTOL 30 * LM_MACHEP   /* users are recommended to require this */

/* If the above values do not work, the following seem good for an x86:
 LM_MACHEP     .555e-16
 LM_DWARF      9.9e-324
 LM_SQRT_DWARF 1.e-160
 LM_SQRT_GIANT 1.e150
 LM_USER_TOL   1.e-14
   The following values should work on any machine:
 LM_MACHEP     1.2e-16
 LM_DWARF      1.0e-38
 LM_SQRT_DWARF 3.834e-20
 LM_SQRT_GIANT 1.304e19
 LM_USER_TOL   1.e-14
*/

/* Predefined control parameter sets (msgfile=NULL means stdout). */
const lm_control_struct lm_control_double = {
    LM_USERTOL, LM_USERTOL, LM_USERTOL, LM_USERTOL,
    100., 100, 1, NULL, 0, -1, -1};
const lm_control_struct lm_control_float = {
    1.e-7, 1.e-7, 1.e-7, 1.e-7,
    100., 100, 1, NULL, 0, -1, -1};

/******************************************************************************/
/*  Message texts (indexed by status.info)                                    */
/******************************************************************************/

const char* lm_infmsg[] = {
    "found zero (sum of squares below underflow limit)",
    "converged  (the relative error in the sum of squares is at most tol)",
    "converged  (the relative error of the parameter vector is at most tol)",
    "converged  (both errors are at most tol)",
    "trapped    (by degeneracy; increasing epsilon might help)",
    "exhausted  (number of function calls exceeding preset patience)",
    "failed     (ftol<tol: cannot reduce sum of squares any further)",
    "failed     (xtol<tol: cannot improve approximate solution any further)",
    "failed     (gtol<tol: cannot improve approximate solution any further)",
    "crashed    (not enough memory)",
    "exploded   (fatal coding error: improper input parameters)",
    "stopped    (break requested within function evaluation)",
    "found nan  (function value is not-a-number or infinite)"};

const char* lm_shortmsg[] = {
    "found zero",
    "converged (f)",
    "converged (p)",
    "converged (2)",
    "degenerate",
    "call limit",
    "failed (f)",
    "failed (p)",
    "failed (o)",
    "no memory",
    "invalid input",
    "user break",
    "found nan"};

/******************************************************************************/
/*  Monitoring auxiliaries.                                                   */
/******************************************************************************/

void lm_print_pars(int nout, const double* par, FILE* fout)
{
    int i;
    for (i = 0; i < nout; ++i)
        fprintf(fout, " %16.9g", par[i]);
    fprintf(fout, "\n");
}

/******************************************************************************/
/*  lmmin (main minimization routine)                                         */
/******************************************************************************/

void lmmin(const int n, double* x, const int m, const void* data,
           void (*evaluate)(const double* par, const int m_dat,
                            const void* data, double* fvec, int* userbreak),
           const lm_control_struct* C, lm_status_struct* S)
{
    int j, i;
    double actred, dirder, fnorm, fnorm1, gnorm, pnorm, prered, ratio, step,
        sum, temp, temp1, temp2, temp3;

    /***  Initialize internal variables.  ***/

    int maxfev = C->patience * (n+1);

    int inner_success; /* flag for loop control */
    double lmpar = 0;  /* Levenberg-Marquardt parameter */
    double delta = 0;
    double xnorm = 0;
    double eps = sqrt(MAX(C->epsilon, LM_MACHEP)); /* for forward differences */

    int nout = C->n_maxpri == -1 ? n : MIN(C->n_maxpri, n);

    /* Reinterpret C->msgfile=NULL as stdout (which is unavailable for
       compile-time initialization of lm_control_double and similar). */
    FILE* msgfile = C->msgfile ? C->msgfile : stdout;

    /***  Default status info; must be set before first return statement.  ***/

    S->outcome = 0; /* status code */
    S->userbreak = 0;
    S->nfev = 0; /* function evaluation counter */

    /***  Check input parameters for errors.  ***/

    if (n <= 0) {
        fprintf(stderr, "lmmin: invalid number of parameters %i\n", n);
        S->outcome = 10;
        return;
    }
    if (m < n) {
        fprintf(stderr, "lmmin: number of data points (%i) "
                        "smaller than number of parameters (%i)\n",
                m, n);
        S->outcome = 10;
        return;
    }
    if (C->ftol < 0 || C->xtol < 0 || C->gtol < 0) {
        fprintf(stderr,
                "lmmin: negative tolerance (at least one of %g %g %g)\n",
                C->ftol, C->xtol, C->gtol);
        S->outcome = 10;
        return;
    }
    if (maxfev <= 0) {
        fprintf(stderr, "lmmin: nonpositive function evaluations limit %i\n",
                maxfev);
        S->outcome = 10;
        return;
    }
    if (C->stepbound <= 0) {
        fprintf(stderr, "lmmin: nonpositive stepbound %g\n", C->stepbound);
        S->outcome = 10;
        return;
    }
    if (C->scale_diag != 0 && C->scale_diag != 1) {
        fprintf(stderr, "lmmin: logical variable scale_diag=%i, "
                        "should be 0 or 1\n",
                C->scale_diag);
        S->outcome = 10;
        return;
    }

    /***  Allocate work space.  ***/

    /* Allocate total workspace with just one system call */
    char* ws;
    if ((ws = malloc((2*m + 5*n + m*n) * sizeof(double) +
                     n * sizeof(int))) == NULL) {
        S->outcome = 9;
        return;
    }

    /* Assign workspace segments. */
    char* pws = ws;
    double* fvec = (double*)pws;
    pws += m * sizeof(double) / sizeof(char);
    double* diag = (double*)pws;
    pws += n * sizeof(double) / sizeof(char);
    double* qtf = (double*)pws;
    pws += n * sizeof(double) / sizeof(char);
    double* fjac = (double*)pws;
    pws += n * m * sizeof(double) / sizeof(char);
    double* wa1 = (double*)pws;
    pws += n * sizeof(double) / sizeof(char);
    double* wa2 = (double*)pws;
    pws += n * sizeof(double) / sizeof(char);
    double* wa3 = (double*)pws;
    pws += n * sizeof(double) / sizeof(char);
    double* wf = (double*)pws;
    pws += m * sizeof(double) / sizeof(char);
    int* Pivot = (int*)pws;
    pws += n * sizeof(int) / sizeof(char);

    /* Initialize diag. */
    if (!C->scale_diag)
        for (j = 0; j < n; j++)
            diag[j] = 1;

    /***  Evaluate function at starting point and calculate norm.  ***/

    if (C->verbosity) {
        fprintf(msgfile, "lmmin start ");
        lm_print_pars(nout, x, msgfile);
    }
    (*evaluate)(x, m, data, fvec, &(S->userbreak));
    if (C->verbosity > 4)
        for (i = 0; i < m; ++i)
            fprintf(msgfile, "    fvec[%4i] = %18.8g\n", i, fvec[i]);
    S->nfev = 1;
    if (S->userbreak)
        goto terminate;
    fnorm = lm_enorm(m, fvec);
    if (C->verbosity)
        fprintf(msgfile, "  fnorm = %18.8g\n", fnorm);

    if (!isfinite(fnorm)) {
        S->outcome = 12; /* nan */
        goto terminate;
    } else if (fnorm <= LM_DWARF) {
        S->outcome = 0; /* sum of squares almost zero, nothing to do */
        goto terminate;
    }

    /***  The outer loop: compute gradient, then descend.  ***/

    for (int outer = 0;; ++outer) {

        /** Calculate the Jacobian. **/
        for (j = 0; j < n; j++) {
            temp = x[j];
            step = MAX(eps * eps, eps * fabs(temp));
            x[j] += step; /* replace temporarily */
            (*evaluate)(x, m, data, wf, &(S->userbreak));
            ++(S->nfev);
            if (S->userbreak)
                goto terminate;
            for (i = 0; i < m; i++)
                fjac[j*m+i] = (wf[i] - fvec[i]) / step;
            x[j] = temp; /* restore */
        }
        if (C->verbosity >= 10) {
            /* print the entire matrix */
            printf("\nlmmin Jacobian\n");
            for (i = 0; i < m; i++) {
                printf("  ");
                for (j = 0; j < n; j++)
                    printf("%.5e ", fjac[j*m+i]);
                printf("\n");
            }
        }

        /** Compute the QR factorization of the Jacobian. **/

        /* fjac is an m by n array. The upper n by n submatrix of fjac is made
         *   to contain an upper triangular matrix R with diagonal elements of
         *   nonincreasing magnitude such that
         *
         *         P^T*(J^T*J)*P = R^T*R
         *
         *         (NOTE: ^T stands for matrix transposition),
         *
         *   where P is a permutation matrix and J is the final calculated
         *   Jacobian. Column j of P is column Pivot(j) of the identity matrix.
         *   The lower trapezoidal part of fjac contains information generated
         *   during the computation of R.
         *
         * Pivot is an integer array of length n. It 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 Pivot(j) of the identity matrix.
         */

        lm_qrfac(m, n, fjac, Pivot, wa1, wa2, wa3);
        /* return values are Pivot, wa1=rdiag, wa2=acnorm */

        /** Form Q^T * fvec, and store first n components in qtf. **/
        for (i = 0; i < m; i++)
            wf[i] = fvec[i];

        for (j = 0; j < n; j++) {
            temp3 = fjac[j*m+j];
            if (temp3 != 0) {
                sum = 0;
                for (i = j; i < m; i++)
                    sum += fjac[j*m+i] * wf[i];
                temp = -sum / temp3;
                for (i = j; i < m; i++)
                    wf[i] += fjac[j*m+i] * temp;
            }
            fjac[j*m+j] = wa1[j];
            qtf[j] = wf[j];
        }

        /**  Compute norm of scaled gradient and detect degeneracy. **/
        gnorm = 0;
        for (j = 0; j < n; j++) {
            if (wa2[Pivot[j]] == 0)
                continue;
            sum = 0;
            for (i = 0; i <= j; i++)
                sum += fjac[j*m+i] * qtf[i];
            gnorm = MAX(gnorm, fabs(sum / wa2[Pivot[j]] / fnorm));
        }

        if (gnorm <= C->gtol) {
            S->outcome = 4;
            goto terminate;
        }

        /** Initialize or update diag and delta. **/
        if (!outer) { /* first iteration only */
            if (C->scale_diag) {
                /* diag := norms of the columns of the initial Jacobian */
                for (j = 0; j < n; j++)
                    diag[j] = wa2[j] ? wa2[j] : 1;
                /* xnorm := || D x || */
                for (j = 0; j < n; j++)
                    wa3[j] = diag[j] * x[j];
                xnorm = lm_enorm(n, wa3);
                if (C->verbosity >= 2) {
                    fprintf(msgfile, "lmmin diag  ");
                    lm_print_pars(nout, x, msgfile); // xnorm
                    fprintf(msgfile, "  xnorm = %18.8g\n", xnorm);
                }
                /* Only now print the header for the loop table. */
                if (C->verbosity >= 3) {
                    fprintf(msgfile, "  o  i     lmpar    prered"
                                     "          ratio    dirder      delta"
                                     "      pnorm                 fnorm");
                    for (i = 0; i < nout; ++i)
                        fprintf(msgfile, "               p%i", i);
                    fprintf(msgfile, "\n");
                }
            } else {
                xnorm = lm_enorm(n, x);
            }
            if (!isfinite(xnorm)) {
                S->outcome = 12; /* nan */
                goto terminate;
            }
            /* Initialize the step bound delta. */
            if (xnorm)
                delta = C->stepbound * xnorm;
            else
                delta = C->stepbound;
        } else {
            if (C->scale_diag) {
                for (j = 0; j < n; j++)
                    diag[j] = MAX(diag[j], wa2[j]);
            }
        }

        /** The inner loop. **/
        int inner = 0;
        do {

            /** Determine the Levenberg-Marquardt parameter. **/
            lm_lmpar(n, fjac, m, Pivot, diag, qtf, delta, &lmpar,
                     wa1, wa2, wf, wa3);
            /* used return values are fjac (partly), lmpar, wa1=x, wa3=diag*x */

            /* Predict scaled reduction. */
            pnorm = lm_enorm(n, wa3);
            if (!isfinite(pnorm)) {
                S->outcome = 12; /* nan */
                goto terminate;
            }
            temp2 = lmpar * SQR(pnorm / fnorm);
            for (j = 0; j < n; j++) {
                wa3[j] = 0;
                for (i = 0; i <= j; i++)
                    wa3[i] -= fjac[j*m+i] * wa1[Pivot[j]];
            }
            temp1 = SQR(lm_enorm(n, wa3) / fnorm);
            if (!isfinite(temp1)) {
                S->outcome = 12; /* nan */
                goto terminate;
            }
            prered = temp1 + 2*temp2;
            dirder = -temp1 + temp2; /* scaled directional derivative */

            /* At first call, adjust the initial step bound. */
            if (!outer && pnorm < delta)
                delta = pnorm;

            /** Evaluate the function at x + p. **/
            for (j = 0; j < n; j++)
                wa2[j] = x[j] - wa1[j];
            (*evaluate)(wa2, m, data, wf, &(S->userbreak));
            ++(S->nfev);
            if (S->userbreak)
                goto terminate;
            fnorm1 = lm_enorm(m, wf);
            if (!isfinite(fnorm1)) {
                S->outcome = 12; /* nan */
                goto terminate;
            }

            /** Evaluate the scaled reduction. **/

            /* Actual scaled reduction. */
            actred = 1 - SQR(fnorm1 / fnorm);

            /* Ratio of actual to predicted reduction. */
            ratio = prered ? actred / prered : 0;

            if (C->verbosity == 2) {
                fprintf(msgfile, "lmmin (%i:%i) ", outer, inner);
                lm_print_pars(nout, wa2, msgfile); // fnorm1,
            } else if (C->verbosity >= 3) {
                printf("%3i %2i %9.2g %9.2g %14.6g"
                       " %9.2g %10.3e %10.3e %21.15e",
                       outer, inner, lmpar, prered, ratio,
                       dirder, delta, pnorm, fnorm1);
                for (i = 0; i < nout; ++i)
                    fprintf(msgfile, " %16.9g", wa2[i]);
                fprintf(msgfile, "\n");
            }

            /* Update the step bound. */
            if (ratio <= 0.25) {
                if (actred >= 0)
                    temp = 0.5;
                else if (actred > -99) /* -99 = 1-1/0.1^2 */
                    temp = MAX(dirder / (2*dirder + actred), 0.1);
                else
                    temp = 0.1;
                delta = temp * MIN(delta, pnorm / 0.1);
                lmpar /= temp;
            } else if (ratio >= 0.75) {
                delta = 2 * pnorm;
                lmpar *= 0.5;
            } else if (!lmpar) {
                delta = 2 * pnorm;
            }

            /**  On success, update solution, and test for convergence. **/

            inner_success = ratio >= 1e-4;
            if (inner_success) {

                /* Update x, fvec, and their norms. */
                if (C->scale_diag) {
                    for (j = 0; j < n; j++) {
                        x[j] = wa2[j];
                        wa2[j] = diag[j] * x[j];
                    }
                } else {
                    for (j = 0; j < n; j++)
                        x[j] = wa2[j];
                }
                for (i = 0; i < m; i++)
                    fvec[i] = wf[i];
                xnorm = lm_enorm(n, wa2);
                if (!isfinite(xnorm)) {
                    S->outcome = 12; /* nan */
                    goto terminate;
                }
                fnorm = fnorm1;
            }

            /* Convergence tests. */
            S->outcome = 0;
            if (fnorm <= LM_DWARF)
                goto terminate; /* success: sum of squares almost zero */
            /* Test two criteria (both may be fulfilled). */
            if (fabs(actred) <= C->ftol && prered <= C->ftol && ratio <= 2)
                S->outcome = 1; /* success: x almost stable */
            if (delta <= C->xtol * xnorm)
                S->outcome += 2; /* success: sum of squares almost stable */
            if (S->outcome != 0) {
                goto terminate;
            }

            /** Tests for termination and stringent tolerances. **/
            if (S->nfev >= maxfev) {
                S->outcome = 5;
                goto terminate;
            }
            if (fabs(actred) <= LM_MACHEP && prered <= LM_MACHEP &&
                ratio <= 2) {
                S->outcome = 6;
                goto terminate;
            }
            if (delta <= LM_MACHEP * xnorm) {
                S->outcome = 7;
                goto terminate;
            }
            if (gnorm <= LM_MACHEP) {
                S->outcome = 8;
                goto terminate;
            }

            /** End of the inner loop. Repeat if iteration unsuccessful. **/
            ++inner;
        } while (!inner_success);

    }; /***  End of the outer loop.  ***/

terminate:
    S->fnorm = lm_enorm(m, fvec);
    if (C->verbosity >= 2)
        printf("lmmin outcome (%i) xnorm %g ftol %g xtol %g\n", S->outcome,
               xnorm, C->ftol, C->xtol);
    if (C->verbosity & 1) {
        fprintf(msgfile, "lmmin final ");
        lm_print_pars(nout, x, msgfile); // S->fnorm,
        fprintf(msgfile, "  fnorm = %18.8g\n", S->fnorm);
    }
    if (S->userbreak) /* user-requested break */
        S->outcome = 11;

    /***  Deallocate the workspace.  ***/
    free(ws);

} /*** lmmin. ***/

/******************************************************************************/
/*  lm_lmpar (determine Levenberg-Marquardt parameter)                        */
/******************************************************************************/

void lm_lmpar(const int n, double* r, const int ldr, const int* Pivot,
              const double* diag, const double* qtb, const double delta,
              double* par, double* x, double* Sdiag, double* aux, double* xdi)
/*     Given an m 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 a parameter value par such that if x solves the system
 *
 *          A*x = b  and  sqrt(par)*D*x = 0
 *
 *     in the least squares sense, and dxnorm is the Euclidean norm of D*x,
 *     then either par=0 and (dxnorm-delta) < 0.1*delta, or par>0 and
 *     abs(dxnorm-delta) < 0.1*delta.
 *
 *     Using lm_qrsolv, this subroutine completes the solution of the
 *     problem if it is provided with the necessary information from the
 *     QR factorization, with column pivoting, of A. That is, if A*P = Q*R,
 *     where P is a permutation matrix, Q has orthogonal columns, and R is
 *     an upper triangular matrix with diagonal elements of nonincreasing
 *     magnitude, then lmpar expects the full upper triangle of R, the
 *     permutation matrix P, and the first n components of Q^T*b. On output
 *     lmpar also provides an upper triangular matrix S such that
 *
 *          P^T*(A^T*A + par*D*D)*P = S^T*S.
 *
 *     S is employed within lmpar and may be of separate interest.
 *
 *     Only a few iterations are generally needed for convergence of the
 *     algorithm. If, however, the limit of 10 iterations is reached, then
 *     the output par will contain the best value obtained so far.
 *
 *     Parameters:
 *
 *      n is a positive integer INPUT variable set to the order of r.
 *
 *      r is an n by n array. On INPUT the full upper triangle must contain
 *        the full upper triangle of the matrix R. On OUTPUT the full upper
 *        triangle is unaltered, and the strict lower triangle contains the
 *        strict upper triangle (transposed) of the upper triangular matrix S.
 *
 *      ldr is a positive integer INPUT variable not less than n which
 *        specifies the leading dimension of the array R.
 *
 *      Pivot is an integer INPUT array of length n which defines the
 *        permutation matrix P such that A*P = Q*R. Column j of P is column
 *        Pivot(j) of the identity matrix.
 *
 *      diag is an INPUT array of length n which must contain the diagonal
 *        elements of the matrix D.
 *
 *      qtb is an INPUT array of length n which must contain the first
 *        n elements of the vector Q^T*b.
 *
 *      delta is a positive INPUT variable which specifies an upper bound
 *        on the Euclidean norm of D*x.
 *
 *      par is a nonnegative variable. On INPUT par contains an initial
 *        estimate of the Levenberg-Marquardt parameter. On OUTPUT par
 *        contains the final estimate.
 *
 *      x is an OUTPUT array of length n which contains the least-squares
 *        solution of the system A*x = b, sqrt(par)*D*x = 0, for the output par.
 *
 *      Sdiag is an array of length n needed as workspace; on OUTPUT it
 *        contains the diagonal elements of the upper triangular matrix S.
 *
 *      aux is a multi-purpose work array of length n.
 *
 *      xdi is a work array of length n. On OUTPUT: diag[j] * x[j].
 *
 */
{
    int i, iter, j, nsing;
    double dxnorm, fp, fp_old, gnorm, parc, parl, paru;
    double sum, temp;
    static double p1 = 0.1;

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

    nsing = n;
    for (j = 0; j < n; j++) {
        aux[j] = qtb[j];
        if (r[j*ldr+j] == 0 && nsing == n)
            nsing = j;
        if (nsing < n)
            aux[j] = 0;
    }
    for (j = nsing-1; j >= 0; j--) {
        aux[j] = aux[j] / r[j+ldr*j];
        temp = aux[j];
        for (i = 0; i < j; i++)
            aux[i] -= r[j*ldr+i] * temp;
    }

    for (j = 0; j < n; j++)
        x[Pivot[j]] = aux[j];

    /*** Initialize the iteration counter, evaluate the function at the origin,
         and test for acceptance of the Gauss-Newton direction. ***/

    for (j = 0; j < n; j++)
        xdi[j] = diag[j] * x[j];
    dxnorm = lm_enorm(n, xdi);
    fp = dxnorm - delta;
    if (fp <= p1 * delta) {
#ifdef LMFIT_DEBUG_MESSAGES
        printf("debug lmpar nsing=%d, n=%d, terminate[fp<=p1*del]\n", nsing, n);
#endif
        *par = 0;
        return;
    }

    /*** If the Jacobian is not rank deficient, the Newton step provides a
         lower bound, parl, for the zero of the function. Otherwise set this
         bound to zero. ***/

    parl = 0;
    if (nsing >= n) {
        for (j = 0; j < n; j++)
            aux[j] = diag[Pivot[j]] * xdi[Pivot[j]] / dxnorm;

        for (j = 0; j < n; j++) {
            sum = 0;
            for (i = 0; i < j; i++)
                sum += r[j*ldr+i] * aux[i];
            aux[j] = (aux[j] - sum) / r[j+ldr*j];
        }
        temp = lm_enorm(n, aux);
        parl = fp / delta / temp / temp;
    }

    /*** Calculate an upper bound, paru, for the zero of the function. ***/

    for (j = 0; j < n; j++) {
        sum = 0;
        for (i = 0; i <= j; i++)
            sum += r[j*ldr+i] * qtb[i];
        aux[j] = sum / diag[Pivot[j]];
    }
    gnorm = lm_enorm(n, aux);
    paru = gnorm / delta;
    if (paru == 0)
        paru = LM_DWARF / MIN(delta, p1);

    /*** If the input par lies outside of the interval (parl,paru),
         set par to the closer endpoint. ***/

    *par = MAX(*par, parl);
    *par = MIN(*par, paru);
    if (*par == 0)
        *par = gnorm / dxnorm;

    /*** Iterate. ***/

    for (iter = 0;; iter++) {

        /** Evaluate the function at the current value of par. **/
        if (*par == 0)
            *par = MAX(LM_DWARF, 0.001 * paru);
        temp = sqrt(*par);
        for (j = 0; j < n; j++)
            aux[j] = temp * diag[j];

        lm_qrsolv(n, r, ldr, Pivot, aux, qtb, x, Sdiag, xdi);
        /* return values are r, x, Sdiag */

        for (j = 0; j < n; j++)
            xdi[j] = diag[j] * x[j]; /* used as output */
        dxnorm = lm_enorm(n, xdi);
        fp_old = fp;
        fp = dxnorm - delta;

        /** If the function is small enough, accept the current value
            of par. Also test for the exceptional cases where parl
            is zero or the number of iterations has reached 10. **/
        if (fabs(fp) <= p1 * delta ||
            (parl == 0 && fp <= fp_old && fp_old < 0) || iter == 10) {
#ifdef LMFIT_DEBUG_MESSAGES
            printf("debug lmpar nsing=%d, iter=%d, "
                   "par=%.4e [%.4e %.4e], delta=%.4e, fp=%.4e\n",
                   nsing, iter, *par, parl, paru, delta, fp);
#endif
            break; /* the only exit from the iteration. */
        }

        /** Compute the Newton correction. **/
        for (j = 0; j < n; j++)
            aux[j] = diag[Pivot[j]] * xdi[Pivot[j]] / dxnorm;

        for (j = 0; j < n; j++) {
            aux[j] = aux[j] / Sdiag[j];
            for (i = j+1; i < n; i++)
                aux[i] -= r[j*ldr+i] * aux[j];
        }
        temp = lm_enorm(n, aux);
        parc = fp / delta / temp / temp;

        /** Depending on the sign of the function, update parl or paru. **/
        if (fp > 0)
            parl = MAX(parl, *par);
        else /* fp < 0 [the case fp==0 is precluded by the break condition] */
            paru = MIN(paru, *par);

        /** Compute an improved estimate for par. **/
        *par = MAX(parl, *par + parc);
    }

} /*** lm_lmpar. ***/

/******************************************************************************/
/*  lm_qrfac (QR factorization, from lapack)                                  */
/******************************************************************************/

void lm_qrfac(const int m, const int n, double* A, int* Pivot, double* Rdiag,
              double* Acnorm, double* W)
/*
 *     This subroutine uses Householder transformations with column pivoting
 *     to compute a QR factorization of the m by n matrix A. That is, qrfac
 *     determines an orthogonal matrix Q, a permutation matrix P, and an
 *     upper trapezoidal matrix R with diagonal elements of nonincreasing
 *     magnitude, such that A*P = Q*R. The Householder transformation for
 *     column k, k = 1,2,...,n, is of the form
 *
 *          I - 2*w*wT/|w|^2
 *
 *     where w has zeroes in the first k-1 positions.
 *
 *     Parameters:
 *
 *      m is an INPUT parameter set to the number of rows of A.
 *
 *      n is an INPUT parameter set to the number of columns of A.
 *
 *      A is an m by n array. On INPUT, A contains the matrix for which the
 *        QR factorization is to be computed. On OUTPUT the strict upper
 *        trapezoidal part of A contains the strict upper trapezoidal part
 *        of R, and the lower trapezoidal part of A contains a factored form
 *        of Q (the non-trivial elements of the vectors w described above).
 *
 *      Pivot is an integer OUTPUT array of length n that describes the
 *        permutation matrix P. Column j of P is column Pivot(j) of the
 *        identity matrix.
 *
 *      Rdiag is an OUTPUT array of length n which contains the diagonal
 *        elements of R.
 *
 *      Acnorm is an OUTPUT array of length n which contains the norms of
 *        the corresponding columns of the input matrix A. If this information
 *        is not needed, then Acnorm can share storage with Rdiag.
 *
 *      W is a work array of length n.
 *
 */
{
    int i, j, k, kmax;
    double ajnorm, sum, temp;

#ifdef LMFIT_DEBUG_MESSAGES
    printf("debug qrfac\n");
#endif

    /** Compute initial column norms;
        initialize Pivot with identity permutation. ***/
    for (j = 0; j < n; j++) {
        W[j] = Rdiag[j] = Acnorm[j] = lm_enorm(m, &A[j*m]);
        Pivot[j] = j;
    }

    /** Loop over columns of A. **/
    assert(n <= m);
    for (j = 0; j < n; j++) {

        /** Bring the column of largest norm into the pivot position. **/
        kmax = j;
        for (k = j+1; k < n; k++)
            if (Rdiag[k] > Rdiag[kmax])
                kmax = k;

        if (kmax != j) {
            /* Swap columns j and kmax. */
            k = Pivot[j];
            Pivot[j] = Pivot[kmax];
            Pivot[kmax] = k;
            for (i = 0; i < m; i++) {
                temp = A[j*m+i];
                A[j*m+i] = A[kmax*m+i];
                A[kmax*m+i] = temp;
            }
            /* Half-swap: Rdiag[j], W[j] won't be needed any further. */
            Rdiag[kmax] = Rdiag[j];
            W[kmax] = W[j];
        }

        /** Compute the Householder reflection vector w_j to reduce the
            j-th column of A to a multiple of the j-th unit vector. **/
        ajnorm = lm_enorm(m-j, &A[j*m+j]);
        if (ajnorm == 0) {
            Rdiag[j] = 0;
            continue;
        }

        /* Let the partial column vector A[j][j:] contain w_j := e_j+-a_j/|a_j|,
           where the sign +- is chosen to avoid cancellation in w_jj. */
        if (A[j*m+j] < 0)
            ajnorm = -ajnorm;
        for (i = j; i < m; i++)
            A[j*m+i] /= ajnorm;
        A[j*m+j] += 1;

        /** Apply the Householder transformation U_w := 1 - 2*w_j.w_j/|w_j|^2
            to the remaining columns, and update the norms. **/
        for (k = j+1; k < n; k++) {
            /* Compute scalar product w_j * a_j. */
            sum = 0;
            for (i = j; i < m; i++)
                sum += A[j*m+i] * A[k*m+i];

            /* Normalization is simplified by the coincidence |w_j|^2=2w_jj. */
            temp = sum / A[j*m+j];

            /* Carry out transform U_w_j * a_k. */
            for (i = j; i < m; i++)
                A[k*m+i] -= temp * A[j*m+i];

            /* No idea what happens here. */
            if (Rdiag[k] != 0) {
                temp = A[m*k+j] / Rdiag[k];
                if (fabs(temp) < 1) {
                    Rdiag[k] *= sqrt(1 - SQR(temp));
                    temp = Rdiag[k] / W[k];
                } else
                    temp = 0;
                if (temp == 0 || 0.05 * SQR(temp) <= LM_MACHEP) {
                    Rdiag[k] = lm_enorm(m-j-1, &A[m*k+j+1]);
                    W[k] = Rdiag[k];
                }
            }
        }

        Rdiag[j] = -ajnorm;
    }
} /*** lm_qrfac. ***/

/******************************************************************************/
/*  lm_qrsolv (linear least-squares)                                          */
/******************************************************************************/

void lm_qrsolv(const int n, double* r, const int ldr, const int* Pivot,
               const double* diag, const double* qtb, double* x,
               double* Sdiag, double* W)
/*
 *     Given an m by n matrix A, an n by n diagonal matrix D, and an
 *     m-vector b, the problem is to determine an x which solves the
 *     system
 *
 *          A*x = b  and  D*x = 0
 *
 *     in the least squares sense.
 *
 *     This subroutine completes the solution of the problem if it is
 *     provided with the necessary information from the QR factorization,
 *     with column pivoting, of A. That is, if A*P = Q*R, where P is a
 *     permutation matrix, Q has orthogonal columns, and R is an upper
 *     triangular matrix with diagonal elements of nonincreasing magnitude,
 *     then qrsolv expects the full upper triangle of R, the permutation
 *     matrix P, and the first n components of Q^T*b. The system
 *     A*x = b, D*x = 0, is then equivalent to
 *
 *          R*z = Q^T*b,  P^T*D*P*z = 0,
 *
 *     where x = P*z. If this system does not have full rank, then a least
 *     squares solution is obtained. On output qrsolv also provides an upper
 *     triangular matrix S such that
 *
 *          P^T*(A^T*A + D*D)*P = S^T*S.
 *
 *     S is computed within qrsolv and may be of separate interest.
 *
 *     Parameters:
 *
 *      n is a positive integer INPUT variable set to the order of R.
 *
 *      r is an n by n array. On INPUT the full upper triangle must contain
 *        the full upper triangle of the matrix R. On OUTPUT the full upper
 *        triangle is unaltered, and the strict lower triangle contains the
 *        strict upper triangle (transposed) of the upper triangular matrix S.
 *
 *      ldr is a positive integer INPUT variable not less than n which
 *        specifies the leading dimension of the array R.
 *
 *      Pivot is an integer INPUT array of length n which defines the
 *        permutation matrix P such that A*P = Q*R. Column j of P is column
 *        Pivot(j) of the identity matrix.
 *
 *      diag is an INPUT array of length n which must contain the diagonal
 *        elements of the matrix D.
 *
 *      qtb is an INPUT array of length n which must contain the first
 *        n elements of the vector Q^T*b.
 *
 *      x is an OUTPUT array of length n which contains the least-squares
 *        solution of the system A*x = b, D*x = 0.
 *
 *      Sdiag is an OUTPUT array of length n which contains the diagonal
 *        elements of the upper triangular matrix S.
 *
 *      W is a work array of length n.
 *
 */
{
    int i, kk, j, k, nsing;
    double qtbpj, sum, temp;
    double _sin, _cos, _tan, _cot; /* local variables, not functions */

    /*** Copy R and Q^T*b to preserve input and initialize S.
         In particular, save the diagonal elements of R in x. ***/

    for (j = 0; j < n; j++) {
        for (i = j; i < n; i++)
            r[j*ldr+i] = r[i*ldr+j];
        x[j] = r[j*ldr+j];
        W[j] = qtb[j];
    }

    /*** Eliminate the diagonal matrix D using a Givens rotation. ***/

    for (j = 0; j < n; j++) {

        /*** Prepare the row of D to be eliminated, locating the diagonal
             element using P from the QR factorization. ***/

        if (diag[Pivot[j]] != 0) {
            for (k = j; k < n; k++)
                Sdiag[k] = 0;
            Sdiag[j] = diag[Pivot[j]];

            /*** The transformations to eliminate the row of D modify only
                 a single element of Q^T*b beyond the first n, which is
                 initially 0. ***/

            qtbpj = 0;
            for (k = j; k < n; k++) {

                /** Determine a Givens rotation which eliminates the
                    appropriate element in the current row of D. **/
                if (Sdiag[k] == 0)
                    continue;
                kk = k + ldr * k;
                if (fabs(r[kk]) < fabs(Sdiag[k])) {
                    _cot = r[kk] / Sdiag[k];
                    _sin = 1 / hypot(1, _cot);
                    _cos = _sin * _cot;
                } else {
                    _tan = Sdiag[k] / r[kk];
                    _cos = 1 / hypot(1, _tan);
                    _sin = _cos * _tan;
                }

                /** Compute the modified diagonal element of R and
                    the modified element of (Q^T*b,0). **/
                r[kk] = _cos * r[kk] + _sin * Sdiag[k];
                temp = _cos * W[k] + _sin * qtbpj;
                qtbpj = -_sin * W[k] + _cos * qtbpj;
                W[k] = temp;

                /** Accumulate the tranformation in the row of S. **/
                for (i = k+1; i < n; i++) {
                    temp = _cos * r[k*ldr+i] + _sin * Sdiag[i];
                    Sdiag[i] = -_sin * r[k*ldr+i] + _cos * Sdiag[i];
                    r[k*ldr+i] = temp;
                }
            }
        }

        /** Store the diagonal element of S and restore
            the corresponding diagonal element of R. **/
        Sdiag[j] = r[j*ldr+j];
        r[j*ldr+j] = x[j];
    }

    /*** Solve the triangular system for z. If the system is singular, then
        obtain a least-squares solution. ***/

    nsing = n;
    for (j = 0; j < n; j++) {
        if (Sdiag[j] == 0 && nsing == n)
            nsing = j;
        if (nsing < n)
            W[j] = 0;
    }

    for (j = nsing-1; j >= 0; j--) {
        sum = 0;
        for (i = j+1; i < nsing; i++)
            sum += r[j*ldr+i] * W[i];
        W[j] = (W[j] - sum) / Sdiag[j];
    }

    /*** Permute the components of z back to components of x. ***/

    for (j = 0; j < n; j++)
        x[Pivot[j]] = W[j];

} /*** lm_qrsolv. ***/

/******************************************************************************/
/*  lm_enorm (Euclidean norm)                                                 */
/******************************************************************************/

double lm_enorm(int n, const double* x)
/*     This function calculates the Euclidean norm of an n-vector x.
 *
 *     The Euclidean norm is computed by accumulating the sum of squares
 *     in three different sums. The sums of squares for the small and large
 *     components are scaled so that no overflows occur. Non-destructive
 *     underflows are permitted. Underflows and overflows do not occur in
 *     the computation of the unscaled sum of squares for the intermediate
 *     components. The definitions of small, intermediate and large components
 *     depend on two constants, LM_SQRT_DWARF and LM_SQRT_GIANT. The main
 *     restrictions on these constants are that LM_SQRT_DWARF**2 not underflow
 *     and LM_SQRT_GIANT**2 not overflow.
 *
 *     Parameters:
 *
 *      n is a positive integer INPUT variable.
 *
 *      x is an INPUT array of length n.
 */
{
    int i;
    double agiant, s1, s2, s3, xabs, x1max, x3max;

    s1 = 0;
    s2 = 0;
    s3 = 0;
    x1max = 0;
    x3max = 0;
    agiant = LM_SQRT_GIANT / n;

    /** Sum squares. **/
    for (i = 0; i < n; i++) {
        xabs = fabs(x[i]);
        if (xabs > LM_SQRT_DWARF) {
            if (xabs < agiant) {
                s2 += SQR(xabs);
            } else if (xabs > x1max) {
                s1 = 1 + s1 * SQR(x1max / xabs);
                x1max = xabs;
            } else {
                s1 += SQR(xabs / x1max);
            }
        } else if (xabs > x3max) {
            s3 = 1 + s3 * SQR(x3max / xabs);
            x3max = xabs;
        } else if (xabs != 0) {
            s3 += SQR(xabs / x3max);
        }
    }

    /** Calculate the norm. **/
    if (s1 != 0)
        return x1max * sqrt(s1 + (s2 / x1max) / x1max);
    else if (s2 != 0)
        if (s2 >= x3max)
            return sqrt(s2 * (1 + (x3max / s2) * (x3max * s3)));
        else
            return sqrt(x3max * ((s2 / x3max) + (x3max * s3)));
    else
        return x3max * sqrt(s3);

} /*** lm_enorm. ***/