/*********************************************************************
* FUNCTIONS FOR THE LEAST-SQUARES SOLUTION OF M NONLINEAR EQUATIONS  *
* IN N VARIABLES USING THE Levenberg-Marquardt ALGORITHM.            *
* ------------------------------------------------------------------ *
*  REFERENCE                                                         *
*  From F77 program By:                                              *
*  ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. MARCH 1980.         *
*  BURTON S. GARBOW, KENNETH E. HILLSTROM, JORGE J. MORE             *
*                                                                    *
*                        Visual C++ Release By J-P Moreau, Paris.    *
*                                    (www.jpmoreau.fr)               *
*********************************************************************/
#include <basis.h>      //For REAL, etc.
#include <vmblock.h>    //For memory allocations.

REAL   zp25 = 0.25, zp5 = 0.5, five = 5.0, eight = 8.0,
       ten = 10.0, c13 = 13.0, c14 = 14.0, c29 = 29.0, c45 = 45.0,
       p5 = 0.5, p25 = 0.25, p05 = 0.05, epsmch = 2.25e-16;

int  nprob, nfev, njev;

     void ssqfcn (int, int, REAL *, REAL *, int);
     REAL enorm(int, REAL *);
     void lmdif1(int, int, REAL *, REAL *, REAL, int *, int *iwa);


     void fdjac2(int, int, REAL *, REAL *, REAL **, int *, REAL);
     void qrfac(int, int, REAL **, bool, int *, REAL *, REAL *);
     void lmpar(int, REAL **, int *, REAL *, REAL *, REAL, REAL *, REAL *, REAL *);
     void lmder(int, int, REAL *, REAL *, REAL **, REAL, REAL, REAL, int, int, REAL, 
		        int, int *, int *, int *, int *);
     void qrsolv(int, REAL **, int *, REAL *, REAL *, REAL *, REAL *);


     //Utility functions
     REAL Dot_Product(int n, REAL *a, REAL *b) {
       int i; REAL sum;
       sum = ZERO;
       for (i=1; i<=n; i++) sum += a[i]*b[i];
       return sum;
     }

     REAL MAX(REAL a, REAL b) {
       if (a>=b) return a; else return b;
     }

     REAL MIN(REAL a, REAL b) {
       if (a<=b) return a; else return b;
     }

     REAL Sqr(REAL x) {
       return x*x;
     }


void ssqfcn (int m, int n, REAL *x, REAL *fvec, int nprob) {
/********************************************************************
!     Function SSQFCN

!     THIS Function DEFINES THE FUNCTIONS OF THREE NONLINEAR
!     LEAST SQUARES PROBLEMS. 

!     THE Function STATEMENT IS

!       Function SSQFCN(M, N, X, FVEC, NPROB)

!     WHERE

!       M AND N ARE POSITIVE INTEGER INPUT VARIABLES. N MUST NOT
!         EXCEED M.

!       X IS AN INPUT ARRAY OF LENGTH N (OF TYPE pVec).

!       FVEC IS AN OUTPUT ARRAY OF LENGTH M WHICH CONTAINS THE NPROB
!         FUNCTION EVALUATED AT X (OF TYPE pVec).

!       NPROB IS A POSITIVE INTEGER INPUT VARIABLE WHICH DEFINES THE
!         NUMBER OF THE PROBLEM. NPROB MUST NOT EXCEED 18.

!     ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. MARCH 1980.
!     BURTON S. GARBOW, KENNETH E. HILLSTROM, JORGE J. MORE

!********************************************************************/

// FUNCTION ROUTINE SELECTOR
  switch(nprob) {

    case 1:     // Example #1 (n=2)
//    X1^2 + X1 + X2^2 - 2           = 0
//    X1^2 + X2 - X2^2 - 1 + log(X1) = 0 }
      fvec[1] = x[1]*x[1] + x[1] + x[2]*x[2] - TWO;
      fvec[2] = x[1]*x[1] + x[2] - x[2]*x[2] - ONE + log(x[1]);
      break;
    case 2:     // Example #2 (n=4)
//    10.0*x + x2 + x3  + x4 - 20.0 + Sqr(sin(x1)) + Sqr(cos(x2)) = 0
//    x1 + 20.0*x2 + x3 + x4 - 48.0 + one/pow^6                   = 0
//    Sqr(x1 + x2) + 30.0*x3 + x4 - 97.0 + log(x1) + log(x2+x3)   = 0
//    x1     + x2  + x3 + 40.0*x4 - 166.0 + Sqr(x1)               = 0  }
      fvec[1] = 10.0*x[1] + x[2] + x[3] + x[4] - 20.0 + Sqr(sin(x[1])) + Sqr(cos(x[2]));
      fvec[2] = x[1] + 20.0*x[2] + x[3] + x[4] - 48.0 + ONE/(x[1]*x[1]*x[1]*x[1]*x[1]*x[1]);
      fvec[3] = Sqr(x[1] + x[2]) + 30.0*x[3] + x[4] - 97.0 + log(x[1]) + log(x[2]+x[3]);
      fvec[4] = x[1] + x[2] + x[3] + 40.0*x[4] - 166.0 + Sqr(x[1]);
      break;
	  case 3:   // Example #3 (n=6)  Stiff system
/*    Hiebert's 2nd Chemical Engineering Problem

      source: Hiebert; Sandia Technical Report #SAND80-0181
              Sandia National Laboratories, Albuquerque, NM (1980)

      X1 + X2 + X4 - .001 = 0
      X5 + X6 -55         = 0
      X1 + X2 + X3 + 2X5 + X6 - 110.001 = 0
      X1 - 0.1X2        = 0
      X1 - 10000 X3 X4  = 0
      X5 - 5.5e15 X3 X6 = 0
      solution: (8.264e-5, 8.264e-4, 9.091e-5, 9.091e-5, 55, 1.1e-10)  */
      fvec[1] = x[1] + x[2] + x[4] - 0.001;
      fvec[2] = x[5] + x[6] - 55.0;
      fvec[3] = x[1] + x[2] + x[3] + TWO * x[5] + x[6] - 110.001;
      fvec[4] = x[1] - 0.1 * x[2];
      fvec[5] = x[1] - 1.0E04 * x[3] * x[4];
      fvec[6] = x[5] - 5.5E15 * x[3] * x[6];
  }

} // ssqfcn()

