clear all;
clear;
close all;
rng(1); % random seed
global ns average quantization Q1 Q2 Q3 fs M D sb_nbr Apass Astop dens

%% Parameter

ns = 2e6;           % Number of Sample
fs = 40e9;          % Sampling Frequency
M = 20;             % Polyphase factor
D = 8;              % Decimator

sb_nbr = 3;        % Sub-band Number in OPFB : 1 -> M

average = 0;        % PSD smoothing : 0 = OFF ; 1 = ON
quantization = 1;   % 0 = OFF ; 1 = ON

Q1 = 6;             % DG Quantization
Q2 = 8;             % TAP Quantizantion
Q3 = 6;             % PFB Quantization

Apass = 0.25;       % Passband Ripple (dB)
Astop = 50;         % Stopband Attenuation (dB)
dens  = 50;         % Density Factor

%% Input Signal

bw = fs/M; % OPFB Output Bandwidth

f1 = bw*(sb_nbr);
f2 = bw*(sb_nbr-0.625+0.25*mod(sb_nbr,M/gcd(M,D)));

t = 0:1/fs:(ns-1)/fs;
sw1    = 1*sin(2*pi*f1*t);
sw2    = 1*sin(2*pi*f1*t);
noise = randn([1 ns]);
nsw   = noise+sw1+sw2;

% win   = hanning(ns);
% if average == 0
%     [PSD, FREQ] = periodogram(nsw,win,ns,fs);
% else
%     [PSD, FREQ] = pwelch(nsw,ns/100,[],ns,fs);
% end
% figure;
% MAX = max(10*log(PSD));
% plot(FREQ/1e9, 10*log(PSD)-MAX);
% title('Real Input signal');
% xlim([0,fs/1e9]);
% grid on;

%% Digitizer Anti-aliasing Filter

f     = [0 0.1 0.9 1];
m     = [0 1 1 0];
b     = fir2(1000,f,m);
s_dg_real = conv(noise+sw1,b,'valid');
s_dg_imag = conv(noise+sw2,b,'valid');
s_dg_bb = 0*s_dg_real+1i*s_dg_imag;
ns_dg_bb = size(s_dg_bb,2);

if quantization == 1
    s_dg_bb_fi = fi(s_dg_bb,1,Q1,0);
    s_dg_bb    = s_dg_bb_fi.data;
end

win = hanning(ns_dg_bb);
if average == 0
    [PSD, FREQ] = periodogram(s_dg_bb,win,ns_dg_bb,fs);
else
    [PSD, FREQ] = pwelch(s_dg_bb,ns_dg_bb/100,[],ns_dg_bb,fs);
end
figure;
MAX = max(10*log(PSD));
plot(FREQ/1e9, 10*log(PSD)-MAX);
title('Dual complex input signal');
xlim([0,fs/1e9]);
grid on;


%% Oversampled Polyphase Filter Bank

% All frequency values are in GHz.
Fs    = fs/1e9;                            % Sampling Frequency
Fpass = (1-(1-D/M))*Fs/M;                  % Passband Frequency
Fstop = Fs/M;                              % Stopband Frequency
Dpass = 1-10^(-Apass/20);                  % Passband Ripple
Dstop = 10^(-Astop/20);                    % Stopband Attenuation

% Calculate the order from the parameters using FIRPMORD.
[N, Fo, Ao, W] = firpmord([Fpass, Fstop]/(Fs), [1 0], [Dpass, Dstop]);
Ndec0 = N;
if (mod(N,M) ~= M-1)
    N = N + M-1 - mod(N,M);
end

% Calculate the coefficients using the FIRPM function.
Hdec0 = firpm(N, Fo, Ao, W, {dens});

% Calculate polyphase filter
Hdec0 = fliplr(Hdec0);
len = length(Hdec0);
nrows = len/M;
Hdec0_pol = zeros(M,nrows);
for m=1:M
    for i=1:nrows
        Hdec0_pol(m,i)=Hdec0(1,(i-1)*M+m); 
    end
end
if quantization == 1 
    Hdec0_pol_fi = fi(Hdec0_pol,1,Q2);
    Hdec0_pol    = Hdec0_pol_fi.data;
end

% Demux Input
ns_dg_rs1 = floor((ns_dg_bb-M)/D);
s_dg_rs1 = zeros(M,ns_dg_rs1);
for m=1:M  
    for i=1:ns_dg_rs1
        s_dg_rs1(m,i) = s_dg_bb(1,(i-1)*D+m);
    end
end
s_dg_rs1 = flipud(s_dg_rs1);

