import torch
import numpy as np
import matplotlib.pyplot as plt
import time

# ==========================================
# 0. 全局设置与 CUDA 引擎
# ==========================================
plt.rcParams['axes.unicode_minus'] = False
plt.rcParams['font.sans-serif'] = ['Microsoft YaHei']
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")

# 锁定随机数种子，对标 MATLAB 的 rng(2024)
torch.manual_seed(2024)
np.random.seed(2024)

# 颜色设置对标 MATLAB
c_ana = '#77AC30'
c_dig = '#000000'
c_dft = '#4DBEEE'
c_omp = '#D95319'
c_tsc = '#0072BD'

# ==========================================
# 1. 物理层参数初始化 (对标 setpar.m)
# ==========================================
N_ant, N_RF = 24, 4
target_angle = 5.0
clutter_angle = -15.0
L_snapshots = 100
noise_power = 1.0
INR_linear = 10**(30/10)

fc = 4.9e9
lam = 3e8 / fc
# 完全复刻 MATLAB 中的非均匀阵列 (NULA) 物理间隔
a = np.arange(0, 11*43 + 1, 43)
b = np.array([11*43 + 63])
c = 11*43 + 63 + np.arange(43, 11*43 + 1, 43)
delta_d = 1e-3 * np.concatenate((a, b, c))  # Shape: (24,)
delta_d_tensor = torch.tensor(delta_d, dtype=torch.float32, device=device).unsqueeze(1) # Shape: (24, 1)

def gen_a(theta_deg):
    """对标 gen_a.m, 注意此处的 -1j 以及物理位置 delta_d"""
    theta_rad = torch.deg2rad(torch.tensor(theta_deg, dtype=torch.float32, device=device))
    return torch.exp(-1j * 2 * torch.pi * delta_d_tensor * torch.sin(theta_rad) / lam)

a_target = gen_a(target_angle)
a_clutter = gen_a(clutter_angle)

# ==========================================
# 2. 动态生成目标约束扇区与字典
# ==========================================
theta_coarse_prior = round(target_angle)
window_half_width = 5.0

dic_angles_constrained = torch.arange(theta_coarse_prior - window_half_width, theta_coarse_prior + window_half_width + 0.1, 0.1, device=device)
A_dic_constrained = torch.cat([gen_a(th) / np.sqrt(N_ant) for th in dic_angles_constrained], dim=1)

dic_angles_full = torch.arange(-90, 90.5, 0.5, device=device)
A_dic_full = torch.cat([gen_a(th) / np.sqrt(N_ant) for th in dic_angles_full], dim=1)

angles_dft = torch.linspace(theta_coarse_prior - window_half_width, theta_coarse_prior + window_half_width, N_RF, device=device)
F_RF_dft = torch.cat([gen_a(th) / np.sqrt(N_ant) for th in angles_dft], dim=1)

# ==========================================
# 3. 提取防发散的慢时间先验射频矩阵 F_RF
# ==========================================
print('正在进行慢时间 (Slow-Time) 模拟射频网络硬件配置...')
a_prior_broad = torch.zeros((N_ant, 1), dtype=torch.complex64, device=device)
for theta_b in torch.arange(theta_coarse_prior - window_half_width, theta_coarse_prior + window_half_width + 0.1, 0.5, device=device):
    a_prior_broad += gen_a(theta_b)
a_prior_broad /= torch.norm(a_prior_broad)

R_interference_prior = noise_power * torch.eye(N_ant, device=device) + INR_linear * (a_clutter @ a_clutter.mH)
w_digital_prior = torch.linalg.inv(R_interference_prior) @ a_prior_broad
w_digital_prior /= torch.norm(w_digital_prior)

