function er_plot(f,N,fs,varargin)
% function er_plot(f,N,fs,varargin)
%   Function to compute output and generate plots for given input transfer functions.
%   Plots generated are: 1) Magnitude response (specify 'dB' or 'mag' in varargin{1}), 
%   2) Phase response (in rads, specify 'normal' or 'unwrap' in varagin{2}), 3) Impulse 
%   repsonse, 4) Pole/Zero plot.  Takes transfer function parameters in struct F, length 
%   of two-sided impulse repsonse in N, sampling frequency in FS.  Last
%   input argument renames figure window to specified name (e.g., 'Figure Name').
%
%   EXAMPLE:
%   >> er_plot(f,128,1000,'db','unwrap','1')
%   
%   This command generates plots for the transfer function coefficients in
%   F, with a sample response length of 128, a sampling frequency of 1KHZ,
%   the magnitude response plotted in DB, the phase response is UNWRAPPED,
%   and the figure is given the name '1'.
%
%   Author:  Evan Ruzanski, CU-Boulder, ECEN5632 MATLAB assignment, FA2004
warning off

% Handle variable input arguments
magtype = upper(varargin{1}); 
phasetype = upper('normal');
if ((nargin == 5) | (nargin == 6))
    phasetype = upper(varargin{2});
end

% Unpack transfer functions
numd = f(1).tf_complete;
dend = f(2).tf_complete;

% Set figure, color properties and name
figure
set(gcf,'Color','White')
figs = get(0,'children');
figname = varargin{3};
figs = figs(find(strcmp(get(figs,'type'),'figure')));
figs = figs(find(strcmp(get(figs,'name'),figname)));
H = gcf;
set(H,'name',figname);

%%%%% Plot I:  Magnitude response of transfer function %%%%%

% I.A: Compute magnitude and phase response values
dend = dend(:).';
numd = numd(:).';
ndend = max(size(dend));
nnumd = max(size(numd));
fftn = 1024; % Take default 2^10 pt. FFT
ss = 2; % For half of unit circle
w = (0:fftn-1)'*2*pi/fftn/ss; % Set frequency vector
nfft = lcm(fftn,max(ndend,nnumd));
H1 = (fft([numd zeros(1,ss*nfft - nnumd)]) ./ fft([dend zeros(1,ss*nfft - ndend)])).'; % Perform FFT
H1 = H1(1+(0:fftn-1)*nfft/fftn);
switch upper(magtype) % Generate magnitude response vector
    case {'DB'}
        H = 20*log10(abs(H1));
    case {'MAG'}
        H = abs(H1);
end

% I.B: Generate magnitude plots
subplot(2,2,1)
ff  = w*fs/2/pi;
plot(ff,H);
grid
title('Magnitude spectrum, |H(e^{j\theta})|','FontName','Arial','FontSize',14)
xlabel('Hz','FontName','Arial','FontSize',10,'FontWeight','Bold')
switch upper(magtype)
    case {'DB'}
        ylabel('dB','FontName','Arial','FontSize',10,'FontWeight','Bold')
    case {'MAG'}
        ylabel('Amplitude','FontName','Arial','FontSize',10,'FontWeight','Bold')
end

%%%%% Plot II:  Phase response of transfer function (in rads) %%%%%

% II.A: Generate phase plot (use data from previous section)
P = angle(H1); % Generate phase response vector
subplot(2,2,2)
switch upper(phasetype)
    case {'UNWRAP'}
        P = unwrap(P);
        plot(ff,P);
    case {'NORMAL'}
        plot(ff,P);
end
grid
title('Phase spectrum, arg[H(e^{j\theta})]','FontName','Arial','FontSize',14)
xlabel('Hz','FontName','Arial','FontSize',10,'FontWeight','Bold')
ylabel('radians','FontName','Arial','FontSize',10,'FontWeight','Bold')

%%%%% Plot III:  Impulse response of transfer function %%%%%

% III.A: Compute impulse repsonse values

% Decompose given transfer function into causal and anticausal sections using partial fraction decomposition

%%%%% Create denominator polynomials %%%%%
lzc = shiftcheck(dend); % Strip leading/trailing zeros from shifts only
tzc = shiftcheck(fliplr(dend));
p = roots(dend); % Find poles
if (isempty(p) == 1) % Set (assumed causal) FIR case
    denc = dend;
    denac = [1];
    pc = [];
    pac = [];
elseif (lzc == tzc) % Set IIR case
    pm = abs(p); % Separate poles of causal, anticausal sections
    pc = [];
    pac = [];
    for k = 1:length(p)
        if (pm(k) < 1) % No poles at zero => trailing zero in denominator
            pc = [pc ; p(k)]; % Column vector of causal poles
        else
            pac = [pac ; p(k)]; % Column vector of anticausal poles
        end
    end
