% Matlab Fixed-Point FIR filter demo part 3
% Simulate FIR filter with all fixed point values
% DRB 15-Nov-2013
% For ECE4703
% =================================================================
% User parameters below
% =================================================================
M = 25;         % FIR filter order 
Nh = 8;         % number of total bits for filter coefficients
Mh = 7;         % number of fractional bits for filter coefficients 
Nx = 16;        % number of total bits for inputs
Mx = 15;        % number of fractional bits for inputs 
Nm = 16;        % number of total bits for multiplier outputs
Nv = 16;        % number of total bits for sum output
Ny = 16;        % number of total bits for final output
px = 4;         % input right shift
ph = 2;         % coefficient right shift
pm = 1;         % multiplier output right shift
N = 5000;       % length of input signal
a = 0.9;        % fractoion of full-scale for input signal 
% =================================================================

% preliminary calculations
My = Mx;                 % number of fractional bits for final output
Mm = Mh+Mx-px-ph;        % number of fractional bits for multiplier outputs
Mv = Mm-pm;              % number of fractional bits in sum before final output 
pv = Mv-My;              % final right shift before final output
            
% make math object with desired properties
F1 = fimath('CastBeforeSum',1,...
            'OverflowMode','wrap',...
            'ProductMode','SpecifyPrecision',...
            'ProductWordLength',Nm,...
            'ProductFractionLength',Mm,...
            'SumMode','SpecifyPrecision',...
            'SumWordLength',Nv,...
            'SumFractionLength',Mv);

% design FIR filter
h = firls(M,[0 .1 .2 .5]*2,[1 1 0 0]);  % lowpass (full precision)
hq = sfi(h,Nh,Mh);                      % quantized coefficients 

% generate random input signal
x = a*(2*rand(1,N)-1);
xq = sfi(x,Nx,Mx);                      % quantized inputs 

% allocate space for variables
yq = sfi(zeros(1,N),Ny,My);
multiplier_output = sfi(zeros(1,M+1),Nm,Mm);

tic
mmo = 0;        % track maximum multiplier output
mv = 0;         % track maximum accumulator
% do FIR filtering
for n=1:N,
    v = sfi(0,Nv,Mv);
    for m=0:M, % h has M+1 coefficients
        if (n-m)>0,
            
            multiplier_output(m+1) = F1.mpy(hq(m+1),xq(n-m));
            v = F1.add(v,multiplier_output(m+1));
            
            % diagnostics
            amo = abs(double(multiplier_output(m+1)));
            if amo>mmo,
                mmo = amo;
            end
            av = abs(double(v));
            if av>mv,
                mv = av;
            end
            
        end
    end
    yq(n) = v;
end
toc

% do full-precision filter
y = filter(h,1,x);

% plot
fs = 44100;
[Pyq,f] = pwelch(double(yq),128,64,128,fs); % estimate psd of left channel
[Py,f] = pwelch(double(y),128,64,128,fs); % estimate psd of left channel
plot(f/1e3,10*log10(Py),f/1e3,10*log10(Pyq)); % plot psd in dB
grid on
xlabel('frequency (kHz)');
ylabel('magnitude response (dB)');
legend('full precision','quantized');
