/* -*- Mode: C; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
/* vim:set sw=2 sts=2 et cin: */
/*
 * This file is part of the MUSE Instrument Pipeline
 * Copyright (C) 2005-2014 European Southern Observatory
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 */
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <string.h>
#ifndef _OPENMP
#define omp_get_max_threads() 1
#else
#include <omp.h>
#endif

/*----------------------------------------------------------------------------*
  The following lines come from lmmin.h.
 *----------------------------------------------------------------------------*/
/* User-supplied subroutines. */

/* Compact high-level interface. */

/* Collection of control parameters. */
typedef struct {
  double ftol;      /* relative error desired in the sum of squares. */
  double xtol;      /* relative error between last two approximations. */
  double gtol;      /* orthogonality desired between fvec and its derivs. */
  double epsilon;   /* step used to calculate the jacobian. */
  double stepbound; /* initial bound to steps in the outer loop. */
  double fnorm;     /* norm of the residue vector fvec. */
  int maxcall;      /* maximum number of iterations. */
  int nfev;	      /* actual number of iterations. */
  int info;	      /* status of minimization. */
} lm_control_type;

/* Initialize control parameters with default values. */
static void lm_initialize_control(lm_control_type * control);

/* Refined calculation of Eucledian norm, typically used in printout routine. */
static double lm_enorm(int, double *);

/* The actual minimization. */
static void lm_minimize(int m_dat, int n_par, double *par,
                        void (*evaluate) (double *par, int m_dat, double *fvec,
                                          void *data, int *info),
                        void (*printout) (const char *func, int n_par, 
                                          double *par, int m_dat,
                                          double *fvec, void *data, int iflag,
                                          int iter, int nfev),
                        void *data, lm_control_type * control);


/* Legacy low-level interface. */

/* Alternative to lm_minimize, allowing full control, and read-out
   of auxiliary arrays. For usage, see implementation of lm_minimize. */
static void lm_lmdif(int m, int n, double *x, double *fvec, double ftol,
                     double xtol, double gtol, int maxfev, double epsfcn,
                     double *diag, int mode, double factor, int *info, 
                     int *nfev, double *fjac, int *ipvt, double *qtf, 
                     double *wa1, double *wa2, double *wa3, double *wa4,
                     void (*evaluate) (double *par, int m_dat, double *fvec,
                                       void *data, int *info),
                     void (*printout) (const char *func, int n_par, 
                                       double *par, int m_dat,
                                       double *fvec, void *data, int iflag,
                                       int iter, int nfev),
                     void *data);

/* static const char *lm_infmsg[]; */
/* static const char *lm_shortmsg[]; */
/*---------------------------------------------------------------------------*
  End of lmmin.h.
 *---------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*
  Definition of high level CPL interface.
 *---------------------------------------------------------------------------*/

#include "muse_optimize.h"
#include "muse_utils.h"
/**
   @addtogroup muse_utils
   @{
*/

typedef struct {
  muse_cpl_evaluate_func *func;
  int npar;
  void *data;
  cpl_error_code retval;
} muse_cpl_optimize_eval_struct;

/**
  @private
   @brief User provided function to evaluate.

   This function just calls the function specified in the data array.
 */
static void 
lm_evaluate_cpl (double *par, int m_dat, double *fvec, void *data, int *info) {
  muse_cpl_optimize_eval_struct *s = data;

  cpl_array *cpl_par = cpl_array_wrap_double(par, s->npar);
  cpl_array *cpl_fvec = cpl_array_wrap_double(fvec, m_dat);

  s->retval = (*(s->func))(s->data, cpl_par, cpl_fvec);
  
  cpl_array_unwrap(cpl_par);
  cpl_array_unwrap(cpl_fvec);
  
  if (s->retval != CPL_ERROR_NONE) {
    *info = -1;
  }
}

static const cpl_error_code error_code_tbl[] = { // see also lm_infmsg
    CPL_ERROR_ILLEGAL_INPUT, 
    CPL_ERROR_NONE,
    CPL_ERROR_NONE,
    CPL_ERROR_NONE,
    CPL_ERROR_SINGULAR_MATRIX,
    CPL_ERROR_CONTINUE,
    CPL_ERROR_CONTINUE,
    CPL_ERROR_CONTINUE,
    CPL_ERROR_CONTINUE,
    CPL_ERROR_ILLEGAL_OUTPUT,
    CPL_ERROR_NONE,
};

static const char *lm_infmsg[] = { /* from lmmin.c */
  "fatal coding error (improper input parameters)",
  "success (the relative error in the sum of squares is at most tol)",
  "success (the relative error between x and the solution is at most tol)",
  "success (both errors are at most tol)",
  "trapped by degeneracy (fvec is orthogonal to the columns of the jacobian)"
  "timeout (number of calls to fcn has reached maxcall*(n+1))",
  "failure (ftol<tol: cannot reduce sum of squares any further)",
  "failure (xtol<tol: cannot improve approximate solution any further)",
  "failure (gtol<tol: cannot improve approximate solution any further)",
  "exception (not enough memory)",
  "exception (break requested within function evaluation)"
};

