lmmin.cxx

Go to the documentation of this file.

/*
 * Project:  LevenbergMarquardtLeastSquaresFitting
 *
 * File:     lmmin.c
 *
 * Contents: Levenberg-Marquardt core implementation,
 *           and simplified user interface.
 *
 * Author:   Joachim Wuttke <j.wuttke@fz-juelich.de>
 *
 * Homepage: joachimwuttke.de/lmfit
 */
 
/*
 * lmfit is released under the LMFIT-BEER-WARE licence:
 *
 * In writing this software, I borrowed heavily from the public domain,
 * especially from work by Burton S. Garbow, Kenneth E. Hillstrom,
 * Jorge J. Moré, Steve Moshier, and the authors of lapack. To avoid
 * unneccessary complications, I put my additions and amendments also
 * into the public domain. Please retain this notice. Otherwise feel
 * free to do whatever you want with this stuff. If we meet some day,
 * and you think this work is worth it, you can buy me a beer in return.
 */
 
#include "lmmin.h"
 
#include <cfloat>
#include <cmath>
#include <cstdio>
#include <cstdlib>
 
/*****************************************************************************/
/*  set numeric constants                                                    */
/*****************************************************************************/
 
/* machine-dependent constants 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
*/
 
const lm_control_struct lm_control_double = {LM_USERTOL, LM_USERTOL, LM_USERTOL, LM_USERTOL, 100., 100, 1, 0};
const lm_control_struct lm_control_float = {1.e-7, 1.e-7, 1.e-7, 1.e-7, 100., 100, 0, 0};
 
/*****************************************************************************/
/*  set message texts (indexed by status.info)                               */
/*****************************************************************************/
 
const char *lm_infmsg[] = {"success (sum of squares below underflow limit)",
                           "success (the relative error in the sum of squares is at most tol)",
                           "success (the relative error between x and the solution is at most tol)",
                           "success (both errors are at most tol)",
                           "trapped by degeneracy (fvec is orthogonal to the columns of the jacobian)",
                           "timeout (number of calls to fcn has reached maxcall*(n+1))",
                           "failure (ftol<tol: cannot reduce sum of squares any further)",
                           "failure (xtol<tol: cannot improve approximate solution any further)",
                           "failure (gtol<tol: cannot improve approximate solution any further)",
                           "exception (not enough memory)",
                           "fatal coding error (improper input parameters)",
                           "exception (break requested within function evaluation)"};
 
const char *lm_shortmsg[] = {"success (0)", "success (f)", "success (p)",   "success (f,p)",
                             "degenerate",  "call limit",  "failed (f)",    "failed (p)",
                             "failed (o)",  "no memory",   "invalid input", "user break"};
 
/*****************************************************************************/
/*  lm_printout_std (default monitoring routine)                             */
/*****************************************************************************/
 
void lm_printout_std(int n_par, const double *par, int m_dat, const void *data, const double *fvec, int printflags,
                     int iflag, int iter, int nfev)
/*
 *       data  : for soft control of printout behaviour, add control
 *                 variables to the data struct
 *       iflag : 0 (init) 1 (outer loop) 2(inner loop) -1(terminated)
 *       iter  : outer loop counter
 *       nfev  : number of calls to *evaluate
 */
{
   int i = 0;
 
   if (!printflags)
      return;
 
   if (printflags & 1) {
      /* location of printout call within lmdif */
      if (iflag == 2) {
         printf("trying step in gradient direction  ");
      } else if (iflag == 1) {
         printf("determining gradient (iteration %2d)", iter);
      } else if (iflag == 0) {
         printf("starting minimization              ");
      } else if (iflag == -1) {
         printf("terminated after %3d evaluations   ", nfev);
      }
   }
 
   if (printflags & 2) {
      printf("  par: ");
      for (i = 0; i < n_par; ++i)
         printf(" %18.11g", par[i]);
      printf(" => norm: %18.11g", lm_enorm(m_dat, fvec));
   }
 
   if (printflags & 3)
      printf("\n");
 
   if ((printflags & 8) || ((printflags & 4) && iflag == -1)) {
      printf("  residuals:\n");
      for (i = 0; i < m_dat; ++i)
         printf("    fvec[%2d]=%12g\n", i, fvec[i]);
   }
}
 
