%% 
plot(Time, Cam_Meas_Roll*(180/pi)); hold on;
plot(Time, Quad_Meas_Roll*(180/pi));

%%
plot(Time, pitch_velocity*(180/pi)); hold on;
plot(Time, roll_velocity*(180/pi));
plot(Time, yaw_velocity*(180/pi));

%% 
figure;
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data*(180/pi)); hold on;
plot(expData.Time.data, expData.Pitch_trim_add_Sum.data*(180/pi) + 2.2);
legend('Camera Pitch', 'Quad Pitch');
xlabel('seconds'); ylabel('degrees');

%% 
figure;
plot(time, (VRPNRollConstant + pi)*(180/pi)); hold on;
plot(time, RollConstant*(180/pi));
legend('Camera Roll', 'Quad Roll');
xlabel('seconds'); ylabel('degrees');

%% 
%plot(Time, X_setpoint); hold on;
markerSize = 3;
ax1 = subplot(2,1,1);
plot(time, (XSetpointConstant - VRPNXConstant), '-o', 'MarkerSize', markerSize); hold on;
plot(time, -XposPIDCorrection * (180/pi), '-o', 'MarkerSize', markerSize); hold off;
legend('X Error', 'X PID output');


ax2 = subplot(2,1,2);
plot(time, (180/pi)*PitchConstant, '-o', 'MarkerSize', markerSize); hold on;
plot(time, (180/pi)*PitchPIDCorrection, '-o', 'MarkerSize', markerSize);
legend('Pitch Error', 'Pitch PID output');

linkaxes([ax1, ax2], 'x');
%%
ax2 = subplot(2,2,1);
plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.VRPN_X_Constant.data);
title('X error');

ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_pos_PID_Correction.data);
title('x output');

ax3 = subplot(2,2,3);
plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
title('pitch output');
legend('output', 'Pitch x10');

ax4 = subplot(2,2,4);
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.gyro_y.data .* 100000);
legend('output', 'Pitch rate x100000');
title('pitch rate output');

linkaxes([ax1, ax2, ax3, ax4], 'x');
%% X pos controller flow
figure;
ax2 = subplot(2,2,1);
plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.OF_Integrate_X_Integrated.data);
title('X error');

ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_pos_PID_Correction.data);
title('x output');

ax3 = subplot(2,2,3);
plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
title('pitch output');
legend('output', 'Pitch x10');

ax4 = subplot(2,2,4);
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.gyro_y.data);
legend('output', 'Pitch rate');
title('pitch rate output');

linkaxes([ax1, ax2, ax3, ax4], 'x');
%%
plot(time, 1044.26 .* (PitchPIDCorrection - gyro_y));hold on;
%plot(time, PitchRatePIDCorrection);
%%
ax2 = subplot(2, 1, 1);
plot(time, YawConstant);
ax1 = subplot(2, 1, 2);
plot(time, gyro_z); hold on;
plot(time, YawRatePIDCorrection);
linkaxes([ax1, ax2], 'x');

%%
all_motors = expData.Signal_Mixer_MOTOR_0.data + expData.Signal_Mixer_MOTOR_1.data + ...
    expData.Signal_Mixer_MOTOR_2.data + expData.Signal_Mixer_MOTOR_3.data;
ax1 = subplot(1, 2, 1);
plot(expData.Time.data, all_motors ./ 4); hold on;
plot(expData.Time.data, expData.RC_Throttle_Constant.data); hold on;
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.Roll_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.Yaw_Rate_PID_Correction.data);
legend('average motors', 'throttle', 'pitch', 'roll', 'yaw');
ax2 = subplot(1, 2, 2);
plot(expData.Time.data, -expData.VRPN_Alt_Constant.data);
legend('Z, meters');
linkaxes([ax1, ax2], 'x');

%%
ax1 = subplot(1, 2, 1);
plot(expData.Time.data, expData.Pitch_Constant.data .* (180 / pi)); hold on; grid minor
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* (180 / pi));
legend('imu', 'vrpn');
ax2 = subplot(1, 2, 2);
plot(expData.Time.data, expData.Roll_Constant.data .* (180 / pi)); hold on; grid minor
plot(expData.Time.data, expData.VRPN_Roll_Constant.data .* (180 / pi));
legend('imu', 'vrpn');
linkaxes([ax1, ax2], 'x');

%%
ax1 = subplot(3, 1, 1);
plot(expData.Time.data, expData.accel_x.data);
ax2 = subplot(3, 1, 2);
plot(expData.Time.data, expData.accel_y.data);
ax3 = subplot(3, 1, 3);
plot(expData.Time.data, expData.accel_z.data);
linkaxes([ax1, ax2, ax3], 'x');
%%
ax1 = subplot(3, 1, 1);
plot(expData.Time.data, expData.gyro_x.data);
ax2 = subplot(3, 1, 2);
plot(expData.Time.data, expData.gyro_y.data);
ax3 = subplot(3, 1, 3);
plot(expData.Time.data, expData.gyro_z.data);
linkaxes([ax1, ax2, ax3], 'x');
%%
figure;
ax2 = subplot(2,2,1);
raw_derivative = -diff(expData.VRPN_X_Constant.data) / 0.04;
plot(expData.Time.data, expData.X_Vel_Correction.data - (expData.RC_Pitch_Constant.data * 5)); hold on;
%plot(expData.Time.data, expData.X_Vel_Correction.data); hold on;
%plot(expData.Time.data, [0; raw_derivative]);
title('X velocity error');

ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_Vel_PID_Correction.data);
title('x vel output');

ax3 = subplot(2,2,3);
plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
title('pitch output');
legend('output', 'Pitch x10');

ax4 = subplot(2,2,4);
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.gyro_y.data .* 100000);
legend('output', 'Pitch rate x100000');
title('pitch rate output');

