function [f1,p,GRAD,OPTIONS] = leastsqf(x,y,pin,wt,FUN,OPTIONS)
%LEASTSQF Solves the non-linear least squares problem for parameter estimation:
%function [f1,p,GRAD,OPTIONS] = leastsqf(x,y,pin,wt,FUN,OPTIONS)
%	min  sum {FUN(x,p).^2}	where  x may be vectors or matrix  .
%	      p
%
%function [f1,p,GRAD,OPTIONS] = leastsqf(x,y,pin,wt,FUN,OPTIONS)
%	starts at the vector pin and finds a minimum to the
%	sum of squares of the functions described in FUN. FUN is usually
%	an  M-file which returns an objective functions: F=FUN(x,p).
%
%	OPTIONS(2) is a measure of the precision required for the
%	values of p at the solution. OPTIONS(3) is a measure of the precision
%	required of the objective function at the solution. See HELP FOPTIONS. 
%
%	The default algorithm is the Levenberg-Marquardt method with a 
%	mixed quadratic and cubic line search procedure.  A Gauss-Newton
%	method is selected by setting  OPTIONS(5)=1. 
%
% x=vec or matrix of indep variables, 1 row/observation: x(i)=[x1(i)..xn(i)]
% y=vec of observed values, same no. of rows as x.
% e.g. y=p0 +p1*x1 +p2*x1^2 +p3*x2 +p4/x2 ==> y=f([x1 x2],[p0..p4])
% wt=vec(dim=1 or length(y))of stat. wts:(sqrts of var(y))^-1; default=1
% pin=vector of initial parameters to be adjusted by leasqr.
% FUN=name of function in quotes,of the form y=f(x,p) y MUST RETURN AS COL
%	    OUTPUT VARIABLES
% f1=vec function values computed in function func.
% p=vec of final parameters. i.e, the solution.
% GRAD = Jacobian Matrix at Final Parameter values length(p)* length(f)
%       Note: GRAD is scaled by wt. True Jac=GRAD*diag(wt)^(-1)**
% OPTIONS - SEE HELP FOPTIONS
%
%	Copyright (c) 1990 by the MathWorks, Inc.
%	Andy Grace 7-9-90.
%     MODIFIED FOR LS PARAMETER ESTIMATION (A.J. apr 91)

%Arthur Jutan 
%Dept of Chemical and Biochemical Enginering
%Univ of Western Ontario
%London Ontario Canada, N6A 5B9

%

% ------------Initialization----------------
XOUT=pin(:);p=pin(:);
[nvars]=length(XOUT);

evalstr = [FUN];
if ~any(FUN<48)
	evalstr=[evalstr, '(x,p)'];
end
evalstr=['(y-', evalstr,').*wt']; % form weighted error(NB scales Jacobian)**

if nargin < 6, OPTIONS=[]; end
	       GRADFUN=[]; %force no analytical grad (jacobian) calc

if length(GRADFUN)
	evalstr2 = [GRADFUN];
	if ~any(GRADFUN<48)
		evalstr2 = [evalstr2, '(x'];
		for i=1:nargin - 4
		    evalstr2 = [evalstr2,',P',num2str(i)];
		end
		evalstr2 = [evalstr2, ')'];
	end
end