/** @brief Default printout method adopted to CPL.
 *  @param func  Function name
 *  @param n_par Number of parameters
 *  @param par   Vector of parameters
 *  @param m_dat Number of datapoints
 *  @param fvec  Vector of datapoints
 *  @param data  For soft control of printout behaviour, add control
 *               variables to the data struct.
 *  @param iflag 0 (init) 1 (outer loop) 2(inner loop) -1(terminated)
 *  @param iter  outer loop counter
 *  @param nfev  number of calls to *evaluate
 */
static void
muse_cpl_optimize_print(const char *func, int n_par, double *par,
                        int m_dat, double *fvec,
                        void *data, int iflag, int iter, int nfev) {
  if (iflag == 2) {
    cpl_msg_debug(func, "trying step in gradient direction");
  } else if (iflag == 1) {
    cpl_msg_debug(func, "determining gradient (iteration %d)", iter);
  } else if (iflag == 0) {
    cpl_msg_debug(func, "starting minimization");
  } else if (iflag == -1) {
    cpl_msg_debug(func, "terminated after %d evaluations", nfev);
  }

  char *parstring = cpl_calloc(n_par * 15 + 30, sizeof(char));
  snprintf(parstring, 5, "par:");
  int i;
  for (i = 0; i < n_par; ++i) {
    snprintf(parstring + strlen(parstring), 15, " %7.3g", par[i]);
  }
  snprintf(parstring + strlen(parstring), 25, " => norm: %7g", 
           lm_enorm(m_dat, fvec));
  cpl_msg_debug(func, "%s", parstring);
  cpl_free(parstring);

#if 0 /* still not adopted */
  double f, y, t;
  muse_cpl_optimize_eval_struct *mydata 
    = (muse_cpl_optimize_eval_struct *) data;

  if (iflag == -1) {
    cpl_msg_debug(func, "  fitting data as follows:", mydata);
    for (i = 0; i < m_dat; ++i) {
      t = (mydata->tvec)[i];
      y = (mydata->yvec)[i];
      f = mydata->f(t, par);
      cpl_msg_debug(func, "    t[%2d]=%12g y=%12g fit=%12g residue=%12g",
                    i, t, y, f, y - f);
    }
  }
#else /* just to silence "unused parameter" warning: */
  UNUSED_ARGUMENT(data);
#endif
}

/**
   @brief Minimize a function with the Levenberg-Marquardt algorithm.
   @param aData Any data to be forwarded to the evaluation function.
   @param aPar Array containing the fits parameters.
   @param aSize Number of data points.
   @param aFunction Function that evaluates the fit. 
   @param aCtrl Structure that controls the algorithm, or NULL for defaults.
   @retval CPL_ERROR_NONE Everything went OK. 
   @cpl_ensure_code{aPar != NULL, CPL_ERROR_NULL_INPUT}
   @cpl_ensure_code{cpl_array_get_size(aPar) > 0, CPL_ERROR_ILLEGAL_INPUT}
   @cpl_ensure_code{aSize > 0, CPL_ERROR_ILLEGAL_INPUT}
   @cpl_ensure_code{aFunction != NULL, CPL_ERROR_NULL_INPUT}
   @retval CPL_ERROR_SINGULAR_MATRIX Return vector is orthogonal to the columns of the jacobian
   @retval CPL_ERROR_CONTINUE The algorithm did not converge.
   @retval CPL_ERROR_ILLEGAL_OUTPUT Memory allocation failed

   @remark If the evaluation function did not return <tt>CPL_ERROR_NONE</tt>,
   the minimization stops and returns with the return value of the evaluation
   function.

   The interface is loosely modeled after cpl_fit_lvmq(). However it can be
   used with functions that calc the whole result vector at once, it does not
   need to provide a dervitative, and in may forward any data to the
   evaluation function.

   @note The implementation used here is copied from
   http://www.messen-und-deuten.de/lmfit/ .

*/
cpl_error_code 
muse_cpl_optimize_lvmq(void *aData, cpl_array *aPar, int aSize,
                       muse_cpl_evaluate_func *aFunction,
                       muse_cpl_optimize_control_t *aCtrl) {
    
  cpl_ensure_code(aPar != NULL, CPL_ERROR_NULL_INPUT);
  int npars = cpl_array_get_size(aPar);
  cpl_ensure_code(npars > 0, CPL_ERROR_ILLEGAL_INPUT);
  cpl_ensure_code(aSize > 0, CPL_ERROR_ILLEGAL_INPUT);
  cpl_ensure_code(aFunction != NULL, CPL_ERROR_NULL_INPUT);
  
  muse_cpl_optimize_eval_struct s = {
    aFunction, 
    npars,
    aData,
    CPL_ERROR_NONE
  };
  
  void (*debug_func) (const char *, int, double *, int, double *, void *, 
                      int, int, int) = NULL;
  lm_control_type control;
  lm_initialize_control(&control);
  if (aCtrl != NULL) {
    if (aCtrl->maxcall > 0) {
      control.maxcall = aCtrl->maxcall;
    }
    if (aCtrl->ftol > 0) {
      control.ftol = aCtrl->ftol;
    }
    if (aCtrl->xtol > 0) {
      control.xtol = aCtrl->xtol;
    }
    if (aCtrl->gtol > 0) {
      control.gtol = aCtrl->gtol;
    }
    if (aCtrl->debug == CPL_TRUE) {
      debug_func = muse_cpl_optimize_print;
    }
  }

  double timeinit = cpl_test_get_walltime();
  double cpuinit = cpl_test_get_cputime();
  lm_minimize(aSize, 
              cpl_array_get_size(aPar),
              cpl_array_get_data_double(aPar),
              lm_evaluate_cpl, 
              debug_func,
              &s, &control);

  double timefini = cpl_test_get_walltime();
  double cpufini = cpl_test_get_cputime();
  
  cpl_msg_debug(__func__, "Minimizing finished after %i steps: %s", 
                control.nfev / ((int)cpl_array_get_size(aPar)+1),
                lm_infmsg[control.info]);
  
  cpl_msg_debug(__func__, "processing time %.3fs (%.3fs CPU, %d CPUs)", 
                timefini - timeinit, cpufini - cpuinit, omp_get_max_threads());


  cpl_error_code res = error_code_tbl[control.info];

  cpl_ensure_code(res == CPL_ERROR_NONE, res);

  return s.retval;
}

