clear all; close all; clc;

% Read data and transfer to variables
T = readmatrix('../simOut.csv');
t           = T(:, 1);

x           = T(:, 2);
y           = T(:, 3);
z           = T(:, 4);

vx          = T(:, 5);
vy          = T(:, 6);
vz          = T(:, 7);

ax          = T(:, 8);
ay          = T(:, 9);
az          = T(:, 10);

yaw         = T(:, 11);
pitch       = T(:, 12);
roll        = T(:, 13);

yawdot      = T(:, 14);
pitchdot    = T(:, 15);	 
rolldot     = T(:, 16);

Servo1      = T(:, 17);
Servo2      = T(:, 18);

PIDx      = T(:, 20);
PIDy      = T(:, 21);

% Acceleration
subplot(3, 1, 1)
plot(t, az)
title('Acceleration vs Time')
xlabel('Time (s)')
ylabel('Acceleration (g''s)')

% Velocity
subplot(3, 1, 2)
plot(t, vz)
title('Velocity vs Time')
xlabel('Time (s)')
ylabel('Velocity (m/s)')

% Altitude
subplot(3, 1, 3)
plot(t, z)
title('Altitude vs Time')
xlabel('Time (s)')
ylabel('Altitude (m)')
ylim([0 z(1)+5])
%saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')

figure(2)

% Euler Angles
subplot(2, 1, 1)
hold on;
plot(t, yaw)
plot(t, pitch)
plot(t, roll)
title('Euler Angles vs Time')
xlabel('Time (ms)')
ylabel('Euler Angles (deg)')
legend("yaw", "pitch", "roll")

% Angular Velocity
subplot(2, 1, 2)
hold on;
plot(t, yawdot)
plot(t, pitchdot)
plot(t, rolldot)
title('Angular Velocity vs Time')
xlabel('Time (ms)')
ylabel('Angular Velocity (deg/s)')
%saveas(gcf,'outputs/Euler Angles vs Time.png')
legend("yawdot", "pitchdot", "rolldot")

figure(3)

% Servo 1 Position
subplot(2, 1, 1)
plot(t, Servo1)
title('Servo 1 Position vs Time')
xlabel('Time (ms)')
ylabel('Servo 1 Position (rad)')

% Servo 2 Position
subplot(2, 1, 2)
plot(t, Servo2)
title('Servo 2 Position vs Time')
xlabel('Time (ms)')
ylabel('Servo 2 Position (rad)')
%saveas(gcf,'outputs/Servo Position vs Time.png')

figure(4)

% Servo 1 Position
subplot(2, 1, 1)
plot(t, PIDx)
title('PIDx vs Time')
xlabel('Time (ms)')
ylabel('PIDx')

% Servo 2 Position
subplot(2, 1, 2)
plot(t, PIDy)
title('PIDy vs Time')
xlabel('Time (ms)')
ylabel('PIDy')
%saveas(gcf,'outputs/Servo Position vs Time.png')