import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
import os

# ============================================================
# 0. 全局绘图设置（尽量兼容中文）
# ============================================================
plt.rcParams['font.sans-serif'] = [
    'Microsoft YaHei', 'SimHei', 'PingFang SC',
    'Noto Sans CJK SC', 'Arial Unicode MS', 'DejaVu Sans'
]
plt.rcParams['axes.unicode_minus'] = False

# ============================================================
# 1. 用户参数区（你可以根据论文需要自行调整）
# ============================================================
ARRAY_TYPE = 'ULA'          # 'ULA' 或 'NULA'
N_ant = 24                  # 阵元数
N_RF = 4                    # 射频链数
d_lambda = 0.5              # 阵元间距（单位：波长），ULA 用
target_angle = 5.0         # 目标角度
clutter_angle = -15.0       # 强杂波角度
sector_min = 0.0            # TSC-OMP 扇区下界
sector_max = 10.0           # TSC-OMP 扇区上界
clutter_band_half = 2.0     # 杂波邻域半宽（用于统计“杂波误选率”）
noise_power = 1.0
SNR_dB = -15.0              # 故意设置得较低，便于显现伪峰
INR_dB = 35.0               # 强干扰
L_snapshots = 48            # 快拍数，故意设置得不高，便于显现伪峰
diag_loading_ratio = 0.01   # 对角加载比例
MC = 400                    # Monte Carlo 次数

# 字典设置
full_grid = np.arange(-90.0, 90.0 + 0.5, 0.5)    # 传统OMP：全空间字典
tsc_grid = np.arange(sector_min, sector_max + 0.1, 0.1)  # TSC-OMP：局部字典

# 宽波束先验
broad_step = 0.5

# 输出文件夹
save_dir = 'pseudo_peak_results'
os.makedirs(save_dir, exist_ok=True)


# ============================================================
# 2. 阵列位置定义
# ============================================================
def get_array_positions(array_type='ULA', N=24, d=0.5):
    """
    返回阵列位置（单位：波长）
    """
    if array_type.upper() == 'ULA':
        pos = np.arange(-(N - 1) / 2, (N - 1) / 2 + 1) * d
        return pos.astype(float)

    elif array_type.upper() == 'NULA':
        # 一个示例性的 NULA 阵列位置（非均匀、递增排序、近似关于0分布）
        # 你也可以改成与你论文代码完全一致的 NULA 位置
        pos = np.array([
            -5.75, -5.10, -4.40, -3.85, -3.15, -2.60,
            -2.05, -1.45, -0.95, -0.35,  0.20,  0.75,
             1.25,  1.85,  2.45,  3.05,  3.70,  4.30,
             4.95,  5.65,  6.35,  7.05,  7.85,  8.70
        ])
        pos = pos - np.mean(pos)
        return pos.astype(float)

    else:
        raise ValueError("ARRAY_TYPE 只能是 'ULA' 或 'NULA'")


array_pos = get_array_positions(ARRAY_TYPE, N_ant, d_lambda)


# ============================================================
# 3. 基本函数
# ============================================================
def steering_vector(theta_deg, array_pos):
    """
    阵列导向矢量 a(theta)
    theta_deg : 角度（度）
    array_pos : 阵元位置（单位：波长）
    """
    theta_rad = np.deg2rad(theta_deg)
    phase = 2 * np.pi * array_pos * np.sin(theta_rad)
    a = np.exp(1j * phase).reshape(-1, 1)
    return a


def build_dictionary(angle_grid, array_pos, normalize=True):
    """
    构造字典矩阵 A = [a(theta1), a(theta2), ...]
    """
    A = np.hstack([steering_vector(ang, array_pos) for ang in angle_grid])
    if normalize:
        A = A / np.sqrt(A.shape[0])
    return A


def omp_trace(w_opt, A_dic, angle_grid, N_RF):
    """
    传统 OMP / TSC-OMP 的公共实现：只是在字典上不同

    返回：
        F_RF        : 最终模拟矩阵
        chosen_ang  : 每一步选中的角度
        proj_hist   : 每一步投影谱 |A^H r|
    """
    res = w_opt.copy()
    F_RF = np.zeros((A_dic.shape[0], 0), dtype=np.complex128)
    chosen_ang = []
    proj_hist = []

    for _ in range(N_RF):
        proj = np.abs(A_dic.conj().T @ res).flatten()
        proj_hist.append(proj.copy())

        idx = np.argmax(proj)
        chosen_ang.append(float(angle_grid[idx]))

        atom = A_dic[:, idx:idx+1]
        F_RF = np.hstack((F_RF, atom))

        coeff = np.linalg.pinv(F_RF) @ w_opt
        res = w_opt - F_RF @ coeff

    return F_RF, np.array(chosen_ang), proj_hist