def extract_omp(w_opt, A_dic):
    res = w_opt.clone()
    F_RF_mat = None
    for k in range(N_RF):
        proj = A_dic.mH @ res
        idx = torch.argmax(torch.abs(proj))
        new_atom = A_dic[:, idx:idx+1]
        F_RF_mat = new_atom if F_RF_mat is None else torch.cat((F_RF_mat, new_atom), dim=1)
        res = w_digital_prior - F_RF_mat @ (torch.linalg.pinv(F_RF_mat) @ w_digital_prior)
    return F_RF_mat

F_RF_omp_full_static = extract_omp(w_digital_prior, A_dic_full)
F_RF_omp_static = extract_omp(w_digital_prior, A_dic_constrained)

# ==========================================
# 4. 计算波束方向图与理论 SINR
# ==========================================
print('正在计算静态波束方向图与理论 SINR...')
w_analog = a_target / np.sqrt(N_ant)

def calc_hybrid_weight(F_RF, R_in_stat):
    a_eff = F_RF.mH @ a_target
    R_eff = F_RF.mH @ R_in_stat @ F_RF
    R_eff += (0.05 * torch.trace(R_eff).real / N_RF) * torch.eye(N_RF, device=device)
    inv_R_eff = torch.linalg.inv(R_eff)
    F_BB = (inv_R_eff @ a_eff) / (a_eff.mH @ inv_R_eff @ a_eff)
    w_hyb = F_RF @ F_BB
    return w_hyb / torch.norm(w_hyb)


#随机数
#sig_power_static = 10**(0/10)
#s_t_st = np.sqrt(sig_power_static/2) * (torch.randn(1, L_snapshots, device=device) + 1j*torch.randn(1, L_snapshots, device=device))
#s_c_st = np.sqrt(INR_linear/2) * (torch.randn(1, L_snapshots, device=device) + 1j*torch.randn(1, L_snapshots, device=device))
#n_t_st = np.sqrt(noise_power/2) * (torch.randn(N_ant, L_snapshots, device=device) + 1j*torch.randn(N_ant, L_snapshots, device=device))
#X_static = a_target @ s_t_st + a_clutter @ s_c_st + n_t_st
#R_in_static = (X_static @ X_static.mH) / L_snapshots

sig_power_static = 10**(0/10)
# 直接使用无穷大快拍下的理论理想协方差矩阵，消除所有随机扰动
R_in_static = noise_power * torch.eye(N_ant, device=device) + \
              sig_power_static * (a_target @ a_target.mH) + \
              INR_linear * (a_clutter @ a_clutter.mH)

w_hybrid_dft = calc_hybrid_weight(F_RF_dft, R_in_static)
w_hybrid_omp_full = calc_hybrid_weight(F_RF_omp_full_static, R_in_static)
w_hybrid_omp = calc_hybrid_weight(F_RF_omp_static, R_in_static)

R_in_static_dl = R_in_static + (0.01 * torch.trace(R_in_static).real / N_ant) * torch.eye(N_ant, device=device)
inv_R_stat = torch.linalg.inv(R_in_static_dl)
w_digital_ideal = (inv_R_stat @ a_target) / (a_target.mH @ inv_R_stat @ a_target)
w_digital_ideal /= torch.norm(w_digital_ideal)

theta_scan = torch.arange(-90, 90.1, 0.1, device=device)
A_scan_full = torch.cat([gen_a(th) for th in theta_scan], dim=1)

def get_bp(w): return 10 * torch.log10((torch.abs(A_scan_full.mH @ w)**2).squeeze() / torch.max(torch.abs(A_scan_full.mH @ w)**2))
BP_Ana = get_bp(w_analog).cpu().numpy()
BP_Dig = get_bp(w_digital_ideal).cpu().numpy()
BP_DFT = get_bp(w_hybrid_dft).cpu().numpy()
BP_OMP_FULL = get_bp(w_hybrid_omp_full).cpu().numpy()
BP_OMP = get_bp(w_hybrid_omp).cpu().numpy()
theta_scan_np = theta_scan.cpu().numpy()

