function [hs,es] = er_firlsinv(gnum,gden,fnum,fden,n,m)
% function [hs,es] = er_firlsinv(gnum,gden,fnum,fden,n,m)
%   Function to find deterministic least squares inverse FIR 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 (to create deterministic autocorrelation sequence R) and second transfer function 
%   numerator coefficient vector in FNUM and denominator coefficient vector in FDEN (to 
%   generate normal equation result vector  Q).  Order of FIR filter is given in N.  Length 
%   of inverse z-transform is given in M.
%   NOTE: Zero-padding is not needed for input vectors to equalize lengths over all vectors.
%
%   EXAMPLE:
%   >> [hs,es] = er_firlsinv([0.25 0 -1],[1],[0 0 0 0 0 0 0 0 0 0 0 0 1],[1],10,32);
%
%   CONSTRAINTS:  G and F must be rational, G having no poles or zeros on unit circle, F 
%   having no poles on unit circle.
%
%   Author:  Evan Ruzanski, CU-Boulder, ECEN5632 MATLAB assignment, FA2004

% Long numerical format for accurate screen display
format long
warning off

% %%%%%%%%%% PERFORM FRONT-END PROCESSING %%%%%%%%%%
% Generate autocorrelation sequence
Rnum = conv(gnum,fliplr(gnum));
Rden = conv(gden,fliplr(gden));
r = iztrans(Rnum,Rden,m);

% Take causal part of autocorrelation sequence
[max_val ind] = max(r); 
r = r(ind:ind + n);

% Generate normal equation result vector
Qnum = conv(fnum,fliplr(gnum));
Qden = conv(fden,fliplr(gden));
q = iztrans(Qnum,Qden,m);

% Take causal part of normal equation result vector sequence
q = q(ind:ind + n);

%%%%%%%%%% SOLVE TOEPLITZ SYSTEM OF EQUATIONS USING LEVINSON RECURSION %%%%%%%%%% 
%%%%%%%%%% (via Fig. 7.2.2 in Roberts, Mullis, p. 233) %%%%%%%%%%
% Initialize
alpha = r(1);
a(1,1) = 1;
hh(1) = q(1)/r(1);

% Body
for k = 1:n
    a(k + 1,k) = 0;
    a(1,k + 1) = 1;
    
    % Intermediate step: compute first sum
    summ = 0;
    for j = 0:k - 1
        summ = summ + r(k + 1 - j)*a(j + 1,k);
    end
    
    beta = summ/alpha;
    for j = 1:k
        a(j + 1,k + 1) = a(j + 1,k) - beta*a(k + 1 - j,k);
    end
    alpha = alpha*(1 - beta^2);
    
    % Intermediate step: compute second sum
    summ = 0;
    for j = 0:k - 1
        summ = summ + r(k + 1 - j)*hh(j + 1);
    end
    
    hh(k + 1) = (q(k + 1) - summ)/alpha;
    for j = 0:k - 1
        hh(j + 1) = hh(j + 1) + a(k + 1 - j,k + 1)*hh(k + 1);
    end
end

%%%%%%%%%% GENERATE ERROR SEQUENCE AND DISPLAY %%%%%%%%%%
% Generate V(h) = ||f|| - q'h 
vnum = conv(fnum,fliplr(fnum));
vden = conv(fden,fliplr(fden));
normf = iztrans(vnum,vden,m);
normf = sum(normf.^2);
Vmin = normf - q*hh';
disp('Mean square error: ');
Vmin
% Generate error sequence
g = iztrans(gnum,gden,m);
f = iztrans(fnum,fden,m);
f2 = conv(g,hh);
e = f - f2(1:m);

%%%%%%%%%% PACK OUTPUT INTO STRUCT FOR PLOTTING %%%%%%%%%%
hs(1).tf_complete = hh;
hs(2).tf_complete = 1;
es(1).tf_complete = e;
es(2).tf_complete = 1;

%%%%%%%%%% DECLARE LOCAL FUNCTIONS %%%%%%%%%%
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