/*****************************************************************************/
/*  lm_minimize (intermediate-level interface)                               */
/*****************************************************************************/
 
void lmmin(int n_par, double *par, int m_dat, const void *data,
           void (*evaluate)(const double *par, int m_dat, const void *data, double *fvec, int *info),
           const lm_control_struct *control, lm_status_struct *status,
           void (*printout)(int n_par, const double *par, int m_dat, const void *data, const double *fvec,
                            int printflags, int iflag, int iter, int nfev))
{
 
   /*** allocate work space. ***/
 
   double *fvec, *diag, *fjac, *qtf, *wa1, *wa2, *wa3, *wa4; // NOLINT
   int *ipvt = nullptr, j = 0;
 
   int n = n_par;
   int m = m_dat;
 
   if ((fvec = (double *)malloc(m * sizeof(double))) == nullptr ||
       (diag = (double *)malloc(n * sizeof(double))) == nullptr ||
       (qtf = (double *)malloc(n * sizeof(double))) == nullptr ||
       (fjac = (double *)malloc(n * m * sizeof(double))) == nullptr ||
       (wa1 = (double *)malloc(n * sizeof(double))) == nullptr ||
       (wa2 = (double *)malloc(n * sizeof(double))) == nullptr ||
       (wa3 = (double *)malloc(n * sizeof(double))) == nullptr ||
       (wa4 = (double *)malloc(m * sizeof(double))) == nullptr || (ipvt = (int *)malloc(n * sizeof(int))) == nullptr) {
      status->info = 9; // NOLINT
      return;
   }
 
   if (!control->scale_diag)
      for (j = 0; j < n_par; ++j)
         diag[j] = 1;
 
   /*** perform fit. ***/
 
   status->info = 0;
 
   /* this goes through the modified legacy interface: */
   lm_lmdif(m, n, par, fvec, control->ftol, control->xtol, control->gtol, control->maxcall * (n + 1), control->epsilon,
            diag, (control->scale_diag ? 1 : 2), control->stepbound, &(status->info), &(status->nfev), fjac, ipvt, qtf,
            wa1, wa2, wa3, wa4, evaluate, printout, control->printflags, data);
 
   if (printout)
      (*printout)(n, par, m, data, fvec, control->printflags, -1, 0, status->nfev);
   status->fnorm = lm_enorm(m, fvec);
   if (status->info < 0)
      status->info = 11;
 
   /*** clean up. ***/
 
   free(fvec);
   free(diag);
   free(qtf);
   free(fjac);
   free(wa1);
   free(wa2);
   free(wa3);
   free(wa4);
   free(ipvt);
} /*** lmmin. ***/
 
/*****************************************************************************/
/*  lm_lmdif (low-level, modified legacy interface for full control)         */
/*****************************************************************************/
 
void lm_lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double delta, double *par, double *x,
              double *sdiag, double *aux, double *xdi);
void lm_qrfac(int m, int n, double *a, int pivot, int *ipvt, double *rdiag, double *acnorm, double *wa);
void lm_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, double *wa);
 
#define MIN(a, b) (((a) <= (b)) ? (a) : (b))
#define MAX(a, b) (((a) >= (b)) ? (a) : (b))
#define SQR(x) (x) * (x)
 
