%% 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
pitch_setpoint_model_data = pitch_setpoint_model.signals.values(indices_40ms);
pitchrate_setpoint_model_data = pitchrate_setpoint_model.signals.values(indices_40ms);
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
roll_setpoint_model_data = roll_setpoint_model.signals.values(indices_40ms);
rollrate_setpoint_model_data = rollrate_setpoint_model.signals.values(indices_40ms);
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_40ms);
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(2, 1, indices_5ms);
pitch_accel = reshape(pitch_accel, [length(pitch_accel) , 1] );
roll_accel = angle_IMU_reading.signals.values(1, 1, indices_5ms);
roll_accel = reshape(roll_accel, [length(roll_accel) , 1] );



%% Plot x control structure

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

% Plot pitch controller output
subplot(2, 2, 2);
stairs(time, pitchrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_40ms, 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
subplot(2, 2, 3);
stairs(time, x_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, x_command_model_data, '.-'); hold off;
title('X Command');
xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');

% Plot x position
subplot(2, 2, 4);
stairs(time, x_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, x_position_model_data, '.-'); hold off;
title('X Position');
xlabel('Time (s)');
ylabel('Position (m)');
legend('Log', 'Model', 'location', 'northwest');

%% Plot y control structure

% Plot longitude controller output
figure(2); subplot(2, 2, 1);
stairs(time, pitch_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, pitch_setpoint_model_data, '.-'); hold off;
title('Longitude Controller Output ');
xlabel('Time (s)');
ylabel('\phi (rad)');
legend('Log', 'Model', 'location', 'northwest');

% Plot roll controller output
subplot(2, 2, 2);
stairs(time, rollrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_40ms, 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
subplot(2, 2, 3);
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');
 
% Plot y position
subplot(2, 2, 4);
stairs(time, y_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_position_model_data, '.-'); hold off;
title('Y Position');
xlabel('Time (s)');
ylabel('Position (m)');
legend('Log', 'Model', 'location', 'northwest');

%% Plot z control structure

% Plot z controller command
figure(3); subplot(2, 1, 1);
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 z position
subplot(2, 1, 2);
stairs(time, z_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, z_position_model_data, '.-'); hold off;
title('Z Position');
xlabel('Time (s)');
ylabel('Position (m)');
legend('Log', 'Model', 'location', 'northwest');

%% Plot yaw control structure

% Plot roll controller output
figure(4); subplot(2, 2, 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
subplot(2, 2, 2);
stairs(time, yaw_command, '.-'); hold on; grid minor;
stairs(time_model_40ms, yaw_command_model_data, '.-'); hold off;
title('Yaw Command');
xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');

% Plot yaw position
subplot(2, 2, 3);
stairs(time, yaw_value, '.-'); hold on; grid minor;
stairs(time_model_40ms, yaw_value_model_data, '.-'); hold off;
title('Yaw Position');
xlabel('Time (s)');
ylabel('Value (rad)');
legend('Log', 'Model', 'location', 'northwest');

%% Plot PWM Commands
figure(5); 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');

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');

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');

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');

%% Plot output of complimentary filter

figure(6); subplot(2, 1, 1);
stairs(time, pitch_measured_IMU, '.-'); hold on; grid minor;
stairs(time_model_5ms, pitch_accel, '.-'); hold off;
title('Pitch Complimentary Filter Output');
xlabel('Time (s)');
ylabel('Pitch Angle (rad)');
legend('Log', 'Model', 'location', 'northwest');

subplot(2, 1, 2);
stairs(time, roll_measured_IMU, '.-'); hold on; grid minor;
stairs(time_model_5ms, roll_accel, '.-'); hold off;
title('Roll Complimentary Filter Output');
xlabel('Time (s)');
ylabel('Roll Angle (rad)');
legend('Log', 'Model', 'location', 'northwest');