be/src/thirdparty/mpfit/mpfit.c

/* 
 * MINPACK-1 Least Squares Fitting Library
 *
 * Original public domain version by B. Garbow, K. Hillstrom, J. More'
 *   (Argonne National Laboratory, MINPACK project, March 1980)
 * See the file DISCLAIMER for copyright information.
 * 
 * Tranlation to C Language by S. Moshier (moshier.net)
 * 
 * Enhancements and packaging by C. Markwardt
 *   (comparable to IDL fitting routine MPFIT
 *    see http://cow.physics.wisc.edu/~craigm/idl/idl.html)
 */

/* Main mpfit library routines (double precision) 
   $Id: mpfit.c,v 1.24 2013/04/23 18:37:38 craigm Exp $
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "mpfit.h"

/* Forward declarations of functions in this module */
static int mp_fdjac2(mp_func funct,
	      int m, int n, int *ifree, int npar, double *x, double *fvec,
	      double *fjac, int ldfjac, double epsfcn,
	      double *wa, void *priv, int *nfev,
	      double *step, double *dstep, int *dside,
	      int *qulimited, double *ulimit,
	      int *ddebug, double *ddrtol, double *ddatol,
	      double *wa2, double **dvecptr);
static void mp_qrfac(int m, int n, double *a, int lda, 
	      int pivot, int *ipvt, int lipvt,
	      double *rdiag, double *acnorm, double *wa);
static void mp_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag,
	       double *qtb, double *x, double *sdiag, double *wa);
static void mp_lmpar(int n, double *r, int ldr, int *ipvt, int *ifree, double *diag,
	      double *qtb, double delta, double *par, double *x,
	      double *sdiag, double *wa1, double *wa2);
static double mp_enorm(int n, double *x);
static double mp_dmax1(double a, double b);
static double mp_dmin1(double a, double b);
static int mp_min0(int a, int b);
static int mp_covar(int n, double *r, int ldr, int *ipvt, double tol, double *wa);

/* Macro to call user function */
#define mp_call(funct, m, n, x, fvec, dvec, priv) (*(funct))(m,n,x,fvec,dvec,priv)

/* Macro to safely allocate memory */
#define mp_malloc(dest,type,size) \
  dest = (type *) malloc( sizeof(type)*size ); \
  if (dest == 0) { \
    info = MP_ERR_MEMORY; \
    goto CLEANUP; \
  } else { \
    int _k; \
    for (_k=0; _k<(size); _k++) dest[_k] = 0; \
  } 

/*
*     **********
*
*     subroutine mpfit
*
*     the purpose of mpfit is to minimize the sum of the squares of
*     m nonlinear functions in n variables by a modification of
*     the levenberg-marquardt algorithm. the user must provide a
*     subroutine which calculates the functions. the jacobian is
*     then calculated by a finite-difference approximation.
*
*     mp_funct funct - function to be minimized
*     int m          - number of data points
*     int npar       - number of fit parameters
*     double *xall   - array of n initial parameter values
*                      upon return, contains adjusted parameter values
*     mp_par *pars   - array of npar structures specifying constraints;
*                      or 0 (null pointer) for unconstrained fitting
*                      [ see README and mpfit.h for definition & use of mp_par]
*     mp_config *config - pointer to structure which specifies the
*                      configuration of mpfit(); or 0 (null pointer)
*                      if the default configuration is to be used.
*                      See README and mpfit.h for definition and use
*                      of config.
*     void *private  - any private user data which is to be passed directly
*                      to funct without modification by mpfit().
*     mp_result *result - pointer to structure, which upon return, contains
*                      the results of the fit.  The user should zero this
*                      structure.  If any of the array values are to be 
*                      returned, the user should allocate storage for them
*                      and assign the corresponding pointer in *result.
*                      Upon return, *result will be updated, and
*                      any of the non-null arrays will be filled.
*
*
* FORTRAN DOCUMENTATION BELOW
*
*
*     the subroutine statement is
*
*	subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
*			 diag,mode,factor,nprint,info,nfev,fjac,
*			 ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4)
*
*     where
*
*	fcn is the name of the user-supplied subroutine which
*	  calculates the functions. fcn must be declared
*	  in an external statement in the user calling
*	  program, and should be written as follows.
*
*	  subroutine fcn(m,n,x,fvec,iflag)
*	  integer m,n,iflag
*	  double precision x(n),fvec(m)
*	  ----------
*	  calculate the functions at x and
*	  return this vector in fvec.
*	  ----------
*	  return
*	  end
*
*	  the value of iflag should not be changed by fcn unless
*	  the user wants to terminate execution of lmdif.
*	  in this case set iflag to a negative integer.
*
*	m is a positive integer input variable set to the number
*	  of functions.
*
*	n is a positive integer input variable set to the number
*	  of variables. n must not exceed m.
*
*	x is an array of length n. on input x must contain
*	  an initial estimate of the solution vector. on output x
*	  contains the final estimate of the solution vector.
*
*	fvec is an output array of length m which contains
*	  the functions evaluated at the output x.
*
*	ftol is a nonnegative input variable. termination
*	  occurs when both the actual and predicted relative
*	  reductions in the sum of squares are at most ftol.
*	  therefore, ftol measures the relative error desired
*	  in the sum of squares.
*
*	xtol is a nonnegative input variable. termination
*	  occurs when the relative error between two consecutive
*	  iterates is at most xtol. therefore, xtol measures the
*	  relative error desired in the approximate solution.
*
*	gtol is a nonnegative input variable. termination
*	  occurs when the cosine of the angle between fvec and
*	  any column of the jacobian is at most gtol in absolute
*	  value. therefore, gtol measures the orthogonality
*	  desired between the function vector and the columns
*	  of the jacobian.
*
*	maxfev is a positive integer input variable. termination
*	  occurs when the number of calls to fcn is at least
*	  maxfev by the end of an iteration.
*
*	epsfcn is an input variable used in determining a suitable
*	  step length for the forward-difference approximation. this
*	  approximation assumes that the relative errors in the
*	  functions are of the order of epsfcn. if epsfcn is less
*	  than the machine precision, it is assumed that the relative
*	  errors in the functions are of the order of the machine
*	  precision.
*
*	diag is an array of length n. if mode = 1 (see
*	  below), diag is internally set. if mode = 2, diag
*	  must contain positive entries that serve as
*	  multiplicative scale factors for the variables.
*
*	mode is an integer input variable. if mode = 1, the
*	  variables will be scaled internally. if mode = 2,
*	  the scaling is specified by the input diag. other
*	  values of mode are equivalent to mode = 1.
*
*	factor is a positive input variable used in determining the
*	  initial step bound. this bound is set to the product of
*	  factor and the euclidean norm of diag*x if nonzero, or else
*	  to factor itself. in most cases factor should lie in the
*	  interval (.1,100.). 100. is a generally recommended value.
*
*	nprint is an integer input variable that enables controlled
*	  printing of iterates if it is positive. in this case,
*	  fcn is called with iflag = 0 at the beginning of the first
*	  iteration and every nprint iterations thereafter and
*	  immediately prior to return, with x and fvec available
*	  for printing. if nprint is not positive, no special calls
*	  of fcn with iflag = 0 are made.
*
*	info is an integer output variable. if the user has
*	  terminated execution, info is set to the (negative)
*	  value of iflag. see description of fcn. otherwise,
*	  info is set as follows.
*
*	  info = 0  improper input parameters.
*
*	  info = 1  both actual and predicted relative reductions
*		    in the sum of squares are at most ftol.
*
*	  info = 2  relative error between two consecutive iterates
*		    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 fcn has reached or
*		    exceeded maxfev.
*
*	  info = 6  ftol is too small. no further reduction in
*		    the sum of squares is possible.
*
*	  info = 7  xtol is too small. no further improvement in
*		    the approximate solution x is possible.
*
*	  info = 8  gtol is too small. fvec is orthogonal to the
*		    columns of the jacobian to machine precision.
*
*	nfev is an integer output variable set to the number of
*	  calls to fcn.
*
*	fjac is an output m by n array. the upper n by n submatrix
*	  of fjac contains an upper triangular matrix r with
*	  diagonal elements of nonincreasing magnitude such that
*
*		 t     t	   t
*		p *(jac *jac)*p = r *r,
*
*	  where p is a permutation matrix and jac is the final
*	  calculated jacobian. column j of p is column ipvt(j)
*	  (see below) of the identity matrix. the lower trapezoidal
*	  part of fjac contains information generated during
*	  the computation of r.
*
*	ldfjac is a positive integer input variable not less than m
*	  which specifies the leading dimension of the array fjac.
*
*	ipvt is an integer output array of length n. ipvt
*	  defines a permutation matrix p such that jac*p = q*r,
*	  where jac is the final calculated jacobian, q is
*	  orthogonal (not stored), and r is upper triangular
*	  with diagonal elements of nonincreasing magnitude.
*	  column j of p is column ipvt(j) of the identity matrix.
*
*	qtf is an output array of length n which contains
*	  the first n elements of the vector (q transpose)*fvec.
*
*	wa1, wa2, and wa3 are work arrays of length n.
*
*	wa4 is a work array of length m.
*
*     subprograms called
*
*	user-supplied ...... fcn
*
*	minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac
*
*	fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod
*
*     argonne national laboratory. minpack project. march 1980.
*     burton s. garbow, kenneth e. hillstrom, jorge j. more
*
* ********** */