SNR_dB_range = np.arange(-10, 22, 2)
SINR_Ana, SINR_Dig, SINR_DFT, SINR_OMP_FULL, SINR_OMP = [], [], [], [], []
for snr in SNR_dB_range:
    sig_p = 10**(snr/10)
    def calc_sinr(w): return (sig_p * torch.abs(w.mH @ a_target)**2) / torch.real(w.mH @ R_interference_prior @ w)
    SINR_Ana.append(10 * torch.log10(calc_sinr(w_analog)).item())
    SINR_Dig.append(10 * torch.log10(calc_sinr(w_digital_ideal)).item())
    SINR_DFT.append(10 * torch.log10(calc_sinr(w_hybrid_dft)).item())
    SINR_OMP_FULL.append(10 * torch.log10(calc_sinr(w_hybrid_omp_full)).item())
    SINR_OMP.append(10 * torch.log10(calc_sinr(w_hybrid_omp)).item())

# ==========================================
# 5. 基于张量 Batch 操作的极致蒙特卡洛评估
# ==========================================
print('正在启动 CUDA 高维张量批量计算 (Batched Evaluation)...')
Monte_Carlo = 500
scan_grid = torch.arange(theta_coarse_prior - window_half_width, theta_coarse_prior + window_half_width + 0.01, 0.01, device=device)
A_scan = torch.cat([gen_a(th) for th in scan_grid], dim=1) 

A_eff_dft = F_RF_dft.mH @ A_scan               
A_eff_omp_full = F_RF_omp_full_static.mH @ A_scan
A_eff_omp = F_RF_omp_static.mH @ A_scan

RMSE_Ana, RMSE_Dig, RMSE_DFT, RMSE_OMP_FULL, RMSE_OMP = [], [], [], [], []
start_time = time.time()

for snr in SNR_dB_range:
    sig_power = 10**(snr/10)
    s_t = np.sqrt(sig_power/2) * (torch.randn(Monte_Carlo, 1, L_snapshots, device=device) + 1j*torch.randn(Monte_Carlo, 1, L_snapshots, device=device))
    s_c = np.sqrt(INR_linear/2) * (torch.randn(Monte_Carlo, 1, L_snapshots, device=device) + 1j*torch.randn(Monte_Carlo, 1, L_snapshots, device=device))
    n_t = np.sqrt(noise_power/2) * (torch.randn(Monte_Carlo, N_ant, L_snapshots, device=device) + 1j*torch.randn(Monte_Carlo, N_ant, L_snapshots, device=device))
    
    X_mc = a_target.unsqueeze(0) * s_t + a_clutter.unsqueeze(0) * s_c + n_t
    R_in = torch.bmm(X_mc, X_mc.mH) / L_snapshots
    trace_R_in = torch.diagonal(R_in, dim1=-2, dim2=-1).sum(-1).real
    R_in_dl = R_in + (0.05 * trace_R_in / N_ant).view(-1, 1, 1) * torch.eye(N_ant, device=device).unsqueeze(0)
    inv_R_in_dl = torch.linalg.inv(R_in_dl)
    
    P_ana = torch.einsum('si, bij, js -> bs', A_scan.conj().T, R_in, A_scan).abs()
    P_dig = torch.einsum('si, bij, js -> bs', A_scan.conj().T, inv_R_in_dl, A_scan).abs()
    
    def process_hybrid_batched(F_RF, A_eff_hyb):
        Y = F_RF.mH.unsqueeze(0) @ X_mc
        R_hyb = torch.bmm(Y, Y.mH) / L_snapshots
        trace_R = torch.diagonal(R_hyb, dim1=-2, dim2=-1).sum(-1).real
        R_hyb += (0.05 * trace_R / N_RF).view(-1, 1, 1) * torch.eye(N_RF, device=device).unsqueeze(0)
        inv_R_hyb = torch.linalg.inv(R_hyb)
        P_hyb = torch.einsum('si, bij, js -> bs', A_eff_hyb.conj().T, inv_R_hyb, A_eff_hyb).abs()
        return scan_grid[torch.argmin(P_hyb, dim=1)] 

    ang_ana = scan_grid[torch.argmax(P_ana, dim=1)]
    ang_dig = scan_grid[torch.argmin(P_dig, dim=1)]
    ang_dft = process_hybrid_batched(F_RF_dft, A_eff_dft)
    ang_omp_full = process_hybrid_batched(F_RF_omp_full_static, A_eff_omp_full)
    ang_omp = process_hybrid_batched(F_RF_omp_static, A_eff_omp)

    RMSE_Ana.append(torch.sqrt(torch.mean((ang_ana - target_angle)**2)).item())
    RMSE_Dig.append(torch.sqrt(torch.mean((ang_dig - target_angle)**2)).item())
    RMSE_DFT.append(torch.sqrt(torch.mean((ang_dft - target_angle)**2)).item())
    RMSE_OMP_FULL.append(torch.sqrt(torch.mean((ang_omp_full - target_angle)**2)).item())
    RMSE_OMP.append(torch.sqrt(torch.mean((ang_omp - target_angle)**2)).item())

