clc; clear; close all;

% 1. Specifications
BW=20e3; OSR=60; fs=2*BW*OSR; bits=1; N=2^22; m=1747; fin=fs*m/N;
t=(0:N-1)/fs;     
x = 0.75*sin(2*pi*fin*t); 

% 2. Coefficients
k1=0.3928; k2=0.27; k3=0.1548;
a1=2.0381; a2=2.7065; a3=2.6064; b3=1;

% 3. 3rd Order CIFF Modulator (Vectorized State Storage)
v1 = zeros(1, N); v2 = zeros(1, N); v3 = zeros(1, N); y  = zeros(1, N);
levels = 2^bits;
quant_levels = linspace(-1,1,levels);

for n = 2:N
    v1(n) = v1(n-1) + k1 * (x(n-1) - y(n-1));   % Integrator 1 (Delayed)
    v2(n) = v2(n-1) + k2 * v1(n);   % Integrator 2 (Delay-free)
    v3(n) = v3(n-1) + k3 * v2(n-1);  % Integrator 3 (Delayed)
    u = (b3 * x(n)) + (a3 * v3(n)) + (a2 * v2(n)) + (a1 * v1(n));  % Summation
    [~,idx] = min(abs(u - quant_levels));
    y(n) = quant_levels(idx);
end

% 4. SNR Calculation
v_win = y .* hann(N)';
Y = fft(v_win);
P2 = abs(Y/N).^2;         % Two-sided power spectrum
P1 = P2(1:N/2+1);         % One-sided power spectrum
P1(2:end-1) = 2*P1(2:end-1);

F = (0:N/2)*(fs/N); % Frequency axis
bin_width = fs / N;
signal_bin = round(fin / bin_width) + 1;
in_band = (F <= BW);  % Ideal Brickwall filter
Ps = sum(P1(signal_bin-1 : signal_bin+1));
Pn = sum(P1(in_band)) - Ps;

SNR = 10 * log10(Ps / Pn);
ENOB = (SNR - 1.76) / 6.02;

fprintf('Measured SNR:  %.2f dB\n', SNR);
fprintf('ENOB: %.2f bits\n', ENOB);

fprintf('Max |u1| = %.2f\n', max(abs(v1)));
fprintf('Max |u2| = %.2f\n', max(abs(v2)));
fprintf('Max |u3| = %.2f\n', max(abs(v3)));


figure(1)
subplot(2,1,1);
plot(F, P1); hold on; xline(BW, '--r');
grid on; title('Linear Frequency'); ylabel('Mag'); xlabel('Hz');

subplot(2,1,2);
semilogx(F, P1); hold on; xline(BW, '--r');
grid on; title('Log Frequency'); ylabel('Mag'); xlabel('Hz');