void fcn (int m, int n, REAL *x, REAL *fvec, int *iflag) {
/****************************************************************

!  THE CALLING SEQUENCE OF FCN SHOULD BE IDENTICAL TO THE
!  CALLING SEQUENCE OF THE FUNCTION SUBROUTINE IN THE NONLINEAR
!  LEAST-SQUARES SOLVER. FCN SHOULD ONLY CALL THE TESTING
!  FUNCTION SUBROUTINE SSQFCN WITH THE APPROPRIATE VALUE OF
!  PROBLEM NUMBER (NPROB).

!  SUBPROGRAMS CALLED

!    USER SUPPLIED: SSQFCN

!  ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. MARCH 1980.
!  BURTON S. GARBOW, KENNETH E. HILLSTROM, JORGE J. MORE

!***************************************************************/
  ssqfcn (m, n, x, fvec, nprob);
  if (*iflag == 1)  nfev++;
  if (*iflag == 2)  njev++;
} // fcn()

void lmdif(int, int, REAL *, REAL *, REAL, REAL, REAL, int, REAL, 
           int, REAL, int, int *, int *, REAL **, int *);

void lmdif1(int m, int n, REAL *x, REAL *fvec, REAL tol, int *info, int *iwa) {
 
/* From F90 Code converted using TO_F90 by Alan Miller
!  **************************************************************************

!  Function lmdif1

!  The purpose of lmdif1 is to minimize the sum of the squares of m nonlinear
!  functions in n variables by a modification of the Levenberg-Marquardt
!  algorithm.  This is done by using the more general least-squares
!  solver lmdif.  The user must provide a subroutine which calculates the
!  functions.  The jacobian is then calculated by a forward-difference
!  approximation.

!  the subroutine statement is

!    Function lmdif1(m, n, x, fvec, tol, info, iwa)

!  where

!    fcn is the name of the user-supplied Function which calculates
!      the functions (removed from list of arguments).
!      fcn should be written as follows:

!      Function fcn (m, n:integer; x, fvec:pVec; var iflag:integer);
!      begin
!      ----------
!      calculate the functions at x and return this vector in fvec.
!      ----------
!      end;

!      the value of iflag should not be changed by fcn unless
!      the user wants to terminate execution of lmdif1.
!      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.

!    tol is a nonnegative input variable.  Termination occurs when the
!      algorithm estimates either that the relative error in the sum of
!      squares is at most tol or that the relative error between x and the
!      solution is at most tol.

!    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  algorithm estimates that the relative error
!                in the sum of squares is at most tol.

!      info = 2  algorithm estimates that the relative error
!                between x and the solution is at most tol.

!      info = 3  conditions for info = 1 and info = 2 both hold.

!      info = 4  fvec is orthogonal to the columns of the
!                jacobian to machine precision.

!      info = 5  number of calls to fcn has reached or exceeded 200*(n+1).

!      info = 6  tol is too small. no further reduction in
!                the sum of squares is possible.

!      info = 7  tol is too small.  No further improvement in
!                the approximate solution x is possible.

!    iwa is an integer work array of length n.

!    wa is a work array of length lwa.

!    lwa is a positive integer input variable not less than m*n+5*n+m.

!  subprograms called

!    user-supplied ...... fcn

!    minpack-supplied ... lmdif

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!  **************************************************************************/
  int maxfev, mode, nprint;
  REAL epsfcn, ftol, gtol, xtol;
  REAL **fjac;

  REAL factor = 100.0;

  void *vmblock = NULL;

  *info = 0;
  
 // allocate memory for matrix fjac
  vmblock = vminit();
  fjac = (REAL **) vmalloc(vmblock, MATRIX,  n+1, n+1);
 
  if (! vmcomplete(vmblock)) {
    LogError ("No Memory", 0, __FILE__, __LINE__);
    return;
  }    

// check the input parameters for errors
  if (n <= 0 || m < n || tol < ZERO) goto e10;

  maxfev = 200*(n + 1);
  ftol = tol;
  xtol = tol;
  gtol = ZERO;
  epsfcn = ZERO;
  mode = 1;
  nprint = 0;

  lmdif(m, n, x, fvec, ftol, xtol, gtol, maxfev, epsfcn,
           mode, factor, nprint, info, &nfev, fjac, iwa);

  if (*info == 8) *info = 4;

e10: vmfree(vmblock);

} // lmdif1()


