function [hs,es] = er_iirlsinv(Gnum,Gden,Fnum,Fden)
% function [hs,es] = er_iirlsinv(Gnum,Gden,Fnum,Fden)
%   Function to find deterministic least squares inverse causal IIR filter coefficients
%   HS and return error vector in ES, to minimize ||f - conv(g,h)||.  Takes first filter 
%   transfer function numerator coefficients in GNUM and denominator coefficients in 
%   GDEN and second transfer function numerator coefficient vector in FNUM and denominator 
%   coefficient vector in FDEN.  
%   NOTE: Zero-padding is not needed for input vectors to equalize lengths over all vectors.
%
%   This function finds the filter, H(z) = D(z)X, where D(z) = A1(z)/(C1(z)B1(z)B2(z)), 
%   where C1(z) is the polynomial created from the poles of F(z) inside the unit
%   circle, B1(z) is constructed from the zeros of G(z) inside the unit circle, B2(z) is
%   created from the reciprocals of the zeros of G(z) outside the unit circle, and A1(z) 
%   is created from the poles of G(z) inside the unit circle.
%
%   X is FIR and found to minimize ||F(z) - (D(z)G(z))X||^2.  Uses
%   ER_FIRLSINV.M to compute this.
%
%   EXAMPLE:
%
%   >> [hs,es] = er_iirlsinv([1 2 3],[1 0 -1/4],[0 0 0 0 0 0 0 0 0 0 1],[1]);
%
%   Mean square error: 
%
%   Vmin =
%
%   1.431870069394124e-005
%
%   CONSTRAINTS:  G and F must be rational.
%
%   Author:  Evan Ruzanski, CU-Boulder, ECEN5632 MATLAB assignment, FA2004

% Long numerical format for accurate screen display
format long
warning off
m = 128;

% Split polynomials into causal/anticausal sections
[numc_g,numac_g] = poly_split(Gnum);
[denc_g,denac_g] = poly_split(Gden);
[numc_f,numac_f] = poly_split(Fnum);
[denc_f,denac_f] = poly_split(Fden);

% Create intermediate polynomial, D(z)
Dnum = denc_g;
Dden_temp = conv(denc_f,numc_g);
Dden = conv(Dden_temp,1./numac_g);

% Create intermediate polynomial, D(z)G(z)
DGnum = numac_g
DGden_temp = conv(denc_f,1./numac_g);
DGden = conv(DGden_temp,denac_g)

% Find X to minimize ||F(z) - (D(z)G(z))X||^2
[x,e] = er_firlsinv(DGnum,DGden,Fnum,Fden,m/2 -1,4*m); % Choose order arbitrarily
x = x(1).tf_complete;
clc

% Take inverse z-transform of D(z)
d = iztrans(Dnum,Dden,m);
d = d(m/2 + 1:length(d));

% Construct h(n)
h = conv(d,x);
h = h(1:m/2);

% Take inverse of F(z) and G(z)
f = iztrans(Fnum,Fden,m);
f = f(m/2 + 1:length(f));
g = iztrans(Gnum,Gden,m);
g = g(m/2 + 1:length(g));

%%%%%%%%%% GENERATE ERROR SEQUENCE %%%%%%%%%%
% Generate error sequence
f_temp = conv(g,h);
f_temp = f_temp(1:m/2);
e = f - f_temp;

% Generate V(h) = ||f - conv(g,h)||^2
disp('Mean square error: ');
Vmin = sum(e.^2)

%%%%%%%%%% PACK OUTPUT INTO STRUCT FOR PLOTTING %%%%%%%%%%
hs(1).tf_complete = h;
hs(2).tf_complete = 1;
es(1).tf_complete = e;function [plyc,plyac] = poly_split(plynm);
% TF_SPLIT Separates given polynomial into causal and anti-causal sections
z = roots(plynm);
pc = [];
pac = [];
for k = 1:length(z);
    if abs(z(k)) < 1
        pc = [pc; z(k)];
    else
        pac = [pac; z(k)];
    end
end
plyc = poly(pc);
plyac = poly(pac);
es(2).tf_complete = 1;

%%%%%%%%%% DECLARE LOCAL FUNCTIONS %%%%%%%%%%
function [plyc,plyac] = poly_split(plynm);
% TF_SPLIT Separates given polynomial into causal and anti-causal sections
z = roots(plynm);
pc = [];
pac = [];
for k = 1:length(z);
    if abs(z(k)) < 1
        pc = [pc; z(k)];
    else
        pac = [pac; z(k)];
    end
end
plyc = poly(pc);
plyac = poly(pac);

function h = iztrans(numd,dend,N);
% IZTRANS Take inverse z-transform of given transfer function

% STEP 1:  Decompose given transfer function into causal and anticausal
% sections (using partial fraction decomposition)

% 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));
% lzc = 0;
% tzc = 0;
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 ((length(denc) == length(dend)) & isempty(pac) == 1) % All causal
    numc = numd2;
    numac = [1];
    firflag = 0;
elseif ((length(denac) == length(dend)) & 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
sample_n = -N/2+(tzc-lzc):N/2+(tzc-lzc)-1; % Impulse response vector
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

function ct = shiftcheck(a)
% SHIFTCHECK Count number of leading zeros in input 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