print(f"CUDA 并行测角完毕！耗时: {time.time() - start_time:.4f} 秒。")

# ==========================================
# 6. 绘图：单图输出与汇总输出
# ==========================================
bp_data = [BP_Ana, BP_Dig, BP_DFT, BP_OMP_FULL, BP_OMP]
bp_labels = ['纯模拟架构', '纯数字架构(性能上限)', 'DFT混合架构', '传统OMP混合架构', 'TSC-OMP混合架构(所提)']
bp_colors = [c_ana, c_dig, c_dft, c_omp, c_tsc]
bp_ls = ['-.', '-', '--', ':', '-']

# 6.1 分别绘制 5 张独立的波束方向图
for i in range(5):
    fig, ax = plt.subplots(figsize=(8, 6))
    ax.plot(theta_scan_np, bp_data[i], color=bp_colors[i], linestyle=bp_ls[i], linewidth=2.5, label='主波束')
    ax.axvline(x=target_angle, color='g', linestyle='--', linewidth=2, label=f'机动目标 ({target_angle}°)')
    ax.axvline(x=clutter_angle, color='r', linestyle='--', linewidth=2, label=f'强杂波 ({clutter_angle}°)')
    ax.set_ylim([-60, 0])
    ax.set_xlim([-60, 60])
    ax.grid(True, linestyle='--', alpha=0.6)
    ax.set_title(f'波束方向图 - {bp_labels[i]}', fontsize=14, fontweight='bold')
    ax.set_xlabel(r'角度 $\theta (^\circ)$', fontsize=12)
    ax.set_ylabel('归一化增益 (dB)', fontsize=12)
    
    # 调整图例到下方
    plt.tight_layout()
    fig.subplots_adjust(bottom=0.22)
    ax.legend(loc='upper center', bbox_to_anchor=(0.5, -0.12), ncol=3, fontsize=10)
    
    file_name = f'BP_Single_{i+1}_{bp_labels[i].split("(")[0]}.png'
    plt.savefig(file_name, dpi=300, bbox_inches='tight')

# 6.2 绘制五大架构综合对比波束图
fig_all, ax_all = plt.subplots(figsize=(10, 6))
for i in range(5):
    ax_all.plot(theta_scan_np, bp_data[i], color=bp_colors[i], linestyle=bp_ls[i], linewidth=2.5, label=bp_labels[i].split("(")[0])
ax_all.axvline(x=target_angle, color='g', linestyle='--', linewidth=2)
ax_all.axvline(x=clutter_angle, color='r', linestyle='--', linewidth=2)
ax_all.text(target_angle + 1, -5, f'机动目标 ({target_angle}°)', color='g', fontweight='bold')
ax_all.text(clutter_angle - 1, -5, f'强杂波 ({clutter_angle}°)', color='r', fontweight='bold', ha='right')
ax_all.set_ylim([-60, 0])
ax_all.set_xlim([-60, 60])
ax_all.grid(True, linestyle='--', alpha=0.6)
ax_all.set_title('五大架构波束方向图综合对比', fontsize=14, fontweight='bold')
ax_all.set_xlabel(r'角度 $\theta (^\circ)$', fontsize=12)
ax_all.set_ylabel('归一化增益 (dB)', fontsize=12)