int mpfit(mp_func funct, int m, int npar,
	  double *xall, mp_par *pars, mp_config *config, void *private_data, 
	  mp_result *result)
{
  mp_config conf;
  int i, j, info, iflag, nfree, npegged, iter;
  int qanylim = 0;

  int ij,jj,l;
  double actred,delta,dirder,fnorm,fnorm1,gnorm, orignorm;
  double par,pnorm,prered,ratio;
  double sum,temp,temp1,temp2,temp3,xnorm, alpha;
  static double one = 1.0;
  static double p1 = 0.1;
  static double p5 = 0.5;
  static double p25 = 0.25;
  static double p75 = 0.75;
  static double p0001 = 1.0e-4;
  static double zero = 0.0;
  int nfev = 0;

  double *step = 0, *dstep = 0, *llim = 0, *ulim = 0;
  int *pfixed = 0, *mpside = 0, *ifree = 0, *qllim = 0, *qulim = 0;
  int *ddebug = 0;
  double *ddrtol = 0, *ddatol = 0;

  double *fvec = 0, *qtf = 0;
  double *x = 0, *xnew = 0, *fjac = 0, *diag = 0;
  double *wa1 = 0, *wa2 = 0, *wa3 = 0, *wa4 = 0;
  double **dvecptr = 0;
  int *ipvt = 0;

  int ldfjac;

  /* Default configuration */
  conf.ftol = 1e-10;
  conf.xtol = 1e-10;
  conf.gtol = 1e-10;
  conf.stepfactor = 100.0;
  conf.nprint = 1;
  conf.epsfcn = MP_MACHEP0;
  conf.maxiter = 200;
  conf.douserscale = 0;
  conf.maxfev = 0;
  conf.covtol = 1e-14;
  conf.nofinitecheck = 0;
  
  if (config) {
    /* Transfer any user-specified configurations */
    if (config->ftol > 0) conf.ftol = config->ftol;
    if (config->xtol > 0) conf.xtol = config->xtol;
    if (config->gtol > 0) conf.gtol = config->gtol;
    if (config->stepfactor > 0) conf.stepfactor = config->stepfactor;
    if (config->nprint >= 0) conf.nprint = config->nprint;
    if (config->epsfcn > 0) conf.epsfcn = config->epsfcn;
    if (config->maxiter > 0) conf.maxiter = config->maxiter;
    if (config->maxiter == MP_NO_ITER) conf.maxiter = 0;
    if (config->douserscale != 0) conf.douserscale = config->douserscale;
    if (config->covtol > 0) conf.covtol = config->covtol;
    if (config->nofinitecheck > 0) conf.nofinitecheck = config->nofinitecheck;
    conf.maxfev = config->maxfev;
  }

  info = MP_ERR_INPUT; /* = 0 */
  iflag = 0;
  nfree = 0;
  npegged = 0;

  /* Basic error checking */
  if (funct == 0) {
    return MP_ERR_FUNC;
  }

  if ((m <= 0) || (xall == 0)) {
    return MP_ERR_NPOINTS;
  }
  
  if (npar <= 0) {
    return MP_ERR_NFREE;
  }

  fnorm = -1.0;
  fnorm1 = -1.0;
  xnorm = -1.0;
  delta = 0.0;

  /* FIXED parameters? */
  mp_malloc(pfixed, int, npar);
  if (pars) for (i=0; i<npar; i++) {
    pfixed[i] = (pars[i].fixed)?1:0;
  }

  /* Finite differencing step, absolute and relative, and sidedness of deriv */
  mp_malloc(step,  double, npar);
  mp_malloc(dstep, double, npar);
  mp_malloc(mpside, int, npar);
  mp_malloc(ddebug, int, npar);
  mp_malloc(ddrtol, double, npar);
  mp_malloc(ddatol, double, npar);
  if (pars) for (i=0; i<npar; i++) {
    step[i] = pars[i].step;
    dstep[i] = pars[i].relstep;
    mpside[i] = pars[i].side;
    ddebug[i] = pars[i].deriv_debug;
    ddrtol[i] = pars[i].deriv_reltol;
    ddatol[i] = pars[i].deriv_abstol;
  }
    
  /* Finish up the free parameters */
  nfree = 0;
  mp_malloc(ifree, int, npar);
  for (i=0, j=0; i<npar; i++) {
    if (pfixed[i] == 0) {
      nfree++;
      ifree[j++] = i;
    }
  }
  if (nfree == 0) {
    info = MP_ERR_NFREE;
    goto CLEANUP;
  }
  
  if (pars) {
    for (i=0; i<npar; i++) {
      if ( (pars[i].limited[0] && (xall[i] < pars[i].limits[0])) ||
	   (pars[i].limited[1] && (xall[i] > pars[i].limits[1])) ) {
	info = MP_ERR_INITBOUNDS;
	goto CLEANUP;
      }
      if ( (pars[i].fixed == 0) && pars[i].limited[0] && pars[i].limited[1] &&
	   (pars[i].limits[0] >= pars[i].limits[1])) {
	info = MP_ERR_BOUNDS;
	goto CLEANUP;
      }
    }

    mp_malloc(qulim, int, nfree);
    mp_malloc(qllim, int, nfree);
    mp_malloc(ulim, double, nfree);
    mp_malloc(llim, double, nfree);

    for (i=0; i<nfree; i++) {
      qllim[i] = pars[ifree[i]].limited[0];
      qulim[i] = pars[ifree[i]].limited[1];
      llim[i]  = pars[ifree[i]].limits[0];
      ulim[i]  = pars[ifree[i]].limits[1];
      if (qllim[i] || qulim[i]) qanylim = 1;
    }
  }

  /* Sanity checking on input configuration */
  if ((npar <= 0) || (conf.ftol <= 0) || (conf.xtol <= 0) ||
      (conf.gtol <= 0) || (conf.maxiter < 0) ||
      (conf.stepfactor <= 0)) {
    info = MP_ERR_PARAM;
    goto CLEANUP;
  }

  /* Ensure there are some degrees of freedom */
  if (m < nfree) {
    info = MP_ERR_DOF;
    goto CLEANUP;
  }

  /* Allocate temporary storage */
  mp_malloc(fvec, double, m);
  mp_malloc(qtf, double, nfree);
  mp_malloc(x, double, nfree);
  mp_malloc(xnew, double, npar);
  mp_malloc(fjac, double, m*nfree);
  ldfjac = m;
  mp_malloc(diag, double, npar);
  mp_malloc(wa1, double, npar);
  mp_malloc(wa2, double, npar);
  mp_malloc(wa3, double, npar);
  mp_malloc(wa4, double, m);
  mp_malloc(ipvt, int, npar);
  mp_malloc(dvecptr, double *, npar);

  /* Evaluate user function with initial parameter values */
  iflag = mp_call(funct, m, npar, xall, fvec, 0, private_data);
  nfev += 1;
  if (iflag < 0) {
    goto CLEANUP;
  }

  fnorm = mp_enorm(m, fvec);
  orignorm = fnorm*fnorm;

  /* Make a new copy */
  for (i=0; i<npar; i++) {
    xnew[i] = xall[i];
  }

  /* Transfer free parameters to 'x' */
  for (i=0; i<nfree; i++) {
    x[i] = xall[ifree[i]];
  }

  /* Initialize Levelberg-Marquardt parameter and iteration counter */

  par = 0.0;
  iter = 1;
  for (i=0; i<nfree; i++) {
    qtf[i] = 0;
  }

  /* Beginning of the outer loop */
 OUTER_LOOP:
  for (i=0; i<nfree; i++) {
    xnew[ifree[i]] = x[i];
  }
  
  /* XXX call iterproc */

  /* Calculate the jacobian matrix */
  iflag = mp_fdjac2(funct, m, nfree, ifree, npar, xnew, fvec, fjac, ldfjac,
		    conf.epsfcn, wa4, private_data, &nfev,
		    step, dstep, mpside, qulim, ulim,
		    ddebug, ddrtol, ddatol, wa2, dvecptr);
  if (iflag < 0) {
    goto CLEANUP;
  }

  /* Determine if any of the parameters are pegged at the limits */
  if (qanylim) {
    for (j=0; j<nfree; j++) {
      int lpegged = (qllim[j] && (x[j] == llim[j]));
      int upegged = (qulim[j] && (x[j] == ulim[j]));
      sum = 0;

      /* If the parameter is pegged at a limit, compute the gradient
	 direction */
      if (lpegged || upegged) {
	ij = j*ldfjac;
	for (i=0; i<m; i++, ij++) {
	  sum += fvec[i] * fjac[ij];
	}
      }
      /* If pegged at lower limit and gradient is toward negative then
	 reset gradient to zero */
      if (lpegged && (sum > 0)) {
	ij = j*ldfjac;
	for (i=0; i<m; i++, ij++) fjac[ij] = 0;
      }
      /* If pegged at upper limit and gradient is toward positive then
	 reset gradient to zero */
      if (upegged && (sum < 0)) {
	ij = j*ldfjac;
	for (i=0; i<m; i++, ij++) fjac[ij] = 0;
      }
    }
  } 

  /* Compute the QR factorization of the jacobian */
  mp_qrfac(m,nfree,fjac,ldfjac,1,ipvt,nfree,wa1,wa2,wa3);

  /*
   *	 on the first iteration and if mode is 1, scale according
   *	 to the norms of the columns of the initial jacobian.
   */
  if (iter == 1) {
    if (conf.douserscale == 0) {
      for (j=0; j<nfree; j++) {
	diag[ifree[j]] = wa2[j];
	if (wa2[j] == zero ) {
	  diag[ifree[j]] = one;
	}
      }
    }

    /*
     *	 on the first iteration, calculate the norm of the scaled x
     *	 and initialize the step bound delta.
     */
    for (j=0; j<nfree; j++ ) {
      wa3[j] = diag[ifree[j]] * x[j];
    }
    
    xnorm = mp_enorm(nfree, wa3);
    delta = conf.stepfactor*xnorm;
    if (delta == zero) delta = conf.stepfactor;
  }

  /*
   *	 form (q transpose)*fvec and store the first n components in
   *	 qtf.
   */
  for (i=0; i<m; i++ ) {
    wa4[i] = fvec[i];
  }

  jj = 0;
  for (j=0; j<nfree; j++ ) {
    temp3 = fjac[jj];
    if (temp3 != zero) {
      sum = zero;
      ij = jj;
      for (i=j; i<m; i++ ) {
	sum += fjac[ij] * wa4[i];
	ij += 1;	/* fjac[i+m*j] */
      }
      temp = -sum / temp3;
      ij = jj;
      for (i=j; i<m; i++ ) {
	wa4[i] += fjac[ij] * temp;
	ij += 1;	/* fjac[i+m*j] */
      }
    }
    fjac[jj] = wa1[j];
    jj += m+1;	/* fjac[j+m*j] */
    qtf[j] = wa4[j];
  }

  /* ( From this point on, only the square matrix, consisting of the
     triangle of R, is needed.) */

  
  if (conf.nofinitecheck) {
    /* Check for overflow.  This should be a cheap test here since FJAC
       has been reduced to a (small) square matrix, and the test is
       O(N^2). */
    int off = 0, nonfinite = 0;

    for (j=0; j<nfree; j++) {
      for (i=0; i<nfree; i++) {
	if (mpfinite(fjac[off+i]) == 0) nonfinite = 1;
      }
      off += ldfjac;
    }

    if (nonfinite) {
      info = MP_ERR_NAN;
      goto CLEANUP;
    }
  }


  /*
   *	 compute the norm of the scaled gradient.
   */
  gnorm = zero;
  if (fnorm != zero) {
    jj = 0;
    for (j=0; j<nfree; j++ ) {
      l = ipvt[j];
      if (wa2[l] != zero) {
	sum = zero;
	ij = jj;
	for (i=0; i<=j; i++ ) {
	  sum += fjac[ij]*(qtf[i]/fnorm);
	  ij += 1; /* fjac[i+m*j] */
	}
	gnorm = mp_dmax1(gnorm,fabs(sum/wa2[l]));
      }
      jj += m;
    }
  }

  /*
   *	 test for convergence of the gradient norm.
   */
  if (gnorm <= conf.gtol) info = MP_OK_DIR;
  if (info != 0) goto L300;
  if (conf.maxiter == 0) {
    info = MP_MAXITER;
    goto L300;
  }

  /*
   *	 rescale if necessary.
   */
  if (conf.douserscale == 0) {
    for (j=0; j<nfree; j++ ) {
      diag[ifree[j]] = mp_dmax1(diag[ifree[j]],wa2[j]);
    }
  }

  /*
   *	 beginning of the inner loop.
   */
 L200:
  /*
   *	    determine the levenberg-marquardt parameter.
   */
  mp_lmpar(nfree,fjac,ldfjac,ipvt,ifree,diag,qtf,delta,&par,wa1,wa2,wa3,wa4);
  /*
   *	    store the direction p and x + p. calculate the norm of p.
   */
  for (j=0; j<nfree; j++ ) {
    wa1[j] = -wa1[j];
  }

  alpha = 1.0;
  if (qanylim == 0) {
    /* No parameter limits, so just move to new position WA2 */
    for (j=0; j<nfree; j++ ) {
      wa2[j] = x[j] + wa1[j];
    }

  } else {
    /* Respect the limits.  If a step were to go out of bounds, then 
     * we should take a step in the same direction but shorter distance.
     * The step should take us right to the limit in that case.
     */
    for (j=0; j<nfree; j++) {
      int lpegged = (qllim[j] && (x[j] <= llim[j]));
      int upegged = (qulim[j] && (x[j] >= ulim[j]));
      int dwa1 = fabs(wa1[j]) > MP_MACHEP0;
      
      if (lpegged && (wa1[j] < 0)) wa1[j] = 0;
      if (upegged && (wa1[j] > 0)) wa1[j] = 0;

      if (dwa1 && qllim[j] && ((x[j] + wa1[j]) < llim[j])) {
	alpha = mp_dmin1(alpha, (llim[j]-x[j])/wa1[j]);
      }
      if (dwa1 && qulim[j] && ((x[j] + wa1[j]) > ulim[j])) {
	alpha = mp_dmin1(alpha, (ulim[j]-x[j])/wa1[j]);
      }
    }
    
    /* Scale the resulting vector, advance to the next position */
    for (j=0; j<nfree; j++) {
      double sgnu, sgnl;
      double ulim1, llim1;

      wa1[j] = wa1[j] * alpha;
      wa2[j] = x[j] + wa1[j];

      /* Adjust the output values.  If the step put us exactly
       * on a boundary, make sure it is exact.
       */
      sgnu = (ulim[j] >= 0) ? (+1) : (-1);
      sgnl = (llim[j] >= 0) ? (+1) : (-1);
      ulim1 = ulim[j]*(1-sgnu*MP_MACHEP0) - ((ulim[j] == 0)?(MP_MACHEP0):0);
      llim1 = llim[j]*(1+sgnl*MP_MACHEP0) + ((llim[j] == 0)?(MP_MACHEP0):0);

      if (qulim[j] && (wa2[j] >= ulim1)) {
	wa2[j] = ulim[j];
      }
      if (qllim[j] && (wa2[j] <= llim1)) {
	wa2[j] = llim[j];
      }
    }

  }

  for (j=0; j<nfree; j++ ) {
    wa3[j] = diag[ifree[j]]*wa1[j];
  }

  pnorm = mp_enorm(nfree,wa3);
  
  /*
   *	    on the first iteration, adjust the initial step bound.
   */
  if (iter == 1) {
    delta = mp_dmin1(delta,pnorm);
  }

  /*
   *	    evaluate the function at x + p and calculate its norm.
   */
  for (i=0; i<nfree; i++) {
    xnew[ifree[i]] = wa2[i];
  }

  iflag = mp_call(funct, m, npar, xnew, wa4, 0, private_data);
  nfev += 1;
  if (iflag < 0) goto L300;

  fnorm1 = mp_enorm(m,wa4);

  /*
   *	    compute the scaled actual reduction.
   */
  actred = -one;
  if ((p1*fnorm1) < fnorm) {
    temp = fnorm1/fnorm;
    actred = one - temp * temp;
  }

  /*
   *	    compute the scaled predicted reduction and
   *	    the scaled directional derivative.
   */
  jj = 0;
  for (j=0; j<nfree; j++ ) {
    wa3[j] = zero;
    l = ipvt[j];
    temp = wa1[l];
    ij = jj;
    for (i=0; i<=j; i++ ) {
      wa3[i] += fjac[ij]*temp;
      ij += 1; /* fjac[i+m*j] */
    }
    jj += m;
  }

  /* Remember, alpha is the fraction of the full LM step actually
   * taken
   */

  temp1 = mp_enorm(nfree,wa3)*alpha/fnorm;
  temp2 = (sqrt(alpha*par)*pnorm)/fnorm;
  prered = temp1*temp1 + (temp2*temp2)/p5;
  dirder = -(temp1*temp1 + temp2*temp2);

  /*
   *	    compute the ratio of the actual to the predicted
   *	    reduction.
   */
  ratio = zero;
  if (prered != zero) {
    ratio = actred/prered;
  }

  /*
   *	    update the step bound.
   */
  
  if (ratio <= p25) {
    if (actred >= zero) {
      temp = p5; 
    } else {
      temp = p5*dirder/(dirder + p5*actred);
    }
    if (((p1*fnorm1) >= fnorm)
	|| (temp < p1) ) {
      temp = p1;
    }
    delta = temp*mp_dmin1(delta,pnorm/p1);
    par = par/temp;
  } else {
    if ((par == zero) || (ratio >= p75) ) {
      delta = pnorm/p5;
      par = p5*par;
    }
  }

  /*
   *	    test for successful iteration.
   */
  if (ratio >= p0001) {
    
    /*
     *	    successful iteration. update x, fvec, and their norms.
     */
    for (j=0; j<nfree; j++ ) {
      x[j] = wa2[j];
      wa2[j] = diag[ifree[j]]*x[j];
    }
    for (i=0; i<m; i++ ) {
      fvec[i] = wa4[i];
    }
    xnorm = mp_enorm(nfree,wa2);
    fnorm = fnorm1;
    iter += 1;
  }
  
  /*
   *	    tests for convergence.
   */
  if ((fabs(actred) <= conf.ftol) && (prered <= conf.ftol) && 
      (p5*ratio <= one) ) {
    info = MP_OK_CHI;
  }
  if (delta <= conf.xtol*xnorm) {
    info = MP_OK_PAR;
  }
  if ((fabs(actred) <= conf.ftol) && (prered <= conf.ftol) && (p5*ratio <= one)
      && ( info == 2) ) {
    info = MP_OK_BOTH;
  }
  if (info != 0) {
    goto L300;
  }
  
  /*
   *	    tests for termination and stringent tolerances.
   */
  if ((conf.maxfev > 0) && (nfev >= conf.maxfev)) {
    /* Too many function evaluations */
    info = MP_MAXITER;
  }
  if (iter >= conf.maxiter) {
    /* Too many iterations */
    info = MP_MAXITER;
  }
  if ((fabs(actred) <= MP_MACHEP0) && (prered <= MP_MACHEP0) && (p5*ratio <= one) ) {
    info = MP_FTOL;
  }
  if (delta <= MP_MACHEP0*xnorm) {
    info = MP_XTOL;
  }
  if (gnorm <= MP_MACHEP0) {
    info = MP_GTOL;
  }
  if (info != 0) {
    goto L300;
  }
  
  /*
   *	    end of the inner loop. repeat if iteration unsuccessful.
   */
  if (ratio < p0001) goto L200;
  /*
   *	 end of the outer loop.
   */
  goto OUTER_LOOP;

 L300:
  /*
   *     termination, either normal or user imposed.
   */
  if (iflag < 0) {
    info = iflag;
  }
  iflag = 0;

  for (i=0; i<nfree; i++) {
    xall[ifree[i]] = x[i];
  }
  
  if ((conf.nprint > 0) && (info > 0)) {
    iflag = mp_call(funct, m, npar, xall, fvec, 0, private_data);
    nfev += 1;
  }

  /* Compute number of pegged parameters */
  npegged = 0;
  if (pars) for (i=0; i<npar; i++) {
    if ((pars[i].limited[0] && (pars[i].limits[0] == xall[i])) ||
	(pars[i].limited[1] && (pars[i].limits[1] == xall[i]))) {
      npegged ++;
    }
  }

  /* Compute and return the covariance matrix and/or parameter errors */
  if (result && (result->covar || result->xerror)) {
    mp_covar(nfree, fjac, ldfjac, ipvt, conf.covtol, wa2);
    
    if (result->covar) {
      /* Zero the destination covariance array */
      for (j=0; j<(npar*npar); j++) result->covar[j] = 0;
      
      /* Transfer the covariance array */
      for (j=0; j<nfree; j++) {
	for (i=0; i<nfree; i++) {
	  result->covar[ifree[j]*npar+ifree[i]] = fjac[j*ldfjac+i];
	}
      }
    }

    if (result->xerror) {
      for (j=0; j<npar; j++) result->xerror[j] = 0;

      for (j=0; j<nfree; j++) {
	double cc = fjac[j*ldfjac+j];
	if (cc > 0) result->xerror[ifree[j]] = sqrt(cc);
      }
    }
  }      

  if (result) {
    strcpy(result->version, MPFIT_VERSION);
    result->bestnorm = mp_dmax1(fnorm,fnorm1);
    result->bestnorm *= result->bestnorm;
    result->orignorm = orignorm;
    result->status   = info;
    result->niter    = iter;
    result->nfev     = nfev;
    result->npar     = npar;
    result->nfree    = nfree;
    result->npegged  = npegged;
    result->nfunc    = m;
    
    /* Copy residuals if requested */
    if (result->resid) {
      for (j=0; j<m; j++) result->resid[j] = fvec[j];
    }
  }


 CLEANUP:
  if (fvec) free(fvec);
  if (qtf)  free(qtf);
  if (x)    free(x);
  if (xnew) free(xnew);
  if (fjac) free(fjac);
  if (diag) free(diag);
  if (wa1)  free(wa1);
  if (wa2)  free(wa2);
  if (wa3)  free(wa3);
  if (wa4)  free(wa4);
  if (ipvt) free(ipvt);
  if (pfixed) free(pfixed);
  if (step) free(step);
  if (dstep) free(dstep);
  if (mpside) free(mpside);
  if (ddebug) free(ddebug);
  if (ddrtol) free(ddrtol);
  if (ddatol) free(ddatol);
  if (ifree) free(ifree);
  if (qllim) free(qllim);
  if (qulim) free(qulim);
  if (llim)  free(llim);
  if (ulim)  free(ulim);
  if (dvecptr) free(dvecptr);

  return info;
}


