%% 全空域四扇区动态先验鲁棒性测试 (多核并行加速版)
clear; clc; close all;
rng(2024); 

par = setpar(); 
N_ant = par.M; N_RF = par.Nrf;
clutter_angle = -15; a_clutter = gen_a(clutter_angle, par);
INR_linear = 10^(30/10); noise_power = 1;
R_interference = noise_power * eye(N_ant) + INR_linear * (a_clutter * a_clutter');

disp('正在构建全空域字典 (传统OMP使用)...');
dic_angles_full = -90:0.5:90; A_dic_full = zeros(N_ant, length(dic_angles_full));
for i = 1:length(dic_angles_full), A_dic_full(:,i) = gen_a(dic_angles_full(i), par) / sqrt(N_ant); end

angle_ranges = {0:1:10, 11:1:20, 21:1:30, 31:1:40};
snr_test_list = [0, 5, 10, 15, 20]; 
Monte_Carlo = 500;                  
L_snapshots = 100;
color_array = {'#0072BD', '#D95319', '#EDB120', '#7E2F8E', '#77AC30'}; 

disp('正在启动多核并行引擎 (parfor) 进行全空域极速测试...');
for fig_idx = 1:4
    test_angles = angle_ranges{fig_idx};
    fprintf('\n>>> 正在进行第 %d 张图的并行仿真：扇区 %d° ~ %d° ...\n', fig_idx, min(test_angles), max(test_angles));
    
    RMSE_DFT_All = zeros(length(snr_test_list), length(test_angles));
    RMSE_OMP_FULL_All = zeros(length(snr_test_list), length(test_angles));
    RMSE_OMP_All = zeros(length(snr_test_list), length(test_angles));
    
    figure('Name', sprintf('扇区 %d°~%d° 测试', min(test_angles), max(test_angles)), 'Color', 'w', 'Position', [50+fig_idx*50, 50+fig_idx*50, 850, 600]);
    hold on; grid on;
    
    for snr_idx = 1:length(snr_test_list)
        snr_test = snr_test_list(snr_idx);
        sig_power = 10^(snr_test/10); 
        
        err_dft = zeros(length(test_angles), Monte_Carlo);
        err_omp_full = zeros(length(test_angles), Monte_Carlo);
        err_omp = zeros(length(test_angles), Monte_Carlo);
        
        for a_idx = 1:length(test_angles)
            true_target_angle = test_angles(a_idx) + 0.04; 
            theta_prior = round(true_target_angle); window = 5; 
            
            dic_tsc = (theta_prior - window) : 0.1 : (theta_prior + window);
            A_dic_tsc = zeros(N_ant, length(dic_tsc));
            for i = 1:length(dic_tsc), A_dic_tsc(:,i) = gen_a(dic_tsc(i), par) / sqrt(N_ant); end
            
            angles_dft = linspace(theta_prior - window, theta_prior + window, N_RF);
            F_RF_dft = zeros(N_ant, N_RF);
            for i = 1:N_RF, F_RF_dft(:,i) = gen_a(angles_dft(i), par) / sqrt(N_ant); end
            
            a_prior_broad = zeros(N_ant, 1);
            for theta_b = (theta_prior - window) : 0.5 : (theta_prior + window)
                a_prior_broad = a_prior_broad + gen_a(theta_b, par);
            end
            a_prior_broad = a_prior_broad / norm(a_prior_broad);
            w_dig_prior = inv(R_interference) * a_prior_broad; w_dig_prior = w_dig_prior / norm(w_dig_prior);
            
            res_full = w_dig_prior; F_RF_omp_full = [];
            for k = 1:N_RF
                proj = A_dic_full' * res_full; [~, midx] = max(abs(proj));
                F_RF_omp_full = [F_RF_omp_full, A_dic_full(:, midx)]; res_full = w_dig_prior - F_RF_omp_full * (pinv(F_RF_omp_full)*w_dig_prior);
            end
            res_tsc = w_dig_prior; F_RF_omp = [];
            for k = 1:N_RF
                proj = A_dic_tsc' * res_tsc; [~, midx] = max(abs(proj));
                F_RF_omp = [F_RF_omp, A_dic_tsc(:, midx)]; res_tsc = w_dig_prior - F_RF_omp * (pinv(F_RF_omp)*w_dig_prior);
            end
            
            scan_grid = (theta_prior - window) : 0.01 : (theta_prior + window);
            
            % 为 parfor 准备的切片临时变量
            err_dft_tmp = zeros(1, Monte_Carlo); err_omp_full_tmp = zeros(1, Monte_Carlo); err_omp_tmp = zeros(1, Monte_Carlo);
            
            % 【核心提速】：parfor 多核并行循环！
            parfor mc = 1:Monte_Carlo
                s_t = sqrt(sig_power/2)*(randn(1,L_snapshots) + 1j*randn(1,L_snapshots));
                s_c = sqrt(INR_linear/2)*(randn(1,L_snapshots) + 1j*randn(1,L_snapshots));
                n_t = sqrt(noise_power/2)*(randn(N_ant,L_snapshots) + 1j*randn(N_ant,L_snapshots));
                X = gen_a(true_target_angle, par)*s_t + a_clutter*s_c + n_t;
                
                Y_dft = F_RF_dft' * X; Y_omp_full = F_RF_omp_full' * X; Y_omp = F_RF_omp' * X;
                R_dft = (Y_dft*Y_dft')/L_snapshots; R_dft = R_dft + (0.05*trace(R_dft)/N_RF)*eye(N_RF);
                R_omp_full = (Y_omp_full*Y_omp_full')/L_snapshots; R_omp_full = R_omp_full + (0.05*trace(R_omp_full)/N_RF)*eye(N_RF);
                R_omp = (Y_omp*Y_omp')/L_snapshots; R_omp = R_omp + (0.05*trace(R_omp)/N_RF)*eye(N_RF);
                
                inv_dft = inv(R_dft); inv_full = inv(R_omp_full); inv_tsc = inv(R_omp);
                P_dft = zeros(1, length(scan_grid)); P_full = zeros(1, length(scan_grid)); P_tsc = zeros(1, length(scan_grid));
                
                for s_idx = 1:length(scan_grid)
                    a_test = gen_a(scan_grid(s_idx), par);
                    P_dft(s_idx) = 1/abs((F_RF_dft'*a_test)'*inv_dft*(F_RF_dft'*a_test));
                    P_full(s_idx) = 1/abs((F_RF_omp_full'*a_test)'*inv_full*(F_RF_omp_full'*a_test));
                    P_tsc(s_idx) = 1/abs((F_RF_omp'*a_test)'*inv_tsc*(F_RF_omp'*a_test));
                end
                
                [~, m1] = max(P_dft); err_dft_tmp(mc) = (scan_grid(m1) - true_target_angle)^2;
                [~, m2] = max(P_full); err_omp_full_tmp(mc) = (scan_grid(m2) - true_target_angle)^2;
                [~, m3] = max(P_tsc); err_omp_tmp(mc) = (scan_grid(m3) - true_target_angle)^2;
            end
            err_dft(a_idx, :) = err_dft_tmp; err_omp_full(a_idx, :) = err_omp_full_tmp; err_omp(a_idx, :) = err_omp_tmp;
        end
        RMSE_DFT_All(snr_idx, :) = sqrt(mean(err_dft, 2))'; RMSE_OMP_FULL_All(snr_idx, :) = sqrt(mean(err_omp_full, 2))'; RMSE_OMP_All(snr_idx, :) = sqrt(mean(err_omp, 2))';
        
        c_str = color_array{snr_idx};
        semilogy(test_angles, RMSE_DFT_All(snr_idx, :), '--^', 'Color', c_str, 'LineWidth', 1.5, 'MarkerSize', 5);
        semilogy(test_angles, RMSE_OMP_FULL_All(snr_idx, :), ':x', 'Color', c_str, 'LineWidth', 2, 'MarkerSize', 7);
        semilogy(test_angles, RMSE_OMP_All(snr_idx, :), '-o', 'Color', c_str, 'LineWidth', 2.5, 'MarkerSize', 6);
    end
    xlim([min(test_angles), max(test_angles)]); xticks(min(test_angles):1:max(test_angles));
    set(gca, 'YScale', 'log', 'YMinorGrid', 'on', 'XMinorGrid', 'off', 'FontSize', 12); ylim([1e-3, 20]); yticks(10.^(-3:1:2));         
    xlabel('目标真实出现角度 \theta (\circ)', 'FontSize', 14, 'FontWeight', 'bold'); ylabel('测角均方根误差 RMSE (\circ)', 'FontSize', 14, 'FontWeight', 'bold'); title(sprintf('宽波束先验引导下，扇区 (%d°~%d°) 动态波门鲁棒性评估', min(test_angles), max(test_angles)), 'FontSize', 15, 'FontWeight', 'bold');
    
    legend_str = {};
    for snr_idx = 1:length(snr_test_list)
        legend_str{end+1} = sprintf('SNR=%ddB, DFT', snr_test_list(snr_idx)); legend_str{end+1} = sprintf('SNR=%ddB, 传统OMP(发散)', snr_test_list(snr_idx)); legend_str{end+1} = sprintf('SNR=%ddB, TSC-OMP(稳健)', snr_test_list(snr_idx));
    end
    lgd = legend(legend_str, 'Location', 'southoutside', 'FontSize', 10); set(lgd, 'NumColumns', 3); 
end
disp('全部四组扇区极速测试完毕！');