%% Pull data from model and plot

% Simulate the model to update parameters
sim('test_model.slx');

% Determine model time for various sampling times and scale it according to
% logged data timescale
indices_40ms = find(pitch_setpoint_model.time >= time(1));
indices_5ms = find(x_command_model.time >= time(1));
time_model_40ms = pitch_setpoint_model.time(indices_40ms);
time_model_5ms = x_command_model.time(indices_5ms);

% Pull x control structure data
x_vel_setpoint_model_data = x_vel_setpoint_model.signals.values(indices_40ms);
pitch_setpoint_model_data = pitch_setpoint_model.signals.values(indices_40ms);
pitchrate_setpoint_model_data = pitchrate_setpoint_model.signals.values(indices_5ms);
x_command_model_data = x_command_model.signals.values(indices_5ms);
x_position_model_data = x_position_model.signals.values(indices_40ms);

% Pull y control structure data
y_vel_setpoint_model_data = y_vel_setpoint_model.signals.values(indices_40ms);
roll_setpoint_model_data = roll_setpoint_model.signals.values(indices_40ms);
rollrate_setpoint_model_data = rollrate_setpoint_model.signals.values(indices_5ms);
y_command_model_data = y_command_model.signals.values(indices_5ms);
y_position_model_data = y_position_model.signals.values(indices_40ms);

% Pull z control structure data
z_command_model_data = z_command_model.signals.values(indices_40ms);
z_position_model_data = z_position_model.signals.values(indices_40ms);

% Pull yaw control structure data
yawrate_setpoint_model_data = yawrate_setpoint_model.signals.values(indices_40ms);
yaw_command_model_data = yaw_command_model.signals.values(indices_5ms);
yaw_value_model_data = yaw_value_model.signals.values(indices_40ms);

% Pull duty cycle commands from model
PWM0_model = motorCommands.signals.values(indices_5ms, 1);
PWM1_model = motorCommands.signals.values(indices_5ms, 2);
PWM2_model = motorCommands.signals.values(indices_5ms, 3);
PWM3_model = motorCommands.signals.values(indices_5ms, 4);

% Pull accelerometer readings from model
pitch_accel = angle_IMU_reading.signals.values(indices_5ms, 2);
roll_accel = angle_IMU_reading.signals.values(indices_5ms, 1);

% Pull mahony filter data
pitch_accel_mahony = mahony_reading.signals.values(indices_5ms, 2);
roll_accel_mahony = mahony_reading.signals.values(indices_5ms, 1);

%% Plot x control structure

% Plot x position controller output
figure(1); ax1 = subplot(2, 2, 1);
stairs(time, x_vel_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, x_vel_setpoint_model_data, '.-'); hold off;
title('X Position Controller Output');
xlabel('Time (s)');
ylabel('Velocity (m/s)');
legend('Log', 'Model', 'location', 'northwest');

% Plot lateral controller output
ax2 = subplot(2, 2, 2);
stairs(time, pitch_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, pitch_setpoint_model_data, '.-'); hold off;
title('X Velocity Controller Output');
xlabel('Time (s)');
ylabel('\theta (rad)');
legend('Log', 'Model', 'location', 'northwest');

% Plot pitch controller output
ax3 = subplot(2, 2, 3);
stairs(time, pitchrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, pitchrate_setpoint_model_data, '.-'); hold off;
title('Pitch Controller Output');
xlabel('Time (s)');
ylabel('d\theta/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');

% Plot x controller command
ax4 = subplot(2, 2, 4);
stairs(time, x_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, x_command_model_data, '.-'); hold off;
title('Pitch Rate Controller Output');
xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');

linkaxes([ax1, ax2, ax3, ax4], 'x');

%% Plot y control structure

% Plot y position controller output
figure(2); ax1 = subplot(2, 2, 1);
stairs(time, y_vel_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_vel_setpoint_model_data, '.-'); hold off;
title('Y Position Controller Output');
xlabel('Time (s)');
ylabel('Velocity (m/s)');
legend('Log', 'Model', 'location', 'northwest');

% Plot y velocity controller output
ax2 = subplot(2, 2, 2);
stairs(time, roll_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, roll_setpoint_model_data, '.-'); hold off;
title('Y Velocity Controller Output ');
xlabel('Time (s)');
ylabel('\phi (rad)');
legend('Log', 'Model', 'location', 'northwest');


% Plot roll controller output
ax3 = subplot(2, 2, 3);
stairs(time, rollrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, rollrate_setpoint_model_data, '.-'); hold off;
title('Roll Controller Output');
xlabel('Time (s)');
ylabel('d\phi/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');

% Plot y controller command
ax4 = subplot(2, 2, 4);
stairs(time, y_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, y_command_model_data, '.-'); hold off;
title('Y Command');
xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');

linkaxes([ax1, ax2, ax3, ax4], 'x');

%% Plot z control structure

% Plot z controller command
figure(3);
stairs(time, z_command, '.-'); hold on; grid minor;
stairs(time_model_40ms, z_command_model_data, '.-'); hold off;
title('Z Command');
xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');

%% Plot yaw control structure

% Plot yaw controller output
figure(4); ax1 = subplot(2, 1, 1);
stairs(time, yawrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_40ms, yawrate_setpoint_model_data, '.-'); hold off;
title('Yaw Controller Output');
xlabel('Time (s)');
ylabel('d\psi/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');