/************************fdjac2.c*************************/

static 
int mp_fdjac2(mp_func funct,
	      int m, int n, int *ifree, int npar, double *x, double *fvec,
	      double *fjac, int ldfjac, double epsfcn,
	      double *wa, void *priv, int *nfev,
	      double *step, double *dstep, int *dside,
	      int *qulimited, double *ulimit,
	      int *ddebug, double *ddrtol, double *ddatol,
	      double *wa2, double **dvec)
{
/*
*     **********
*
*     subroutine fdjac2
*
*     this subroutine computes a forward-difference approximation
*     to the m by n jacobian matrix associated with a specified
*     problem of m functions in n variables.
*
*     the subroutine statement is
*
*	subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)
*
*     where
*
*	fcn is the name of the user-supplied subroutine which
*	  calculates the functions. fcn must be declared
*	  in an external statement in the user calling
*	  program, and should be written as follows.
*
*	  subroutine fcn(m,n,x,fvec,iflag)
*	  integer m,n,iflag
*	  double precision x(n),fvec(m)
*	  ----------
*	  calculate the functions at x and
*	  return this vector in fvec.
*	  ----------
*	  return
*	  end
*
*	  the value of iflag should not be changed by fcn unless
*	  the user wants to terminate execution of fdjac2.
*	  in this case set iflag to a negative integer.
*
*	m is a positive integer input variable set to the number
*	  of functions.
*
*	n is a positive integer input variable set to the number
*	  of variables. n must not exceed m.
*
*	x is an input array of length n.
*
*	fvec is an input array of length m which must contain the
*	  functions evaluated at x.
*
*	fjac is an output m by n array which contains the
*	  approximation to the jacobian matrix evaluated at x.
*
*	ldfjac is a positive integer input variable not less than m
*	  which specifies the leading dimension of the array fjac.
*
*	iflag is an integer variable which can be used to terminate
*	  the execution of fdjac2. see description of fcn.
*
*	epsfcn is an input variable used in determining a suitable
*	  step length for the forward-difference approximation. this
*	  approximation assumes that the relative errors in the
*	  functions are of the order of epsfcn. if epsfcn is less
*	  than the machine precision, it is assumed that the relative
*	  errors in the functions are of the order of the machine
*	  precision.
*
*	wa is a work array of length m.
*
*     subprograms called
*
*	user-supplied ...... fcn
*
*	minpack-supplied ... dpmpar
*
*	fortran-supplied ... dabs,dmax1,dsqrt
*
*     argonne national laboratory. minpack project. march 1980.
*     burton s. garbow, kenneth e. hillstrom, jorge j. more
*
      **********
*/
  int i,j,ij;
  int iflag = 0;
  double eps,h,temp;
  static double zero = 0.0;
  int has_analytical_deriv = 0, has_numerical_deriv = 0;
  int has_debug_deriv = 0;
  
  temp = mp_dmax1(epsfcn,MP_MACHEP0);
  eps = sqrt(temp);
  ij = 0;
  ldfjac = 0;   /* Prevent compiler warning */
  if (ldfjac){} /* Prevent compiler warning */

  for (j=0; j<npar; j++) dvec[j] = 0;

  /* Initialize the Jacobian derivative matrix */
  for (j=0; j<(n*m); j++) fjac[j] = 0;

  /* Check for which parameters need analytical derivatives and which
     need numerical ones */
  for (j=0; j<n; j++) {  /* Loop through free parameters only */
    if (dside && dside[ifree[j]] == 3 && ddebug[ifree[j]] == 0) {
      /* Purely analytical derivatives */
      dvec[ifree[j]] = fjac + j*m;
      has_analytical_deriv = 1;
    } else if (dside && ddebug[ifree[j]] == 1) {
      /* Numerical and analytical derivatives as a debug cross-check */
      dvec[ifree[j]] = fjac + j*m;
      has_analytical_deriv = 1;
      has_numerical_deriv = 1;
      has_debug_deriv = 1;
    } else {
      has_numerical_deriv = 1;
    }
  }

  /* If there are any parameters requiring analytical derivatives,
     then compute them first. */
  if (has_analytical_deriv) {
    iflag = mp_call(funct, m, npar, x, wa, dvec, priv);
    if (nfev) *nfev = *nfev + 1;
    if (iflag < 0 ) goto DONE;
  }

  if (has_debug_deriv) {
    printf("FJAC DEBUG BEGIN\n");
    printf("#  %10s %10s %10s %10s %10s %10s\n", 
	   "IPNT", "FUNC", "DERIV_U", "DERIV_N", "DIFF_ABS", "DIFF_REL");
  }

  /* Any parameters requiring numerical derivatives */
  if (has_numerical_deriv) for (j=0; j<n; j++) {  /* Loop thru free parms */
    int dsidei = (dside)?(dside[ifree[j]]):(0);
    int debug  = ddebug[ifree[j]];
    double dr = ddrtol[ifree[j]], da = ddatol[ifree[j]];
    
    /* Check for debugging */
    if (debug) {
      printf("FJAC PARM %d\n", ifree[j]);
    }

    /* Skip parameters already done by user-computed partials */
    if (dside && dsidei == 3) continue;

    temp = x[ifree[j]];
    h = eps * fabs(temp);
    if (step  &&  step[ifree[j]] > 0) h = step[ifree[j]];
    if (dstep && dstep[ifree[j]] > 0) h = fabs(dstep[ifree[j]]*temp);
    if (h == zero)                    h = eps;

    /* If negative step requested, or we are against the upper limit */
    if ((dside && dsidei == -1) || 
	(dside && dsidei == 0 && 
	 qulimited && ulimit && qulimited[j] && 
	 (temp > (ulimit[j]-h)))) {
      h = -h;
    }

    x[ifree[j]] = temp + h;
    iflag = mp_call(funct, m, npar, x, wa, 0, priv);
    if (nfev) *nfev = *nfev + 1;
    if (iflag < 0 ) goto DONE;
    x[ifree[j]] = temp;

    if (dsidei <= 1) {
      /* COMPUTE THE ONE-SIDED DERIVATIVE */
      if (! debug) {
	/* Non-debug path for speed */
	for (i=0; i<m; i++, ij++) {
	  fjac[ij] = (wa[i] - fvec[i])/h; /* fjac[i+m*j] */
	}
      } else {
	/* Debug path for correctness */
	for (i=0; i<m; i++, ij++) {
	  double fjold = fjac[ij];
	  fjac[ij] = (wa[i] - fvec[i])/h; /* fjac[i+m*j] */
	  if ((da == 0 && dr == 0 && (fjold != 0 || fjac[ij] != 0)) ||
	      ((da != 0 || dr != 0) && (fabs(fjold-fjac[ij]) > da + fabs(fjold)*dr))) {
	    printf("   %10d %10.4g %10.4g %10.4g %10.4g %10.4g\n", 
		   i, fvec[i], fjold, fjac[ij], fjold-fjac[ij], 
		   (fjold == 0)?(0):((fjold-fjac[ij])/fjold));
	  }
	}
      } /* end debugging */

    } else {  /* dside > 2 */
      /* COMPUTE THE TWO-SIDED DERIVATIVE */
      for (i=0; i<m; i++) {
	wa2[i] = wa[i];
      }

      /* Evaluate at x - h */
      x[ifree[j]] = temp - h;
      iflag = mp_call(funct, m, npar, x, wa, 0, priv);
      if (nfev) *nfev = *nfev + 1;
      if (iflag < 0 ) goto DONE;
      x[ifree[j]] = temp;

      /* Now compute derivative as (f(x+h) - f(x-h))/(2h) */
      if (! debug ) {
	/* Non-debug path for speed */
	for (i=0; i<m; i++, ij++) {
	  fjac[ij] = (fjac[ij] - wa[i])/(2*h); /* fjac[i+m*j] */
	}
      } else {
	/* Debug path for correctness */
	for (i=0; i<m; i++, ij++) {
	  double fjold = fjac[ij];
	  fjac[ij] = (wa2[i] - wa[i])/(2*h); /* fjac[i+m*j] */
	  if ((da == 0 && dr == 0 && (fjold != 0 || fjac[ij] != 0)) ||
	      ((da != 0 || dr != 0) && (fabs(fjold-fjac[ij]) > da + fabs(fjold)*dr))) {
	    printf("   %10d %10.4g %10.4g %10.4g %10.4g %10.4g\n", 
		   i, fvec[i], fjold, fjac[ij], fjold-fjac[ij], 
		   (fjold == 0)?(0):((fjold-fjac[ij])/fjold));
	  }
	}
      } /* end debugging */
      
    } /* if (dside > 2) */
  } /* if (has_numerical_derivative) */

  if (has_debug_deriv) {
    printf("FJAC DEBUG END\n");
  }

 DONE:
  if (iflag < 0) return iflag;
  return 0; 
  /*
   *     last card of subroutine fdjac2.
   */
}


