%% МОДЕЛИРОВАНИЕ ПОЛОСОВОГО ФИЛЬТРА С ДИФФЕРЕНЦИАТОРОМ
clear all; close all; clc;

%% 1. ПАРАМЕТРЫ ФИЛЬТРА (подставь свои значения)
b0 = 0.000392540911;
b1 = 0.0;
b2 = -0.000392540911;
a1 = -1.99915338;
a2 = 0.999214947;

% Или рассчитай новые:
center_freq = 50;       % Центральная частота (Гц)
sample_freq = 40000;     % Частота дискретизации (Гц)
bandwidth = 5;         % Ширина полосы (Гц)

% %% 2. РАСЧЕТ КОЭФФИЦИЕНТОВ (если нужно)
% if 1  % Поставь 0 если используешь готовые коэффициенты выше
%     % Отношение частот
%     fc_ratio = center_freq / sample_freq;
%     bandwidth_ratio = bandwidth / center_freq;
% 
%     % Расчет коэффициентов полосового фильтра
%     w0 = 2 * pi * fc_ratio;
%     Q = 1 / bandwidth_ratio;
%     alpha = sin(w0) / (2 * Q);
%     cos_w0 = cos(w0);
% 
%     % Коэффициенты биквадратного полосового фильтра
%     b0_bp = alpha;
%     b1_bp = 0;
%     b2_bp = -alpha;
%     a0_bp = 1 + alpha;
%     a1_bp = -2 * cos_w0;
%     a2_bp = 1 - alpha;
% 
%     % Нормализация (a0 = 1)
%     b0 = b0_bp / a0_bp;
%     b1 = b1_bp / a0_bp;
%     b2 = b2_bp / a0_bp;
%     a1 = a1_bp / a0_bp;
%     a2 = a2_bp / a0_bp;
% end

fprintf('Коэффициенты фильтра:\n');
fprintf('b0 = %.6f\n', b0);
fprintf('b1 = %.6f\n', b1);
fprintf('b2 = %.6f\n', b2);
fprintf('a1 = %.6f\n', a1);
fprintf('a2 = %.6f\n', a2);

%% 3. ПРОВЕРКА УСТОЙЧИВОСТИ
poles = roots([1, a1, a2]);
fprintf('\nПолюса фильтра:\n');
for i = 1:length(poles)
    fprintf('  pole%d = %.6f %+.6fj (|pole| = %.6f)\n', ...
            i, real(poles(i)), imag(poles(i)), abs(poles(i)));
end

if any(abs(poles) >= 1)
    fprintf('⚠️  ВНИМАНИЕ: Фильтр НЕУСТОЙЧИВ!\n');
else
    fprintf('✅ Фильтр устойчив\n');
end

%% 5. ЧАСТОТНАЯ ХАРАКТЕРИСТИКА ПОЛНОГО ФИЛЬТРА (с дифференциатором)
% Дифференциатор: H_diff(z) = 1 - z^-1
b_diff = [1, -1];
a_diff = 1;

% % Каскадное соединение: дифференциатор + полосовой фильтр
% b_total = conv(b_diff, b_bp);  % Перемножение полиномов
% a_total = conv(a_diff, a_bp);
% полосовой фильтр
b_total = [b0, b1, b2];
a_total = [1, a1, a2];

[h_total, f_total] = freqz(b_total, a_total, 2048, sample_freq);

figure();
subplot(1,2,1);
plot(f_total, 20*log10(abs(h_total)), 'r', 'LineWidth', 2);
grid on; hold on;
xline(center_freq, '--r', sprintf('%d Гц', center_freq), 'LineWidth', 1.5);
xlim([0, sample_freq/2]);
ylim([-60, 20]);
xlabel('Частота (Гц)');
ylabel('Усиление (дБ)');
title('АЧХ полного фильтра (дифференциатор + полосовой)');

subplot(1,2,2);
plot(f_total, angle(h_total)*180/pi, 'r', 'LineWidth', 2);
grid on; hold on;
xline(center_freq, '--r', sprintf('%d Гц', center_freq), 'LineWidth', 1.5);
xlim([0, sample_freq/2]);
xlabel('Частота (Гц)');
ylabel('Фаза (градусы)');
title('ФЧХ полного фильтра (дифференциатор + полосовой)');

