% The function  BODEAS returns the Bode asymptotic diagrams of the LTI
% system defined by the input Ws (as tf or ss). The asymptotic diagrams are
% compared with those yielded by BODE function.
 
function bodeas(Ws)

% Line width of the diagrams
lwidth=3;

% Definition of the transfer function
s=tf('s');

% Transform to zpk form
Ws_zpk=zpk(Ws);

% Zeros and poles vectors
zv=Ws_zpk.z{1};
pv=Ws_zpk.p{1};

% Number of zeros/poles in the origin
nz0=sum(zv==0);
np0=sum(pv==0);
nzp0=nz0+np0;

% Build a matrix of singular values where each row is
% (abs value, singular value, type) and type={0,1} (1=zero, -1=pole)
SVM=[abs(zv), ones(length(zv),1);
    abs(pv), -ones(length(pv),1)];

if ~isempty(SVM)
    % Sort the matrix SVM
    SVM=sortrows(SVM,1);
end

% Build the matrix for phase diagram
SVP=[];
for i=1:length(zv)
    if zv(i)~=0
        SVP=[SVP;abs(zv(i))/10 -sign(real(zv(i)));abs(zv(i))*10 sign(real(zv(i)))];
    end
end
for i=1:length(pv)
    if pv(i)~=0
        SVP=[SVP;abs(pv(i))/10 sign(real(pv(i)));abs(pv(i))*10 -sign(real(pv(i)))];
    end
end

if ~isempty(SVP)
    % Sort the matrix SVP
    SVP=sortrows(SVP,1);
end

% Draw the magnitude diagram
figure 
subplot(2,1,1)

% Create new axes with logarithmic w axis
h1=semilogx(1,0);

% Hold the plot
hold on

% Draw the grid
grid

delete(h1)

% If there are singular values in the origin
% delete the corresponding rows from SVM matrix
if nzp0
    SVM=SVM(nzp0+1:end,:);
end

% Counter
ctr=1;

% Extended vector of singular values
if isempty(SVM)
    % If there are no singular values, w ranges from 0.1 to 10
    wv=[0.1,10];
else
    wv=[SVM(1,1)/200;SVM(:,1);SVM(end,1)*200];
end
    
% The initial slope is determined by the number of zeros/poles in the
% origin
slop=20*(nz0-np0);

M_as=[];
M_as=[M_as; 20*log10(abs(dcgain(Ws*s^np0/s^nz0)))+slop*log10(wv(1))];
    
for ctr=1:size(SVM,1)
   
    if slop~=0
        M_as=[M_as;M_as(end)+slop*(log10(SVM(ctr,1))-log10(wv(ctr)))];
    else
        M_as=[M_as;M_as(end)];
    end
    
    % Compute new slope
    slop=slop+SVM(ctr,2)*20;
    %semilogx(wv,Mv,'b','LineWidth',lwidth);    
end
   
% Vector of n_pt points logarithmically spaced
if slop~=0
    M_as=[M_as; M_as(end)+slop*(log10(wv(end))-log10(wv(end-1)))];
else
    M_as=[M_as;M_as(end)];
end

semilogx(wv,M_as,'b-.','LineWidth',lwidth);

% Draw the phase diagram
subplot(2,1,2)
h2=semilogx(1,0);
hold on
grid
delete(h2)

% Counter
ctr=1;

% Extended vector of singular values
if isempty(SVP)
    % If there are no singular values, w ranges from 0.1 to 10
    wv=[0.01,100];
else
    wv=[SVP(1,1)/2; SVP(:,1); SVP(end,1)*2];
end

% Bounds of w axis
wmin=wv(1)*0.1;
wmax=wv(end)*10;

% The initial phase value is determined by the number of 
% zeros/poles in the origin and by the sign of the gain
if dcgain(Ws*s^np0/s^nz0)>0
    phase_in=90*(nz0-np0);
else
    phase_in=90*(nz0-np0)-180;
end

% Vector of phase values
P_as=phase_in;

% The initial phase diagram slope is always zero
slop=0;

for ctr=1:size(SVP,1)
    if slop~=0
        P_as=[P_as; P_as(end)+slop*(log10(SVP(ctr,1))-log10(wv(ctr)))];
    else
        P_as=[P_as; P_as(end)];
    end
    
    % Compute new slope
    slop=slop+SVP(ctr,2)*45;   
end

P_as=[P_as; P_as(end)];
P_as=[P_as(1); P_as; P_as(end)];
wv=[wv(1)*0.1; wv; wv(end)*10];
semilogx(wv,P_as,'b-.','LineWidth',lwidth);

% Plot real magnitude diagram
wv=logspace(log10(wmin),log10(wmax),200);
[Mw,Pw]=bode(Ws_zpk,wv);

subplot(2,1,1)
semilogx(wv,20*log10(squeeze(Mw)),'r-','LineWidth',lwidth);

mlim=axis;
axis([wmin,wmax,mlim(3),mlim(4)+5]);

subplot(2,1,2)
semilogx(wv,squeeze(Pw),'r-','LineWidth',lwidth);

plim=axis;
axis([wmin,wmax,plim(3),plim(4)+5]);