/************************qrfac.c*************************/
 
static 
void mp_qrfac(int m, int n, double *a, int lda, 
	      int pivot, int *ipvt, int lipvt,
	      double *rdiag, double *acnorm, double *wa)
{
/*
*     **********
*
*     subroutine qrfac
*
*     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
*
*			    t
*	    i - (1/u(k))*u*u
*
*     where u has zeros in the first k-1 positions. the form of
*     this transformation and the method of pivoting first
*     appeared in the corresponding linpack subroutine.
*
*     the subroutine statement is
*
*	subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa)
*
*     where
*
*	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).
*
*	lda is a positive integer input variable not less than m
*	  which specifies the leading dimension of the array a.
*
*	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. ipvt
*	  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.
*
*	lipvt is a positive integer input variable. if pivot is false,
*	  then lipvt may be as small as 1. if pivot is true, then
*	  lipvt must be at least n.
*
*	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.
*
*     subprograms called
*
*	minpack-supplied ... dpmpar,enorm
*
*	fortran-supplied ... dmax1,dsqrt,min0
*
*     argonne national laboratory. minpack project. march 1980.
*     burton s. garbow, kenneth e. hillstrom, jorge j. more
*
*     **********
*/
  int i,ij,jj,j,jp1,k,kmax,minmn;
  double ajnorm,sum,temp;
  static double zero = 0.0;
  static double one = 1.0;
  static double p05 = 0.05;

  lda = 0;      /* Prevent compiler warning */
  lipvt = 0;    /* Prevent compiler warning */
  if (lda) {}   /* Prevent compiler warning */
  if (lipvt) {} /* Prevent compiler warning */

  /*
   *     compute the initial column norms and initialize several arrays.
   */
  ij = 0;
  for (j=0; j<n; j++) {
    acnorm[j] = mp_enorm(m,&a[ij]);
    rdiag[j] = acnorm[j];
    wa[j] = rdiag[j];
    if (pivot != 0)
      ipvt[j] = j;
    ij += m; /* m*j */
  }
  /*
   *     reduce a to r with householder transformations.
   */
  minmn = mp_min0(m,n);
  for (j=0; j<minmn; j++) {
    if (pivot == 0)
      goto L40;
    /*
     *	 bring the column of largest norm into the pivot position.
     */
    kmax = j;
    for (k=j; k<n; k++)
      {
	if (rdiag[k] > rdiag[kmax])
	  kmax = k;
      }
    if (kmax == j)
      goto L40;
      
    ij = m * j;
    jj = m * kmax;
    for (i=0; i<m; i++)
      {
	temp = a[ij]; /* [i+m*j] */
	a[ij] = a[jj]; /* [i+m*kmax] */
	a[jj] = temp;
	ij += 1;
	jj += 1;
      }
    rdiag[kmax] = rdiag[j];
    wa[kmax] = wa[j];
    k = ipvt[j];
    ipvt[j] = ipvt[kmax];
    ipvt[kmax] = k;
      
  L40:
    /*
     *	 compute the householder transformation to reduce the
     *	 j-th column of a to a multiple of the j-th unit vector.
     */
    jj = j + m*j;
    ajnorm = mp_enorm(m-j,&a[jj]);
    if (ajnorm == zero)
      goto L100;
    if (a[jj] < zero)
      ajnorm = -ajnorm;
    ij = jj;
    for (i=j; i<m; i++)
      {
	a[ij] /= ajnorm;
	ij += 1; /* [i+m*j] */
      }
    a[jj] += one;
    /*
     *	 apply the transformation to the remaining columns
     *	 and update the norms.
     */
    jp1 = j + 1;
    if (jp1 < n)
      {
	for (k=jp1; k<n; k++)
	  {
	    sum = zero;
	    ij = j + m*k;
	    jj = j + m*j;
	    for (i=j; i<m; i++)
	      {
		sum += a[jj]*a[ij];
		ij += 1; /* [i+m*k] */
		jj += 1; /* [i+m*j] */
	      }
	    temp = sum/a[j+m*j];
	    ij = j + m*k;
	    jj = j + m*j;
	    for (i=j; i<m; i++)
	      {
		a[ij] -= temp*a[jj];
		ij += 1; /* [i+m*k] */
		jj += 1; /* [i+m*j] */
	      }
	    if ((pivot != 0) && (rdiag[k] != zero))
	      {
		temp = a[j+m*k]/rdiag[k];
		temp = mp_dmax1( zero, one-temp*temp );
		rdiag[k] *= sqrt(temp);
		temp = rdiag[k]/wa[k];
		if ((p05*temp*temp) <= MP_MACHEP0)
		  {
		    rdiag[k] = mp_enorm(m-j-1,&a[jp1+m*k]);
		    wa[k] = rdiag[k];
		  }
	      }
	  }
      }
      
  L100:
    rdiag[j] = -ajnorm;
  }
  /*
   *     last card of subroutine qrfac.
   */
}