% Circular shifting
s_dg_rs2 = s_dg_rs1;
ns_dg_rs2 = ns_dg_rs1;
% for i = 1:ns_dg_rs2
%    s_dg_rs2(:,i) = flip((s_dg_bb((i-1)*D+1:(i-1)*D+M))');
% end
for i=1:ns_dg_rs2
    for m=1:M    
        s_dg_rs2(m,i) = s_dg_rs1(1+mod( (m-1) + mod((i-1)*D,M) ,M),i); 
    end
end

% Apply polyphase filter
ns_pol = ns_dg_rs2-(nrows-1);
s_pol = zeros(M,ns_pol);

for m=1:M
    s_pol(m,:) = conv(s_dg_rs2(m,:),Hdec0_pol(m,:),'valid');
end

s_pol(:,1:M) = 0*s_pol(:,1:M);
s_pol(10,1:10) = 1i;
s_pol_fi = fi(s_pol,1,19,10);
s_pol = s_pol_fi.data;

% DFT
s_pol_dft = zeros(M,ns_pol);

if M == 10
    s_pol_rearranged = [s_pol(1,:); s_pol(3,:); s_pol(5,:); s_pol(7,:); s_pol(9,:); s_pol(2,:); s_pol(4,:); s_pol(6,:); s_pol(8,:); s_pol(10,:)];
elseif M == 20
    s_pol_rearranged = [s_pol(1,:); s_pol(5,:); s_pol(9,:); s_pol(13,:); s_pol(17,:); s_pol(3,:); s_pol(7,:); s_pol(11,:); s_pol(15,:); s_pol(19,:); s_pol(2,:); s_pol(6,:); s_pol(10,:); s_pol(14,:); s_pol(18,:); s_pol(4,:); s_pol(8,:); s_pol(12,:); s_pol(16,:); s_pol(20,:)];
end

%%
for i = 1:M/5
    s_pol_dft(1+5*(i-1):5*i,:) = winograd5(s_pol_rearranged(1+5*(i-1):5*i,:), 8);
end

%F = fimath('RoundingMethod','Floor','ProductMode','KeepMSB', 'ProductFractionLength',6);
wn5 = fi(exp(-2*1i*pi*[0:4]/10), 1, 8, 6);
wn5 = wn5.data;
wn10 = fi(exp(-2*1i*pi*[0:9]/20), 1, 8, 6);
wn10 = wn10.data;
for i = 1:M/10
    s_pol_dft(1+10*(i-1):10*i, :) = radix2(s_pol_dft(1+10*(i-1):10*i, :), wn5);
end
if M == 20
    s_pol_dft = radix2(s_pol_dft, wn10);
end

% for i=1:ns_pol
%     s_pol_dft(:,i) = fft(s_pol(:,i));
% end
%s_pol_dft(2:end, :) = flip(s_pol_dft(2:end, :));
% for i=1:ns_pol
%     for p=1:M
%         for m=1:M
%             s_pol_dft(p,i) = s_pol_dft(p,i) + s_pol(m,i)*exponential(mod((m-1)*(p-1), M)+1);
%         end
%     end
% end

tb_input    = s_pol_fi.data*2^s_pol_fi.FractionLength;
tb_input    = [real(tb_input);imag(tb_input)];
tb_input    = reshape(tb_input, 1, size(tb_input,1)*size(tb_input,2));
tb_output   = fi(s_pol_dft,1,6,4);
tb_output   = tb_output.data*2^(4);
tb_output   = floor([real(tb_output);imag(tb_output)]);
tb_output    = reshape(tb_output, 1, size(tb_output,1)*size(tb_output,2));


%% Sub-band Selection

sb_nbr = 9;
% 
% if sb_nbr < M/2    
%     s_pol_sel = s_pol_dft(sb_nbr+1,:)+conj(s_pol_dft(M-sb_nbr+1,:));
% elseif sb_nbr == M/2
%     s_pol_sel = s_pol_dft(sb_nbr+1,:);
% if sb_nbr < M  
%     s_pol_sel = -1i*s_pol_dft(sb_nbr+1,:)+conj(-1i*s_pol_dft(M-sb_nbr+1,:));
% elseif sb_nbr == M
%     s_pol_sel = s_pol_dft(1,:);
% end

s_pol_sel = s_pol_dft(sb_nbr,:);
% s_pol_sel = s_pol_dft(sb_nbr+1,:)+conj(s_pol_dft(M-sb_nbr+1,:));
% s_pol_sel = -1i*s_pol_dft(sb_nbr+1,:)+conj(-1i*s_pol_dft(M-sb_nbr+1,:));

s_pol_sel=s_pol_sel.*exp(-1i*pi*[1:ns_pol])/2;

win = hanning(ns_pol);
if average == 0
    [PSD, FREQ] = periodogram(s_pol_sel,win,ns_pol,(fs)/D);
else
    [PSD, FREQ] = pwelch(s_pol_sel,floor(ns_pol/100),[],ns_pol,(fs)/D);
end
figure;
MAX = max(10*log(PSD));
plot(FREQ/1e9, 10*log(PSD)-MAX);
title('Complex output signal');
xlim([0,(fs)/D/1e9]);
grid on;

%% Printf

demux = M;

file_folder = 'D:/Stage/ALMA_OPFB/simu/kx5_DFT - tb_6b_coeffs/tb_txts_files';
write_file(tb_input, strcat(file_folder, '/input.txt'), 2*M, ' %4d');
write_file(tb_input, strcat(file_folder, '/output.txt'), 2*M, ' %10d');

gen_Wn_coeffs_5ndft(M, 8, strcat(file_folder, '/coeffs.vhd'));