function [aw,bw,cw,dw,magfit] = fitd(threl, freq, n, blksz,flag)
% [SS_D,LOGDFIT] = FITD(LOGD,W,N,BLKSZ,FLAG)
% [AD,BD,CD,DD,LOGDFIT] = FITD(LOGD,W,N,BLKSZ,FLAG) produces a stable,
%   minimum-phase state-space realization SS_D of a diagonal transfer
%   function matrix such that the diagonal elements' magnitude bode plots 
%   approximately fit Bode magnitude plot data given in the rows of the
%   matrix LOGD.  Uses the the numerically stable routine "YULEWALK.M".
%
%   Input:     LOGD  matrix whose rows are logs of magnitude bode plots;
%              W frequency vector.
%   Optional:  N vector of desired orders of the approximants (default=0);
%              BLKSZ vector of sizes diagonal blocks in SS_D (default = 1);
%              FLAG (default: display Bode plot; 0: no Bode plot);
%              Bode plot of the fit will be displayed 4 at a time
%              with a "pause" in between every full screen display.
%   Output:    SS_D is a state-space realization of the diagonal transfer
%              function matrix  D(s) =diag(d1(s)I1,...,dn(s)In) where
%              di(s) is the fit to the i-th row of LOGD and I1,...,In are
%              identity matrices of dimensions given in the n-vector BLKSZ.
%              LOGDFIT contains the fit to LOGD, i.e., SS_D's bode plot.

% by Weizheng Wang (University of Southern California)
% with enhancements by
% R. Y. Chiang & M. G. Safonov 
% Copyright (c) 1988 by the MathWorks, Inc.
% All Rights Reserved.
% ------------------------------------------------------------------------

[mmm,nnn] = size(threl);
if mmm > nnn
   threl = threl';
   [mmm,nnn] = size(threl);
end
thre=exp(threl);  
threl=threl/log(10);     % from log to log10
freql = log10(freq);

bodeflag=0;
magfit=[];
if nargout==2 | nargout==5, bodeflag == 1; end

nfreq=max(size(freq));
if nnn ~= nfreq,
   error('MAG and W must have the same number of points')
end

if nargin<3, n=zeros(1,mmm); end
if max(size(n))==1, n=n*ones(1,mmm); end

if nargin <5
   bodeflag=1;% compute bode plot of fit
   flag = 1;  % display bode plot of fit
   if nargin<4,blksz=ones(1,mmm);end
end
if max(size(n))==0, n=zeros(1,mmm); end
if max(size(blksz))==0, blksz=ones(1,mmm); end
if max(size(flag))==0,
   bodeflag=1;% compute bode plot of fit
   flag = 1;  % display bode plot of fit
end


% if scalar blksz, make it a vector
if max(size(blksz))==1, blksz=blksz*ones(1,mmm);end

% Find the best sampling time "ff so that the frequency mapping from "s"
% to "z" is approximately linear between 0 and Pi:
%
mx=max(freql);  mn=min(freql);  
ff=10^(mn+2);
ff=ff/3^(4-mx+mn); % ff is approx. sqrt(max(freq)/min(freq));

%
% Bilinear mapping of the frequency axis from "z" to "s":
%
z=[0:126]/126*pi;
j=sqrt(-1);
w=imag(ff*(exp(j*z)-1)./(exp(j*z)+1));
wl(1)=log10(freq(1));
[x,y]=size(freq);
x=max(x,y);
wl(127)=log10(freq(x));
wl(2:126)=log10(w(2:126));
for i=2:126, if wl(i)>wl(127), wl(i)=wl(127); end; end;
for i=2:126, if wl(i)<wl(1),  wl(i)=wl(1); end; end;
z=z/pi;
% 


% Curve fit the continuous THREL magnitude data, one row at a time:
%
% Now prepare to curve fit THREL magnitude data, one row at a time:

loop = mmm;

if loop ~= max(size(n)),
   error('Number of rows of MAG must equal the dimension of the vector N')
end
count = 0;

for i = 1:loop
  mag =thre(i,:);
  magl = threl(i,:);
  nl = n(1,i);
  count = count + 1;
%      

  if nl==0
    ad=1;
    bd = exp(log(10)*polyfit(freql, magl, 0));
  else
  
  % First do a high order least squares polynomial curve fit to the
  % loglog bode magnitude plot data in ith row of THREL:
    y = nl*2;
    if y < 8, y = 8; end
    if y>nfreq/2, y=round(nfreq/2); end
    if y<1, y=1; end; if nfreq>100, y=10; end;
    p = polyfit(freql, magl, y);

  % 
  % Now obtain a lower order approximation in z-plane via YULEWALK.M:
  %
    dth = polyval(p, wl);
    dth = exp(log(10)*dth);
    dth(1)=mag(1);
    dth(127)=mag(x);	
    [bd,ad]=yulewalk(nl,z,dth);
    % make sure the fitting is minimum phase
    bd = polystab(bd);
  end

% Discrete time state-space (az,bz,cz,dz):
%
  [az,bz,cz,dz]=tf2ss(bd,ad);
%
% Convert the discrete state-space to W-plane:
%
  if nl>0,
    [aw0,bw0,cw0,dw0]=bilin(az,bz,cz,dz,-1,'Tustin',2/ff);
  else
    aw0=[];bw0=[];cw0=[];dw0=dz;
  end
  for j=1:blksz(i);
     if i == 1 & j==1
        aw = aw0; bw = bw0; cw = cw0; dw = dw0;
     else
        [aw,bw,cw,dw] = append(aw,bw,cw,dw,aw0,bw0,cw0,dw0);
     end
  end

%
% Compute and display (if flag==1) the frequency response and fit
%
  if bodeflag==1,
     if nl==0,
         aw0=-1;bw0=0; cw0=0;
     end
     [ga_w0,ph0] = bode(aw0,bw0,cw0,dw0,1,freq);
     magfit=[magfit; log(ga_w0')];
     if flag==1,
        if loop > 1
           if count==1, clg; subplot(221); end;
	   if count==2, subplot(222); end;
           if count==3, subplot(223); end;
           if count==4, subplot(224); end;
        end;
        loglog(freq,mag,'x',freq,ga_w0);
        title([ 'YULE-WALKER ORDER ' num2str(n(i)) ' FIT']);
        xlabel('R/S (x: data; solid: fit)')
        ylabel(['Mag D' num2str(i)]);
     end
  end
  if count == 4     % reset the counter after four loops
     count = 0;
     pause          % pause the 4-window plot on the screen
     clg
  end
%
end  % of the loop
%
if nargout <3,
    ss_w = mksys(aw,bw,cw,dw);
    aw = ss_w;
    bw=magfit;
end
%
% -------- End of FITD.M % WW/RYC/MGS %




