/*---------------------------------------------------------------------------
 * @(#) curvefit.i: non-linaear fitting in Yorick by Eric THIEBAUT.
 *---------------------------------------------------------------------------
 * 	$Id: curvefit.i,v 1.1 1995/12/18 11:45:07 eric Exp $
 *---------------------------------------------------------------------------
 * History:
 *	November 2, 1995 by Eric THIEBAUT: 1st version from IDL CURVEFIT.PRO
 *---------------------------------------------------------------------------
 */
require, "string.i";

func curvefit(function, x, y, w, &a,&sigma, &chisqr,
	      lambda=, itmax=, factor=, quiet=)
{
//+
// NAME:
//	CURVEFIT
// PURPOSE:
//	Non-linear least squares fit to a function of an
//	arbitrary number of parameters.
//	Function may be any non-linear function where
//	the partial derivatives are known or can be approximated.
// CATEGORY:
//	E2 - Curve and Surface Fitting
// CALLING SEQUENCE:
//	YFIT = CURVEFIT(X,Y,W,A,SIGMA)
// INPUTS:
//	X = Row vector of independent variables.
//	Y = Row vector of dependent variable, same length as x.
//	W = Row vector of weights, same length as x and y.
//		For no weighting: w(i) = 1.,
//		instrumental weighting w(i) = 1./y(i),
//		statistical weighting w(i) = 1./Var(y(i)),
//		etc.
//	A = Vector of nterms length containing the initial estimate
//		for each parameter.
//
// OUTPUTS:
//	A = Vector of parameters containing fit.
//	Function result = YFIT = Vector of calculated
//		values.
// OPTIONAL OUTPUT PARAMETERS:
//	Sigma = Vector of standard deviations for parameters
//		A.
//	
// COMMON BLOCKS:
//	NONE.
// SIDE EFFECTS:
//	The function to be fit must be defined and called FUNCT.
//	For an example see FUNCT in the IDL User's Libaray.
//	Call to FUNCT is:
//	FUNCT,X,A,F,PDER
// where:
//	X = Vector of NPOINT independent variables, input.
//	A = Vector of NTERMS function parameters, input.
//	F = Vector of NPOINT values of function, y(i) = funct(x(i)), output.
//	PDER = Array, (NPOINT, NTERMS), of partial derivatives of funct.
//		PDER(I,J) = DErivative of function at ith point with
//		respect to jth parameter.  Optional output parameter.
//		PDER should not be calculated if parameter is not
//		supplied in call (Unless you want to waste some time).
// RESTRICTIONS:
//	NONE.
// PROCEDURE:
//	Copied from "CURFIT", least squares fit to a non-linear
//	function, pages 237-239, Bevington, Data Reduction and Error
//	Analysis for the Physical Sciences.
//
//	"This method is the Gradient-expansion algorithm which
//	compines the best features of the gradient search with
//	the method of linearizing the fitting function."
//
//	Iterations are perform until the chi square changes by
//	only 0.1% or until 15 attempts at minimisation (with maximum of
//	20 iterations) have been performed.
//
//	The initial guess of the parameter values should be
//	as close to the actual values as possible or the solution
//	may not converge.
//
// MODIFICATION HISTORY:
//	Written, DMS, RSI, September, 1982.
//	       Amended to output CHISQR, and to set A=0 if no convergence
//	T.J.Harris, Dept. of Physics, University of Adeliade,  August 1990.
//-
    itmax  = scalar(itmax,  20,   type=long,   gt=1,  arg="ITMAX");
    factor = scalar(factor, 10.,  type=double, gt=1., arg="FACTOR");
    lambda = scalar(lambda, 1e-3, type=double, gt=0., arg="LAMBDA");
    verbose = !anyof(quiet);

    a = double(a);	// make params floating-point
    nterms = numberof(a);

    // Degrees of freedom:
    nfree = min(numberof(y), numberof(x)) - nterms;
    //nfree = numberof(where(w * (y + !y) != 0.) ) - nterms;
    if (nfree <= 0)
	error, "curvefit - not enough data points.";
    
    //Subscripts of diagonal elements:
    diag = indgen(1:nterms^2:nterms+1);

    iter=0;
    do {

	if (++iter > itmax)
	    error, "exceeded maximum number of iterations";
	
	//
	// Compute Alpha and Beta matrices:	
	//
        yfit = function(x, a, pder, deriv=1);
        beta = (w * (y - yfit))(+) * pder(+,);
        alpha = w(+) * (pder(,,-) * pder(,-,))(+,..);
	c = alpha(diag);
	c = sqrt(c(,-) * c(-,));
	
	// Present chi squared:
        chisq1 = sum(w*(y-yfit)^2)/nfree;
	if (chisq1 == 0.0) {
	    write, "CURVEFIT: WARNING Chi2 maybe too small to be true!";
	    break;
	}
	if (verbose)
	    write, format=" %3d: Chi2 =%g", iter, chisq1;

	//
	// Invert modified curvature matrix to find new parameters.
	//
	count = 0;
        do {
	    if (++count > 20)
		error, "cannot improve Chi2";
	    
            d = alpha / c;
            d(diag) = 1.+lambda;
            d = LUsolve(d);
            b = a + (d / c)(,+) * beta(+);	// new parameters
	    yfit = function(x, b);
	    chisqr = sum(w * (y - yfit)^2) / nfree;	//new chisqr
	    lambda *= factor;				//assume fit got worse
	    if (verbose)
		write, format=" %g", chisqr;
	    
        } while (chisqr >= chisq1);
	
	if (verbose)
	    write, "\n";
	
        lambda /= factor^2;	//decrease lambda by factor
        a = b;			//save new parameter estimate

    } while ((chisq1-chisqr)/chisq1 > .001);

    sigma = sqrt(d(diag)/alpha(diag));	//return sigma's
    return yfit;
}