elseif (lzc ~= tzc)
    pm = abs(p); % Separate poles of causal, anticausal sections
    pc = [];
    pac = [];
    for k = 1:length(p)
        if ((pm(k) < 1) & (pm(k) ~= 0)) % No poles at zero => trailing zero in denominator
            pc = [pc ; p(k)] ;% Column vector of causal poles
        elseif (pm(k) ~= 0)
            pac = [pac ; p(k)]; % Column vector of anticausal poles
        end
    end
end
denc = poly(pc); % Causal section, ascending powers of z^(-n)
denac = poly(1./pac); % Anticausal section, ascending powers of z^(n)

% Ensure equal lengths
if (length(denc) > length(denac)) 
    denac = [denac zeros(1,length(denc)-length(denac))]; % Trailing zeros does not change tf
elseif (length(denac) > length(denc))
    denc = [denc zeros(1,length(denac)-length(denc))];
else
    denc = denc;
    denac = denac;
end

%%%%% Create numerator polynomials %%%%%
lzc = shiftcheck(numd); % Strip leading/trailing zeros from shifts only
tzc = shiftcheck(fliplr(numd));
if (lzc ~= tzc)
    numd2 = numd(lzc + 1:length(numd)-tzc);
else
    numd2 = numd;
end
if ((isempty(pc) == 1) & (isempty(pac) == 1)) % Check (assumed causal) FIR case 
    numc = numd2;
    numac = [1];
    firflag = 1;
elseif (isempty(pac) == 1) % All causal
    numc = numd2;
    numac = [1];
    firflag = 0;
elseif (isempty(pc) == 1) % All anticausal
    numac = fliplr(numd2);
    numc = [1];
    firflag = 0;