void lmdif(int m, int n, REAL *x, REAL *fvec, REAL ftol, REAL xtol, REAL gtol, 
		   int maxfev, REAL epsfcn, int mode, REAL factor, int nprint, int *info, 
		   int *nfev, REAL **fjac, int *ipvt)  {
/*****************************************************************************

!  Function lmdif

!  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 which calculates the
!  functions.  The jacobian is then calculated by a forward-difference
!  approximation.

!  the Function statement is

!    Function lmdif(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 Function which calculates
!      the functions (removed from list of arguments).
!      fcn should be written as follows:

!      Function fcn (m, n:integer; x, fvec:pVec; var iflag:integer);
!      begin
!      ----------
!      calculate the functions at x and return this vector in fvec.
!      ----------
!      end;

!      the value of iflag should not be changed by fcn unless
!      the user wants to terminate execution of lmdif1.
!      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 <= 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

!    dpmpar, enorm, fdjac2, lmpar, max, min, qrfac

!    c++-supplied ... abs, sqrt, modulo (%)

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!  **************************************************************************/
//Labels: e20,e30,e40,e60,e80,e120,e170,e200,e260,e290,e300

  int i, iflag, iter, j, l;
  REAL actred, delta, dirder, fnorm, fnorm1, gnorm,
  par, pnorm, prered, ratio, sum, temp, temp1, temp2, xnorm;
  REAL *diag, *qtf, *tmp, *tmp1, *wa1, *wa2, *wa3, *wa4;

  REAL p1 = 0.1, p75 = 0.75, p0001 = 0.0001;

  void *vmblock = NULL;

// allocate memory for vectors
  vmblock = vminit();
  diag = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  qtf  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  tmp  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  tmp1 = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0); 
  wa1  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  wa2  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  wa3  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  wa4  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0); 

  if (! vmcomplete(vmblock)) {
    LogError ("No Memory", 0, __FILE__, __LINE__);
    return;
  }    

  *info = 0;
  iflag = 0;
  *nfev = 0;

// check the input parameters for errors

  if (n <= 0 || m < n || ftol < ZERO || xtol < ZERO || gtol < ZERO
        || maxfev <= 0 || factor <= ZERO)  goto e300;

  if (mode != 2) goto e20;

  for (j = 1; j<=n; j++)
    if (diag[j] <= ZERO) goto e300;

// evaluate the function at the starting point and calculate its norm

e20: iflag = 1;
  fcn(m, n, x, fvec, &iflag);
  *nfev = 1;
  if (iflag < 0) goto e300;

  fnorm = enorm(m, fvec);

// initialize levenberg-marquardt parameter and iteration counter.

  par = ZERO;
  iter = 1;

// beginning of the outer loop.

// calculate the jacobian matrix.

e30:iflag = 2;
  fdjac2(m, n, x, fvec, fjac, &iflag, epsfcn);
  *nfev = *nfev + n;
  if (iflag < 0) goto e300;

// If requested, call fcn to enable printing of iterates

  if (nprint <= 0) goto e40;
  iflag = 0;
  if ((iter-1 % nprint) == 0)  fcn(m, n, x, fvec, &iflag);
  if (iflag < 0) goto e300;

// Compute the qr factorization of the jacobian

e40: qrfac(m, n, fjac, true, ipvt, wa1, wa2);

// On the first iteration and if mode is 1, scale according
// to the norms of the columns of the initial jacobian.

  if (iter != 1) goto e80;
  if (mode == 2) goto e60;
  for (j = 1; j<=n; j++) {
    diag[j] = wa2[j];
    if (wa2[j] == ZERO)  diag[j] = ONE;
  }

// On the first iteration, calculate the norm of the scaled x
// and initialize the step bound delta.

e60: for (j=1; j<=n; j++)  wa3[j] = diag[j]*x[j];
  xnorm = enorm(n, wa3);
  delta = factor*xnorm;
  if (delta == ZERO)  delta = factor;

// Form (q transpose)*fvec and store the first n components in qtf.

e80: for (j=1; j<=m; j++)  wa4[j] = fvec[j];
  for  (j=1; j<=n; j++) {
    if (fjac[j][j] == ZERO) goto e120;
    for (i=j; i<=m; i++) {
      tmp[i-j+1]=fjac[i][j];
      tmp1[i-j+1]=wa4[i];
    }   
    sum = Dot_Product(m-j+1, tmp, tmp1);
    temp = -sum/fjac[j][j];
    for (i=j; i<=m; i++)
      wa4[i] = wa4[i] + fjac[i][j]*temp;
e120:fjac[j][j] = wa1[j];
    qtf[j] = wa4[j];
  }

// compute the norm of the scaled gradient.

  gnorm = ZERO;
  if (fnorm == ZERO) goto e170;
  for (j=1; j<=n; j++) {
    l = ipvt[j];
    if (wa2[l] == ZERO) break;;
    sum = ZERO;
    for (i=1; i<=j; i++)
      sum += fjac[i][j]*(qtf[i]/fnorm);
    gnorm = MAX(gnorm, ABS(sum/wa2[l]));
  }

// test for convergence of the gradient norm.

e170:if (gnorm <= gtol)  *info = 4;
  if (*info != 0) goto e300;

// rescale if necessary

  if (mode == 2) goto e200;
  for (j=1; j<=n; j++) 
    diag[j] = MAX(diag[j], wa2[j]);

// beginning of the inner loop.

// determine the Levenberg-Marquardt parameter.

e200: lmpar(n, fjac, ipvt, diag, qtf, delta, &par, wa1, wa2);

// store the direction p and x + p. calculate the norm of p

  for (j=1; j<=n; j++) {
    wa1[j] = -wa1[j];
    wa2[j] = x[j] + wa1[j];
    wa3[j] = diag[j]*wa1[j];
  }
  pnorm = enorm(n, wa3);

// on the first iteration, adjust the initial step bound.

  if (iter == 1)  delta = MIN(delta, pnorm);

// evaluate the function at x + p and calculate its norm.

  iflag = 1;
  fcn(m, n, wa2, wa4, &iflag);
  *nfev = *nfev + 1;
  if (iflag < 0) goto e300;
  fnorm1 = enorm(m, wa4);

