Fs = 20e6; % Input sample rate
A = 60;
h1 = fir_kaiser(0.005,0.495,A);
h2 = fir_kaiser(0.01,0.49,A);
h3 = fir_kaiser(0.02,0.18,A);
h4 = fir_kaiser(0.09,0.11,A);
D1 = 2;
D2 = 2;
D3 = 5;
D4 = 5;

f = [-0.5:0.001:0.5].'; % Column vector
H1 = @(f) exp(1j*2*pi*f*[0:length(h1)-1])*h1.';
H2 = @(f) exp(1j*2*pi*f*[0:length(h2)-1])*h2.';
H3 = @(f) exp(1j*2*pi*f*[0:length(h3)-1])*h3.';
H4 = @(f) exp(1j*2*pi*f*[0:length(h4)-1])*h4.';

%X1 = ones(size(f));
X1 = @(f) cos(2*pi*50*f).^2;
Y1 = @(f) H1(f).*X1(f);
Z1 = @(f) (1/2)*(Y1(f) + Y1(f-1/2));
X2 = @(f) Z1(f/2);

subplot(511); plot(f,20*log10(abs(X1(f)))); ylim([-100 0]);
title('Stage 1: Kaier FIR + Decimate by 2');
subplot(512); plot(f,20*log10(abs(Y1(f)))); ylim([-100 0]);
subplot(513); plot(f,20*log10(abs(Y1(f))));  ylim([-100 0]); hold on; ax = axis;
subplot(513); plot(f,20*log10(abs(Y1(f-1/2)))); ylim([-100 0]);
subplot(513); plot(0.005*[1 1],ax(3:4),'r');
subplot(513); plot(0.495*[1 1],ax(3:4),'r'); hold off;
subplot(514); plot(f,20*log10(abs(Z1(f)))); ylim([-100 0]);
subplot(515); plot(f,20*log10(abs(X2(f)))); ylim([-100 0]);
pause;

Y2 = @(f) H2(f).*X2(f);
Z2 = @(f) (1/2)*(Y2(f) + Y2(f-1/2));
X3 = @(f) Z2(f/2);

subplot(511); plot(f,20*log10(abs(X2(f)))); ylim([-100 0]);
title('Stage 2: Kaier FIR + Decimate by 2');
subplot(512); plot(f,20*log10(abs(Y2(f)))); ylim([-100 0]);
subplot(513); plot(f,20*log10(abs(Y2(f)))); ylim([-100 0]); hold on; ax = axis;
subplot(513); plot(f,20*log10(abs(Y2(f-1/2)))); ylim([-100 0]);
subplot(513); plot(0.01*[1 1],ax(3:4),'r');
subplot(513); plot(0.49*[1 1],ax(3:4),'r'); hold off;
subplot(514); plot(f,20*log10(abs(Z2(f)))); ylim([-100 0]);
subplot(515); plot(f,20*log10(abs(X3(f)))); ylim([-100 0]);
pause;

Y3 = @(f) H3(f).*X3(f);
Z3 = @(f) (1/5)*(Y3(f) + Y3(f-1/5) + Y3(f-2/5) + Y3(f-3/5) + Y3(f-4/5));
X4 = @(f) Z3(f/5);

subplot(511); plot(f,20*log10(abs(X3(f)))); ylim([-100 0]);
title('Stage 3: Kaier FIR + Decimate by 5');
subplot(512); plot(f,20*log10(abs(Y3(f)))); ylim([-100 0]);
subplot(513); plot(f,20*log10(abs(Y3(f)))); ylim([-100 0]); hold on; ax = axis;
subplot(513); plot(f,20*log10(abs(Y3(f-1/5)))); ylim([-100 0]);
subplot(513); plot(f,20*log10(abs(Y3(f-2/5)))); ylim([-100 0]);
subplot(513); plot(f,20*log10(abs(Y3(f-3/5)))); ylim([-100 0]);
subplot(513); plot(f,20*log10(abs(Y3(f-4/5)))); ylim([-100 0]);
subplot(513); plot(0.02*[1 1],ax(3:4),'r');
subplot(513); plot(0.18*[1 1],ax(3:4),'r'); hold off; ylim([-100 0]);
subplot(514); plot(f,20*log10(abs(Z3(f)))); ylim([-100 0]);
subplot(515); plot(f,20*log10(abs(X4(f)))); ylim([-100 0]);
pause;

Y4 = @(f) H4(f).*X4(f);
Z4 = @(f) (1/5)*(Y4(f) + Y4(f-1/5) + Y4(f-2/5) + Y4(f-3/5) + Y4(f-4/5));
X5 = @(f) Z4(f/5);

subplot(511); plot(f,20*log10(abs(X4(f)))); ylim([-100 0]);
title('Stage 4: Kaier FIR + Decimate by 5');
subplot(512); plot(f,20*log10(abs(Y4(f)))); ylim([-120 0]);
subplot(513); plot(f,20*log10(abs(Y4(f)))); ylim([-100 0]); hold on; ax = axis;
subplot(513); plot(f,20*log10(abs(Y4(f-1/5)))); ylim([-100 0]);
subplot(513); plot(f,20*log10(abs(Y4(f-2/5)))); ylim([-100 0]);
subplot(513); plot(f,20*log10(abs(Y4(f-3/5)))); ylim([-100 0]);
subplot(513); plot(f,20*log10(abs(Y4(f-4/5)))); ylim([-100 0]);
subplot(513); plot(0.09*[1 1],ax(3:4),'r');
subplot(513); plot(0.11*[1 1],ax(3:4),'r'); hold off; ylim([-120 0]);
subplot(514); plot(f,20*log10(abs(Z4(f)))); ylim([-100 0]);
subplot(515); plot(f,20*log10(abs(X5(f)))); ylim([-100 0]);
