}#if BUG>1	/* DEBUG: 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);	if (iter == 1) { /* first iteration */	    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;	}/*** 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;	if (fnorm != 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] / fnorm;		gnorm = MAX(gnorm, fabs(sum / wa2[ipvt[j]]));	    }	}	if (gnorm <= gtol) {	    *info = 4;	    return;	}/*** outer: rescale if necessary. ***/	if (mode != 2) {	    for (j = 0; j < n; j++)		diag[j] = MAX(diag[j], wa2[j]);	}/*** the inner loop. ***/	do {#if BUG	    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, wa3, wa4);/*** inner: store the direction p and x + p; calculate the norm of p. ***/	    for (j = 0; j < n; j++) {		wa1[j] = -wa1[j];		wa2[j] = x[j] + wa1[j];		wa3[j] = diag[j] * wa1[j];	    }	    pnorm = lm_enorm(n, wa3);/*** inner: on the first iteration, adjust the initial step bound. ***/	    if (*nfev <= 1 + n)		delta = MIN(delta, pnorm);            /* evaluate the function at x + p and calculate its norm. */	    *info = 0;	    (*evaluate) (wa2, m, wa4, data, info);	    (*printout) (n, x, m, wa4, data, 2, iter, ++(*nfev));	    if (*info < 0)		return; /* user requested break. */	    fnorm1 = lm_enorm(m, wa4);#if BUG	    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;#if BUG	    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 <= p25) {		if (actred >= 0.)		    temp = p5;		else		    temp = p5 * dirder / (dirder + p5 * actred);		if (p1 * fnorm1 >= fnorm || temp < p1)		    temp = p1;		delta = temp * MIN(delta, pnorm / p1);		par /= temp;	    } else if (par == 0. || ratio >= p75) {		delta = pnorm / p5;		par *= p5;	    }/*** 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++;	    }#if BUG	    else {		printf("ATTN: iteration considered unsuccessful\n");	    }#endif/*** inner: tests for convergence ( otherwise *info = 1, 2, or 3 ). ***/	    *info = 0; /* do not terminate (unless overwritten by nonzero) */	    if (fabs(actred) <= ftol && prered <= ftol && p5 * 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;	    if (fabs(actred) <= LM_MACHEP &&		prered <= LM_MACHEP && p5 * ratio <= 1)		*info = 6;	    if (delta <= LM_MACHEP * xnorm)		*info = 7;	    if (gnorm <= LM_MACHEP)		*info = 8;	    if (*info != 0)		return;/*** inner: end of the loop. repeat if iteration unsuccessful. ***/	} while (ratio < p0001);/*** outer: end of the loop. ***/    } while (1);} /*** lm_lmdif. ***/void lm_lmpar(int n, double *r, int ldr, int *ipvt, double *diag,	      double *qtb, double delta, double *par, double *x,	      double *sdiag, double *wa1, double *wa2){/*     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. * *     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. * *     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 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. * */    int i, iter, j, nsing;    double dxnorm, fp, fp_old, gnorm, parc, parl, paru;    double sum, temp;    static double p1 = 0.1;    static double p001 = 0.001;#if BUG    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++) {	wa1[j] = qtb[j];	if (r[j * ldr + j] == 0 && nsing == n)	    nsing = j;	if (nsing < n)	    wa1[j] = 0;    }#if BUG    printf("nsing %d ", nsing);#endif    for (j = nsing - 1; j >= 0; j--) {	wa1[j] = wa1[j] / r[j + ldr * j];	temp = wa1[j];	for (i = 0; i < j; i++)	    wa1[i] -= r[j * ldr + i] * temp;    }    for (j = 0; j < n; j++)	x[ipvt[j]] = wa1[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++)	wa2[j] = diag[j] * x[j];    dxnorm = lm_enorm(n, wa2);    fp = dxnorm - delta;    if (fp <= p1 * delta) {#if BUG	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++)	    wa1[j] = diag[ipvt[j]] * wa2[ipvt[j]] / dxnorm;	for (j = 0; j < n; j++) {	    sum = 0.;	    for (i = 0; i < j; i++)		sum += r[j * ldr + i] * wa1[i];	    wa1[j] = (wa1[j] - sum) / r[j + ldr * j];	}	temp = lm_enorm(n, wa1);	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];	wa1[j] = sum / diag[ipvt[j]];    }    gnorm = lm_enorm(n, wa1);    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;#if BUG    printf("lmpar/ parl %.4e  par %.4e  paru %.4e\n", parl, *par, paru);#endif/*** lmpar: iterate. ***/    for (;; iter++) {        /** evaluate the function at the current value of par. **/	if (*par == 0.)	    *par = MAX(LM_DWARF, p001 * paru);	temp = sqrt(*par);	for (j = 0; j < n; j++)	    wa1[j] = temp * diag[j];	lm_qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);	for (j = 0; j < n; j++)	    wa2[j] = diag[j] * x[j];	dxnorm = lm_enorm(n, wa2);	fp_old = fp;	fp = dxnorm - delta;                /** if the function is small enough, accept the current value            of par. Also test for the exceptional cases where parl            is zero or the number of iterations has reached 10. **/	if (fabs(fp) <= p1 * delta	    || (parl == 0. && fp <= fp_old && fp_old < 0.)	    || iter == 10)	    break; /* the only exit from the iteration. */                /** compute the Newton correction. **/	for (j = 0; j < n; j++)	    wa1[j] = diag[ipvt[j]] * wa2[ipvt[j]] / dxnorm;	for (j = 0; j < n; j++) {	    wa1[j] = wa1[j] / sdiag[j];	    for (i = j + 1; i < n; i++)