clc, clear all

%% Конфигурация фильтра
fs = 1000000/25;        % Частота дискретизации = 40000 Гц
fc = 50;                % Центральная частота фильтра = 50 Гц
fc_ratio = fc/fs;       % 50/40000 = 0.00125
bandwidth_ratio = 0.3;  % Полоса 30%

% Создаем фильтр
[b, a] = create_bandpass_filter(fc_ratio, bandwidth_ratio);

%% 1. Расчет ФЧХ для графика (в Гц)
% Частоты для подробного графика
freqslow = 40:0.1:49;      % [40 41 ... 49] Гц
freqshigh = 51:0.1:60;     % [51 52 ... 60] Гц  
freqscenter = 49:0.1:51; % [49.0 49.1 ... 51.0] Гц
freqs_hz = [freqslow, freqscenter, freqshigh]; % Все в Гц

% Преобразуем в нормированные частоты (f/fs)
freqs_ratio = freqs_hz / fs; % Разделить на fs

% Рассчитываем фазы для этих частот
phases = zeros(size(freqs_ratio));
for i = 1:length(freqs_ratio)
    phases(i) = calc_phase_at_freq(b, a, freqs_ratio(i));
end

% График ФЧХ (по частоте в Гц)
figure;
plot(freqs_hz, phases, 'b-', 'LineWidth', 2);
xlabel('Частота, Гц');
ylabel('Фаза, градусы');
title(sprintf('ФЧХ фильтра: fc=%d Гц, fs=%d Гц', fc, fs));
grid on;
xlim([40 60]);

%% 2. Табличный расчет (используем ТЕ ЖЕ данные!)
% Используем ТЕ ЖЕ частоты для таблицы
table_freqs_ratio = freqs_ratio;  % нормированные частоты
table_phases = phases;            % соответствующие фазы
table_freqs_hz = freqs_hz;        % частоты в Гц

% Вывод таблицы
fprintf('=== Таблица фазовых сдвигов ===\n');
fprintf('f/fs\t\tФаза, °\t\tЧастота, Гц\n');
fprintf('-------------------------------\n');
for i = 1:length(table_freqs_hz)
    fprintf('%.6f\t%6.1f\t\t%6.1f\n', ...
            table_freqs_ratio(i), table_phases(i), table_freqs_hz(i));
end

%% 3. Проверка поиска по таблице (ближайшее значение)
% Теперь target_freq ДОЛЖНА быть в нормированных частотах!
% Например, хотим найти фазу для 53 Гц:
target_freq_hz = 53;                       % 53 Гц
target_freq_ratio = target_freq_hz / fs;   % 53/40000 = 0.001325

% Ищем в таблице по нормированным частотам
phase_from_table = get_phase_from_table(table_phases, table_freqs_ratio, target_freq_ratio);
phase_exact = calc_phase_at_freq(b, a, target_freq_ratio);

fprintf('\n=== Проверка поиска по таблице ===\n');
fprintf('Целевая частота: %.3f Гц (%.6f f/fs)\n', target_freq_hz, target_freq_ratio);
fprintf('Из таблицы: %.1f°\n', phase_from_table);
fprintf('Точное значение: %.1f°\n', phase_exact);
fprintf('Погрешность: %.1f°\n', abs(phase_exact - phase_from_table));

%% 4. Дополнительная проверка на разных частотах
test_freqs_hz = [45, 49.5, 50, 50.5, 55]; % Частоты в Гц для проверки
fprintf('\n=== Проверка на разных частотах ===\n');
fprintf('Частота, Гц\tИз таблицы\tТочное\t\tПогрешность\n');
fprintf('-----------------------------------------------\n');

for i = 1:length(test_freqs_hz)
    test_freq_hz = test_freqs_hz(i);
    test_freq_ratio = test_freq_hz / fs;
    
    % Из таблицы
    phase_table = get_phase_from_table(table_phases, table_freqs_ratio, test_freq_ratio);
    
    % Точное значение
    phase_exact = calc_phase_at_freq(b, a, test_freq_ratio);
    
    fprintf('%6.1f\t\t%6.1f\t\t%6.1f\t\t%6.1f\n', ...
            test_freq_hz, phase_table, phase_exact, abs(phase_exact - phase_table));
end

%% 5. Сравнение идеальной ФЧХ и табличной (ступенчатой)
figure;
% Идеальная ФЧХ (гладкая)
plot(table_freqs_hz, table_phases, 'b-', 'LineWidth', 2);
hold on;
% Табличная ФЧХ (ступенчатая)
stairs(table_freqs_hz, table_phases, 'r-', 'LineWidth', 1.5);
% Точки таблицы
plot(table_freqs_hz, table_phases, 'ko', 'MarkerSize', 5, 'MarkerFaceColor', 'k');

xlabel('Частота, Гц');
ylabel('Фаза, градусы');
title('Сравнение идеальной и табличной ФЧХ');
legend('Идеальная ФЧХ', 'Табличная ФЧХ (ступеньки)', 'Точки таблицы', 'Location', 'best');
grid on;
xlim([40 60]);








%% ФУНКЦИИ
function [b, a] = create_bandpass_filter(fc_ratio, bandwidth_ratio)
    w0 = 2 * pi * fc_ratio;
    Q = 1 / bandwidth_ratio;
    alpha = sin(w0) / (2 * Q);
    cos_w0 = cos(w0);
    
    b0_bp = alpha;
    b1_bp = 0.0;
    b2_bp = -alpha;
    a0_bp = 1.0 + alpha;
    a1_bp = -2.0 * cos_w0;
    a2_bp = 1.0 - alpha;
    
    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;
    
    b = [b0, b1, b2];
    a = [1, a1, a2];
end

function phase_deg = calc_phase_at_freq(b, a, freq_ratio)
    omega = 2 * pi * freq_ratio;
    
    cos_w = cos(omega);
    sin_w = sin(omega);
    cos_2w = cos(2 * omega);
    sin_2w = sin(2 * omega);
    
    num_real = b(1) + b(2) * cos_w + b(3) * cos_2w;
    num_imag = -b(2) * sin_w - b(3) * sin_2w;
    
    den_real = 1 + a(2) * cos_w + a(3) * cos_2w;
    den_imag = -a(2) * sin_w - a(3) * sin_2w;
    
    den_sqr = den_real^2 + den_imag^2;
    if den_sqr < 1e-12
        phase_deg = 0;
        return;
    end
    
    H_real = (num_real * den_real + num_imag * den_imag) / den_sqr;
    H_imag = (num_imag * den_real - num_real * den_imag) / den_sqr;
    
    phase_rad = atan2(H_imag, H_real);
    phase_deg = phase_rad * 180 / pi;
    
    while phase_deg > 180
        phase_deg = phase_deg - 360;
    end
    while phase_deg < -180
        phase_deg = phase_deg + 360;
    end
end

function phase_table = calc_phase_table(b, a, freq_points)
    num_points = length(freq_points);
    phase_table = zeros(1, num_points);
    
    for i = 1:num_points
        phase_table(i) = calc_phase_at_freq(b, a, freq_points(i));
    end
end

function phase = get_phase_from_table(phase_table, freq_table, target_freq)
    [~, idx] = min(abs(freq_table - target_freq));
    phase = phase_table(idx);
end