/************************qrsolv.c*************************/

static 
void mp_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag,
	       double *qtb, double *x, double *sdiag, double *wa)
{
/*
*     **********
*
*     subroutine qrsolv
*
*     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 ,	  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
*
*		   t	   t
*	    r*z = q *b ,  p *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
*
*	     t	 t		 t
*	    p *(a *a + d*d)*p = s *s .
*
*     s is computed within qrsolv and may be of separate interest.
*
*     the subroutine statement is
*
*	subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa)
*
*     where
*
*	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.
*
*     subprograms called
*
*	fortran-supplied ... dabs,dsqrt
*
*     argonne national laboratory. minpack project. march 1980.
*     burton s. garbow, kenneth e. hillstrom, jorge j. more
*
*     **********
*/
  int i,ij,ik,kk,j,jp1,k,kp1,l,nsing;
  double cosx,cotan,qtbpj,sinx,sum,tanx,temp;
  static double zero = 0.0;
  static double p25 = 0.25;
  static double p5 = 0.5;
  
  /*
   *     copy r and (q transpose)*b to preserve input and initialize s.
   *     in particular, save the diagonal elements of r in x.
   */
  kk = 0;
  for (j=0; j<n; j++) {
    ij = kk;
    ik = kk;
    for (i=j; i<n; i++)
      {
	r[ij] = r[ik];
	ij += 1;   /* [i+ldr*j] */
	ik += ldr; /* [j+ldr*i] */
      }
    x[j] = r[kk];
    wa[j] = qtb[j];
    kk += ldr+1; /* j+ldr*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.
     */
    l = ipvt[j];
    if (diag[l] == zero)
      goto L90;
    for (k=j; k<n; k++)
      sdiag[k] = zero;
    sdiag[j] = diag[l];
    /*
     *	 the transformations to eliminate the row of d
     *	 modify only a single element of (q transpose)*b
     *	 beyond the first n, which is initially zero.
     */
    qtbpj = zero;
    for (k=j; k<n; k++)
      {
	/*
	 *	    determine a givens rotation which eliminates the
	 *	    appropriate element in the current row of d.
	 */
	if (sdiag[k] == zero)
	  continue;
	kk = k + ldr * k;
	if (fabs(r[kk]) < fabs(sdiag[k]))
	  {
	    cotan = r[kk]/sdiag[k];
	    sinx = p5/sqrt(p25+p25*cotan*cotan);
	    cosx = sinx*cotan;
	  }
	else
	  {
	    tanx = sdiag[k]/r[kk];
	    cosx = p5/sqrt(p25+p25*tanx*tanx);
	    sinx = cosx*tanx;
	  }
	/*
	 *	    compute the modified diagonal element of r and
	 *	    the modified element of ((q transpose)*b,0).
	 */
	r[kk] = cosx*r[kk] + sinx*sdiag[k];
	temp = cosx*wa[k] + sinx*qtbpj;
	qtbpj = -sinx*wa[k] + cosx*qtbpj;
	wa[k] = temp;
	/*
	 *	    accumulate the tranformation in the row of s.
	 */
	kp1 = k + 1;
	if (n > kp1)
	  {
	    ik = kk + 1;
	    for (i=kp1; i<n; i++)
	      {
		temp = cosx*r[ik] + sinx*sdiag[i];
		sdiag[i] = -sinx*r[ik] + cosx*sdiag[i];
		r[ik] = temp;
		ik += 1; /* [i+ldr*k] */
	      }
	  }
      }
  L90:
    /*
     *	 store the diagonal element of s and restore
     *	 the corresponding diagonal element of r.
     */
    kk = j + ldr*j;
    sdiag[j] = r[kk];
    r[kk] = 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] == zero) && (nsing == n))
      nsing = j;
    if (nsing < n)
      wa[j] = zero;
  }
  if (nsing < 1)
    goto L150;
  
  for (k=0; k<nsing; k++) {
    j = nsing - k - 1;
    sum = zero;
    jp1 = j + 1;
    if (nsing > jp1)
      {
	ij = jp1 + ldr * j;
	for (i=jp1; i<nsing; i++)
	  {
	    sum += r[ij]*wa[i];
	    ij += 1; /* [i+ldr*j] */
	  }
      }
    wa[j] = (wa[j] - sum)/sdiag[j];
  }
 L150:
  /*
   *     permute the components of z back to components of x.
   */
  for (j=0; j<n; j++) {
    l = ipvt[j];
    x[l] = wa[j];
  }
  /*
   *     last card of subroutine qrsolv.
   */
}