void lm_lmdif(int m, int n, double *x, double *fvec, double ftol, double xtol, double gtol, int maxfev, double epsfcn,
              double *diag, int mode, double factor, int *info, int *nfev, double *fjac, int *ipvt, double *qtf,
              double *wa1, double *wa2, double *wa3, double *wa4,
              void (*evaluate)(const double *par, int m_dat, const void *data, double *fvec, int *info),
              void (*printout)(int n_par, const double *par, int m_dat, const void *data, const double *fvec,
                               int printflags, int iflag, int iter, int nfev),
              int printflags, const void *data)
{
   /*
    *   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 evaluate which calculates the functions. The jacobian
    *   is then calculated by a forward-difference approximation.
    *
    *   The multi-parameter interface lm_lmdif is for users who want
    *   full control and flexibility. Most users will be better off using
    *   the simpler interface lmmin provided above.
    *
    *   Parameters:
    *
    *      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 lm_fcn is at least
    *        maxfev by the end of an iteration.
    *
    *      epsfcn is an input variable used in choosing a step length for
    *        the forward-difference approximation. The relative errors in
    *        the functions are assumed to be of the order of epsfcn.
    *
    *      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.
    *
    *      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 (0.1,100.0). Generally, the value 100.0 is recommended.
    *
    *      info is an integer OUTPUT variable that indicates the termination
    *        status of lm_lmdif as follows:
    *
    *        info < 0  termination requested by user-supplied routine *evaluate;
    *
    *        info = 0  fnorm almost vanishing;
    *
    *        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
    *                  is at most 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 lm_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;
    *
    *        info =10  improper input parameters;
    *
    *      nfev is an OUTPUT variable set to the number of calls to the
    *        user-supplied routine *evaluate.
    *
    *      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
    *
    *              pT*(jacT*jac)*p = rT*r
    *
    *              (NOTE: T stands for matrix transposition),
    *
    *        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.
    *
    *      ipvt is an integer OUTPUT 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 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, used among others to hold
    *        residuals from evaluate.
    *
    *      evaluate points to the subroutine which calculates the
    *        m nonlinear functions. Implementations should be written as follows:
    *
    *        void evaluate( double* par, int m_dat, void *data,
    *                       double* fvec, int *info )
    *        {
    *           // for ( i=0; i<m_dat; ++i )
    *           //     calculate fvec[i] for given parameters par;
    *           // to stop the minimization,
    *           //     set *info to a negative integer.
    *        }
    *
    *      printout points to the subroutine which informs about fit progress.
    *        Call with printout=0 if no printout is desired.
    *        Call with printout=lm_printout_std to use the default implementation.
    *
    *      printflags is passed to printout.
    *
    *      data is an input pointer to an arbitrary structure that is passed to
    *        evaluate. Typically, it contains experimental data to be fitted.
    *
    */
   int i = 0, iter = 0, j = 0;
   double actred = NAN, delta = NAN, dirder = NAN, eps = NAN, fnorm = NAN, fnorm1 = NAN, gnorm = NAN, par = NAN,
          pnorm = NAN, prered = NAN, ratio = NAN, step = NAN, sum = NAN, temp = NAN, temp1 = NAN, temp2 = NAN,
          temp3 = NAN, xnorm = NAN;
   static double p1 = 0.1;
   static double p0001 = 1.0e-4;
 
   *nfev = 0; /* function evaluation counter */
   iter = 0;  /* outer loop counter */
   par = 0;   /* levenberg-marquardt parameter */
   delta = 0; /* to prevent a warning (initialization within if-clause) */
   xnorm = 0; /* ditto */
   temp = MAX(epsfcn, LM_MACHEP);
   eps = sqrt(temp); /* for calculating the Jacobian by forward differences */
 
   /*** lmdif: check input parameters for errors. ***/
 
   if ((n <= 0) || (m < n) || (ftol < 0.) || (xtol < 0.) || (gtol < 0.) || (maxfev <= 0) || (factor <= 0.)) {
      *info = 10; // invalid parameter
      return;
   }
   if (mode == 2) {             /* scaling by diag[] */
      for (j = 0; j < n; j++) { /* check for nonpositive elements */
         if (diag[j] <= 0.0) {
            *info = 10; // invalid parameter
            return;
         }
      }
   }
#ifdef LMFIT_DEBUG_MESSAGES
   printf("lmdif\n");
#endif
 
   /*** lmdif: evaluate function at starting point and calculate norm. ***/
 
   *info = 0;
   (*evaluate)(x, m, data, fvec, info);
   ++(*nfev);
   if (printout)
      (*printout)(n, x, m, data, fvec, printflags, 0, 0, *nfev);
   if (*info < 0)
      return;
   fnorm = lm_enorm(m, fvec);
   if (fnorm <= LM_DWARF) {
      *info = 0;
      return;
   }
 
   /*** lmdif: the outer loop. ***/
 
   do {
#ifdef LMFIT_DEBUG_MESSAGES
      printf("lmdif/ outer loop iter=%d nfev=%d fnorm=%.10e\n", iter, *nfev, fnorm);
#endif
 
      /*** outer: calculate the Jacobian. ***/
 
      for (j = 0; j < n; j++) {
         temp = x[j];
         step = MAX(eps * eps, eps * fabs(temp));
         x[j] = temp + step; /* replace temporarily */
         *info = 0;
         (*evaluate)(x, m, data, wa4, info);
         ++(*nfev);
         if (printout)
            (*printout)(n, x, m, data, wa4, printflags, 1, iter, *nfev);
         if (*info < 0)
            return; /* user requested break */
         for (i = 0; i < m; i++)
            fjac[j * m + i] = (wa4[i] - fvec[i]) / step;
         x[j] = temp; /* restore */
      }
#ifdef LMFIT_DEBUG_MAtRIX
      /* print the entire matrix */
      for (i = 0; i < m; i++) {
         for (j = 0; j < n; j++)
            printf("%.5e ", fjac[j * m + i]);
         printf("\n");
      }
#endif
 
      /*** outer: compute the qr factorization of the Jacobian. ***/
 
      lm_qrfac(m, n, fjac, 1, ipvt, wa1, wa2, wa3);
      /* return values are ipvt, wa1=rdiag, wa2=acnorm */
 
      if (!iter) {
         /* first iteration only */
         if (mode != 2) {
            /* diag := norms of the columns of the initial Jacobian */
            for (j = 0; j < n; j++) {
               diag[j] = wa2[j];
               if (wa2[j] == 0.)
                  diag[j] = 1.;
            }
         }
         /* use diag to scale x, then calculate the norm */
         for (j = 0; j < n; j++)
            wa3[j] = diag[j] * x[j];
         xnorm = lm_enorm(n, wa3);
         /* initialize the step bound delta. */
         delta = factor * xnorm;
         if (delta == 0.)
            delta = factor;
      } else {
         if (mode != 2) {
            for (j = 0; j < n; j++)
               diag[j] = MAX(diag[j], wa2[j]);
         }
      }
 
      /*** outer: form (q transpose)*fvec and store first n components in qtf. ***/
 
      for (i = 0; i < m; i++)
         wa4[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] * wa4[i];
            temp = -sum / temp3;
            for (i = j; i < m; i++)
               wa4[i] += fjac[j * m + i] * temp;
         }
         fjac[j * m + j] = wa1[j];
         qtf[j] = wa4[j];
      }
 
      /*** outer: compute norm of scaled gradient and test for convergence. ***/
 
      gnorm = 0;
      for (j = 0; j < n; j++) {
         if (wa2[ipvt[j]] == 0)
            continue;
         sum = 0.;
         for (i = 0; i <= j; i++)
            sum += fjac[j * m + i] * qtf[i];
         gnorm = MAX(gnorm, fabs(sum / wa2[ipvt[j]] / fnorm));
      }
 
      if (gnorm <= gtol) {
         *info = 4;
         return;
      }
 
      /*** the inner loop. ***/
      do {
#ifdef LMFIT_DEBUG_MESSAGES
         printf("lmdif/ inner loop iter=%d nfev=%d\n", iter, *nfev);
#endif
 
         /*** inner: determine the levenberg-marquardt parameter. ***/
 
         lm_lmpar(n, fjac, m, ipvt, diag, qtf, delta, &par, wa1, wa2, wa4, wa3);
         /* used return values are fjac (partly), par, wa1=x, wa3=diag*x */
 
         for (j = 0; j < n; j++)
            wa2[j] = x[j] - wa1[j]; /* new parameter vector ? */
 
         pnorm = lm_enorm(n, wa3);
 
         /* at first call, adjust the initial step bound. */
 
         if (*nfev <= 1 + n)
            delta = MIN(delta, pnorm);
 
         /*** inner: evaluate the function at x + p and calculate its norm. ***/
 
         *info = 0;
         (*evaluate)(wa2, m, data, wa4, info);
         ++(*nfev);
         if (printout)
            (*printout)(n, wa2, m, data, wa4, printflags, 2, iter, *nfev);
         if (*info < 0)
            return; /* user requested break. */
 
         fnorm1 = lm_enorm(m, wa4);
#ifdef LMFIT_DEBUG_MESSAGES
         printf("lmdif/ pnorm %.10e  fnorm1 %.10e  fnorm %.10e"
                " delta=%.10e par=%.10e\n",
                pnorm, fnorm1, fnorm, delta, par);
#endif
 
         /*** inner: compute the scaled actual reduction. ***/
 
         if (p1 * fnorm1 < fnorm)
            actred = 1 - SQR(fnorm1 / fnorm);
         else
            actred = -1;
 
         /*** inner: compute the scaled predicted reduction and
              the scaled directional derivative. ***/
 
         for (j = 0; j < n; j++) {
            wa3[j] = 0;
            for (i = 0; i <= j; i++)
               wa3[i] -= fjac[j * m + i] * wa1[ipvt[j]];
         }
         temp1 = lm_enorm(n, wa3) / fnorm;
         temp2 = sqrt(par) * pnorm / fnorm;
         prered = SQR(temp1) + 2 * SQR(temp2);
         dirder = -(SQR(temp1) + SQR(temp2));
 
         /*** inner: compute the ratio of the actual to the predicted reduction. ***/
 
         ratio = prered != 0 ? actred / prered : 0;
#ifdef LMFIT_DEBUG_MESSAGES
         printf("lmdif/ actred=%.10e prered=%.10e ratio=%.10e"
                " sq(1)=%.10e sq(2)=%.10e dd=%.10e\n",
                actred, prered, prered != 0 ? ratio : 0., SQR(temp1), SQR(temp2), dirder);
#endif
 
         /*** inner: update the step bound. ***/
 
         if (ratio <= 0.25) {
            if (actred >= 0.)
               temp = 0.5;
            else
               temp = 0.5 * dirder / (dirder + 0.5 * actred);
            if (p1 * fnorm1 >= fnorm || temp < p1)
               temp = p1;
            delta = temp * MIN(delta, pnorm / p1);
            par /= temp;
         } else if (par == 0. || ratio >= 0.75) {
            delta = pnorm / 0.5;
            par *= 0.5;
         }
 
         /*** inner: test for successful iteration. ***/
 
         if (ratio >= p0001) {
            /* yes, success: update x, fvec, and their norms. */
            for (j = 0; j < n; j++) {
               x[j] = wa2[j];
               wa2[j] = diag[j] * x[j];
            }
            for (i = 0; i < m; i++)
               fvec[i] = wa4[i];
            xnorm = lm_enorm(n, wa2);
            fnorm = fnorm1;
            iter++;
         }
#ifdef LMFIT_DEBUG_MESSAGES
         else {
            printf("AtTN: iteration considered unsuccessful\n");
         }
#endif
 
         /*** inner: test for convergence. ***/
 
         if (fnorm <= LM_DWARF) {
            *info = 0;
            return;
         }
 
         *info = 0;
         if (fabs(actred) <= ftol && prered <= ftol && 0.5 * ratio <= 1)
            *info = 1;
         if (delta <= xtol * xnorm)
            *info += 2;
         if (*info != 0)
            return;
 
         /*** inner: tests for termination and stringent tolerances. ***/
 
         if (*nfev >= maxfev) {
            *info = 5;
            return;
         }
         if (fabs(actred) <= LM_MACHEP && prered <= LM_MACHEP && 0.5 * ratio <= 1) {
            *info = 6;
            return;
         }
         if (delta <= LM_MACHEP * xnorm) {
            *info = 7;
            return;
         }
         if (gnorm <= LM_MACHEP) {
            *info = 8;
            return;
         }
 
         /*** inner: end of the loop. repeat if iteration unsuccessful. ***/
 
      } while (ratio < p0001);
 
      /*** outer: end of the loop. ***/
 
   } while (true);
 
} /*** lm_lmdif. ***/
 
