import pandas as pd
import matplotlib.pyplot as plt

# Load the CSV exported by the PID controller
df = pd.read_csv("data.csv")

# --- Plot target vs current ---
for coord in ["x", "y", "z"]:
    plt.figure()
    plt.plot(df["time"], df["target_" + coord], label="target_" + coord)
    plt.plot(df["time"], df["current_" + coord], label="current_" + coord)
    plt.plot(df["time"], df["mot_" + coord].clip(-1,1), label="mot_" + coord)
    plt.plot(df["time"], df["dmot_" + coord].clip(-1,1), label="desired mot_" + coord)
    plt.xlabel("Time (s)")
    plt.ylabel("Angular velocity & Motor Offset")
    plt.title(f"Target vs Current Angular Velocity with Motor Diff ({coord})")
    plt.legend(loc='upper right')
    plt.grid(True)
    plt.show()

# for coord in ["x", "y", "z"]:
#     plt.figure()
#     plt.plot(df["time"], df["target_" + coord], label="target_" + coord)
#     plt.plot(df["time"], df["current_" + coord], label="current_" + coord)
#     plt.xlabel("Time (s)")
#     plt.ylabel("Angular velocity")
#     plt.title(f"Target vs Current Angular Velocity ({coord})")
#     plt.legend()
#     plt.grid(True)
#     plt.show()


# plt.figure()
# plt.plot(df["time"], df["target_x"], label="target_x")
# plt.plot(df["time"], df["current_x"], label="current_x")
# plt.plot(df["time"], df["target_y"], label="target_y")
# plt.plot(df["time"], df["current_y"], label="current_y")
# plt.plot(df["time"], df["target_z"], label="target_z")
# plt.plot(df["time"], df["current_z"], label="current_z")
# plt.xlabel("Time (s)")
# plt.ylabel("Angular velocity")
# plt.title("Target vs Current Angular Velocity")
# plt.legend()
# plt.grid(True)
# plt.show()
# --- Plot error over time ---
plt.figure()
plt.plot(df["time"], df["error_x"], label="error_x")
plt.plot(df["time"], df["error_y"], label="error_y")
plt.plot(df["time"], df["error_z"], label="error_z")
plt.xlabel("Time (s)")
plt.ylabel("Error")
plt.title("PID Error Over Time")
plt.legend(loc='upper right')
plt.grid(True)
plt.show()