// compute the scaled actual reduction.

  actred = -ONE;
  if (p1*fnorm1 < fnorm)  actred = ONE - Sqr(fnorm1/fnorm);

// Compute the scaled predicted reduction and
// the scaled directional derivative.

  for (j=1; j<=n; j++) {
    wa3[j] = ZERO;
    l = ipvt[j];
    temp = wa1[l];
    for (i=1; i<=j; i++)
      wa3[i] = wa3[i] + fjac[i][j]*temp;
  }
  temp1 = enorm(n,wa3)/fnorm;
  temp2 = (SQRT(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;
    if (actred < ZERO)  temp = p5*dirder/(dirder + p5*actred);
    if (p1*fnorm1 >= fnorm || temp < p1)  temp = p1;
    delta = temp*MIN(delta,pnorm/p1);
    par = par/temp;
  }
  else {
    if (par != ZERO && ratio < p75) goto e260;
    delta = pnorm/p5;
    par = p5*par;
  }

// test for successful iteration.

e260: if (ratio < p0001) goto e290;

// successful iteration. update x, fvec, and their norms.

  for (j=1; j<=n; j++) {
    x[j] = wa2[j];
    wa2[j] = diag[j]*x[j];
  }
  for (j=1; j<=m; j++)  fvec[j] = wa4[j];
  xnorm = enorm(n, wa2);
  fnorm = fnorm1;
  iter++;

// tests for convergence.

e290: if (ABS(actred) <= ftol &&  prered <= ftol && p5*ratio <= ONE)  *info = 1;
  if (delta <= xtol*xnorm)  *info = 2;
  if (ABS(actred) <= ftol && prered <= ftol && p5*ratio <= ONE && *info == 2) *info = 3;
  if (*info != 0) goto e300;

// tests for termination and stringent tolerances.

  if (*nfev >= maxfev) *info = 5;
  if (ABS(actred) <= epsmch && prered <= epsmch && p5*ratio <= ONE)  *info = 6;
  if (delta <= epsmch*xnorm)  *info = 7;
  if (gnorm <= epsmch) *info = 8;
  if (*info != 0) goto e300;

// end of the inner loop. repeat if iteration unsuccessful.

  if (ratio < p0001) goto e200;

// end of the outer loop.

  goto e30;

// termination, either normal or user imposed.

e300: if (iflag < 0)  *info = iflag;
  iflag = 0;
  if (nprint > 0)  fcn(m, n, x, fvec, &iflag);

  vmfree(vmblock);

} // lmdif()


void lmder1(int m, int n, REAL *x, REAL *fvec, REAL **fjac, REAL tol,
            int *info, int *ipvt)  {
/*******************************************************************

!  Function lmder1

!  The purpose of lmder1 is to minimize the sum of the squares of
!  m nonlinear functions in n variables by a modification of the
!  levenberg-marquardt algorithm.  This is done by using the more
!  general least-squares solver lmder.  The user must provide a
!  subroutine which calculates the functions and the jacobian.

!  the Function statement is

!    Function lmder1(m, n, x, fvec, fjac, tol, info, ipvt)

!  where

!    fcn is the name of the user-supplied Function which calculates
!      the functions (removed from list of arguments).
!      fcn should be written as follows:

!      Function fcn (m, n:integer; x, fvec:pVec; var iflag:integer);
!      begin
!      ----------
!      calculate the functions at x and return this vector in fvec.
!      ----------
!      end;

!      the value of iflag should not be changed by fcn unless
!      the user wants to terminate execution of lmdif1.
!      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.

!    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.

!    tol is a nonnegative input variable. termination occurs
!      when the algorithm estimates either that the relative
!      error in the sum of squares is at most tol or that
!      the relative error between x and the solution is at most tol.

!    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  algorithm estimates that the relative error
!                in the sum of squares is at most tol.

!      info = 2  algorithm estimates that the relative error
!                between x and the solution is at most tol.

!      info = 3  conditions for info = 1 and info = 2 both hold.

!      info = 4  fvec is orthogonal to the columns of the
!                jacobian to machine precision.

!      info = 5  number of calls to fcn with iflag = 1 has reached 100*(n+1).

!      info = 6  tol is too small.  No further reduction in
!                the sum of squares is possible.

!      info = 7  tol is too small.  No further improvement in
!                the approximate solution x is possible.

!    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.

!    wa is a work array of length lwa.

!    lwa is a positive integer input variable not less than 5*n+m.

!  subprograms called

!    user-supplied ...... fcn

!    minpack-supplied ... lmder

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!***************************************************************************/
  int maxfev, mode, njev, nprint;
  REAL ftol, gtol, xtol, factor = 100.0;

  *info = 0;

// check the input parameters for errors.

  if (n <= 0 || m < n || tol < ZERO) return;

// call lmder

  maxfev = 100*(n + 1);
  ftol = tol;
  xtol = tol;
  gtol = ZERO;
  mode = 1;
  nprint = 0;
  lmder(m, n, x, fvec, fjac, ftol, xtol, gtol, maxfev,
           mode, factor, nprint, info, &nfev, &njev, ipvt);
  if (*info == 8)  *info = 4;

} // lmder1()