def build_broad_prior_vector(array_pos, sector_min, sector_max, step=0.5):
    """
    宽波束先验导向向量：对目标扇区做叠加
    """
    a_broad = np.zeros((len(array_pos), 1), dtype=np.complex128)
    for th in np.arange(sector_min, sector_max + 1e-12, step):
        a_broad += steering_vector(th, array_pos)
    a_broad /= np.linalg.norm(a_broad)
    return a_broad


def build_sample_prior(array_pos,
                       target_angle,
                       clutter_angle,
                       SNR_dB,
                       INR_dB,
                       noise_power,
                       L_snapshots,
                       diag_loading_ratio,
                       sector_min,
                       sector_max,
                       broad_step=0.5):
    """
    构造有限快拍样本协方差下的先验数字权值 w_prior
    这是让传统 OMP 更容易显现伪峰风险的关键
    """
    N_ant = len(array_pos)
    sig_p = 10 ** (SNR_dB / 10.0)
    INR_linear = 10 ** (INR_dB / 10.0)

    a_t = steering_vector(target_angle, array_pos)
    a_c = steering_vector(clutter_angle, array_pos)

    s_t = np.sqrt(sig_p / 2) * (
        np.random.randn(1, L_snapshots) + 1j * np.random.randn(1, L_snapshots)
    )
    s_c = np.sqrt(INR_linear / 2) * (
        np.random.randn(1, L_snapshots) + 1j * np.random.randn(1, L_snapshots)
    )
    n_t = np.sqrt(noise_power / 2) * (
        np.random.randn(N_ant, L_snapshots) + 1j * np.random.randn(N_ant, L_snapshots)
    )

    X = a_t @ s_t + a_c @ s_c + n_t
    R_hat = (X @ X.conj().T) / L_snapshots

    dl = diag_loading_ratio * np.real(np.trace(R_hat)) / N_ant
    R_hat = R_hat + dl * np.eye(N_ant)

    a_broad = build_broad_prior_vector(array_pos, sector_min, sector_max, broad_step)

    # 用低快拍样本协方差构造“先验数字权值”
    w_prior = np.linalg.solve(R_hat, a_broad)
    w_prior /= np.linalg.norm(w_prior)

    return w_prior


def count_stats(chosen_angles, sector_min, sector_max, clutter_angle, clutter_band_half):
    """
    统计：
        P_hit      : 目标扇区命中率
        P_clutter  : 杂波误选率
        P_out      : 扇区外误选率
    """
    chosen_angles = np.array(chosen_angles)

    hit = np.sum((chosen_angles >= sector_min) & (chosen_angles <= sector_max))
    clutter = np.sum((chosen_angles >= clutter_angle - clutter_band_half) &
                     (chosen_angles <= clutter_angle + clutter_band_half))
    out = np.sum((chosen_angles < sector_min) | (chosen_angles > sector_max))

    return hit, clutter, out


def compute_badness_score(proj, angle_grid, sector_min, sector_max, clutter_angle, clutter_band_half):
    """
    用于自动挑选一个“传统OMP伪峰最明显”的代表性实验样本
    """
    proj = np.array(proj).flatten()

    target_mask = (angle_grid >= sector_min) & (angle_grid <= sector_max)
    clutter_mask = (angle_grid >= clutter_angle - clutter_band_half) & (angle_grid <= clutter_angle + clutter_band_half)
    out_mask = ~target_mask

    target_peak = np.max(proj[target_mask]) if np.any(target_mask) else 1e-12
    clutter_peak = np.max(proj[clutter_mask]) if np.any(clutter_mask) else 0.0
    out_peak = np.max(proj[out_mask]) if np.any(out_mask) else 0.0

    score = max(clutter_peak / (target_peak + 1e-12),
                out_peak / (target_peak + 1e-12))
    return score


# ============================================================
# 4. 构造字典
# ============================================================
A_dic_full = build_dictionary(full_grid, array_pos, normalize=True)
A_dic_tsc = build_dictionary(tsc_grid, array_pos, normalize=True)

# ============================================================
# 5. Monte Carlo 验证
# ============================================================
all_angles_full = []
all_angles_tsc = []

