clear all; close all; clc;

function [NarrowBand,Wideband,ff,f_lo,fs_lo, FreqVector] = CIC_Response(M,N,iPrescaler,FSample)
  R = 2^iPrescaler;                    % Decimation factor
  fs_lo = FSample / R;            % Post-decimation sampling frequency

  fc = FSample/R/2;      % Ideal cutoff frequency
  Fo = R*fc/FSample;     % Normalized cutoff frequency
  
  p = 2e3;                    % Granularity
  s = 0.25 / p;               % Step size 1/4 *
  fpass = 0:s:Fo;             % Pass band frequency samples
  ff = [fpass] * 2;     % Normalized frequency samples, 0 <= f <= 1
  
  
  f = linspace(0,1,4*length(ff));                                   % Whole 
  f_lo = [ff ff+1 ff+2 ff+3];                                       % Just 4x period
  Wideband =   R^-M*abs(sin(pi*f*R)./sin(pi*f)).^M;    % MAGIC STUFF
  NarrowBand = (R^-M*abs(R*N*sin(pi*N*f_lo) ./ (pi*N*f_lo)).^M);  % MAGIC STUFF
  FreqVector = f_lo*fs_lo;
end

function [H_IIR,w,b,a,sos,g] = IIR_Response(fSample_CIC, Prescaler_CIC, attenuation, ripple, Cf_amount,IIR_Bitwidth)
%=============================
% IIR Filter Design 
%=============================
  
fs = fSample_CIC/(2^Prescaler_CIC); % Input frequency for IIR
fc = fs/8;                          % Cutoff frequency / 2 
f = [fc] /(fs/2);                   % normalize cutoff frequency (Nyquist)
%[b,a] = butter(Cf_amount,f);       % Butterworth Filter
%[b,a] = ellip(Cf_amount,ripple,attenuation,f); % elliptic filter
%[b,a] = cheby1(Cf_amount,ripple,f);   %Chebyshev 2 filter
[b,a] = cheby2(Cf_amount,attenuation,f);   %Chebyshev 2 filter
[H_IIR, w] = freqz(b,a,512,"whole");            % return transfer function and frequency in radiant
figure('name','IIR Phase')
freqz(b,a,512,"whole");            % return transfer function and frequency in radiant
%=============================
% IIR Filter Plot
%=============================
sos = tf2sos(b,a);  
figure('name','IIR Filter');
subplot(211)
plot(w*(fSample_CIC/2^Prescaler_CIC)/(2*pi),20*log10(abs(H_IIR)));
title([{'IIR Filter Transfer function'};{sprintf('Stages = %i, Attenuation = %i, Ripple = %idB',Cf_amount, attenuation, ripple)}]);
xlabel('Frequency (Hz)');
ylabel('|H(e^{j\omega})| (dB)');
subplot(212)
zplane(b,a); 
title([{'IIR Filter Pole-Zero-Plot'};{sprintf('Stages = %i, Attenuation = %i, Ripple = %idB',Cf_amount, attenuation, ripple)}]);
%=============================
% IIR coefficient smoothing
%=============================
a = floor(a*2^IIR_Bitwidth)/2^IIR_Bitwidth;
b = floor(b*2^IIR_Bitwidth)/2^IIR_Bitwidth;
[H_IIR, w] = freqz(b,a,512,"whole");
end
  
  
  fs_CIC       = 10000000;      % Input frequency
  M_CIC        = 3;             % CIC stages
  N_CIC        = 1;             % Delay stages
  Prescaler_CIC= 11;            % Prescaler for CIC
  
  IIR_stages    = 4;            % Number of coefficients for IIR
  IIR_Bitwidth  = 32;           % Width of the coefficients
  IIR_Atten     = 100;          % desired attenuation
  IIR_Ripple    = 2;            % Passband ripple (for ellip)
  