/**@}*/

/*----------------------------------------------------------------------------*
  This comes from lmmin.c.
 *----------------------------------------------------------------------------*/
/*
 * Project:  LevenbergMarquardtLeastSquaresFitting
 *
 * File:     lmmin.c
 *
 * Contents: Levenberg-Marquardt core implementation,
 *           and simplified user interface.
 *
 * Authors:  Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
 *           (lmdif and other routines from the public-domain library
 *           netlib::minpack, Argonne National Laboratories, March 1980);
 *           Steve Moshier (initial C translation);
 *           Joachim Wuttke (conversion into C++ compatible ANSI style,
 *           corrections, comments, wrappers, hosting).
 * 
 * Homepage: www.messen-und-deuten.de/lmfit
 *
 * Licence:  Public domain.
 *
 * Make:     For instance: gcc -c lmmin.c; ar rc liblmmin.a lmmin.o
 */
 
/* *********************** high-level interface **************************** */

/* machine-dependent constants from float.h */
#define LM_MACHEP     DBL_EPSILON   /* resolution of arithmetic */
#define LM_DWARF      DBL_MIN       /* smallest nonzero number */
#if 0
#define LM_SQRT_DWARF sqrt(DBL_MIN) /* square should not underflow */
#define LM_SQRT_GIANT sqrt(DBL_MAX) /* square should not overflow */
#else
#define LM_SQRT_DWARF 1.e-150
#define LM_SQRT_GIANT 1.e150 
#endif
#define LM_USERTOL    30*LM_MACHEP  /* users are recommened to require this */

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


static void lm_initialize_control( lm_control_type * control ) {
  control->maxcall = 100;
  control->epsilon = LM_USERTOL;
  control->stepbound = 100.;
  control->ftol = LM_USERTOL;
  control->xtol = LM_USERTOL;
  control->gtol = LM_USERTOL;
}

static void lm_minimize( int m_dat, int n_par, double *par,
                         void (*evaluate) (double *par, int m_dat, double *fvec,
                                           void *data, int *info),
                         void (*printout) (const char *func, 
                                           int n_par, double *par, int m_dat,
                                           double *fvec, void *data, int iflag,
                                           int iter, int nfev),
                         void *data, lm_control_type * control ) {

/* allocate work space. */

  double *fvec, *diag, *fjac, *qtf, *wa1, *wa2, *wa3, *wa4;
  int *ipvt;
  
  int n = n_par;
  int m = m_dat;
  
  if ( (fvec = (double *) cpl_malloc(m * sizeof(double))) == NULL ||
       (diag = (double *) cpl_malloc(n * sizeof(double))) == NULL ||
       (qtf  = (double *) cpl_malloc(n * sizeof(double))) == NULL ||
       (fjac = (double *) cpl_malloc(n*m*sizeof(double))) == NULL ||
       (wa1  = (double *) cpl_malloc(n * sizeof(double))) == NULL ||
       (wa2  = (double *) cpl_malloc(n * sizeof(double))) == NULL ||
       (wa3  = (double *) cpl_malloc(n * sizeof(double))) == NULL ||
       (wa4  = (double *) cpl_malloc(m * sizeof(double))) == NULL ||
       (ipvt = (int *)    cpl_malloc(n * sizeof(int)   )) == NULL    ) {
    control->info = 9;
    return;
  }

/* perform fit. */

  control->info = 0;
  control->nfev = 0;
  
  /* this goes through the modified legacy interface: */
  lm_lmdif( m, n, par, fvec, control->ftol, control->xtol, control->gtol,
            control->maxcall * (n + 1), control->epsilon, diag, 1,
            control->stepbound, &(control->info),
            &(control->nfev), fjac, ipvt, qtf, wa1, wa2, wa3, wa4,
            evaluate, printout, data );
  
  if ( printout )
    (*printout) (__func__, n, par, m, fvec, data, -1, 0, control->nfev);
  control->fnorm = lm_enorm(m, fvec);
  if ( control->info < 0 )
    control->info = 10;
  
/* clean up. */

  cpl_free(fvec);
  cpl_free(diag);
  cpl_free(qtf);
  cpl_free(fjac);
  cpl_free(wa1);
  cpl_free(wa2);
  cpl_free(wa3);
  cpl_free(wa4);
  cpl_free(ipvt);
} /* lm_minimize. */


