%% 
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); 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');
%%
figure;
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);
figure;
plot(expData.Time.data, expData.Lidar_Constant.data); hold on;
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);

%angle = sqrt(expData.Roll_Constant.data.^2 + expData.VRPN_Pitch_Constant.data.^2);
%corrected = expData.Lidar_Constant.data .* cos(angle);
%plot(expData.Time.data, corrected);
legend('lidar', 'vrpn');

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

%% Sonar
filtered_sonar = [];
last_sonar = expData.Flow_Distance_Constant.data(1);
for i = [1 : length(expData.Flow_Distance_Constant.data)]
    this_sonar = expData.Flow_Distance_Constant.data(i);
    if (abs(this_sonar - last_sonar) < 0.4)
        filtered_sonar(i) = this_sonar;
        last_sonar = this_sonar;
    else
        filtered_sonar(i) = last_sonar;
    end
end
alt_offset = -0.04;
plot(expData.Time.data, -expData.Flow_Distance_Constant.data + alt_offset); hold on;
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
plot(expData.Time.data, -filtered_sonar + alt_offset);
legend('sonar', 'vrpn', 'dumb filter');
%% THE ONE
figure;

% offsetX = -expData.OF_Integrate_X_Integrated.data(1) - expData.VRPN_X_Constant.data(1);
% offsetY = -expData.OF_Integrate_Y_Integrated.data(1) - expData.VRPN_Y_Constant.data(1);
offsetX = 0;
offsetY = 0;

ax1 = subplot(3, 1, 1);
plot(expData.Time.data, expData.OF_Integrate_X_Integrated.data - offsetX); hold on; grid minor
plot(expData.Time.data, expData.VRPN_X_Constant.data);
plot(expData.Time.data, expData.X_Setpoint_Constant.data);
legend('OF X Position', 'VRPN X Position', 'X setpoint');
xlabel('Time (s)');
ylabel('Displacement (m)');
hold off;

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

ax3 = subplot(3, 1, 3);
plot(expData.Time.data, expData.Lidar_Constant.data); hold on; grid minor
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
plot(expData.Time.data, expData.Alt_Setpoint_Constant.data);
legend('Lidar Z Position', 'VRPN Z Position', 'Z setpoint');
xlabel('Time (s)');
ylabel('Displacement (m)');
hold off;



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

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

%% Integarted gyro0.55 yaw
figure;
gyro_yaw = 0.005 * cumtrapz(expData.gyro_z.data + 0.0073);
plot(expData.Time.data, (180/pi) * gyro_yaw); hold on;  
plot(expData.Time.data, expData.Yaw_Constant.data * 180/pi); hold on;
legend('Integrated gyro z', 'actual yaw');
ylabel('Yaw (degrees)');
xlabel('Time (s)');

%%
figure;
angleOffset = 0.62204 + gyro_yaw;

FlowVelX = expData.Flow_Vel_X_Constant.data.*cos(angleOffset) - expData.Flow_Vel_Y_Constant.data.*sin(angleOffset);
FlowVelY = expData.Flow_Vel_X_Constant.data.*sin(angleOffset) + expData.Flow_Vel_Y_Constant.data.*cos(angleOffset);

fc = 10;
FlowVelX = BiquadFilter(FlowVelX, 200, fc);
FlowVelY = BiquadFilter(FlowVelY, 200, fc);

flowX = zeros(1, length(expData.Time.data));

driftX = 0;
driftY = 0;

flowX(1) = expData.VRPN_X_Constant.data(1);
for n = 2:length(flowX)
    flowX(n) = flowX(n-1) + 0.005*(FlowVelX(n) - driftX);
end

flowY = zeros(1, length(expData.Time.data));
flowY(1) = expData.VRPN_Y_Constant.data(1);
for n = 2:length(flowY)
    flowY(n) = flowY(n-1) + 0.005*(FlowVelY(n) - driftY);
end

ax1 = subplot(2, 1, 1);
plot(expData.Time.data, flowX); hold on;
plot(expData.Time.data, expData.VRPN_X_Constant.data);
%legend('OF Integrated X Position');
legend('OF Integrated X', 'VRPN X');
%legend('OF Integrated X Position', 'Approximate Max X Position (measured with tape measure)');
xlabel('Time (s)');
ylabel('Position (m)');
hold off;

ax2 = subplot(2, 1, 2);
plot(expData.Time.data, flowY); hold on;
plot(expData.Time.data, expData.VRPN_Y_Constant.data);
%legend('OF Integrated Y Position');
legend('OF Integrated Y', 'VRPN Y');
%legend('OF Integrated Y Position', 'Approximate Max Y Position (measured with tape measure)');
xlabel('Time (s)');
ylabel('Position (m)');
hold off;

linkaxes([ax1 ax2]);
%%
figure;
ax1 = subplot(2, 1, 1);
plot(expData.Time.data, expData.Flow_Quality_Constant.data);
ax2 = subplot(2, 1, 2);
plot(expData.Time.data, expData.Lidar_Constant.data); hold on;
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
linkaxes([ax1 ax2], 'x');

%%
figure;
ax1 = subplot(3, 1, 1);
plot(expData.Time.data, expData.mag_x.data);
ax2 = subplot(3, 1, 2);
plot(expData.Time.data, expData.mag_y.data);
ax3 = subplot(3, 1, 3);
plot(expData.Time.data, expData.mag_z.data);
linkaxes([ax1 ax2 ax3], 'x');

%% 
figure;
filtX = BiquadFilter(expData.mag_x.data+33.9844, 200, 1);
filtY = BiquadFilter(expData.mag_y.data-40.4922, 200, 1);

magYaw = atan2(-filtY, -filtX);
gyroYaw = cumtrapz(expData.gyro_z.data - 0.008) * 0.005;

ax1 = subplot(3, 1, 1);
plot(expData.Time.data, magYaw*180/pi);
ax2 = subplot(3, 1, 2);
plot(expData.Time.data, gyroYaw*180/pi);
ax3 = subplot(3, 1, 3);
plot(expData.Time.data, expData.Mag_Yaw_Constant.data*180/pi);

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

%% 
mag = expData.mag_z.data;

count = 0;
for n = 2:length(mag)
    if mag(n) == mag(n-1)
        count = count + 1;
        if count >= 10
            disp(['Stall detected at index ' num2str(n)]);
        end
    else
        count = 0;
    end
end