plt.tight_layout()
fig_all.subplots_adjust(bottom=0.22)
ax_all.legend(loc='upper center', bbox_to_anchor=(0.5, -0.12), ncol=5, fontsize=10)
plt.savefig('BP_All_Combined.png', dpi=300, bbox_inches='tight')

# 6.3 绘制 SINR 对比图
fig_sinr, ax_sinr = plt.subplots(figsize=(10, 6))
ax_sinr.plot(SNR_dB_range, SINR_Ana, '-v', color=c_ana, linewidth=2, label='纯模拟')
ax_sinr.plot(SNR_dB_range, SINR_Dig, '-o', color=c_dig, linewidth=2, label='纯数字(上限)')
ax_sinr.plot(SNR_dB_range, SINR_DFT, '-d', color=c_dft, linewidth=2, label='DFT')
ax_sinr.plot(SNR_dB_range, SINR_OMP_FULL, '-*', color=c_omp, linewidth=2, label='传统OMP')
ax_sinr.plot(SNR_dB_range, SINR_OMP, '-s', color=c_tsc, linewidth=2.5, label='TSC-OMP(所提)')
ax_sinr.grid(True, linestyle='--', alpha=0.6)
ax_sinr.set_title(f'全架构输出 SINR 理论逼近测试 (目标 {target_angle}°)', fontsize=14, fontweight='bold')
ax_sinr.set_xlabel('输入 SNR (dB)', fontsize=12)
ax_sinr.set_ylabel('输出 SINR (dB)', fontsize=12)

plt.tight_layout()
fig_sinr.subplots_adjust(bottom=0.22)
ax_sinr.legend(loc='upper center', bbox_to_anchor=(0.5, -0.12), ncol=5, fontsize=10)
plt.savefig('SINR_All_Combined.png', dpi=300, bbox_inches='tight')

# 6.4 绘制 RMSE 对比图
fig_rmse, ax_rmse = plt.subplots(figsize=(10, 6))
ax_rmse.semilogy(SNR_dB_range, RMSE_Ana, '-.v', color=c_ana, linewidth=2, label='纯模拟')
ax_rmse.semilogy(SNR_dB_range, RMSE_Dig, '-o', color=c_dig, linewidth=2, label='纯数字(上限)')
ax_rmse.semilogy(SNR_dB_range, RMSE_DFT, '--d', color=c_dft, linewidth=2, label='DFT')
ax_rmse.semilogy(SNR_dB_range, RMSE_OMP_FULL, ':*', color=c_omp, linewidth=2.5, label='传统OMP')
ax_rmse.semilogy(SNR_dB_range, RMSE_OMP, '-s', color=c_tsc, linewidth=2.5, label='TSC-OMP (所提)')
ax_rmse.grid(True, which="both", ls="--", alpha=0.6)
ax_rmse.set_ylim([1e-3, 20])
ax_rmse.set_title(f'全架构实战恶劣环境 DOA 测角精度评估 (机动目标 {target_angle}°)', fontsize=14, fontweight='bold')
ax_rmse.set_xlabel('信噪比 SNR (dB)', fontsize=12)
ax_rmse.set_ylabel(r'测角 RMSE $(^\circ)$', fontsize=12)

plt.tight_layout()
fig_rmse.subplots_adjust(bottom=0.22)
ax_rmse.legend(loc='upper center', bbox_to_anchor=(0.5, -0.12), ncol=5, fontsize=10)
plt.savefig('RMSE_All_Combined.png', dpi=300, bbox_inches='tight')

plt.show()
print('计算完毕，单图与综合图表均已独立生成。')