full_hit_total = 0
full_clutter_total = 0
full_out_total = 0

tsc_hit_total = 0
tsc_clutter_total = 0
tsc_out_total = 0

# 保存一个“代表性最差样本”，用于画第一步投影谱
best_bad_score = -1.0
rep_full_proj0 = None
rep_tsc_proj0 = None
rep_full_chosen = None
rep_tsc_chosen = None

for mc in range(MC):
    w_prior = build_sample_prior(
        array_pos=array_pos,
        target_angle=target_angle,
        clutter_angle=clutter_angle,
        SNR_dB=SNR_dB,
        INR_dB=INR_dB,
        noise_power=noise_power,
        L_snapshots=L_snapshots,
        diag_loading_ratio=diag_loading_ratio,
        sector_min=sector_min,
        sector_max=sector_max,
        broad_step=broad_step
    )

    # 传统OMP（全空间字典）
    _, chosen_full, proj_hist_full = omp_trace(w_prior, A_dic_full, full_grid, N_RF)

    # TSC-OMP（目标扇区局部字典）
    _, chosen_tsc, proj_hist_tsc = omp_trace(w_prior, A_dic_tsc, tsc_grid, N_RF)

    all_angles_full.extend(chosen_full.tolist())
    all_angles_tsc.extend(chosen_tsc.tolist())

    h, c, o = count_stats(chosen_full, sector_min, sector_max, clutter_angle, clutter_band_half)
    full_hit_total += h
    full_clutter_total += c
    full_out_total += o

    h, c, o = count_stats(chosen_tsc, sector_min, sector_max, clutter_angle, clutter_band_half)
    tsc_hit_total += h
    tsc_clutter_total += c
    tsc_out_total += o

    # 选一个“传统OMP最容易看到伪峰”的样本
    cur_score = compute_badness_score(proj_hist_full[0], full_grid,
                                      sector_min, sector_max,
                                      clutter_angle, clutter_band_half)
    # 优先考虑“传统OMP至少有一个原子落在扇区外或杂波邻域”的样本
    bad_event = np.any((chosen_full < sector_min) | (chosen_full > sector_max)) or \
                np.any((chosen_full >= clutter_angle - clutter_band_half) &
                       (chosen_full <= clutter_angle + clutter_band_half))

    if bad_event and cur_score > best_bad_score:
        best_bad_score = cur_score
        rep_full_proj0 = proj_hist_full[0]
        rep_tsc_proj0 = proj_hist_tsc[0]
        rep_full_chosen = chosen_full.copy()
        rep_tsc_chosen = chosen_tsc.copy()

# 如果一个“明确坏样本”都没抓到，就退而选投影谱最坏的那一个
if rep_full_proj0 is None:
    # 再跑一遍，抓 score 最高的
    best_bad_score = -1.0
    for mc in range(MC):
        w_prior = build_sample_prior(
            array_pos=array_pos,
            target_angle=target_angle,
            clutter_angle=clutter_angle,
            SNR_dB=SNR_dB,
            INR_dB=INR_dB,
            noise_power=noise_power,
            L_snapshots=L_snapshots,
            diag_loading_ratio=diag_loading_ratio,
            sector_min=sector_min,
            sector_max=sector_max,
            broad_step=broad_step
        )
        _, chosen_full, proj_hist_full = omp_trace(w_prior, A_dic_full, full_grid, N_RF)
        _, chosen_tsc, proj_hist_tsc = omp_trace(w_prior, A_dic_tsc, tsc_grid, N_RF)

        cur_score = compute_badness_score(proj_hist_full[0], full_grid,
                                          sector_min, sector_max,
                                          clutter_angle, clutter_band_half)
        if cur_score > best_bad_score:
            best_bad_score = cur_score
            rep_full_proj0 = proj_hist_full[0]
            rep_tsc_proj0 = proj_hist_tsc[0]
            rep_full_chosen = chosen_full.copy()
            rep_tsc_chosen = chosen_tsc.copy()

# ============================================================
# 6. 统计量计算
# ============================================================
total_selections = MC * N_RF

P_hit_full = full_hit_total / total_selections
P_clutter_full = full_clutter_total / total_selections
P_out_full = full_out_total / total_selections

P_hit_tsc = tsc_hit_total / total_selections
P_clutter_tsc = tsc_clutter_total / total_selections
P_out_tsc = tsc_out_total / total_selections

