import os
import re
import sys
import pandas as pd
import matplotlib.pyplot as plt

def main(csv_path: str):
    # 更健壮的读取
    df = pd.read_csv(csv_path, sep=None, engine="python", comment="#")
    if 't' not in df.columns:
        raise RuntimeError("CSV missing 't' column")

    # ——严格区分列名——
    # 允许列名形如：q0, q1, ... | qd0, qd1, ... | qdd0, qdd1, ...
    def grep(pattern):
        r = re.compile(pattern)
        return [c for c in df.columns if r.fullmatch(c)]

    q_cols   = grep(r"q\d+")
    qd_cols  = grep(r"qd\d+")
    qdd_cols = grep(r"qdd\d+")

    # 如果你的列名不是纯数字后缀（比如 q_joint1），把上面的正则改成更宽松的：
    # q_cols   = [c for c in df.columns if c.startswith("q")   and not c.startswith(("qd","qdd"))]
    # qd_cols  = [c for c in df.columns if c.startswith("qd")  and not c.startswith("qdd")]
    # qdd_cols = [c for c in df.columns if c.startswith("qdd")]

    t = df['t'].values

    # 位置
    plt.figure()
    for c in q_cols:
        plt.plot(t, df[c].values, label=c)
    plt.xlabel('time [s]')
    plt.ylabel('position [rad]')
    plt.title('Joint Positions')
    plt.legend()
    plt.grid(True)

    # 速度（只画 qd*）
    plt.figure()
    for c in qd_cols:
        plt.plot(t, df[c].values, label=c)
    plt.xlabel('time [s]')
    plt.ylabel('velocity [rad/s]')
    plt.title('Joint Velocities')
    plt.legend()
    plt.grid(True)

    # 加速度（只画 qdd*）
    plt.figure()
    for c in qdd_cols:
        plt.plot(t, df[c].values, label=c)
    plt.xlabel('time [s]')
    plt.ylabel('acceleration [rad/s^2]')
    plt.title('Joint Accelerations')
    plt.legend()
    plt.grid(True)

    plt.show()

if __name__ == '__main__':
    main("/home/lgv/cmvr/cmvr-es/data/planner/traj.csv")