/************************lmpar.c*************************/

static 
void mp_lmpar(int n, double *r, int ldr, int *ipvt, int *ifree, double *diag,
	      double *qtb, double delta, double *par, double *x,
	      double *sdiag, double *wa1, double *wa2) 
{
  /*     **********
   *
   *     subroutine lmpar
   *
   *     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 ,	  sqrt(par)*d*x = 0 ,
   *
   *     in the least squares sense, and dxnorm is the euclidean
   *     norm of d*x, then either par is zero and
   *
   *	    (dxnorm-delta) .le. 0.1*delta ,
   *
   *     or par is positive and
   *
   *	    abs(dxnorm-delta) .le. 0.1*delta .
   *
   *     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 transpose)*b. on output
   *     lmpar also provides an upper triangular matrix s such that
   *
   *	     t	 t		     t
   *	    p *(a *a + par*d*d)*p = s *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.
   *
   *     the subroutine statement is
   *
   *	subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,
   *			 wa1,wa2)
   *
   *     where
   *
   *	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 output array of length n which contains the
   *	  diagonal elements of the upper triangular matrix s.
   *
   *	wa1 and wa2 are work arrays of length n.
   *
   *     subprograms called
   *
   *	minpack-supplied ... dpmpar,mp_enorm,qrsolv
   *
   *	fortran-supplied ... dabs,mp_dmax1,dmin1,dsqrt
   *
   *     argonne national laboratory. minpack project. march 1980.
   *     burton s. garbow, kenneth e. hillstrom, jorge j. more
   *
   *     **********
   */
  int i,iter,ij,jj,j,jm1,jp1,k,l,nsing;
  double dxnorm,fp,gnorm,parc,parl,paru;
  double sum,temp;
  static double zero = 0.0;
  /* static double one = 1.0; */
  static double p1 = 0.1;
  static double p001 = 0.001;
  
  /*
   *     compute and store in x the gauss-newton direction. if the
   *     jacobian is rank-deficient, obtain a least squares solution.
   */
  nsing = n;
  jj = 0;
  for (j=0; j<n; j++) {
    wa1[j] = qtb[j];
    if ((r[jj] == zero) && (nsing == n))
      nsing = j;
    if (nsing < n)
      wa1[j] = zero;
    jj += ldr+1; /* [j+ldr*j] */
  }

  if (nsing >= 1) {
    for (k=0; k<nsing; k++)
      {
	j = nsing - k - 1;
	wa1[j] = wa1[j]/r[j+ldr*j];
	temp = wa1[j];
	jm1 = j - 1;
	if (jm1 >= 0)
	  {
	    ij = ldr * j;
	    for (i=0; i<=jm1; i++)
	      {
		wa1[i] -= r[ij]*temp;
		ij += 1;
	      }
	  }
      }
  }
  
  for (j=0; j<n; j++) {
    l = ipvt[j];
    x[l] = wa1[j];
  }
  /*
   *     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++)
    wa2[j] = diag[ifree[j]]*x[j];
  dxnorm = mp_enorm(n,wa2);
  fp = dxnorm - delta;
  if (fp <= p1*delta) {
    goto L220;
  }
  /*
   *     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 = zero;
  if (nsing >= n) {
    for (j=0; j<n; j++)
      {
	l = ipvt[j];
	wa1[j] = diag[ifree[l]]*(wa2[l]/dxnorm);
      }
    jj = 0;
    for (j=0; j<n; j++)
      {
	sum = zero;
	jm1 = j - 1;
	if (jm1 >= 0)
	  {
	    ij = jj;
	    for (i=0; i<=jm1; i++)
	      {
		sum += r[ij]*wa1[i];
		ij += 1;
	      }
	  }
	wa1[j] = (wa1[j] - sum)/r[j+ldr*j];
	jj += ldr; /* [i+ldr*j] */
      }
    temp = mp_enorm(n,wa1);
    parl = ((fp/delta)/temp)/temp;
  }
  /*
   *     calculate an upper bound, paru, for the zero of the function.
   */
  jj = 0;
  for (j=0; j<n; j++) {
    sum = zero;
    ij = jj;
    for (i=0; i<=j; i++)
      {
	sum += r[ij]*qtb[i];
	ij += 1;
      }
    l = ipvt[j];
    wa1[j] = sum/diag[ifree[l]];
    jj += ldr; /* [i+ldr*j] */
  }
  gnorm = mp_enorm(n,wa1);
  paru = gnorm/delta;
  if (paru == zero)
    paru = MP_DWARF/mp_dmin1(delta,p1);
  /*
   *     if the input par lies outside of the interval (parl,paru),
   *     set par to the closer endpoint.
   */
  *par = mp_dmax1( *par,parl);
  *par = mp_dmin1( *par,paru);
  if (*par == zero)
    *par = gnorm/dxnorm;

  /*
   *     beginning of an iteration.
   */
 L150:
  iter += 1;
  /*
   *	 evaluate the function at the current value of par.
   */
  if (*par == zero)
    *par = mp_dmax1(MP_DWARF,p001*paru);
  temp = sqrt( *par );
  for (j=0; j<n; j++)
    wa1[j] = temp*diag[ifree[j]];
  mp_qrsolv(n,r,ldr,ipvt,wa1,qtb,x,sdiag,wa2);
  for (j=0; j<n; j++)
    wa2[j] = diag[ifree[j]]*x[j];
  dxnorm = mp_enorm(n,wa2);
  temp = 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 == zero) && (fp <= temp) && (temp < zero))
      || (iter == 10))
    goto L220;
  /*
   *	 compute the newton correction.
   */
  for (j=0; j<n; j++) {
    l = ipvt[j];
    wa1[j] = diag[ifree[l]]*(wa2[l]/dxnorm);
  }
  jj = 0;
  for (j=0; j<n; j++) {
    wa1[j] = wa1[j]/sdiag[j];
    temp = wa1[j];
    jp1 = j + 1;
    if (jp1 < n)
      {
	ij = jp1 + jj;
	for (i=jp1; i<n; i++)
	  {
	    wa1[i] -= r[ij]*temp;
	    ij += 1; /* [i+ldr*j] */
	  }
      }
    jj += ldr; /* ldr*j */
  }
  temp = mp_enorm(n,wa1);
  parc = ((fp/delta)/temp)/temp;
  /*
   *	 depending on the sign of the function, update parl or paru.
   */
  if (fp > zero)
    parl = mp_dmax1(parl, *par);
  if (fp < zero)
    paru = mp_dmin1(paru, *par);
  /*
   *	 compute an improved estimate for par.
   */
  *par = mp_dmax1(parl, *par + parc);
  /*
   *	 end of an iteration.
   */
  goto L150;
  
 L220:
  /*
   *     termination.
   */
  if (iter == 0)
    *par = zero;
  /*
   *     last card of subroutine lmpar.
   */
}


