%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FIR Filter Direct Form Demonstrator Code
% David Hwang -- dhwang@gmu.edu
% NOTE: This simulation never checks integer bit overflow/wraparound (i.e. assumes final result fits in its specified dynamic range)

M = 4; % number of coefficients
N1 = 16; % total bits of input (only used for printing vectors, not for wraparound calculations)
L1 = 15; % fractional bits of input
N2 = 16; % total bits of output (only used for printing vectors, not for wraparound calculations)
L2 = 15; % fractional bits of output
N3 = 16; % total bits of coefficients (only used for printing vectors, not for wraparound calculations)
L3 = 15; % fractional bits of coefficients 
quant_type = 1; % 1 = round to nearest, 0 = truncate
many_quant = 0; % 1 = M quantizers after multipliers, 0 = 1 quantizer after final adder

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Implementation of FIR filter is below

% generate inputs
num_samples = 1200;

rand('state',0);
d_in = -1 + ( (1-2^-L1) +1).*rand(1,num_samples); % random input stream
d_in = floor(d_in*2^L1 + 0.5)/2^L1; % round d_in to a inf.L1 value (where inf = infinity, i.e. don't care about integer bits)

figure(1); plot(d_in);
title('Input data')
xlabel('time')
ylabel('value')

figure(2); pwelch(d_in)
title('Power spectral density of filter input (white signal)')


% generate coefficients
h_flp = 0.7*firpm(M-1,[0 .45 .55 1],[1 1 0 0]); % create halfband filter, requires signal processing toolbox

h_fxp = floor(h_flp*2^L3 + 0.5)/2^L3; % round coefficients to inf.L3 value

h = h_fxp;

figure(3); stem(h);
title('Coefficients');
xlabel('time')
ylabel('value')

figure(4); freqz(h)
title('Frequency Response of h(n)')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% direct form
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
input_delay_line = zeros(1,M);
for t=1:num_samples,
    input_delay_line(2:M) = input_delay_line(1:M-1);
    input_delay_line(1) = d_in(t);
    mult_node_flp = input_delay_line .* h;
    d_out_direct_flp(t) = sum(mult_node_flp);
    
    if many_quant == 1
        mult_node_fxp = floor(mult_node_flp*2^L2 + quant_type*0.5)/2^L2; % quantize mult output to inf.L2 value
        d_out_direct_fxp(t) = sum(mult_node_fxp);
    else
        d_out_direct_fxp(t) = floor(d_out_direct_flp(t)*2^L2 + quant_type*0.5)/2^L2; % quantize adder output to inf.L2 value
    end
     
end

%pwelch(d_out_direct_flp)
figure(5); plot(d_out_direct_fxp);
title('Output data')
xlabel('time')
ylabel('value')

figure(6); pwelch(d_out_direct_fxp)
title('Power spectral density of filter output')

quant_noise = d_out_direct_fxp - d_out_direct_flp;

sqnr_direct = 20*log10(std(d_out_direct_flp)/std(quant_noise)) % compute signal to quantization noise ratio

mean_quant_noise = mean(quant_noise)

% calculate theoretical values
delta = 2^-L2;

var_quantizer = delta^2/12;

quant_noise_theoretical = M^(many_quant) * var_quantizer;

sqnr_direct_theoretical = 20*log10(std(d_out_direct_flp)/sqrt(quant_noise_theoretical))

mean_quant_noise_theoretical  = M^(many_quant) * -delta/2 * (1-quant_type) % assume 0 if round to nearest, compute actual for trunc

% plot first 100 samples of input and output
print_vector(d_in(1:100),'d_in.dat',N1,L1);
print_vector(d_out_direct_fxp(1:100),'d_out.dat',N2,L2);
print_vector(h,'h.dat',N3,L3);