/* the following messages are indexed by the variable info. */

#if 0 /* moved definition of lm_infmsg to the beginning of the file */
static const char *lm_infmsg[] = {
  "fatal coding error (improper input parameters)",
  "success (the relative error in the sum of squares is at most tol)",
  "success (the relative error between x and the solution is at most tol)",
  "success (both errors are at most tol)",
  "trapped by degeneracy (fvec is orthogonal to the columns of the jacobian)"
  "timeout (number of calls to fcn has reached maxcall*(n+1))",
  "failure (ftol<tol: cannot reduce sum of squares any further)",
  "failure (xtol<tol: cannot improve approximate solution any further)",
  "failure (gtol<tol: cannot improve approximate solution any further)",
  "exception (not enough memory)",
  "exception (break requested within function evaluation)"
};

static const char *lm_shortmsg[] = {
  "invalid input",
  "success (f)",
  "success (p)",
  "success (f,p)",
  "degenerate",
  "call limit",
  "failed (f)",
  "failed (p)",
  "failed (o)",
  "no memory",
  "user break"
};
#endif


/* ************************** implementation ******************************* */

#define BUG 0
#if BUG
#include <stdio.h>
#endif

static void lm_qrfac(int m, int n, double *a, int pivot, int *ipvt,
                     double *rdiag, double *acnorm, double *wa);
static void lm_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag,
                      double *qtb, double *x, double *sdiag, double *wa);
static 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);

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


/* the low-level legacy interface for full control. */

/**
 *   The purpose of lmdif is to minimize the sum of the squares of
 *   m nonlinear functions in n variables by a modification of
 *   the levenberg-marquardt algorithm. The user must provide a
 *   subroutine evaluate which calculates the functions. The jacobian
 *   is then calculated by a forward-difference approximation.
 *
 *   The multi-parameter interface lm_lmdif is for users who want
 *   full control and flexibility. Most users will be better off using
 *   the simpler interface lm_minimize provided above.
 *
 *   The parameters are the same as in the legacy FORTRAN implementation,
 *   with the following exceptions:
 *      the old parameter ldfjac which gave leading dimension of fjac has
 *        been deleted because this C translation makes no use of two-
 *        dimensional arrays;
 *      the old parameter nprint has been deleted; printout is now controlled
 *        by the user-supplied routine *printout;
 *      the parameter field *data and the function parameters *evaluate and
 *        *printout have been added; they help avoiding global variables.
 *
 *	@param m is a positive integer input variable set to the number
 *	  of functions.
 *
 *	@param n is a positive integer input variable set to the number
 *	  of variables; n must not exceed m.
 *
 *	@param 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.
 *
 *	@param fvec is an output array of length m which contains
 *	  the functions evaluated at the output x.
 *
 *	@param 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.
 *
 *	@param 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.
 *
 *	@param 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.
 *
 *	@param maxfev is a positive integer input variable. Termination
 *	  occurs when the number of calls to lm_fcn is at least
 *	  maxfev by the end of an iteration.
 *
 *	@param 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.
 *
 *	@param 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.
 *
 *	@param 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.
 *
 *	@param factor is a positive input variable used in determining the
 *	  initial step bound. This bound is set to the product of
 *	  factor and the euclidean norm of diag*x if nonzero, or else
 *	  to factor itself. In most cases factor should lie in the
 *	  interval (0.1,100.0). Generally, the value 100.0 is recommended.
 *
 *	@param info is an integer output variable that indicates the termination
 *        status of lm_lmdif as follows:
 *
 *        info < 0  termination requested by user-supplied routine *evaluate;
 *
 *	  info = 0  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 lm_fcn has reached or
 *		    exceeded maxfev;
 *
 *	  info = 6  ftol is too small: no further reduction in
 *		    the sum of squares is possible;
 *
 *	  info = 7  xtol is too small: no further improvement in
 *		    the approximate solution x is possible;
 *
 *	  info = 8  gtol is too small: fvec is orthogonal to the
 *		    columns of the jacobian to machine precision;
 *
 *	@param nfev is an output variable set to the number of calls to the
 *        user-supplied routine *evaluate.
 *
 *	@param 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.
 *
 *	@param ipvt is an integer output array of length n. It defines a
 *        permutation matrix p such that jac*p = q*r, where jac is
 *        the final calculated jacobian, q is orthogonal (not stored),
 *        and r is upper triangular with diagonal elements of
 *        nonincreasing magnitude. Column j of p is column ipvt(j)
 *        of the identity matrix.
 *
 *	@param qtf is an output array of length n which contains
 *	  the first n elements of the vector (q transpose)*fvec.
 *
 *	@param wa1, wa2, wa3 are work arrays of length n.
 *
 *	@param wa4 is a work array of length m.
 *
 *   The following parameters are newly introduced in this C translation:
 *
 *  @param evaluate is the name of the subroutine which calculates the
 *        m nonlinear functions. A default implementation lm_evaluate_default
 *        is provided in lm_eval.c. Alternative implementations should
 *        be written as follows:
 *
 *        void evaluate ( double* par, int m_dat, double* fvec, 
 *                       void *data, int *info )
 *        {
 *           // for ( i=0; i<m_dat; ++i )
 *           //     calculate fvec[i] for given parameters par;
 *           // to stop the minimization, 
 *           //     set *info to a negative integer.
 *        }
 *
 *  @param printout is the name of the subroutine which nforms about fit progress.
 *        Call with printout=NULL if no printout is desired.
 *        Call with printout=lm_print_default to use the default
 *          implementation provided in lm_eval.c.
 *        Alternative implementations should be written as follows:
 *
 *        void printout ( int n_par, double* par, int m_dat, double* fvec, 
 *                       void *data, int iflag, int iter, int nfev )
 *        {
 *           // iflag : 0 (init) 1 (outer loop) 2(inner loop) -1(terminated)
 *           // iter  : outer loop counter
 *           // nfev  : number of calls to *evaluate
 *        }
 *
 *   @param data is an input pointer to an arbitrary structure that is passed to
 *        evaluate. Typically, it contains experimental data to be fitted.
 *
 */