else % Non-causal
    % Create numerator polynomials using matrix equations from cross-multiplication of numerator, denominator
    lendc = length(denc);
    lendac = length(denac);
    D1(1:lendac,1:lendac) = 0; % Matrix anticausal section
    cnt = 1;
    ptr = lendac - 1;
    for k = 1:lendac
        for m = 1:cnt
            D1(k,m) = denac(ptr + m);
        end
        ptr = ptr - 1;
        cnt = cnt + 1;
    end
    D2(1:lendc,1:lendc) = 0; % Matrix causal section
    cnt = 1;
    ptr = 2;
    for k = 1:lendc
        for m = 1:cnt
            D2(k,lendc + 1 - m) = denc(ptr - m);
        end
        ptr = ptr + 1;
        cnt = cnt + 1;
    end
    D = D1 + D2;
    Dinvrs = inv(D);
    Ds = size(D);
    if length(numd2 < Ds(1))
        numd2 = [zeros(1,Ds(1) - length(numd2)),numd2];
    end
    if (Ds(1) == length(numd2))
        numc = (Dinvrs*numd2')';
    else
        numc = (Dinvrs*numd2(1:Ds(1))')';
    end
    numac = numc;
    firflag = 0;
end

%%%%% Compute impulse response %%%%%
sample_plot = -N/2:N/2-1; % Plot vector
if (firflag ~= 1)
    sample_n = -N/2+(tzc-lzc):N/2+(tzc-lzc)-1; % Impulse response vector
else
    sample_n = -N/2:N/2-1;
end
unit_pulse = (sample_n == 0);

% Check causality of response
% Check causal case
dc = shiftcheck(fliplr(denc));
denc = denc(1:length(denc) - dc);
if length(numc) == length(denc)
    cchk = numc./denc;
elseif length(numc) > length(denc)
    cchk = numc./[denc zeros(1,length(numc)-length(denc))];
elseif length(denc) > length(numc)
    cchk = [numc zeros(1,length(denc)-length(numc))]./denc;
end
cchk = sum(cchk.^2);

% Check anticausal case
dac = shiftcheck(fliplr(denac));
denac = denac(1:length(denac) - dac);
if length(numac) == length(denac)
    acchk = numac./denac;
elseif length(numac) > length(denac)
    acchk = numac./[denac zeros(1,length(numac)-length(denac))];
elseif length(denac) > length(numac)
    acchk = [numac zeros(1,length(denac)-length(numac))]./denac;
end
acchk = sum(acchk.^2);

if ((cchk ~= [1]) | (firflag == 1)) % Causal filtering
    x = unit_pulse;
    u = filter(numc,denc,x);
end

if (acchk ~= [1]) % Anti-causal filtering
    x = unit_pulse;
    x = fliplr(x);
    w = filter(numac,denac,x);
    w = fliplr(w);
end

if ((cchk ~= [1]) & (acchk ~= [1])) % Sum of parallel causal/anti-causal sections
    h = u + w;
elseif (acchk ~= [1])
    h = w;
else
    h = u;
end

%%%%%% III.B: Generate impulse repsonse plot %%%%%
subplot(2,2,3)
if tzc < lzc
    stem(sample_plot,h,'filled'),axis([-N/2+(lzc-tzc) N/2-(lzc-tzc) get(gca,'YLim')]),grid
elseif tzc > lzc
    stem(sample_plot,h,'filled'),axis([-N/2+(tzc-lzc) N/2-(tzc-lzc) get(gca,'YLim')]),grid
else
    stem(sample_plot,h,'filled'),axis([-N/2 N/2 get(gca,'YLim')]),grid
end
title('Impulse response, h[k]','FontName','Arial','FontSize',14)
xlabel('Sample Number','FontName','Arial','FontSize',10,'FontWeight','Bold')
ylabel('Amplitude','FontName','Arial','FontSize',10,'FontWeight','Bold')

%%%%% Plot IV:  Pole/zero plot of transfer function %%%%%

% IV.A: Compute poles/zeros
p1 = length(dend);
q1 = length(numd);
if ((p1 ~= 0) & (firflag == 0))
    pol = roots(dend);
else
    pol = [];
end
if (q1 ~= 0)
    zer = roots(numd);
else
    zer = [];
end
% Pole/zero cancellation
epsilon = 10e-3; % Set parameter for zero/pole cancellation at same location
delta = 10e3; % Set parameter to check for pole/zero at infinity
if ((firflag == 0) & (isempty(pol) == 0) & (sum(pol) ~= 0) & (isempty(zer) == 0)) % Check poles
    pols = pzcanx(pol,zer,epsilon,delta);
elseif (firflag == 0) % No poles to check
    pols = pol;
else % No poles or zeros
    pols = [];
end
if ((isempty(zer) == 0) & (isempty(pol) == 0)) % Check zeros
    zers = pzcanx(zer,pol,epsilon,delta);
elseif (isempty(zer) == 0) % No zeros to check
    zers = zer;
else % No poles or zeros
    zers = [];
end

% IV.B: Generate pole/zero plot
subplot(2,2,4)
t = (0:1/500:1)' * (2*pi); % Set parameters to draw circles of varying radii
for m = 1:5
    l = (6-m)/5;
    r(:,m) = l*sin(t);
    s(:,m) = l*cos(t);
    plot(r(:,m),s(:,m),'k--');hold on;
    text(5*0.15,-5*0.15,num2str(5*0.2));
end
plot(r(:,1),s(:,1),'k');
axis equal % Make circle
axis off
line([-2.5 2.5],[0 0],'Color','k');
line([0 0],[-2.5 2.5],'Color','k');
hold on;
plot(2.5,0,'>',0,2.5,'^','MarkerFaceColor','k','MarkerEdgeColor','k'); % Asthetics for plot
text(1.5,0.2,'Re','FontAngle','italic','FontName','Arial');
text(1.83,0.21,'(z)');
text(0.18,2.1,'Im','FontAngle','italic','FontName','Arial');
text(0.52,2.11,'(z)');
plot(real(pols),imag(pols),'x','MarkerFaceColor','r','MarkerEdgeColor','r','MarkerSize',8,'LineWidth',2); % Plot poles
plot(real(zers),imag(zers),'o','MarkerEdgeColor','b','MarkerSize',5,'LineWidth',1); % Plot zeros
title('Pole/zero plot for H(e^{j\theta})','FontName','Arial','FontSize',14)
hold off

%%%%% DEFINE LOCAL FUNCTIONS %%%%%
function ct = shiftcheck(a)
% SHIFTCHECK Count number of leading zeros in vector
alen = length(a);
epsilon = 10e-9;
% Count front zeros
ct = 0;
for k = 1:alen - 1
    if abs(a(k)) < epsilon
        ct = ct + 1;
        if abs(a(k + 1)) < epsilon
            ct = ct;
        elseif abs(a(k + 1)) > epsilon
            break
        end
    else
        break
    end
end

function pzvec = pzcanx(a,b,epsilon,delta)
% PZCANX Cancels elements in vector A relative to vector B based on
% lower and upper thresholds, EPSILON and DELTA, respectively.
canflag = 0;
pzvec = [];
ctr_1 = 1;
ctr_2 = 1;
for xo = 1:length(a)
    clear canflag;
    for ox = 1:length(b)
        chk = 0;
        rtest = abs(real(a(xo)) - real(b(ox)));
        imtest = abs(imag(a(xo)) - imag(b(ox)));
        if (((rtest < epsilon) & (imtest < epsilon)) | (abs(a(xo)) > delta))
            chk = 1;
        end
        if chk == 0
            canflag(ctr_1) = 0;
        elseif chk == 1
            canflag(ctr_1) = 1;
        end
        ctr_1 = ctr_1 + 1;
    end
    if sum(canflag) == 0
        pzvec(ctr_2) = a(xo);
        ctr_2 = ctr_2 + 1;
    end
end