/************************enorm.c*************************/
 
static 
double mp_enorm(int n, double *x) 
{
  /*
   *     **********
   *
   *     function enorm
   *
   *     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, rdwarf and rgiant. the main
   *     restrictions on these constants are that rdwarf**2 not
   *     underflow and rgiant**2 not overflow. the constants
   *     given here are suitable for every known computer.
   *
   *     the function statement is
   *
   *	double precision function enorm(n,x)
   *
   *     where
   *
   *	n is a positive integer input variable.
   *
   *	x is an input array of length n.
   *
   *     subprograms called
   *
   *	fortran-supplied ... dabs,dsqrt
   *
   *     argonne national laboratory. minpack project. march 1980.
   *     burton s. garbow, kenneth e. hillstrom, jorge j. more
   *
   *     **********
   */
  int i;
  double agiant,floatn,s1,s2,s3,xabs,x1max,x3max;
  double ans, temp;
  double rdwarf = MP_RDWARF;
  double rgiant = MP_RGIANT;
  static double zero = 0.0;
  static double one = 1.0;
  
  s1 = zero;
  s2 = zero;
  s3 = zero;
  x1max = zero;
  x3max = zero;
  floatn = n;
  agiant = rgiant/floatn;
  
  for (i=0; i<n; i++) {
    xabs = fabs(x[i]);
    if ((xabs > rdwarf) && (xabs < agiant))
      {
	/*
	 *	    sum for intermediate components.
	 */
	s2 += xabs*xabs;
	continue;
      }
      
    if (xabs > rdwarf)
      {
	/*
	 *	       sum for large components.
	 */
	if (xabs > x1max)
	  {
	    temp = x1max/xabs;
	    s1 = one + s1*temp*temp;
	    x1max = xabs;
	  }
	else
	  {
	    temp = xabs/x1max;
	    s1 += temp*temp;
	  }
	continue;
      }
    /*
     *	       sum for small components.
     */
    if (xabs > x3max)
      {
	temp = x3max/xabs;
	s3 = one + s3*temp*temp;
	x3max = xabs;
      }
    else	
      {
	if (xabs != zero)
	  {
	    temp = xabs/x3max;
	    s3 += temp*temp;
	  }
      }
  }
  /*
   *     calculation of norm.
   */
  if (s1 != zero) {
    temp = s1 + (s2/x1max)/x1max;
    ans = x1max*sqrt(temp);
    return(ans);
  }
  if (s2 != zero) {
    if (s2 >= x3max)
      temp = s2*(one+(x3max/s2)*(x3max*s3));
    else
      temp = x3max*((s2/x3max)+(x3max*s3));
    ans = sqrt(temp);
  }
  else
    {
      ans = x3max*sqrt(s3);
    }
  return(ans);
  /*
   *     last card of function enorm.
   */
}