/*****************************************************************************/
/*  lm_lmpar (determine Levenberg-Marquardt parameter)                       */
/*****************************************************************************/
 
void lm_lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, 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 value for the parameter
    *     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 qT*b. On output
    *     lmpar also provides an upper triangular matrix s such that
    *
    *          pT*(aT*a + par*d*d)*p = sT*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.
    *
    *      ipvt 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 ipvt(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 transpose)*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 which 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 = 0, iter = 0, j = 0, nsing = 0;
   double dxnorm = NAN, fp = NAN, fp_old = NAN, gnorm = NAN, parc = NAN, parl = NAN, paru = NAN;
   double sum = NAN, temp = NAN;
   static double p1 = 0.1;
 
#ifdef LMFIT_DEBUG_MESSAGES
   printf("lmpar\n");
#endif
 
   /*** lmpar: 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;
   }
#ifdef LMFIT_DEBUG_MESSAGES
   printf("nsing %d ", nsing);
#endif
   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[ipvt[j]] = aux[j];
 
   /*** lmpar: initialize the iteration counter, evaluate the function at the
        origin, and test for acceptance of the gauss-newton direction. ***/
 
   iter = 0;
   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("lmpar/ terminate (fp<p1*delta)\n");
#endif
      *par = 0;
      return;
   }
 
   /*** lmpar: if the jacobian is not rank deficient, the newton
        step provides a lower bound, parl, for the 0. of
        the function. otherwise set this bound to 0.. ***/
 
   parl = 0;
   if (nsing >= n) {
      for (j = 0; j < n; j++)
         aux[j] = diag[ipvt[j]] * xdi[ipvt[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;
   }
 
   /*** lmpar: calculate an upper bound, paru, for the 0. 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[ipvt[j]];
   }
   gnorm = lm_enorm(n, aux);
   paru = gnorm / delta;
   if (paru == 0.)
      paru = LM_DWARF / MIN(delta, p1);
 
   /*** lmpar: 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;
#ifdef LMFIT_DEBUG_MESSAGES
   printf("lmpar/ parl %.4e  par %.4e  paru %.4e\n", parl, *par, paru);
#endif
 
   /*** lmpar: iterate. ***/
 
   for (;; iter++) {
 
      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, ipvt, 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 (fabs(fp) <= p1 * delta || (parl == 0. && fp <= fp_old && fp_old < 0.) || iter == 10)
         break; /* the only exit from the iteration. */
 
      for (j = 0; j < n; j++)
         aux[j] = diag[ipvt[j]] * xdi[ipvt[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;
 
      if (fp > 0)
         parl = MAX(parl, *par);
      else if (fp < 0)
         paru = MIN(paru, *par);
      /* the case fp==0 is precluded by the break condition  */
 
      *par = MAX(parl, *par + parc);
   }
 
} /*** lm_lmpar. ***/
 
/*****************************************************************************/
/*  lm_qrfac (QR factorisation, from lapack)                                 */
/*****************************************************************************/
 
void lm_qrfac(int m, int n, double *a, int pivot, int *ipvt, double *rdiag, double *acnorm, double *wa)
{
   /*
    *     This subroutine uses householder transformations with column
    *     pivoting (optional) 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,...,min(m,n), is of the form
    *
    *          i - (1/u(k))*u*uT
    *
    *     where u has zeroes in the first k-1 positions. The form of
    *     this transformation and the method of pivoting first
    *     appeared in the corresponding linpack subroutine.
    *
    *     Parameters:
    *
    *      m is a positive integer input variable set to the number
    *        of rows of a.
    *
    *      n is a positive integer input variable 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 u vectors described above).
    *
    *      pivot is a logical input variable. If pivot is set true,
    *        then column pivoting is enforced. If pivot is set false,
    *        then no column pivoting is done.
    *
    *      ipvt is an integer OUTPUT array of length lipvt. This array
    *        defines the permutation matrix p such that a*p = q*r.
    *        Column j of p is column ipvt(j) of the identity matrix.
    *        If pivot is false, ipvt is not referenced.
    *
    *      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 coincide
    *        with rdiag.
    *
    *      wa is a work array of length n. If pivot is false, then wa
    *        can coincide with rdiag.
    *
    */
   int i = 0, j = 0, k = 0, kmax = 0, minmn = 0;
   double ajnorm = NAN, sum = NAN, temp = NAN;
 
   /*** qrfac: compute initial column norms and initialize several arrays. ***/
 
   for (j = 0; j < n; j++) {
      acnorm[j] = lm_enorm(m, &a[j * m]);
      rdiag[j] = acnorm[j];
      wa[j] = rdiag[j];
      if (pivot)
         ipvt[j] = j;
   }
#ifdef LMFIT_DEBUG_MESSAGES
   printf("qrfac\n");
#endif
 
   /*** qrfac: reduce a to r with householder transformations. ***/
 
   minmn = MIN(m, n);
   for (j = 0; j < minmn; j++) {
      if (!pivot)
         goto pivot_ok;
 
      kmax = j;
      for (k = j + 1; k < n; k++)
         if (rdiag[k] > rdiag[kmax])
            kmax = k;
      if (kmax == j)
         goto pivot_ok;
 
      for (i = 0; i < m; i++) {
         temp = a[j * m + i];
         a[j * m + i] = a[kmax * m + i];
         a[kmax * m + i] = temp;
      }
      rdiag[kmax] = rdiag[j];
      wa[kmax] = wa[j];
      k = ipvt[j];
      ipvt[j] = ipvt[kmax];
      ipvt[kmax] = k;
 
   pivot_ok:
      ajnorm = lm_enorm(m - j, &a[j * m + j]);
      if (ajnorm == 0.) {
         rdiag[j] = 0;
         continue;
      }
 
      if (a[j * m + j] < 0.)
         ajnorm = -ajnorm;
      for (i = j; i < m; i++)
         a[j * m + i] /= ajnorm;
      a[j * m + j] += 1;
 
      for (k = j + 1; k < n; k++) {
         sum = 0;
 
         for (i = j; i < m; i++)
            sum += a[j * m + i] * a[k * m + i];
 
         temp = sum / a[j + m * j];
 
         for (i = j; i < m; i++)
            a[k * m + i] -= temp * a[j * m + i];
 
         if (pivot && rdiag[k] != 0.) {
            temp = a[m * k + j] / rdiag[k];
            temp = MAX(0., 1 - temp * temp);
            rdiag[k] *= sqrt(temp);
            temp = rdiag[k] / wa[k];
            if (0.05 * SQR(temp) <= LM_MACHEP) {
               rdiag[k] = lm_enorm(m - j - 1, &a[m * k + j + 1]);
               wa[k] = rdiag[k];
            }
         }
      }
 
      rdiag[j] = -ajnorm;
   }
}
 
/*****************************************************************************/
/*  lm_qrsolv (linear least-squares)                                         */
/*****************************************************************************/
 
void lm_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, double *wa)
{
   /*
    *     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 transpose)*b. The system
    *     a*x = b, d*x = 0, is then equivalent to
    *
    *          r*z = qT*b,  pT*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
    *
    *          pT *(aT *a + d*d)*p = sT *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.
    *
    *      ipvt 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 ipvt(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 transpose)*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.
    *
    *      wa is a work array of length n.
    *
    */
   int i = 0, kk = 0, j = 0, k = 0, nsing = 0;
   double qtbpj = NAN, sum = NAN, temp = NAN;
   double _sin = NAN, _cos = NAN, _tan = NAN, _cot = NAN; /* local variables, not functions */
 
   /*** qrsolv: copy r and (q transpose)*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];
      wa[j] = qtb[j];
   }
#ifdef LMFIT_DEBUG_MESSAGES
   printf("qrsolv\n");
#endif
 
   /*** qrsolv: eliminate the diagonal matrix d using a Givens rotation. ***/
 
   for (j = 0; j < n; j++) {
 
      /*** qrsolv: prepare the row of d to be eliminated, locating the
           diagonal element using p from the qr factorization. ***/
 
      if (diag[ipvt[j]] == 0.)
         goto L90;
      for (k = j; k < n; k++)
         sdiag[k] = 0.;
      sdiag[j] = diag[ipvt[j]];
 
      /*** qrsolv: the transformations to eliminate the row of d modify only
           a single element of qT*b beyond the first n, which is initially 0. ***/
 
      qtbpj = 0.;
      for (k = j; k < n; k++) {
 
         if (sdiag[k] == 0.)
            continue;
         kk = k + ldr * k;
         if (fabs(r[kk]) < fabs(sdiag[k])) {
            _cot = r[kk] / sdiag[k];
            _sin = 1 / sqrt(1 + SQR(_cot));
            _cos = _sin * _cot;
         } else {
            _tan = sdiag[k] / r[kk];
            _cos = 1 / sqrt(1 + SQR(_tan));
            _sin = _cos * _tan;
         }
 
         r[kk] = _cos * r[kk] + _sin * sdiag[k];
         temp = _cos * wa[k] + _sin * qtbpj;
         qtbpj = -_sin * wa[k] + _cos * qtbpj;
         wa[k] = temp;
 
         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;
         }
      }
 
   L90:
      sdiag[j] = r[j * ldr + j];
      r[j * ldr + j] = x[j];
   }
 
   /*** qrsolv: 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)
         wa[j] = 0;
   }
 
   for (j = nsing - 1; j >= 0; j--) {
      sum = 0;
      for (i = j + 1; i < nsing; i++)
         sum += r[j * ldr + i] * wa[i];
      wa[j] = (wa[j] - sum) / sdiag[j];
   }
 
   /*** qrsolv: permute the components of z back to components of x. ***/
 
   for (j = 0; j < n; j++)
      x[ipvt[j]] = wa[j];
 
} /*** lm_qrsolv. ***/
 
/*****************************************************************************/
/*  lm_enorm (Euclidean norm)                                                */
/*****************************************************************************/
 
double lm_enorm(int n, const double *x)
{
   /*     Given an n-vector x, this function calculates the
    *     euclidean norm of 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 = 0;
   double agiant = NAN, s1 = NAN, s2 = NAN, s3 = NAN, xabs = NAN, x1max = NAN, x3max = NAN, temp = NAN;
 
   s1 = 0;
   s2 = 0;
   s3 = 0;
   x1max = 0;
   x3max = 0;
   agiant = LM_SQRT_GIANT / n;
 
   for (i = 0; i < n; i++) {
      xabs = fabs(x[i]);
      if (xabs > LM_SQRT_DWARF) {
         if (xabs < agiant) {
            s2 += xabs * xabs;
         } else if (xabs > x1max) {
            temp = x1max / xabs;
            s1 = 1 + s1 * SQR(temp);
            x1max = xabs;
         } else {
            temp = xabs / x1max;
            s1 += SQR(temp);
         }
      } else if (xabs > x3max) {
         temp = x3max / xabs;
         s3 = 1 + s3 * SQR(temp);
         x3max = xabs;
      } else if (xabs != 0.) {
         temp = xabs / x3max;
         s3 += SQR(temp);
      }
   }
 
   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. ***/