linkaxes([ax1, ax2, ax3, ax4], 'x');
%% vel flow
figure;
ax2 = subplot(2,2,1);
plot(expData.Time.data, expData.OF_Offset_Angle_Rotated_X.data - expData.RC_Pitch_Constant.data); hold on;
plot(expData.Time.data, expData.OF_Offset_Angle_Rotated_X.data); hold on;
%plot(expData.Time.data, expData.X_Vel_Correction.data); hold on;
%plot(expData.Time.data, [0; raw_derivative]);
title('X velocity error');

ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_Vel_PID_Correction.data);
title('x vel output');

ax3 = subplot(2,2,3);
plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
plot(expData.Time.data, expData.Pitch_trim_add_Sum.data .* 10);
title('pitch output');
legend('output', 'Pitch x10');

ax4 = subplot(2,2,4);
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.gyro_y.data .* 100000);
legend('output', 'Pitch rate x100000');
title('pitch rate output');

linkaxes([ax1, ax2, ax3, ax4], 'x');
%%
ax1 = subplot(2, 1, 1);
plot(expData.Time.data, expData.Alt_Setpoint_Constant.data - expData.VRPN_Alt_Constant.data); hold on;
plot(expData.Time.data, expData.Alt_Setpoint_Constant.data); hold on;
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
legend('z error', 'z setpoint', 'z position');
xlabel('time (s)');
ylabel('meters');

ax2 = subplot(2, 1, 2);
plot(expData.Time.data, expData.Altitude_PID_Correction.data);
linkaxes([ax1, ax2], 'x');
legend('z PID output');
xlabel('time (s)');
ylabel('1e-8 seconds');


%%
ax1 = subplot(3, 1, 1);
plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.VRPN_X_Constant.data);
title('X error');

ax2 = subplot(3, 1, 2);
plot(expData.Time.data, expData.Y_Setpoint_Constant.data - expData.VRPN_Y_Constant.data);
title('Y error');

ax3 = subplot(3, 1, 3);
plot(expData.Time.data, expData.Alt_Setpoint_Constant.data - expData.VRPN_Alt_Constant.data);
title('Z error');
linkaxes([ax1, ax2, ax3], 'x');
%%
ax1 = subplot(2, 1, 1);
plot(expData.Time.data, expData.Lidar_Constant.data); hold on;
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
legend('lidar', 'vrpn');
title('Z axis');
ylabel('meters');
xlabel('time (s)');

ax2 = subplot(2, 1, 2);
plot(expData.Time.data, expData.Altitude_PID_Correction.data);
title('Altitude PID Correction');
ylabel('PWM values');
xlabel('time (s)');

linkaxes([ax1, ax2], 'x');
%%
figure;
ax1 = subplot(2, 1, 1);
plot(expData.Time.data, expData.OF_Integrate_X_Integrated.data); hold on;
plot(expData.Time.data, expData.VRPN_X_Constant.data);
legend('OF X Position', 'VRPN X Position');
xlabel('Time (s)');
ylabel('Displacement (m)');
hold off;

ax2 = subplot(2, 1, 2);
plot(expData.Time.data, expData.OF_Integrate_Y_Integrated.data); hold on;
plot(expData.Time.data, expData.VRPN_Y_Constant.data);
legend('OF Y Position', 'VRPN Y Position');
xlabel('Time (s)');
ylabel('Displacement (m)');
hold off;

linkaxes([ax1, ax2]);

%% Remove sonar outliers
sonar = expData.Flow_Distance_Constant.data;
filtered_sonar = zeros(length(sonar), 1);
last_sonar = sonar(1);
for i = 1:length(sonar)
    this_sonar = sonar(i);
    if abs(this_sonar - last_sonar) < 0.3
        filtered_sonar(i) = this_sonar;
        last_sonar = this_sonar;
    else
        filtered_sonar(i) = last_sonar;
    end
end
plot(expData.Time.data, sonar); hold on;
plot(expData.Time.data, filtered_sonar);
%% Better sonar outlier removal
sonar = expData.Flow_Distance_Constant.data;
running_med = ApproxMedFilt(sonar, 0.001);
filtered_sonar = zeros(size(sonar));
for i = 1:length(sonar)
    if abs(sonar(i) - running_med(i)) < 0.15
        filtered_sonar(i) = sonar(i);
    else
        filtered_sonar(i) = filtered_sonar(i-1);
    end
end
plot(expData.Time.data, sonar); hold on;
%plot(expData.Time.data, running_med);
plot(expData.Time.data, filtered_sonar);
legend('Raw', 'Median thresholded');
%% complementary filter of accelerometer and sonar
alpha = 0.9975;
dt = 0.005;
fc = 15;

% Remove bias and scale to m/s
z_acceleration = BiquadFilter(-9.8 * (expData.accel_z.data + 0.9968), 200, fc);

sonar_comp = zeros(size(sonar));
sonar_vel = zeros(size(sonar));
sonar_vel(1) = 0;
sonar_comp(1) = filtered_sonar(1);
for n = [2:length(sonar)]
    sonar_vel(n) = alpha * (sonar_vel(n-1) + z_acceleration(n)*dt) + (1 - alpha) * (filtered_sonar(n) - filtered_sonar(n-1)) / dt;
    sonar_comp(n) = alpha * (sonar_comp(n-1) + sonar_vel(n)*dt) + (1 - alpha) * (filtered_sonar(n));
end

figure;
plot(expData.Time.data, sonar_comp); hold on;
plot(expData.Time.data, -expData.VRPN_Alt_Constant.data - 0.025);
title('Massively filtered sonar vs. VRPN');
legend('Filtered sonar', 'VRPN');
ylabel('height (m)');
xlabel('time (s)');
