import matplotlib.pyplot as plt
import pandas as pd

from pathlib import Path

for path in Path(".").iterdir():
    print(path)

plt.style.use("ggplot")

df = pd.read_csv("./public/simOut.csv", delimiter=", ")


# Acceleration
plt.subplot(3, 1, 1)
plt.title("Acceleration, Velocity, and Altitude Plots")
plt.plot(df.t, df.az)
# plt.title("Acceleration vs Time")
plt.xlabel("Time (s)")
plt.ylabel("Acceleration (g" "s)")

# Velocity
plt.subplot(3, 1, 2)
plt.plot(df.t, df.vz)
# plt.title("Velocity vs Time")
plt.xlabel("Time (s)")
plt.ylabel("Velocity (m/s)")

# Altitude
plt.subplot(3, 1, 3)
plt.plot(df.t, df.z)
# plt.title("Altitude vs Time")
plt.xlabel("Time (s)")
plt.ylabel("Altitude (m)\n")

plt.savefig("./public/AVA.png")


# Euler Angles
plt.figure(2)

plt.subplot(2, 1, 1)
plt.title("Deflection and Angular Velocity Plots")
plt.plot(df.t, df.yaw, label="Yaw")
plt.plot(df.t, df.pitch, label="Pitch")
plt.plot(df.t, df.roll, label="Roll")
plt.xlabel("Time (ms)")
plt.ylabel("Euler Angles (deg)")
plt.legend()

# Angular Velocity
plt.subplot(2, 1, 2)
plt.plot(df.t, df.yawdot, label="Yaw")
plt.plot(df.t, df.pitchdot, label="Pitch")
plt.plot(df.t, df.rolldot, label="Roll")
plt.xlabel("Time (ms)")
plt.legend()

plt.savefig("./public/Angles.png")

# Servo Positions
plt.figure(3)

plt.plot(df.t, df.Servo1, label="Servo1")
plt.plot(df.t, df.Servo2, label="Servo2")
plt.xlabel("Time (ms)")
plt.ylabel("Position (rad)")
plt.legend()
plt.title("Servo Positions")

plt.savefig("./public/Servos.png")

plt.show()