% Plot yaw controller command
ax2 = subplot(2, 1, 2);
stairs(time, yaw_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, yaw_command_model_data, '.-'); hold off;
title('Yaw Command');
xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');

linkaxes([ax1, ax2], 'x');

%% Plot PWM Commands
figure(5); ax1 = subplot(2, 2, 1);
stairs(time, PWM0,'.-'); hold on; grid minor;
stairs(time_model_5ms, PWM0_model, '.-'); hold off;
title('PWM0 Value');
xlabel('Time (s)');
ylabel('PWM0 Command');
legend('Log', 'Model', 'location', 'northwest');

ax2 = subplot(2, 2, 2);
stairs(time, PWM1,'.-'); hold on; grid minor;
stairs(time_model_5ms, PWM1_model, '.-'); hold off;
title('PWM1 Value');
xlabel('Time (s)');
ylabel('PWM1 Command');
legend('Log', 'Model', 'location', 'northwest');

ax3 = subplot(2, 2, 3);
stairs(time, PWM2,'.-'); hold on; grid minor;
stairs(time_model_5ms, PWM2_model, '.-'); hold off;
title('PWM2 Value');
xlabel('Time (s)');
ylabel('PWM2 Command');
legend('Log', 'Model', 'location', 'northwest');
    
ax4 = subplot(2, 2, 4);
stairs(time, PWM3,'.-'); hold on; grid minor;
stairs(time_model_5ms, PWM3_model, '.-'); hold off;
title('PWM3 Value');
xlabel('Time (s)');
ylabel('PWM3 Command');
legend('Log', 'Model', 'location', 'northwest');

linkaxes([ax1, ax2, ax3, ax4], 'xy');

%% Plot output of complimentary filter

figure(8); ax1 = subplot(2, 1, 1);
stairs(time, pitch_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
stairs(time, (pitch_measured_IMU) * (180/pi), '.-');
stairs(time_model_5ms, (pitch_accel + 0.045) * (180/pi), '.-');
title('Pitch Complementary Filter Output');
xlabel('Time (s)');
ylabel('Pitch Angle (degrees)');
legend('VRPN','IMU', 'Model', 'location', 'northwest');

ax2 = subplot(2, 1, 2);
stairs(time, roll_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
stairs(time, roll_measured_IMU * (180/pi), '.-');
stairs(time_model_5ms, roll_accel * (180/pi), '.-');
title('Roll Complementary Filter Output');
xlabel('Time (s)');
ylabel('Roll Angle (degrees)');
legend('VRPN','IMU', 'Model', 'location', 'northwest');


linkaxes([ax1, ax2], 'x');

%% Plot VRPN Position

figure(10); ax1 = subplot(3, 1, 1);
stairs(time, x_pos_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, x_position_model_data, '.-');
stairs(time, x_setpoint, '.-');
title('X position');
xlabel('Time (s)');
ylabel('X position');
legend('X position', 'X position model', 'X setpoint');

ax2 = subplot(3, 1, 2);
stairs(time, y_pos_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_position_model_data, '.-');
stairs(time, y_setpoint, '.-');
title('Y position');
xlabel('Time (s)');
ylabel('Y position');
legend('Y position', 'Y position model', 'Y setpoint');

ax3 = subplot(3, 1, 3);
stairs(time, z_pos_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, z_position_model_data, '.-');
stairs(time, z_setpoint, '.-');
title('Z position');
xlabel('Time (s)');
ylabel('Z position');
legend('Z position', 'Z position model', 'Z setpoint');

linkaxes([ax1, ax2, ax3], 'x');

%% Plot Gyro Data
figure(10); ax1 = subplot(3, 1, 1);
stairs(time, raw_gyro_data_x, '.-'); hold on; grid minor;
stairs(time_model_5ms, gyro_data_x, '.-');
title('gyro x');
xlabel('Time (s)');
ylabel('p (rad/s)');

ax2 = subplot(3, 1, 2);
stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor;
stairs(time_model_5ms, gyro_data_y, '.-');
title('gyro y');
xlabel('Time (s)');
ylabel('q (rad/s)');

ax3 = subplot(3, 1, 3);
stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor;
stairs(time_model_5ms, gyro_data_z, '.-');
title('gyro z');
xlabel('Time (s)');
ylabel('r (rad/s)');

linkaxes([ax1, ax2, ax3], 'x');

%% Plot Accel Data
figure(11); ax1 = subplot(3, 1, 1);
stairs(time, raw_accel_data_x, '.-'); hold on; grid minor;
stairs(time_model_5ms, accel_data_x, '.-');
title('accel x');
xlabel('Time (s)');
ylabel('accel x (g)');

ax2 = subplot(3, 1, 2);
stairs(time, raw_accel_data_y, '.-'); hold on; grid minor;
stairs(time_model_5ms, accel_data_y, '.-');
title('accel y');
xlabel('Time (s)');
ylabel('accel y (g)');

ax3 = subplot(3, 1, 3);
stairs(time, raw_accel_data_z, '.-'); hold on; grid minor;
stairs(time_model_5ms, accel_data_z, '.-');
title('accel z');
xlabel('Time (s)');
ylabel('accel (z)');

linkaxes([ax1, ax2, ax3], 'x');