[NarrowBandResponse_CIC,WideBandResponse_CIC,ff_CIC,f_lo_CIC,fs_lo_CIC,F1] = CIC_Response(M_CIC,N_CIC,Prescaler_CIC,fs_CIC); %Get CIC
[H_IIR,w,b,a,sos,g] = IIR_Response(fs_CIC,Prescaler_CIC,IIR_Atten,IIR_Ripple,IIR_stages,IIR_Bitwidth);    % Get IIR


H_IIR4 = [H_IIR' H_IIR' H_IIR' H_IIR'];                     % Add IIR frequency response 4 times
x_iir4 = w*(fs_CIC/2^Prescaler_CIC)/(2*pi);                     % convert radiant to Hz
x_iir4tot = [x_iir4' x_iir4'+x_iir4(end) x_iir4'+2*x_iir4(end) x_iir4'+3*x_iir4(end)]; % Add frequency 4 times

H_IIR_Interp = interp1(x_iir4tot,H_IIR4,F1);                % interpolate IIR response to wider frequency range of CIC NarrowBand

H_both = NarrowBandResponse_CIC .* H_IIR_Interp;            % Multiply both signals
%
%%=========
%% Plotting
%%=========
%
% CIC
figure('name','CIC Frequency Response','Numbertitle','off');
subplot(211);
plot(linspace(0,1,4*length(ff_CIC))*fs_CIC, mag2db(WideBandResponse_CIC), 'b');
grid on;
title([{'Wideband frequency response of CIC filter'};{sprintf('R = %i, M = %i, N = %i',2^Prescaler_CIC,M_CIC,N_CIC)}]);
xlabel('Frequency (Hz)');
ylabel('|H(e^{j\omega})| (dB)');
axis([0,fs_CIC,-1250,0]);
subplot(212);
plot(f_lo_CIC*fs_lo_CIC, mag2db(NarrowBandResponse_CIC));
grid on;
title('Narrowband frequency response of CIC filter');
xlabel('Frequency (Hz)');
ylabel('|H(e^{j\omega})| (dB)');
axis([0,4*fs_lo_CIC,-200,5]);



%figure('name','IIR Freqz');
%freqz(b,a,512,"whole");
%[Htest,w2]=freqz(b,a,1024);


% IIR smoothed
figure('name','IIR Filter smoothed');
subplot(211)
plot(w*(fs_CIC/2^Prescaler_CIC)/(2*pi),20*log10(abs(H_IIR)));
title([{'IIR Filter Transfer function'};{sprintf('Stages = %i, Attenuation = %i, Ripple = %idB, Bits = %i',IIR_stages, IIR_Atten, IIR_Ripple,IIR_Bitwidth)}]);
xlabel('Frequency (Hz)');
ylabel('|H(e^{j\omega})| (dB)');
subplot(212)
zplane(b,a);
title([{'IIR Filter Pole-Zero-Plot'};{sprintf('Stages = %i, Attenuation = %i, Ripple = %idB, Bits = %i',IIR_stages, IIR_Atten, IIR_Ripple,IIR_Bitwidth)}]);


% IIR + CIC Signal
figure('name','CIC with Compensator Filter','Numbertitle','off');
hold on;
plot(F1, mag2db(NarrowBandResponse_CIC), 'b');          % CIC
plot(F1,mag2db(abs(H_IIR_Interp)), 'g');          % 4 times IIR
plot(F1, mag2db(abs(H_both)), 'r');  % IIR + CIC
axis([0,4*fs_lo_CIC,-200,25])
grid on;
title([{'CIC with IIR'};{sprintf('IIR Coefficients = %i, Coefficient precision = %iBit\n R = %i, M = %i, N = %i',IIR_stages,IIR_Bitwidth,2^Prescaler_CIC,M_CIC,N_CIC)}]);
legend('CIC','IIR','Signal');
xlabel('Frequency (Hz)');
ylabel('|H(e^{j\omega})| (dB)');