/************************lmmisc.c*************************/

static 
double mp_dmax1(double a, double b) 
{
  if (a >= b)
    return(a);
  else
    return(b);
}

static 
double mp_dmin1(double a, double b)
{
  if (a <= b)
    return(a);
  else
    return(b);
}

static 
int mp_min0(int a, int b)
{
  if (a <= b)
    return(a);
  else
    return(b);
}

/************************covar.c*************************/
/*
c     **********
c
c     subroutine covar
c
c     given an m by n matrix a, the problem is to determine
c     the covariance matrix corresponding to a, defined as
c
c                    t
c           inverse(a *a) .
c
c     this subroutine completes the solution of the problem
c     if it is provided with the necessary information from the
c     qr factorization, with column pivoting, of a. that is, if
c     a*p = q*r, where p is a permutation matrix, q has orthogonal
c     columns, and r is an upper triangular matrix with diagonal
c     elements of nonincreasing magnitude, then covar expects
c     the full upper triangle of r and the permutation matrix p.
c     the covariance matrix is then computed as
c
c                      t     t
c           p*inverse(r *r)*p  .
c
c     if a is nearly rank deficient, it may be desirable to compute
c     the covariance matrix corresponding to the linearly independent
c     columns of a. to define the numerical rank of a, covar uses
c     the tolerance tol. if l is the largest integer such that
c
c           abs(r(l,l)) .gt. tol*abs(r(1,1)) ,
c
c     then covar computes the covariance matrix corresponding to
c     the first l columns of r. for k greater than l, column
c     and row ipvt(k) of the covariance matrix are set to zero.
c
c     the subroutine statement is
c
c       subroutine covar(n,r,ldr,ipvt,tol,wa)
c
c     where
c
c       n is a positive integer input variable set to the order of r.
c
c       r is an n by n array. on input the full upper triangle must
c         contain the full upper triangle of the matrix r. on output
c         r contains the square symmetric covariance matrix.
c
c       ldr is a positive integer input variable not less than n
c         which specifies the leading dimension of the array r.
c
c       ipvt is an integer input array of length n which defines the
c         permutation matrix p such that a*p = q*r. column j of p
c         is column ipvt(j) of the identity matrix.
c
c       tol is a nonnegative input variable used to define the
c         numerical rank of a in the manner described above.
c
c       wa is a work array of length n.
c
c     subprograms called
c
c       fortran-supplied ... dabs
c
c     argonne national laboratory. minpack project. august 1980.
c     burton s. garbow, kenneth e. hillstrom, jorge j. more
c
c     **********
*/

static 
int mp_covar(int n, double *r, int ldr, int *ipvt, double tol, double *wa)
{
  int i, ii, j, jj, k, l;
  int kk, kj, ji, j0, k0, jj0;
  int sing;
  double one = 1.0, temp, tolr, zero = 0.0;

  /*
   * form the inverse of r in the full upper triangle of r.
   */

#if 0
  for (j=0; j<n; j++) {
    for (i=0; i<n; i++) {
      printf("%f ", r[j*ldr+i]);
    }
    printf("\n");
  }
#endif

  tolr = tol*fabs(r[0]);
  l = -1;
  for (k=0; k<n; k++) {
    kk = k*ldr + k;
    if (fabs(r[kk]) <= tolr) break;

    r[kk] = one/r[kk];
    for (j=0; j<k; j++) {
      kj = k*ldr + j;
      temp = r[kk] * r[kj];
      r[kj] = zero;

      k0 = k*ldr; j0 = j*ldr;
      for (i=0; i<=j; i++) {
	r[k0+i] += (-temp*r[j0+i]);
      }
    }
    l = k;
  }

  /* 
   * Form the full upper triangle of the inverse of (r transpose)*r
   * in the full upper triangle of r
   */

  if (l >= 0) {
    for (k=0; k <= l; k++) {
      k0 = k*ldr; 

      for (j=0; j<k; j++) {
	temp = r[k*ldr+j];

	j0 = j*ldr;
	for (i=0; i<=j; i++) {
	  r[j0+i] += temp*r[k0+i];
	}
      }
      
      temp = r[k0+k];
      for (i=0; i<=k; i++) {
	r[k0+i] *= temp;
      }
    }
  }

  /*
   * For the full lower triangle of the covariance matrix
   * in the strict lower triangle or and in wa
   */
  for (j=0; j<n; j++) {
    jj = ipvt[j];
    sing = (j > l);
    j0 = j*ldr;
    jj0 = jj*ldr;
    for (i=0; i<=j; i++) {
      ji = j0+i;

      if (sing) r[ji] = zero;
      ii = ipvt[i];
      if (ii > jj) r[jj0+ii] = r[ji];
      if (ii < jj) r[ii*ldr+jj] = r[ji];
    }
    wa[jj] = r[j0+j];
  }

  /*
   * Symmetrize the covariance matrix in r
   */
  for (j=0; j<n; j++) {
    j0 = j*ldr;
    for (i=0; i<j; i++) {
      r[j0+i] = r[i*ldr+j];
    }
    r[j0+j] = wa[j];
  }

#if 0
  for (j=0; j<n; j++) {
    for (i=0; i<n; i++) {
      printf("%f ", r[j*ldr+i]);
    }
    printf("\n");
  }
#endif

  return 0;
}