function [b,a]=invfreqs(g,w,nb,na,wf,maxiter,tol,pf)
%INVFREQS Analog filter least squares fit to frequency response data.
%
%	[B,A] = INVFREQS(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]=INVFREQS(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] = INVFREQS(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]=INVFREQS(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]=INVFREQS(H,W,nb,na,Wt,ITER,TOL,'trace') will provide a textual
% 	progress report of the iteration.
%
%	See also FREQZ, FREQS, and INVFREQZ.

%	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-22-92, trace mode made optional
%	Copyright (c) 1984-94 by The MathWorks, Inc.
%	$Revision: 1.11 $  $Date: 1994/01/25 17:59:20 $

nk=0;     % The code is prepared 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

[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+1,nb+nk);
indb=nb:-1:1; indg=na+1:-1:1; inda=na:-1:1;

   OM=ones(1,length(w));
   for kom=1:nm-1
       OM=[OM;(i*w).^kom];
   end

%
% Estimation in the least squares case:
%
    Dva=(OM(inda,:).').*(g*ones(1,na));
    Dvb=-(OM(indb,:).');
    D=[Dva Dvb].*(wf*ones(1,na+nb));
    R=real(D'*D);
    Vd=real(D'*((-g.*OM(na+1,:).').*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
% Stabilizing the denominator:

if na>0,v=roots(a);vind=find(real(v)>0);v(vind)=-v(vind);a=real(poly(v));end

% 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(inda,:).').*(-GC./((a*OM(indg,:)).')*ones(1,na));
    D32=(OM(indb,:).')./((a*OM(indg,:)).'*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=[1 t1(1:na)'];
	b=[zeros(1,nk) t1(na+1:na+nb)'];
        if na>0,v=roots(a);vind=find(real(v)>0);v(vind)=-v(vind);a=real(poly(v));
	end,t1(1:na)=a(2:na+1)'; % Stabilizing the denominator
	GC=((b*OM(indb,:))./(a*OM(indg,:))).';
	V1=((GC-g).*wf)'*((GC-g).*wf);
        if (verb),
	    home, disp(int2str(ll))
        end;
	k=k/2;
	ll=ll+1; if ll==10, gndir=Vd/norm(R)*length(R);k=1;end
	          if ll==20,st=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
