import numpy as np
import matplotlib.pyplot as plt

CSV_PATH = "/home/lgv/cmvr/0-workspace/cmvr-es/data/ik_movel_scurve.csv"   # 改成你的路径，比如 /home/lgv/.../ik_movel_scurve.csv

def load_csv(path: str):
    # 期望列名：t,q1,q2,q3,q4,q5,q6,q7
    data = np.genfromtxt(path, delimiter=",", names=True)
    t = data["t"].astype(float)
    q = np.vstack([data[f"q{i}"] for i in range(1, 8)]).T.astype(float)  # (N,7)
    return t, q

def diff_over_time(x: np.ndarray, t: np.ndarray):
    # x: (N,7), t: (N,)
    dt = np.diff(t)
    # 防止 dt=0
    dt = np.where(dt <= 1e-12, 1e-12, dt)
    dx = np.diff(x, axis=0)
    return dx / dt[:, None]  # (N-1,7)

def main():
    t, q = load_csv(CSV_PATH)
    if len(t) < 3:
        raise RuntimeError("CSV too short (need >= 3 samples).")

    qd = diff_over_time(q, t)              # (N-1,7)
    t_qd = (t[1:] + t[:-1]) * 0.5          # 中点时间
    qdd = diff_over_time(qd, t_qd)         # (N-2,7)
    t_qdd = (t_qd[1:] + t_qd[:-1]) * 0.5

    # 打印统计
    print("=== Stats from CSV ===")
    for j in range(7):
        max_qd = np.max(np.abs(qd[:, j]))
        max_qdd = np.max(np.abs(qdd[:, j]))
        print(f"joint {j+1}: max|qdot|={max_qd:.6f} rad/s,  max|qddot|={max_qdd:.6f} rad/s^2")

    # 画 q(t)
    plt.figure()
    for j in range(7):
        plt.plot(t, q[:, j], label=f"q{j+1}")
    plt.xlabel("t [s]")
    plt.ylabel("q [rad]")
    plt.title("Joint position q(t)")
    plt.grid(True)
    plt.legend()

    # 画 qdot(t)
    plt.figure()
    for j in range(7):
        plt.plot(t_qd, qd[:, j], label=f"qdot{j+1}")
    plt.xlabel("t [s]")
    plt.ylabel("qdot [rad/s]")
    plt.title("Joint velocity qdot(t) (finite difference)")
    plt.grid(True)
    plt.legend()

    # 画 qddot(t)
    plt.figure()
    for j in range(7):
        plt.plot(t_qdd, qdd[:, j], label=f"qddot{j+1}")
    plt.xlabel("t [s]")
    plt.ylabel("qddot [rad/s^2]")
    plt.title("Joint acceleration qddot(t) (finite difference)")
    plt.grid(True)
    plt.legend()

    plt.show()

if __name__ == "__main__":
    main()