void lmder(int m, int n, REAL *x, REAL *fvec, REAL **fjac, REAL ftol, REAL xtol, 
		   REAL gtol, int maxfev, int mode, REAL factor, int nprint, int *info,
		   int *nfev, int *njev, int *ipvt)  {
/*******************************************************************************

!  Function lmder

!  the purpose of lmder 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
!  Function which calculates the functions and the jacobian.

!  the Function statement is

!    Function lmder(m,n,x,fvec,fjac,ldfjac,ftol,xtol,gtol,
!                    maxfev,diag,mode,factor,nprint,info,nfev,
!                    njev,ipvt,qtf,wa1,wa2,wa3,wa4)

!  where:

!    fcn is the name of the user-supplied Function which calculates
!      the functions (removed from list of arguments).
!      fcn should be written as follows:

!      Function fcn (m, n:integer; x, fvec:pVec; var iflag:integer);
!      begin
!      ----------
!      calculate the functions at x and return this vector in fvec.
!      ----------
!      end;

!      the value of iflag should not be changed by fcn unless
!      the user wants to terminate execution of lmdif1.
!      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.

!    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.

!    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 with iflag = 1 has reached maxfev.

!    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, fvec,
!      and fjac available for printing.  fvec and fjac should not be
!      altered.  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 with iflag = 1 has reached 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 with iflag = 1.

!    njev is an integer output variable set to the number of
!      calls to fcn with iflag = 2.

!    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, ssqfcn

!    dpmpar,enorm,lmpar,qrfac,max,min

!    pascal-supplied ... ABS, SQRT, mod

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!*****************************************************************************/
//Labels: e20,e30,e40,e60,e80,e120,e170,e200,e260,e290,e300;
  int i, iflag, iter, j, l;
  REAL actred, delta, dirder, fnorm, fnorm1, gnorm, par, pnorm, prered,
  ratio, sum, temp, temp1, temp2, xnorm;
  REAL *diag, *qtf, *tmp, *tmp1, *wa1, *wa2, *wa3, *wa4;
  REAL p1 = 0.1, p75 = 0.75, p0001 = 0.0001;

  void *vmblock = NULL;

// allocate memory for vectors
  vmblock = vminit();
  diag = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  qtf  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  tmp  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  tmp1 = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0); 
  wa1  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  wa2  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  wa3  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  wa4  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0); 

  if (! vmcomplete(vmblock)) {
    LogError ("No Memory", 0, __FILE__, __LINE__);
    return;
  }    

  *info = 0;
  iflag = 0;
  *nfev = 0;
  *njev = 0;

// check the input parameters for errors.

  if (n <= 0 || m < n || ftol < ZERO || xtol < ZERO || gtol < ZERO
    || maxfev <= 0 || factor <= ZERO) goto e300;
  if (mode != 2) goto e20;
  for (j=1; j<=n; j++)
    if (diag[j] <= ZERO) goto e300;

// evaluate the function at the starting point and calculate its norm.

e20: iflag = 1;
  fcn(m, n, x, fvec, &iflag);
  *nfev = 1;
  if (iflag < 0) goto e300;
  fnorm = enorm(m, fvec);

// initialize levenberg-marquardt parameter and iteration counter.

  par = ZERO;
  iter = 1;

// beginning of the outer loop.

// calculate the jacobian matrix.

e30: iflag = 2;
  fcn(m, n, x, fvec, &iflag);
  *njev = *njev + 1;
  if (iflag < 0) goto e300;

// if requested, call fcn to enable printing of iterates.

  if (nprint <= 0) goto e40;
  iflag = 0;
  if ((iter-1 % nprint) == 0)  fcn(m, n, x, fvec, &iflag);
  if (iflag < 0) goto e300;

// compute the qr factorization of the jacobian

e40: qrfac(m, n, fjac, TRUE, ipvt, wa1, wa2);

// on the first iteration and if mode is 1, scale according
// to the norms of the columns of the initial jacobian.

  if (iter != 1) goto e80;
  if (mode == 2) goto e60;
  for (j=1; j<=n; j++) {
    diag[j] = wa2[j];
    if (wa2[j] == ZERO)  diag[j] = ONE;
  }

// on the first iteration, calculate the norm of the scaled x
// and initialize the step bound delta.

e60:for (j=1; j<=n; j++)  wa3[j] = diag[j]*x[j];
  xnorm = enorm(n,wa3);
  delta = factor*xnorm;
  if (delta == ZERO)  delta = factor;

// form (q transpose)*fvec and store the first n components in qtf

e80:for (j=1; j<=m; j++) wa4[j] = fvec[j];
  for (j=1; j<=n; j++) {
    if (fjac[j][j] == ZERO) goto e120;
    for (i=j; i<=m; i++) {
      tmp[i-j+1]=fjac[i][j];
      tmp1[i-j+1]=wa4[i];
    }
    sum = Dot_Product(m-j+1,tmp,tmp1);
    temp = -sum/fjac[j][j];
    for (i=j; i<=m; i++)  wa4[i] = wa4[i] + fjac[i][j]*temp;
e120: fjac[j][j] = wa1[j];
    qtf[j] = wa4[j];
  }

// compute the norm of the scaled gradient

  gnorm = ZERO;
  if (fnorm == ZERO) goto e170;
  for (j=1; j<=n; j++) {
    l = ipvt[j];
    if (wa2[l] == ZERO) break;
    sum = ZERO;
    for (i=1; i<=j; i++)  sum += fjac[i][j]*(qtf[i]/fnorm);
    gnorm = MAX(gnorm,ABS(sum/wa2[l]));
  }

// test for convergence of the gradient norm

e170:if (gnorm <= gtol)  *info = 4;
  if (*info != 0) goto e300;

// rescale if necessary

  if (mode == 2) goto e200;
  for (j=1; j<=n; j++)  diag[j] = MAX(diag[j], wa2[j]);

// beginning of the inner loop.

// determine the levenberg-marquardt parameter