print("==================================================")
print(f"阵列类型: {ARRAY_TYPE}")
print(f"SNR = {SNR_dB:.1f} dB, INR = {INR_dB:.1f} dB, 快拍数 = {L_snapshots}, MC = {MC}")
print("--------------------------------------------------")
print("传统 OMP（全空间字典）:")
print(f"目标扇区命中率 P_hit      = {P_hit_full:.4f}")
print(f"杂波误选率   P_clutter  = {P_clutter_full:.4f}")
print(f"扇区外误选率 P_out      = {P_out_full:.4f}")
print("--------------------------------------------------")
print("TSC-OMP（目标扇区约束字典）:")
print(f"目标扇区命中率 P_hit      = {P_hit_tsc:.4f}")
print(f"杂波误选率   P_clutter  = {P_clutter_tsc:.4f}")
print(f"扇区外误选率 P_out      = {P_out_tsc:.4f}")
print("==================================================")

# 保存统计结果到 txt
with open(os.path.join(save_dir, 'pseudo_peak_statistics.txt'), 'w', encoding='utf-8') as f:
    f.write(f"阵列类型: {ARRAY_TYPE}\n")
    f.write(f"SNR = {SNR_dB:.1f} dB, INR = {INR_dB:.1f} dB, 快拍数 = {L_snapshots}, MC = {MC}\n\n")
    f.write("传统 OMP（全空间字典）:\n")
    f.write(f"P_hit     = {P_hit_full:.6f}\n")
    f.write(f"P_clutter = {P_clutter_full:.6f}\n")
    f.write(f"P_out     = {P_out_full:.6f}\n\n")
    f.write("TSC-OMP（目标扇区约束字典）:\n")
    f.write(f"P_hit     = {P_hit_tsc:.6f}\n")
    f.write(f"P_clutter = {P_clutter_tsc:.6f}\n")
    f.write(f"P_out     = {P_out_tsc:.6f}\n")

# ============================================================
# 7. 画图1：第一步投影谱对比（最适合论文说明“伪峰”）
# ============================================================
fig1, axes = plt.subplots(2, 1, figsize=(10, 9))

# 上图：传统OMP第一步投影谱
ax = axes[0]
ax.plot(full_grid, rep_full_proj0, 'b-', linewidth=1.8, label='传统OMP第一步投影谱')
ax.axvline(target_angle, color='g', linestyle='--', linewidth=1.5, label=f'目标 {target_angle:.2f}°')
ax.axvline(clutter_angle, color='r', linestyle='--', linewidth=1.5, label=f'杂波 {clutter_angle:.2f}°')

# 标出目标扇区
ymax = 1.05 * np.max(rep_full_proj0)
rect = Rectangle((sector_min, 0), sector_max - sector_min, ymax,
                 facecolor='green', alpha=0.10, edgecolor=None)
ax.add_patch(rect)
ax.text((sector_min + sector_max) / 2, 0.93 * ymax, '目标扇区', color='green',
        ha='center', va='top', fontsize=11)

# 标出杂波邻域
rect2 = Rectangle((clutter_angle - clutter_band_half, 0), 2 * clutter_band_half, ymax,
                  facecolor='red', alpha=0.10, edgecolor=None)
ax.add_patch(rect2)
ax.text(clutter_angle, 0.80 * ymax, '杂波邻域', color='red',
        ha='center', va='top', fontsize=11)

# 标出传统OMP选中的角度
for idx, ang in enumerate(rep_full_chosen):
    val = rep_full_proj0[np.argmin(np.abs(full_grid - ang))]
    ax.plot(ang, val, 'ko')
    ax.text(ang, val + 0.02 * ymax, f'{idx+1}:{ang:.1f}°', fontsize=9, ha='center')

ax.set_title('传统OMP第一步投影谱', fontsize=13, fontweight='bold')
ax.set_xlabel('角度 (°)')
ax.set_ylabel(r'$|a^H(\theta)r^{(0)}|$')
ax.grid(True, linestyle='--', alpha=0.5)
ax.set_xlim([-90, 90])
ax.legend(loc='upper right', fontsize=10)

# 下图：TSC-OMP第一步投影谱
ax = axes[1]
ax.plot(tsc_grid, rep_tsc_proj0, 'm-', linewidth=1.8, label='TSC-OMP第一步投影谱')
ax.axvline(target_angle, color='g', linestyle='--', linewidth=1.5, label=f'目标 {target_angle:.2f}°')