static void lm_lmdif(int m, int n, double *x, double *fvec, double ftol,
                     double xtol, double gtol, int maxfev, double epsfcn,
                     double *diag, int mode, double factor, int *info, 
                     int *nfev, double *fjac, int *ipvt, double *qtf, 
                     double *wa1, double *wa2, double *wa3, double *wa4,
                     void (*evaluate) (double *par, int m_dat, double *fvec,
                                       void *data, int *info),
                     void (*printout) (const char *func, int n_par, double *par, int m_dat,
                                       double *fvec, void *data, int iflag,
                                       int iter, int nfev),
                     void *data)
{
  int iter, j;
  double actred, delta, dirder, eps, fnorm, fnorm1, gnorm, par, pnorm,
    prered, ratio, sum, temp, temp1, temp2, temp3, xnorm;
  static double p1 = 0.1;
  static double p0001 = 1.0e-4;

  *nfev = 0;			/* function evaluation counter */
  iter = 1;			/* outer loop counter */
  par = 0;			/* levenberg-marquardt parameter */
  delta = 0;	 /* to prevent a warning (initialization within if-clause) */
  xnorm = 0;	 /* ditto */
  temp = MAX(epsfcn, LM_MACHEP);
  eps = sqrt(temp); /* for calculating the Jacobian by forward differences */
  
/* lmdif: check input parameters for errors. */

  if ((n <= 0) || (m < n) || (ftol < 0.)
      || (xtol < 0.) || (gtol < 0.) || (maxfev <= 0) || (factor <= 0.)) {
    *info = 0;		// invalid parameter
    return;
  }
  if (mode == 2) {		/* scaling by diag[] */
    for (j = 0; j < n; j++) {	/* check for nonpositive elements */
	    if (diag[j] <= 0.0) {
        *info = 0;	// invalid parameter
        return;
	    }
    }
  }
#if BUG
  printf("lmdif\n");
#endif

/* lmdif: evaluate function at starting point and calculate norm. */

  *info = 0;
  (*evaluate) (x, m, fvec, data, info); ++(*nfev);
  if( printout )
    (*printout) (__func__, n, x, m, fvec, data, 0, 0, *nfev);
  if (*info < 0)
    return;
  fnorm = lm_enorm(m, fvec);
  
/* lmdif: the outer loop. */

  do {
#if BUG
    cpl_msg_debug(__func__, "iter=%d nfev=%d fnorm=%g\n",
                  iter, *nfev, fnorm);
#endif

/* outer: calculate the jacobian matrix. */
    *info = 0;
#if 1
    // FIXME: Removed default(none) clause here, to make the code work with
    //        gcc9 while keeping older versions of gcc working too without
    //        resorting to conditional compilation. Having default(none) here
    //        causes gcc9 to abort the build because of __func__ not being
    //        specified in a data sharing clause. Adding a data sharing clause
    //        makes older gcc versions choke.
    #pragma omp parallel for                                                 \
            shared(n, info, m, x, eps, evaluate, data, printout, nfev, iter, \
                   fjac, fvec)
#endif
    for (j = 0; j < n; j++) {
      if (*info < 0)
        continue;
      double *wa5  = (double *) cpl_malloc(m * sizeof(double));
      double *xd = (double *) cpl_malloc(n * sizeof(double));
      memcpy(xd, x, n * sizeof(double));
	    double step = eps * fabs(xd[j]);
	    if (step == 0.)
        step = eps;
	    xd[j] += step;
	    int infod = 0;
	    (*evaluate) (xd, m, wa5, data, &infod); ++(*nfev);
      if( printout )
        (*printout) (__func__, n, xd, m, wa5, data, 1, iter, *nfev);
	    if (infod < 0) {/* user requested break */
        *info = infod;
        cpl_free(wa5);
        cpl_free(xd);
        continue;
      }
      int i;
      double diff = xd[j] - x[j];
      double *fjacm = fjac + j * m;
      for (i = 0; i < m; i++) { /* changed in 2.3, Mark Bydder */
        fjacm[i] = (wa5[i] - fvec[i]) / diff;
      }
      cpl_free(wa5);
      cpl_free(xd);
    }
    if (*info < 0) 
      return;	/* user requested break */

#if BUG>1
    /* DEBUG: print the entire matrix */
    {
      int i;
      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. */

    int i;
    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); ++(*nfev);
      if( printout )
        (*printout) (__func__, 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 <= 0.25) {
        if (actred >= 0.)
          temp = 0.5;
        else
          temp = 0.5 * dirder / (dirder + 0.55 * actred);
        if (p1 * fnorm1 >= fnorm || temp < p1)
          temp = p1;
        delta = temp * MIN(delta, pnorm / p1);
        par /= temp;
	    } else if (par == 0. || ratio >= 0.75) {
        delta = pnorm / 0.5;
        par *= 0.5;
	    }

/* inner: test for successful iteration. */

	    if (ratio >= p0001) {
        /* yes, success: update x, fvec, and their norms. */
        for (j = 0; j < n; j++) {
          x[j] = wa2[j];
          wa2[j] = diag[j] * x[j];
        }
        for (i = 0; i < m; i++)
          fvec[i] = wa4[i];
        xnorm = lm_enorm(n, wa2);
        fnorm = fnorm1;
        iter++;
	    }
#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 && 0.5 * ratio <= 1)
        *info = 1;
	    if (delta <= xtol * xnorm)
        *info += 2;
	    if (*info != 0)
        return;
      
/* inner: tests for termination and stringent tolerances. */

	    if (*nfev >= maxfev)
        *info = 5;
	    if (fabs(actred) <= LM_MACHEP &&
          prered <= LM_MACHEP && 0.5 * 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. */

/**
 *     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.
 *
 *	@param n is a positive integer input variable set to the order of r.
 *
 *	@param 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.
 *
 *	@param ldr is a positive integer input variable not less than n
 *	  which specifies the leading dimension of the array r.
 *
 *	@param 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.
 *
 *	@param diag is an input array of length n which must contain the
 *	  diagonal elements of the matrix d.
 *
 *	@param qtb is an input array of length n which must contain the first
 *	  n elements of the vector (q transpose)*b.
 *
 *	@param delta is a positive input variable which specifies an upper
 *	  bound on the euclidean norm of d*x.
 *
 *	@param par is a nonnegative variable. on input par contains an
 *	  initial estimate of the levenberg-marquardt parameter.
 *	  on output par contains the final estimate.
 *
 *	@param 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.
 *
 *	@param sdiag is an output array of length n which contains the
 *	  diagonal elements of the upper triangular matrix s.
 *
 *	@param wa1, wa2 are work arrays of length n.
 *
 */
static 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)
{
  int i, iter, j, nsing;
  double dxnorm, fp, fp_old, gnorm, parc, parl, paru;
  double sum, temp;
  static double p1 = 0.1;
  
#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, 0.001 * 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++)
        wa1[i] -= r[j * ldr + i] * wa1[j];
    }
    temp = lm_enorm(n, wa1);
    parc = fp / delta / temp / temp;
    
    /* depending on the sign of the function, update parl or paru. */

    if (fp > 0)
	    parl = MAX(parl, *par);
    else if (fp < 0)
	    paru = MIN(paru, *par);
    /* the case fp==0 is precluded by the break condition  */
    
    /* compute an improved estimate for par. */
        
    *par = MAX(parl, *par + parc);
    
  }
  
} /* lm_lmpar. */