e200: lmpar(n, fjac, ipvt, diag, qtf, delta, &par, wa1, wa2);

// store the direction p and x + p. calculate the norm of p

  for (j=1; j<=n; j++) {
    wa1[j] = -wa1[j];
    wa2[j] = x[j] + wa1[j];
    wa3[j] = diag[j]*wa1[j];
  }
  pnorm = enorm(n, wa3);

// on the first iteration, adjust the initial step bound

  if (iter == 1)   delta = MIN(delta,pnorm);

// evaluate the function at x + p and calculate its norm

  iflag = 1;
  fcn(m, n, wa2, wa4, &iflag);
  *nfev = *nfev + 1;
  if (iflag < 0) goto e300;
  fnorm1 = enorm(m, wa4);

// compute the scaled actual reduction

  actred = -ONE;
  if (p1*fnorm1 < fnorm)  actred = ONE - Sqr(fnorm1/fnorm);

// compute the scaled predicted reduction and
// the scaled directional derivative.

  for (j=1; j<=n; j++) {
    wa3[j] = ZERO;
    l = ipvt[j];
    temp = wa1[l];
    for (i=1; i<=j; i++)  wa3[i] = wa3[i] + fjac[i][j]*temp;
  }
  temp1 = enorm(n,wa3)/fnorm;
  temp2 = (SQRT(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;
    if (actred < ZERO)   temp = p5*dirder/(dirder + p5*actred);
    if (p1*fnorm1 >= fnorm || temp < p1)  temp = p1;
    delta = temp*MIN(delta, pnorm/p1);
    par = par/temp;
  }
  else {
    if (par != ZERO && ratio < p75)  goto e260;
    delta = pnorm/p5;
    par = p5*par;
  }

// test for successful iteration

e260: if (ratio < p0001) goto e290;

// successful iteration. update x, fvec, and their norms

  for (j=1; j<=n; j++) {
    x[j] = wa2[j];
    wa2[j] = diag[j]*x[j];
  }
  for (i=1; i<=m; i++)  fvec[i] = wa4[i];
  xnorm = enorm(n,wa2);
  fnorm = fnorm1;
  iter++;

// tests for convergence

e290:if (ABS(actred) <= ftol && prered <= ftol && p5*ratio <= ONE)  *info = 1;
  if (delta <= xtol*xnorm)  *info = 2;
  if (ABS(actred) <= ftol && prered <= ftol && p5*ratio <= ONE && *info == 2)  *info = 3;
  if (*info != 0) goto e300;

// tests for termination and stringent tolerances

  if (*nfev >= maxfev)  *info = 5;
  if (ABS(actred) <= epsmch && prered <= epsmch && p5*ratio <= ONE)  *info = 6;
  if (delta <= epsmch*xnorm)  *info = 7;
  if (gnorm <= epsmch)  *info = 8;
  if (*info != 0) goto e300;

// end of the inner loop. repeat if iteration unsuccessful

  if (ratio < p0001) goto e200;

// end of the outer loop

  goto e30;

// termination, either normal or user imposed

e300: if (iflag < 0)  *info = iflag;
  iflag = 0;
  if (nprint > 0)  fcn(m, n, x, fvec, &iflag);

  vmfree(vmblock);

} // lmder()


void lmpar(int n, REAL **r, int *ipvt, REAL *diag, REAL *qtb, REAL delta,
           REAL *par, REAL *x, REAL *sdiag)  {
/****************************************************************************

!  Function 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) <= 0.1*delta ,

!  or par is positive and

!        abs(dxnorm-delta) <= 0.1*delta .

!  This Function completes the solution of the problem if it is provided
!  with the necessary information from the r factorization, with column
!  qpivoting, 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 Function statement is

!    Function 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

!    dpmpar, enorm, max, min, qrsolv

!    pascal-supplied ... ABS, SQRT

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!****************************************************************/
//Labels: e120, e150, e220
  int i, iter, j, jm1, jp1, k, l, nsing;
  REAL dxnorm, dwarf, fp, gnorm, parc, parl, paru, sum, temp;
  REAL *tmp, *wa1, *wa2, p1 = 0.1, p001 = 0.001;

  void *vmblock = NULL;

// allocate memory for vectors
  vmblock = vminit();
  tmp = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  wa1 = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  wa2 = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);

  if (! vmcomplete(vmblock)) {
    LogError ("No Memory", 0, __FILE__, __LINE__);
    return;
  }    

// dwarf is the smallest positive magnitude
  dwarf = 2.5e-16;

