import pandas as pd
import matplotlib.pyplot as plt
from pathlib import Path

# -------- File picker --------
RESULTS_DIR = Path("results")

csv_files = sorted(RESULTS_DIR.glob("*.csv"))

if not csv_files:
    raise FileNotFoundError("No CSV files found in results/")

print("Available result files:")
for i, f in enumerate(csv_files):
    print(f"[{i}] {f.name}")

choice = int(input("Select file number: "))
csv_path = csv_files[choice]

print(f"\nLoading: {csv_path}\n")

# -------- Load CSV --------
df = pd.read_csv(csv_path)

# -------- Plot target vs current --------
for coord in ["x"]:
    plt.figure()
    plt.plot(df["time"], df[f"target_{coord}"], label=f"target_{coord}")
    plt.plot(df["time"], df[f"current_{coord}"], label=f"current_{coord}")
    plt.plot(df["time"], df[f"mot_{coord}"].clip(-1, 1), label=f"mot_{coord}")
    plt.plot(df["time"], df[f"dmot_{coord}"].clip(-1, 1), label=f"desired mot_{coord}")

    plt.xlabel("Time (s)")
    plt.ylabel("Angular velocity & Motor Offset")
    plt.title(f"{csv_path.name} — {coord.upper()} axis")
    plt.legend(loc="upper right")
    plt.grid(True)

    ymin, ymax = plt.ylim()
    plt.ylim(min(ymin, -1), max(ymax, 1))

    plt.show()

# -------- Plot error --------
# 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(f"PID Error — {csv_path.name}")
# plt.legend(loc="upper right")
# plt.grid(True)
# plt.show()