/**
 *     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 zeroes in the first k-1 positions. The form of
 *     this transformation and the method of pivoting first
 *     appeared in the corresponding linpack subroutine.
 *
 *	@param m is a positive integer input variable set to the number
 *	  of rows of a.
 *
 *	@param n is a positive integer input variable set to the number
 *	  of columns of a.
 *
 *	@param 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).
 *
 *	@param 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.
 *
 *	@param ipvt is an integer output array of length lipvt. This array
 *	  defines the permutation matrix p such that a*p = q*r.
 *	  Column j of p is column ipvt(j) of the identity matrix.
 *	  If pivot is false, ipvt is not referenced.
 *
 *	@param rdiag is an output array of length n which contains the
 *	  diagonal elements of r.
 *
 *	@param 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.
 *
 *	@param wa is a work array of length n. If pivot is false, then wa
 *	  can coincide with rdiag.
 *
 */
static void lm_qrfac(int m, int n, double *a, int pivot, int *ipvt,
                     double *rdiag, double *acnorm, double *wa)
{
  int i, j, k, minmn;
  double ajnorm;
  static double p05 = 0.05;

/* qrfac: compute initial column norms and initialize several arrays. */

  for (j = 0; j < n; j++) {
    acnorm[j] = lm_enorm(m, &a[j * m]);
    rdiag[j] = acnorm[j];
    wa[j] = rdiag[j];
    if (pivot)
	    ipvt[j] = j;
  }
#if BUG
  printf("qrfac\n");
#endif

/* qrfac: reduce a to r with householder transformations. */

  minmn = MIN(m, n);
  for (j = 0; j < minmn; j++) {
    double *am = a + j *m;
    if (pivot) {
    
      /* bring the column of largest norm into the pivot position. */

      int kmax = j;
      for (k = j + 1; k < n; k++)
        if (rdiag[k] > rdiag[kmax])
          kmax = k;
      if (kmax != j) {
        double *akmaxm = a + kmax * m;
        for (i = 0; i < m; i++) {
          double temp = am[i];
          am[i] = akmaxm[i];
          akmaxm[i] = temp;
        }
        rdiag[kmax] = rdiag[j];
        wa[kmax] = wa[j];
        int ktmp = ipvt[j];
        ipvt[j] = ipvt[kmax];
        ipvt[kmax] = ktmp;
      }
    }
    /* compute the Householder transformation to reduce the
       j-th column of a to a multiple of the j-th unit vector. */

    ajnorm = lm_enorm(m - j, &am[j]);
    if (ajnorm == 0.) {
	    rdiag[j] = 0;
	    continue;
    }

    if (am[j] < 0.)
	    ajnorm = -ajnorm;
    for (i = j; i < m; i++)
	    am[i] /= ajnorm;
    am[j] += 1;
    
    /* apply the transformation to the remaining columns
       and update the norms. */

    for (k = j + 1; k < n; k++) {
	    double sum = 0;
      double *akm = a + k * m;
      
	    for (i = j; i < m; i++)
        sum += am[i] * akm[i];
      
	    double temp = sum / am[j];
      
	    for (i = j; i < m; i++)
        akm[i] -= temp * am[i];
      
	    if (pivot && rdiag[k] != 0.) {
        temp = akm[j] / rdiag[k];
        temp = MAX(0., 1 - temp * temp);
        rdiag[k] *= sqrt(temp);
        temp = rdiag[k] / wa[k];
        if (p05 * SQR(temp) <= LM_MACHEP) {
          rdiag[k] = lm_enorm(m - j - 1, &akm[j + 1]);
          wa[k] = rdiag[k];
        }
	    }
    }

    rdiag[j] = -ajnorm;
  }
}