// compute and store in x the gauss-newton direction. if the
// jacobian is rank-deficient, obtain a least squares solution.

  nsing = n;
  for (j=1; j<=n; j++) {
    wa1[j] = qtb[j];
    if (r[j][j] == ZERO && nsing == n)  nsing = j - 1;
    if (nsing < n)  wa1[j] = ZERO;
  }

  for (k=1; k<=nsing; k++) {
    j = nsing - k + 1;
    wa1[j] = wa1[j]/r[j][j];
    temp = wa1[j];
    jm1 = j - 1;
    for (i=1; i<=jm1; i++)
      wa1[i] = wa1[i] - r[i][j]*temp;
  }
  for (j=1; 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 (i=1; i<=n; i++)  wa2[i] = diag[i]*x[i];
  dxnorm = enorm(n, wa2);
  fp = dxnorm - delta;
  if (fp <= p1*delta) goto e220;

// 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) goto e120;
  for (j=1; j<=n; j++) {
    l = ipvt[j];
    wa1[j] = diag[l]*(wa2[l]/dxnorm);
  }
  for (j=1; j<=n; j++) {
    for (i=1; i<j; i++)  tmp[i]=r[i][j];
    sum = Dot_Product(j-1, tmp, wa1);
    wa1[j] = (wa1[j] - sum)/r[j][j];
  }
  temp = enorm(n,wa1);
  parl = ((fp/delta)/temp)/temp;

// calculate an upper bound, paru, for the ZERO of the function

e120:for (j=1; j<=n; j++) {
    for (i=1; i<=j; i++)  tmp[i]=r[i][j];
    sum = Dot_Product(j, tmp, qtb);
    l = ipvt[j];
    wa1[j] = sum/diag[l];
 }
  gnorm = enorm(n,wa1);
  paru = gnorm/delta;
  if (paru == ZERO)  paru = dwarf/MIN(delta,p1);

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

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

// beginning of an iteration

e150: iter++;

// evaluate the function at the current value of par

  if (*par == ZERO)  *par = MAX(dwarf, p001*paru);
  temp = SQRT(*par);
  for (i=1; i<=n; i++)  wa1[i] = temp*diag[i];

  qrsolv(n, r, ipvt, wa1, qtb, x, sdiag);

  for (i=1; i<=n; i++)  wa2[i] = diag[i]*x[i];
  dxnorm = 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 ((ABS(fp) <= p1*delta || parl == ZERO) && (fp <= temp &&
     temp < ZERO || iter == 10)) goto e220;

// compute the newton correction

  for (j=1; j<=n; j++) {
    l = ipvt[j];
    wa1[j] = diag[l]*(wa2[l]/dxnorm);
  }
  for (j=1; j<=n; j++) {
    wa1[j] = wa1[j]/sdiag[j];
    temp = wa1[j];
    jp1 = j + 1;
    for (i=jp1; i<=n; i++)  wa1[i] = wa1[i] - r[i][j]*temp;
  }
  temp = enorm(n,wa1);
  parc = ((fp/delta)/temp)/temp;

// depending on the sign of the function, update parl or paru

  if (fp > ZERO)  parl = MAX(parl,*par);
  if (fp < ZERO)  paru = MIN(paru,*par);

// compute an improved estimate for par

  *par = MAX(parl, *par + parc);

// end of an iteration

  goto e150;

e220:if (iter == 0)  *par = ZERO;

  vmfree(vmblock);

} // lmpar()


void qrfac(int m, int n, REAL **a, bool pivot, int *ipvt, REAL *rdiag, REAL *acnorm) {
/************************************************************************************

!  Function qrfac

!  This Function 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 Function.

!  the Function statement is

!    Function qrfac(m, n, a, lda, pivot, ipvt, lipvt, rdiag, acnorm, wa)

! N.B. 3 of these arguments have been omitted in this version.

!  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.

!  Functions or functions called:

!    dpmpar, enorm, MAX, MIN

!    pascal-supplied ... SQRT

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!****************************************************************************/
//Labels: e40, e45, e50
  int i, j, jp1, k, kmax, minmn;
  REAL ajnorm, sum, temp;
  REAL *tmp, *tmp1, *wa;

  void *vmblock = NULL;

// allocate memory for vectors
  vmblock = vminit();
  tmp  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  tmp1 = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  wa   = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);

  if (! vmcomplete(vmblock)) {
    LogError ("No Memory", 0, __FILE__, __LINE__);
    return;
  }    

// compute the initial column norms and initialize several arrays

  for (j=1; j<=n; j++) {
    for (i=1; i<=m; i++) tmp[i]=a[i][j];
    acnorm[j] = enorm(m,tmp);
    rdiag[j] = acnorm[j];
    wa[j] = rdiag[j];
    if (pivot)  ipvt[j] = j;
  }

// Reduce a to r with Householder transformations

  if (m<=n)  minmn = m; else minmn = n;
  for (j=1; j<=minmn; j++) {
    
	if (!pivot) goto e40;
  
// 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 e40;
    for (i=1; i<=m; i++) {
      temp = a[i][j];
      a[i][j] = a[i][kmax];
      a[i][kmax] = temp;
    }
    rdiag[kmax] = rdiag[j];
    wa[kmax] = wa[j];
    k = ipvt[j];
    ipvt[j] = ipvt[kmax];
    ipvt[kmax] = k;
  
// Compute the Householder transformation to reduce the
// j-th column of a to a multiple of the j-th unit vector.
  
e40: for (i=j; i<=m; i++)  tmp[i-j+1]=a[i][j];
    ajnorm = enorm(m-j+1, tmp);
    if (ajnorm == ZERO)  goto e50;
    if (a[j][j] < ZERO)  ajnorm = -ajnorm;
    for (i=j; i<=m; i++) a[i][j] /= ajnorm;
    a[j][j] += ONE;
  
// Apply the transformation to the remaining columns and update the norms
  
    jp1 = j + 1;
    for (k=jp1; k<=n; k++) {
	  for (i=j; i<=m; i++) {
        tmp[i-j+1]=a[i][j];
        tmp1[i-j+1]=a[i][k];
      }
      sum = Dot_Product(m-j+1,tmp,tmp1);
      temp = sum/a[j][j];
      for (i=j; i<=m; i++)  a[i][k] -= temp*a[i][j];
      if (!pivot || rdiag[k] == ZERO)  goto e50;
      temp = a[j][k]/rdiag[k];
      rdiag[k] *= SQRT(MAX(ZERO, ONE-temp*temp));
      if (p05*Sqr(rdiag[k]/wa[k]) > epsmch) goto e45;
      for (i=jp1; i<=m; i++) tmp[i-j]=a[i][k];
      rdiag[k] = enorm(m-j, tmp);
      wa[k] = rdiag[k];
e45:;}
    rdiag[j] = -ajnorm;
  } // j loop

