import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

CSV = '/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv'
df = pd.read_csv(CSV)

# ---- 曲线图：q1~q7 (+ 可选 psi) vs index ----
joint_cols_expect = ['q1','q2','q3','q4','q5','q6','q7']
joint_cols = [c for c in joint_cols_expect if c in df.columns]
assert len(joint_cols) == 7, f"CSV 缺少关节列，期望 {joint_cols_expect}，实际 {list(df.columns)}"

x = np.arange(len(df))
plt.figure(figsize=(12, 5))
for c in joint_cols:
    plt.plot(x, df[c].to_numpy(), label=c, linewidth=1.2)

if 'psi' in df.columns:
    plt.plot(x, df['psi'].to_numpy(), '--', label='psi', linewidth=1.2)

plt.xlabel('index')
plt.ylabel('angle (rad)')
plt.title('q1..q7 (and psi) vs index')
plt.grid(True, alpha=0.35)
plt.legend(ncol=4, fontsize=9)
plt.tight_layout()
plt.show()

# ---- 柱状图：最近限位距离（选“最危险帧”） ----
limits = np.array([
    [-0.26,  1.57],
    [-0.78,  1.57],
    [-np.pi,  np.pi],
    [ 0.00,  2.05],
    [-3.00,  3.00],
    [-2.00,  2.00],
    [-0.57,  1.57],
])

Q = df[joint_cols].to_numpy()          # [N,7]
lo = limits[:, 0][None, :]             # [1,7]
hi = limits[:, 1][None, :]

dist_low  = Q - lo                     # 到下限的距离
dist_high = hi - Q                     # 到上限的距离
nearest   = np.minimum(dist_low, dist_high)  # 最近限位（可为负，负值=超限）

# 找“最危险”的 index（全关节最小裕度最小）
min_margin_per_row = nearest.min(axis=1)     # 每帧的最小关节裕度
worst_idx = int(np.argmin(min_margin_per_row))
vals = nearest[worst_idx, :]

psi_text = f", psi={df['psi'].iloc[worst_idx]:.4f}" if 'psi' in df.columns else ""

plt.figure(figsize=(9, 4.5))
plt.bar(joint_cols, vals)
plt.axhline(0.0, linewidth=1, color='k')
plt.ylabel('Nearest distance to limit (rad)')
plt.title(f'Per-joint margin at worst frame (index={worst_idx}{psi_text})')
plt.grid(True, axis='y', alpha=0.35)
plt.tight_layout()
plt.show()

# ---- 可选：整段最小裕度曲线（帮助定位危险段）----
plt.figure(figsize=(12, 3.2))
plt.plot(min_margin_per_row, linewidth=1.2)
plt.axhline(0.0, linewidth=1, color='k')
plt.xlabel('index')
plt.ylabel('min margin (rad)')
plt.title('Minimum per-frame joint margin over sweep (rad)')
plt.grid(True, alpha=0.35)
plt.tight_layout()
plt.show()