/**
 *     Given an m by n matrix a, an n by n diagonal matrix d,
 *     and an m-vector b, the problem is to determine an x which
 *     solves the system
 *
 *	    a*x = b  and  d*x = 0
 *
 *     in the least squares sense.
 *
 *     This subroutine completes the solution of the problem
 *     if it is provided with the necessary information from the
 *     qr factorization, with column pivoting, of a. That is, if
 *     a*p = q*r, where p is a permutation matrix, q has orthogonal
 *     columns, and r is an upper triangular matrix with diagonal
 *     elements of nonincreasing magnitude, then qrsolv expects
 *     the full upper triangle of r, the permutation matrix p,
 *     and the first n components of (q transpose)*b. The system
 *     a*x = b, d*x = 0, is then equivalent to
 *
 *		   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.
 *
 *	@param n is a positive integer input variable set to the order of r.
 *
 *	@param 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.
 *
 *	@param ldr is a positive integer input variable not less than n
 *	  which specifies the leading dimension of the array r.
 *
 *	@param 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.
 *
 *	@param diag is an input array of length n which must contain the
 *	  diagonal elements of the matrix d.
 *
 *	@param qtb is an input array of length n which must contain the first
 *	  n elements of the vector (q transpose)*b.
 *
 *	@param x is an output array of length n which contains the least
 *	  squares solution of the system a*x = b, d*x = 0.
 *
 *	@param sdiag is an output array of length n which contains the
 *	  diagonal elements of the upper triangular matrix s.
 *
 *	@param wa is a work array of length n.
 *
 */
