clc;
close all;
clear;

%% === LOAD INPUTS ===
[y, Fs] = audioread('sem2.wav');
[y2, Fs2] = audioread('flac2.wav');

if size(y,2) > 1, y = mean(y,2); end
if size(y2,2) > 1, y2 = mean(y2,2); end

t = (0:length(y)-1) / Fs;
t2 = (0:length(y2)-1) / Fs2;

%% === VISUALIZE ORIGINAL WAVEFORMS ===
figure;
subplot(2,1,1);
plot(t, y);
xlabel('Time [s]'); ylabel('Amplitude');
title('sem2.wav – Original Signal');
grid on;

subplot(2,1,2);
plot(t2, y2, 'r');
xlabel('Time [s]'); ylabel('Amplitude');
title('flac2.wav – Noise Sample');
grid on;

%% === FREQUENCY ANALYSIS (FFT of noise) ===
N2 = length(y2);
Y2 = fft(y2);
f2 = (0:N2/2-1) * Fs2 / N2;

magY2 = abs(Y2(1:N2/2));
figure;
plot(f2, 20*log10(magY2 + eps), 'r');
xlabel('Frequency [Hz]'); ylabel('Magnitude [dB]');
title('Noise Spectrum (flac2.wav)');
xlim([0 Fs2/2]); grid on;

%% === DETECT DOMINANT NOISE FREQUENCIES ===
[pks, locs] = findpeaks(magY2, 'MinPeakHeight', max(magY2)*0.05, ...
    'MinPeakDistance', round(10*N2/Fs2));
f_peaks = f2(locs);
f_peaks = f_peaks(f_peaks > 20 & f_peaks < Fs/2);
f_peaks = sort(f_peaks(1:min(5,length(f_peaks))));
fprintf('Detected noise frequencies [Hz]: %.1f ', f_peaks);
fprintf('\n');

%% === DESIGN CASCADE OF NOTCH FILTERS (z-plane zeros/poles) ===
r = 0.995;
sos_all = [];
g_all = 1;

for k = 1:length(f_peaks)
    w0 = 2*pi*f_peaks(k)/Fs;
    z = [exp(1j*w0) exp(-1j*w0)];
    p = r*z;
    b = real(poly(z));
    a = real(poly(p));
    b = b / (sum(b)/sum(a)); % normalize DC gain
    [sos_i, g_i] = tf2sos(b, a);
    sos_all = [sos_all; sos_i];
    g_all = g_all * g_i;
end

%% === VISUALIZE FILTER FREQUENCY RESPONSE ===
figure;
freqz(sos_all, 2048, Fs);
title('Cascade of Notch Filters (Pole-Zero Design)');

%% === APPLY FILTER TO sem2.wav ===
y_notch = filtfilt(sos_all, g_all, y);
y_notch = y_notch / (max(abs(y_notch)) + eps) * 0.95;
audiowrite('sem2_notch_filtered.wav', y_notch, Fs);
fprintf('Saved: sem2_notch_filtered.wav\n');

%% === ADDITIONAL SPEECH BANDPASS (100–8000 Hz) ===
[z_bp, p_bp, k_bp] = butter(4, [100 8000]/(Fs/2), 'bandpass');
[sos_bp, g_bp] = zp2sos(z_bp, p_bp, k_bp);
y_final = filtfilt(sos_bp, g_bp, y_notch);
y_final = y_final / (max(abs(y_final)) + eps) * 0.95;
audiowrite('sem2_final.wav', y_final, Fs);
fprintf('Saved: sem2_final.wav (Notch + Bandpass)\n');

%% === TIME DOMAIN COMPARISON ===
figure;
subplot(3,1,1);
plot(t, y, 'b'); title('Original sem2.wav');
xlabel('Time [s]'); ylabel('Amplitude'); grid on;
subplot(3,1,2);
plot(t, y_notch, 'g'); title('After Notch Filtering');
xlabel('Time [s]'); ylabel('Amplitude'); grid on;
subplot(3,1,3);
plot(t, y_final, 'r'); title('Final Output (Notch + Bandpass)');
xlabel('Time [s]'); ylabel('Amplitude'); grid on;

%% === FREQUENCY DOMAIN COMPARISON ===
N = length(y);
f = (0:N/2-1)*Fs/N;
Y = fft(y);
Y_notch = fft(y_notch);
Y_final = fft(y_final);

figure;
subplot(3,1,1);
plot(f, 20*log10(abs(Y(1:N/2))+eps), 'b');
title('Original sem2.wav Spectrum');
xlabel('Frequency [Hz]'); ylabel('Magnitude [dB]');
xlim([0 2000]); grid on;
subplot(3,1,2);
plot(f, 20*log10(abs(Y_notch(1:N/2))+eps), 'g');
title('After Notch Filtering Spectrum');
xlabel('Frequency [Hz]'); ylabel('Magnitude [dB]');
xlim([0 2000]); grid on;
subplot(3,1,3);
plot(f, 20*log10(abs(Y_final(1:N/2))+eps), 'r');
title('After Notch + Bandpass Spectrum');
xlabel('Frequency [Hz]'); ylabel('Magnitude [dB]');
xlim([0 2000]); grid on;

fprintf('\n Processing complete. Files saved:\n');
fprintf('   sem2_notch_filtered.wav (Notch only)\n');
fprintf('   sem2_final.wav (Notch + Bandpass)\n');