e50: vmfree(vmblock);

} // qrfac()


void qrsolv(int n, REAL **r, int *ipvt, REAL *diag, REAL *qtb, REAL *x, REAL *sdiag) {
/**************************************************************************

!  Function 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 Function 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 Function statement is

!    Function qrsolv(n, r, ldr, ipvt, diag, qtb, x, sdiag, wa)

! N.B. Arguments LDR and WA have been removed in this version.

!  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

!    pascal-supplied ... ABS, SQRT

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!**************************************************************************/
  int i, j, k, kp1, l, nsing;
  REAL COS, cotan, qtbpj, SIN, sum, TAN, temp;
  REAL *tmp, *tmp1, *wa;

  void *vmblock = NULL;

// allocate memory for vectors
  vmblock = vminit();
  tmp  = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  tmp1 = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);
  wa   = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);

  if (! vmcomplete(vmblock)) {
    LogError ("No Memory", 0, __FILE__, __LINE__);
    return;
  }    

// Copy r and (q transpose)*b to preserve input and initialize s.
// In particular, save the diagonal elements of r in x

  for (j=1; j<=n; j++) {
    for (i=j; i<=n; i++)  r[i][j] = r[j][i];
    x[j] = r[j][j];
    wa[j] = qtb[j];
  }

// Eliminate the diagonal matrix d using a givens rotation

  for (j=1; 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) break;
    for (i=j; i<=n; i++)  sdiag[i] = 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) break;
      if (ABS(r[k][k]) < ABS(sdiag[k])) {
        cotan = r[k][k]/sdiag[k];
        SIN = p5/SQRT(p25 + p25*cotan*cotan);
        COS = SIN*cotan;
      }
      else {
        TAN = sdiag[k]/r[k][k];
        COS = p5/SQRT(p25 + p25*TAN*TAN);
        SIN = COS*TAN;
      }
    
// Compute the modified diagonal element of r and
// the modified element of ((q transpose)*b,0)
    
      r[k][k] = COS*r[k][k] + SIN*sdiag[k];
      temp = COS*wa[k] + SIN*qtbpj;
      qtbpj = -SIN*wa[k] + COS*qtbpj;
      wa[k] = temp;
    
// Accumulate the tranformation in the row of s
    
      kp1 = k + 1;
      for (i=kp1; i<=n; i++) {
        temp = COS*r[i][k] + SIN*sdiag[i];
        sdiag[i] = -SIN*r[i][k] + COS*sdiag[i];
        r[i][k] = temp;
      }     
	} // k loop
  
// Store the diagonal element of s and restore
// the corresponding diagonal element of r
  
    sdiag[j] = r[j][j];
    r[j][j] = x[j];
  } // j loop

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

  nsing = n;
  for (j=1; j<=n; j++) {
    if (sdiag[j] == ZERO && nsing == n)  nsing = j - 1;
    if (nsing < n)  wa[j] = ZERO;
  }

  for  (k=1; k<=nsing; k++) {
    j = nsing - k + 1;
    for (i=j+1; i<=nsing; i++) {
      tmp[i-j]=r[i][j];
      tmp1[i-j]=wa[i];
    }
    sum = Dot_Product(nsing-j, tmp, tmp1);
    wa[j] = (wa[j] - sum)/sdiag[j];
  }

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

  for (j=1; j<=n; j++) {
    l = ipvt[j];
    x[l] = wa[j];
  }

  vmfree(vmblock);

} // qrsolv()


REAL enorm(int n, REAL *x) {
short i; REAL temp;
  temp=ZERO;
  for (i=1; i<=n; i++) temp += Sqr(x[i]);
  return SQRT(temp);
};


void fdjac2(int m, int n, REAL *x, REAL *fvec, REAL **fjac, int *iflag,
            REAL epsfcn)  {
/************************************************************************

!  Function fdjac2

!  this Function computes a forward-difference approximation
!  to the m by n jacobian matrix associated with a specified
!  problem of m functions in n variables.

!  the Function statement is

!    Function fdjac2(m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)

!  where:

!    fcn is the name of the user-supplied Function which calculates
!      the functions (removed from list of arguments).
!      fcn should be written as follows:

!      Function fcn (m, n:integer; x, fvec:pVec; var iflag:integer);
!      begin
!      ----------
!      calculate the functions at x and return this vector in fvec.
!      ----------
!      end;

!      the value of iflag should not be changed by fcn unless
!      the user wants to terminate execution of lmdif1.
!      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, ssqfcn

!    minpack-supplied ... dpmpa, max

!    pascal-supplied ... ABS, SQRT

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

! *************************************************************************/
  int  i,j;
  REAL eps, h, temp;
  REAL *wa;

  void *vmblock = NULL;

// allocate memory for vector wa
  vmblock = vminit();
  wa   = (REAL *) vmalloc(vmblock, VEKTOR,  n+1, 0);

  if (! vmcomplete(vmblock)) {
    LogError ("No Memory", 0, __FILE__, __LINE__);
    return;
  }    

  eps = SQRT(MAX(epsfcn, epsmch));
  for (j=1; j<=n; j++) {
    temp = x[j];
    h = eps*ABS(temp);
    if (h == ZERO)  h = eps;
    x[j] = temp + h;
    fcn(m, n, x, wa, iflag);
    if (*iflag < 0) return;
    x[j] = temp;
    for (i=1; i<=m; i++)  fjac[i][j] = (wa[i] - fvec[i])/h;
  }

  vmfree(vmblock);

} // fdjac2()

//end of file lm.cpp