static void 
lm_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag,
		      double *qtb, double *x, double *sdiag, double *wa)
{
  int i, kk, j, k, nsing;
  double qtbpj, sum, temp;
  double _sin, _cos, _tan, _cot; /* local variables, not functions */

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

  for (j = 0; j < n; j++) {
    for (i = j; i < n; i++)
	    r[j * ldr + i] = r[i * ldr + j];
    x[j] = r[j * ldr + j];
    wa[j] = qtb[j];
  }
#if BUG
  printf("qrsolv\n");
#endif

/* qrsolv: eliminate the diagonal matrix d using a givens rotation. */

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

/* qrsolv: prepare the row of d to be eliminated, locating the
   diagonal element using p from the qr factorization. */

    if (diag[ipvt[j]] == 0.)
	    goto L90;
    for (k = j; k < n; k++)
	    sdiag[k] = 0.;
    sdiag[j] = diag[ipvt[j]];
    
/* qrsolv: the transformations to eliminate the row of d modify only 
   a single element of (q transpose)*b beyond the first n, which is
   initially 0.. */

    qtbpj = 0.;
    for (k = j; k < n; k++) {
      
      /* determine a givens rotation which eliminates the
         appropriate element in the current row of d. */
      
	    if (sdiag[k] == 0.)
        continue;
	    kk = k + ldr * k;
	    if (fabs(r[kk]) < fabs(sdiag[k])) {
        _cot = r[kk] / sdiag[k];
        _sin = 1 / sqrt(1 + SQR(_cot));
        _cos = _sin * _cot;
	    } else {
        _tan = sdiag[k] / r[kk];
        _cos = 1 / sqrt(1 + SQR(_tan));
        _sin = _cos * _tan;
	    }
      
      /* compute the modified diagonal element of r and
         the modified element of ((q transpose)*b,0). */

	    r[kk] = _cos * r[kk] + _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. */

	    for (i = k + 1; i < n; i++) {
        temp = _cos * r[k * ldr + i] + _sin * sdiag[i];
        sdiag[i] = -_sin * r[k * ldr + i] + _cos * sdiag[i];
        r[k * ldr + i] = temp;
	    }
    }
    
  L90:
    /* store the diagonal element of s and restore
       the corresponding diagonal element of r. */

    sdiag[j] = r[j * ldr + j];
    r[j * ldr + j] = x[j];
  }
  
/* qrsolv: solve the triangular system for z. if the system is
   singular, then obtain a least squares solution. */

  nsing = n;
  for (j = 0; j < n; j++) {
    if (sdiag[j] == 0. && nsing == n)
	    nsing = j;
    if (nsing < n)
	    wa[j] = 0;
  }
  
  for (j = nsing - 1; j >= 0; j--) {
    sum = 0;
    for (i = j + 1; i < nsing; i++)
	    sum += r[j * ldr + i] * wa[i];
    wa[j] = (wa[j] - sum) / sdiag[j];
  }
  
/* qrsolv: permute the components of z back to components of x. */

  for (j = 0; j < n; j++)
    x[ipvt[j]] = wa[j];
  
} /* lm_qrsolv. */

/** 
 *   Given an n-vector x, this function calculates the euclidean norm of x.
 *
 *   The euclidean norm is computed by accumulating the sum of squares in
 *   three different sums. The sums of squares for the small and large
 *   components are scaled so that no overflows occur. Non-destructive
 *   underflows are permitted. Underflows and overflows do not occur in the
 *   computation of the unscaled sum of squares for the intermediate
 *   components.  The definitions of small, intermediate and large components
 *   depend on two constants, LM_SQRT_DWARF and LM_SQRT_GIANT. The main
 *   restrictions on these constants are that LM_SQRT_DWARF**2 not underflow
 *   and LM_SQRT_GIANT**2 not overflow.

 *   @param n is a positive integer input variable.
 *   @param x is an input array of length n.
 */
static double 
lm_enorm(int n, double *x) {
  int i;
  double agiant, s1, s2, s3, xabs, x1max, x3max, temp;

  s1 = 0;
  s2 = 0;
  s3 = 0;
  x1max = 0;
  x3max = 0;
  agiant = LM_SQRT_GIANT / ((double) n);
  
  /* sum squares. */
  for (i = 0; i < n; i++) {
    xabs = fabs(x[i]);
    if (xabs > LM_SQRT_DWARF && xabs < agiant) {
      /*  sum for intermediate components. */
	    s2 += xabs * xabs;
	    continue;
    }

    if (xabs > LM_SQRT_DWARF) {
      /*  sum for large components. */
	    if (xabs > x1max) {
        temp = x1max / xabs;
        s1 = 1 + s1 * SQR(temp);
        x1max = xabs;
	    } else {
        temp = xabs / x1max;
        s1 += SQR(temp);
	    }
	    continue;
    }
    /*  sum for small components. */
    if (xabs > x3max) {
	    temp = x3max / xabs;
	    s3 = 1 + s3 * SQR(temp);
	    x3max = xabs;
    } else {
	    if (xabs != 0.) {
        temp = xabs / x3max;
        s3 += SQR(temp);
	    }
    }
  }
  
  /* calculation of norm. */

  if (s1 != 0)
    return x1max * sqrt(s1 + (s2 / x1max) / x1max);
  if (s2 != 0) {
    if (s2 >= x3max)
	    return sqrt(s2 * (1 + (x3max / s2) * (x3max * s3)));
    else
	    return sqrt(x3max * ((s2 / x3max) + (x3max * s3)));
  }
  
  return x3max * sqrt(s3);
  
} /* lm_enorm. */