%% 6. МОДЕЛИРОВАНИЕ ВО ВРЕМЕННОЙ ОБЛАСТИ
% Создаем тестовый сигнал
duration = 0.2;  % 200 мс
t = 0:1/sample_freq:duration;

% Сигнал 50 Гц + шум
signal_freq = 50;
signal = sin(2*pi*signal_freq*t) + 0.1*randn(size(t));

% Имитация работы твоего кода на C
% Прямая форма II с дифференциатором
x1 = 0; x2 = 0;  % Состояния входа фильтра
y1 = 0; y2 = 0;  % Состояния выхода фильтра
prev_input = 0;  % Для дифференциатора

filtered_signal = zeros(size(signal));

for i = 1:length(signal)
    % 1. Дифференциатор
    diff = signal(i);
    prev_input = signal(i);
    
    % 2. Полосовой фильтр (прямая форма II)
    y = b0*diff + b1*x1 + b2*x2 - a1*y1 - a2*y2;
    
    % 3. Обновление состояний
    x2 = x1;
    x1 = diff;
    y2 = y1;
    y1 = y;
    
    filtered_signal(i) = y;
end

% Встроенная функция MATLAB для сравнения
filtered_matlab = filter(b_total, a_total, signal);

figure();

% Сигнал и выход
subplot(2,1,1);
plot(t, signal, 'b', 'LineWidth', 1);
hold on; grid on;
plot(t, filtered_signal, 'r', 'LineWidth', 2);
plot(t, filtered_matlab, 'g--', 'LineWidth', 1);
xlabel('Время (с)');
ylabel('Амплитуда');
legend('Исходный сигнал', 'Наш фильтр (имитация C)', 'MATLAB filter()', ...
       'Location', 'best');
title(sprintf('Тестовый сигнал: %d Гц + помеха 150 Гц + шум', signal_freq));

% Ошибка между нашей реализацией и MATLAB
subplot(2,1,2);
error = filtered_signal - filtered_matlab;
plot(t, error, 'k', 'LineWidth', 1);
grid on;
xlabel('Время (с)');
ylabel('Ошибка');
title('Разница между нашей реализацией и MATLAB filter()');
fprintf('\nМаксимальная ошибка: %.2e\n', max(abs(error)));

%% 7. АНАЛИЗ НА ЧАСТОТЕ 50 Гц
freq_test = 50;
w_test = 2*pi*freq_test/sample_freq;
z = exp(1i*w_test);

% Передаточная функция полного фильтра
H_z = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2) * (1 - z^-1);

fprintf('\n══════════════════════════════════════════════════\n');
fprintf('АНАЛИЗ НА %d Гц:\n', freq_test);
fprintf('Усиление: %.3f (%.2f дБ)\n', abs(H_z), 20*log10(abs(H_z)));
fprintf('Фазовый сдвиг: %.1f градусов\n', angle(H_z)*180/pi);
fprintf('Задержка: %.2f мс (фазовая)\n', -angle(H_z)*1000/(2*pi*freq_test));
fprintf('══════════════════════════════════════════════════\n');

%% 8. ГРАФИК ПОЛЮСОВ И НУЛЕЙ
figure();

% Единичная окружность
theta = linspace(0, 2*pi, 100);
plot(cos(theta), sin(theta), 'k--', 'LineWidth', 1);
hold on; grid on; axis equal;

% Полюса (красные кресты)
plot(real(poles), imag(poles), 'rx', 'MarkerSize', 15, 'LineWidth', 2);

% Нули полосового фильтра
zeros_bp = roots([b0, b1, b2]);
plot(real(zeros_bp), imag(zeros_bp), 'bo', 'MarkerSize', 10, 'LineWidth', 2);

% Нули дифференциатора (z=1)
plot(1, 0, 'go', 'MarkerSize', 10, 'LineWidth', 2);

xlim([-1.2, 1.2]);
ylim([-1.2, 1.2]);
xlabel('Re(z)');
ylabel('Im(z)');
title('Диаграмма полюсов и нулей фильтра');
legend('Единичная окружность', 'Полюса', 'Нули полосового фильтра', ...
       'Нуль дифференциатора (z=1)', 'Location', 'best');