function [b,a]=invfreqz(g,w,nb,na,wf,maxiter,tol,pf)
%INVFREQZ  Discrete filter least squares fit to frequency response data.
%
%	[B,A] = INVFREQZ(H,W,NB,NA) gives real numerator and denominator 
%	coefficients B and A of orders NB and NA respectively, where
%	H is the desired complex frequency response of the system at frequency
%	points W, and W contains the frequency values in radians/s.
%
%	[B,A]=INVFREQZ(H,W,NB,NA,Wt) allows the fit-errors to the weighted
%	versus frequency.  LENGTH(Wt)=LENGTH(W)=LENGTH(H).
%	Determined by minimization of sum |B-H*A|^2*Wt over the freqs in W.
%
%	[B,A] = INVFREQZ(H,W,NB,NA,Wt,ITER) does another type of fit:
%	Sum |B/A-H|^2*Wt is minimized with respect to the coefficients in B and
%	A by numerical search in at most ITER iterations.  The A-polynomial is 
%	then constrained to be stable.  [B,A]=INVFREQZ(H,W,NB,NA,Wt,ITER,TOL)
%	stops the iterations when the norm of the gradient is less than TOL.
%	The default value of TOL is 0.01.  The default value of Wt is all ones.
%	This default value is also obtained by Wt=[].
%
%	[B,A]=INVFREQZ(H,W,NB,NA,Wt,ITER,TOL,'trace') will provide a textual
%	progress report of the iteration.
%
%	See also FREQZ, FREQS, and INVFREQS.

%	Author(s): J.O. Smith and J.N. Little, 4-23-86
%		   J.N. Little, 4-27-88, revised 
%		   Lennart Ljung, 9-21-92, rewritten
%		   T. Krauss, 10-19-92, trace mode made optional
%	Copyright (c) 1984-94 by The MathWorks, Inc.
%	$Revision: 1.11 $  $Date: 1994/01/25 17:59:21 $

nk=0;T=1; % The code is prepared for arbitrary sampling interval T and for
          % constraining the numerator to begin with nk zeros.

nb=nb+nk+1;
if nargin<5,wf=[];end
if nargin<8,
    verb=0;
elseif (strcmp(pf,'trace')),
    verb=1;
else
    error(['Trace flag ''' pf ''' not recognizeable']);
end
if isempty(wf),wf=ones(length(w),1);end
wf=sqrt(wf);

if length(g)~=length(w),error('The lengths of H and W must coincide'),end
if length(wf)~=length(w),error('The lengths of Wt and W must coincide'),end
if any(w>pi),disp('WARNING: W contains values over the Nyquist frequency!'),end
[rw,cw]=size(w);if rw>cw w=w';end
[rg,cg]=size(g);if cg>rg g=g.';end
[rwf,cwf]=size(wf);if cwf>rwf wf=wf';end

i=sqrt(-1);nm=max(na,nb+nk-1);
OM=exp(-i*[0:nm]'*w*T);
%
% Estimation in the least squares case:
%
    Dva=(OM(2:na+1,:).').*(g*ones(1,na));
    Dvb=-(OM(nk+1:nk+nb,:).');
    D=[Dva Dvb].*(wf*ones(1,na+nb));
    R=real(D'*D);
    Vd=real(D'*(-g.*wf));
    th=R\Vd;
    a=[1 th(1:na)'];b=[zeros(1,nk) th(na+1:na+nb)'];

if nargin<6,return,end

% Now for the iterative minimization
if isempty(maxiter), maxiter = 30; end

if nargin<7,
    tol=0.01;
elseif isempty(tol)
    tol = 0.01;
end
indb=1:length(b);indg=1:length(a);
a=polystab(a); % Stabilizing the denominator

% The initial estimate:

    GC=((b*OM(indb,:))./(a*OM(indg,:))).';
    e=(GC-g).*wf;
    Vcap=e'*e; t=[a(2:na+1) b(nk+1:nk+nb)]';
    if (verb),
        clc, disp(['  INITIAL ESTIMATE'])	
        disp(['Current fit: ' num2str(Vcap)])
        disp(['par-vector'])
        disp(t)
    end;
%
% ** the minimization loop **
%
    gndir=2*tol+1;l=0;st=0;
    while [norm(gndir)>tol l<maxiter st~=1]
        l=l+1;

%     * compute gradient *

        D31=(OM(2:na+1,:).').*(-GC./((a*OM(1:na+1,:)).')*ones(1,na));
        D32=(OM(nk+1:nk+nb,:).')./((a*OM(1:na+1,:)).'*ones(1,nb));
        D3=[D31 D32].*(wf*ones(1,na+nb));
%     * compute Gauss-Newton search direction

        e=(GC-g).*wf;
        R=real(D3'*D3);
        Vd=real(D3'*e);
        gndir=R\Vd;

%     * search along the gndir-direction *

        ll=0;,k=1;V1=Vcap+1;
        while [V1 > Vcap ll<20],

            t1=t-k*gndir; if ll==19,t1=t;end
	           a=polystab([1 t1(1:na)']);
            t1(1:na)=a(2:na+1)';   %Stabilizing denominator
	           b=[zeros(1,nk) t1(na+1:na+nb)'];
	           GC=((b*OM(indb,:))./(a*OM(indg,:))).';
	           V1=((GC-g).*wf)'*((GC-g).*wf); t1=[a(2:na+1) b(nk+1:nk+nb)]';
            if (verb),
                home, disp(int2str(ll))
            end;
            k=k/2;
            ll=ll+1; if ll==20, st=1;end
            if ll==10,gndir=Vd/norm(R)*length(R);k=1;end
        end

        if (verb),
            home
            disp(['      ITERATION # ' int2str(l)])
            disp(['Current fit:  ' num2str(V1) '  Previous fit:  ',...
                  num2str(Vcap)])
            disp(['Current par prev.par   GN-dir'])
            disp([t1 t gndir])
            disp(['Norm of GN-vector: ' num2str(norm(gndir))])
            if st==1, 
                disp(['No improvement of the criterion possible along ',...
                      'the search direction']), 
                disp('Iterations therefore terminated'),
            end
        end
        t=t1; Vcap=V1;
    end