f = eval(evalstr);
f=f(:);
nfun=length(f);
GRAD=zeros(length(XOUT),nfun);
OLDX=XOUT;
MATX=zeros(3,1);
MATL=[f'*f;0;0];
OLDF=f'*f;
FIRSTF=f'*f;
[OLDX,OLDF,OPTIONS]=lsint(XOUT,f,OPTIONS);
EstSum=0.5;
GradFactor=0; 
CHG = 1e-7*abs(XOUT)+1e-7*ones(nvars,1);

OPTIONS(10)=1;
status=-1;

while status~=1

% Work Out Gradients
	if ~length(GRADFUN) | OPTIONS(9)
		OLDF=f;
		CHG = sign(CHG+eps).*min(max(abs(CHG),OPTIONS(16)),OPTIONS(17));
		for gcnt=1:nvars
		    temp = XOUT(gcnt);
		    XOUT(gcnt) = temp +CHG(gcnt);
		    p(:) = XOUT;
		    f(:) = eval(evalstr);
		    GRAD(gcnt,:)=(f-OLDF)'/(CHG(gcnt));
		    XOUT(gcnt) = temp;
		end
		f = OLDF;
		OPTIONS(10)=OPTIONS(10)+nvars;
% Gradient check
		if OPTIONS(9) == 1
		    GRADFD = GRAD;
		    p(:)=XOUT; GRAD = eval(evalstr2);
		    graderr(GRADFD, GRAD, evalstr2);
		    OPTIONS(9) = 0;
		end
	else
		p(:) = XOUT;
		OPTIONS(11)=OPTIONS(11)+1;
		GRAD = eval(evalstr2);
	end
	% Try to set difference to 1e-8 for next iteration
	if nfun==1
		CHG = nfun*1e-8./GRAD;
	else
		CHG = nfun*1e-8./sum(abs(GRAD)')';
	end

	gradf = 2*GRAD*f;
	fnew = f'*f;
%---------------Initialization of Search Direction------------------
	if status==-1
		if cond(GRAD)>1e8
		    SD=-(GRAD*GRAD'+(norm(GRAD)+1)*(eye(nvars)))\(GRAD*f);
		    if OPTIONS(5)==0, GradFactor=norm(GRAD)+1; end
		    how='COND';
		else
%		SD=GRAD'\(GRAD'*X-F)-X;
		    SD=-(GRAD*GRAD'+GradFactor*(eye(nvars)))\(GRAD*f);
		end
		FIRSTF=fnew;
		OLDG=GRAD;
		GDOLD=gradf'*SD;
		OPTIONS(18)=1;
		if OPTIONS(1)>0
		    disp([sprintf('%5.0f %12.3g %12.3g ',OPTIONS(10),fnew,OPTIONS(18)),sprintf('%12.3g  ',GDOLD)]);
		end
		XOUT=XOUT+OPTIONS(18)*SD;
		if OPTIONS(5)==0
		    newf=GRAD'*SD+f;
		    GradFactor=newf'*newf;
		    SD=-(GRAD*GRAD'+GradFactor*(eye(nvars)))\(GRAD*f); 
		end
		newf=GRAD'*SD+f;
		XOUT=XOUT+OPTIONS(18)*SD;
		EstSum=newf'*newf;
		status=0;
		if OPTIONS(7)==0; PCNT=1; end
		
	else
%-------------Direction Update------------------
		gdnew=gradf'*SD;
		if OPTIONS(1)>0, 
		    num=[sprintf('%5.0f %12.3g %12.3g ',OPTIONS(10),fnew,OPTIONS(18)),sprintf('%12.3g  ',gdnew)];
		end
		if gdnew>0 & fnew>FIRSTF

% Case 1: New function is bigger than last and gradient w.r.t. SD -ve
% ... interpolate. 
		    how='inter';
		    [stepsize]=cubici1(fnew,FIRSTF,gdnew,GDOLD,OPTIONS(18));
		    OPTIONS(18)=0.9*stepsize;
		elseif fnew<FIRSTF

%  New function less than old fun. and OK for updating 
%         .... update and calculate new direction. 
		    [newstep,fbest] =cubici3(fnew,FIRSTF,gdnew,GDOLD,OPTIONS(18));
		    if fbest>fnew,fbest=0.9*fnew; end 
		    if gdnew<0
		        how='incstep';
		        if newstep<OPTIONS(18),  newstep=(2*OPTIONS(18)+1e-4); how=[how,'IF']; end
		        OPTIONS(18)=abs(newstep);
		    else 
		        if OPTIONS(18)>0.9
		            how='int_step';
		            OPTIONS(18)=min([1,abs(newstep)]);
		        end
		    end
% SET DIRECTION.
% Gauss-Newton Method    
		    temp=1;
		    if OPTIONS(5)==1 
		        if OPTIONS(18)>1e-8 & cond(GRAD)<1e8
		            SD=GRAD'\(GRAD'*XOUT-f)-XOUT;
		            if SD'*gradf>eps,how='ERROR- GN not descent direction',  end
		            temp=0;
		        else
		            if OPTIONS(1) > 0
		                disp('Conditioning of Gradient Poor - Switching To LM method')
		            end
		            how='CHG2LM';
		            OPTIONS(5)=0;
		            OPTIONS(18)=abs(OPTIONS(18));				
		        end
		    end
		    
		    if (temp)      
% Levenberg_marquardt Method N.B. EstSum is the estimated sum of squares.
%                                 GradFactor is the value of lambda.
% Estimated Residual:
		        if EstSum>fbest
		            GradFactor=GradFactor/(1+OPTIONS(18));
		        else
		            GradFactor=GradFactor+(fbest-EstSum)/(OPTIONS(18)+eps);
		        end
		        SD=-(GRAD*GRAD'+GradFactor*(eye(nvars)))\(GRAD*f); 
		        OPTIONS(18)=1; 
		        estf=GRAD'*SD+f;
		        EstSum=estf'*estf;
		        if OPTIONS(1)>0, num=[num,sprintf('%12.3g ',GradFactor)]; end
		    end
		    gdnew=gradf'*SD;

		    OLDX=XOUT;
% Save Variables
		    FIRSTF=fnew;
		    OLDG=gradf;
		    GDOLD=gdnew;	

		    % If quadratic interpolation set PCNT
		    if OPTIONS(7)==0, PCNT=1; MATX=zeros(3,1);  MATL(1)=fnew; end
		else 
% Halve Step-length
		    how='Red_Step';
		    if fnew==FIRSTF,
		        if OPTIONS(1)>0,
		            disp('No improvement in search direction: Terminating')
		        end
		        status=1;
		    else
		        OPTIONS(18)=OPTIONS(18)/8;
		        if OPTIONS(18)<1e-8
		            OPTIONS(18)=-OPTIONS(18);
		        end
		    end
		end
		XOUT=OLDX+OPTIONS(18)*SD;
		if OPTIONS(1)>0,disp([num,how]),end

	end %----------End of Direction Update-------------------
	if OPTIONS(7)==0, PCNT=1; MATX=zeros(3,1);  MATL(1)=fnew; end
% Check Termination 
	if max(abs(SD))< OPTIONS(2) & (gradf'*SD) < OPTIONS(3) & max(abs(gradf)) < 10*(OPTIONS(3)+OPTIONS(2))
		if OPTIONS(1) > 0
		    disp('Optimization Terminated Successfully')  
		end
		status=1; 
	elseif OPTIONS(10)>OPTIONS(14)
		disp('maximum number of iterations has been exceeded');
		if OPTIONS(1)>0
		    disp('Increase OPTIONS(14)')
		end
		status=1;
	else

% Line search using mixed polynomial interpolation and extrapolation.
		if PCNT~=0
		    while PCNT > 0
			p(:) = XOUT; f(:)  = eval(evalstr); OPTIONS(10)=OPTIONS(10)+1;
		        fnew = f'*f;
		        if fnew<OLDF'*OLDF, OX = XOUT; OLDF=f; end
		        [PCNT,MATL,MATX,steplen,fnew,how]=searchq(PCNT,fnew,OLDX,MATL,MATX,SD,GDOLD,OPTIONS(18),how);
		        OPTIONS(18)=steplen;
		        XOUT=OLDX+steplen*SD;
		        if fnew==FIRSTF,  PCNT=0; end
		    end
		    XOUT = OX;
		    f=OLDF;
		else
		    p(:)=XOUT; f(:) = eval(evalstr); OPTIONS(10)=OPTIONS(10)+1;
		end
	end
end
OPTIONS(8) = fnew;
XOUT=OLDX;
p(:)=XOUT; %param vector
f1(:)=y(:)-f(:)./wt; % calc y=f(x,p) from weighted error vector f(:)