for idx, ang in enumerate(rep_tsc_chosen):
    val = rep_tsc_proj0[np.argmin(np.abs(tsc_grid - ang))]
    ax.plot(ang, val, 'ko')
    ax.text(ang, val + 0.02 * np.max(rep_tsc_proj0), f'{idx+1}:{ang:.1f}°', fontsize=9, ha='center')

ax.set_title('TSC-OMP第一步投影谱', fontsize=13, fontweight='bold')
ax.set_xlabel('角度 (°)')
ax.set_ylabel(r'$|a^H(\theta)r^{(0)}|$')
ax.grid(True, linestyle='--', alpha=0.5)
ax.set_xlim([sector_min, sector_max])
ax.legend(loc='upper right', fontsize=10)

plt.tight_layout()
fig1.savefig(os.path.join(save_dir, f'Fig1_projection_compare_{ARRAY_TYPE}.png'), dpi=300, bbox_inches='tight')

# ============================================================
# 8. 画图2：被选原子角度分布直方图
# ============================================================
fig2, axes = plt.subplots(2, 1, figsize=(10, 8), sharex=False)

# 传统OMP
ax = axes[0]
bins_full = np.arange(-90, 90 + 1, 1.0)
ax.hist(all_angles_full, bins=bins_full, color='steelblue', edgecolor='black', alpha=0.85)
ax.axvline(target_angle, color='g', linestyle='--', linewidth=1.5, label='目标')
ax.axvline(clutter_angle, color='r', linestyle='--', linewidth=1.5, label='杂波')
ax.axvspan(sector_min, sector_max, color='green', alpha=0.10, label='目标扇区')
ax.set_title('传统OMP所选原子角度分布', fontsize=13, fontweight='bold')
ax.set_xlabel('被选原子角度 (°)')
ax.set_ylabel('出现次数')
ax.grid(True, linestyle='--', alpha=0.5)
ax.legend(fontsize=10)

# TSC-OMP
ax = axes[1]
bins_tsc = np.arange(sector_min, sector_max + 0.2, 0.2)
ax.hist(all_angles_tsc, bins=bins_tsc, color='orchid', edgecolor='black', alpha=0.85)
ax.axvline(target_angle, color='g', linestyle='--', linewidth=1.5, label='目标')
ax.axvspan(sector_min, sector_max, color='green', alpha=0.10, label='目标扇区')
ax.set_title('TSC-OMP所选原子角度分布', fontsize=13, fontweight='bold')
ax.set_xlabel('被选原子角度 (°)')
ax.set_ylabel('出现次数')
ax.grid(True, linestyle='--', alpha=0.5)
ax.legend(fontsize=10)

plt.tight_layout()
fig2.savefig(os.path.join(save_dir, f'Fig2_hist_compare_{ARRAY_TYPE}.png'), dpi=300, bbox_inches='tight')

# ============================================================
# 9. 画图3：伪峰统计指标柱状图
# ============================================================
fig3, ax = plt.subplots(figsize=(9, 6))

metrics = ['目标扇区命中率', '杂波误选率', '扇区外误选率']
omp_values = [P_hit_full, P_clutter_full, P_out_full]
tsc_values = [P_hit_tsc, P_clutter_tsc, P_out_tsc]

x = np.arange(len(metrics))
width = 0.34

bars1 = ax.bar(x - width/2, omp_values, width, label='传统OMP', color='steelblue')
bars2 = ax.bar(x + width/2, tsc_values, width, label='TSC-OMP', color='orchid')

for bars in [bars1, bars2]:
    for b in bars:
        h = b.get_height()
        ax.text(b.get_x() + b.get_width()/2, h + 0.01, f'{h:.3f}',
                ha='center', va='bottom', fontsize=10)

ax.set_xticks(x)
ax.set_xticklabels(metrics, fontsize=11)
ax.set_ylim([0, 1.08])
ax.set_ylabel('概率 / 比例', fontsize=12)
ax.set_title('传统OMP与TSC-OMP伪峰统计指标对比', fontsize=13, fontweight='bold')
ax.grid(True, axis='y', linestyle='--', alpha=0.5)
ax.legend(fontsize=11)

plt.tight_layout()
fig3.savefig(os.path.join(save_dir, f'Fig3_stat_compare_{ARRAY_TYPE}.png'), dpi=300, bbox_inches='tight')

# ============================================================
# 10. 显示图像
# ============================================================
plt.show()