%% Crazyflie Logging Parser

[handLog, delimHand] = importdata('hand.txt','\t');
[cflie1Log, delim1] = importdata('cflie1.txt','\t');
[cflie2Log, delim2] = importdata('cflie2.txt','\t');
[cflie3Log, delim3] = importdata('cflie3.txt','\t');
[cflie4Log, delim4] = importdata('cflie4.txt','\t');
%
handTime = handLog.data(:,1);
handRoll = handLog.data(:,3);
handPitch = handLog.data(:,5);
handYaw = handLog.data(:,7);
handX = handLog.data(:,9);
handY = handLog.data(:,11);
handZ = handLog.data(:,13);
handLoopTime = handLog.data(:,15);
handLoopTimeDelta = handLog.data(:,17);
%
cflie1Time = cflie1Log.data(:,1);
cflie1Roll = cflie1Log.data(:,3);
cflie1Pitch = cflie1Log.data(:,5);
cflie1Yaw = cflie1Log.data(:,7);
cflie1X = cflie1Log.data(:,9);
cflie1Y = cflie1Log.data(:,11);
cflie1Z = cflie1Log.data(:,13);
cflie1CamYaw = cflie1Log.data(:,15);
cflie1CorrectedYaw = cflie1Log.data(:,17);
cflie1LoopTimeTotal = cflie1Log.data(:,19);
cflie1LoopTimeTotalDelta = cflie1Log.data(:,21);
cflie1LoopTime = cflie1Log.data(:,23);
cflie1LoopTimeDelta = cflie1Log.data(:,25);
cflie1VrpnPacketDelta = cflie1Log.data(:,27);
cflie1VrpnPacketTime = cflie1Log.data(:,29);
cflie1VrpnPacketBackup = cflie1Log.data(:,31);
% cflie1XSetpoint = cflie1Log.data(:,33);
% cflie1YSetpoint = cflie1Log.data(:,35);
% cflie1ZSetpoint = cflie1Log.data(:,37);
% cflie1YawSetpoint = cflie1Log.data(:,39);
% cflie1PitchDesired = cflie1Log.data(:,41);
% cflie1RollDesired = cflie1Log.data(:,43);
% cflie1YawDesired = cflie1Log.data(:,45);
% cflie1ThrustDesired = cflie1Log.data(:,47);
% cflie1RadioRSSI = cflie1Log.data(:,49);
% cflie1CamYawRad = cflie1Log.data(:,51);
% cflie1CamPitchRad = cflie1Log.data(:,53);
% cflie1CamRollRad = cflie1Log.data(:,55);
% cflie1Motor1 = cflie1Log.data(:,57);
% cflie1Motor2 = cflie1Log.data(:,59);
% cflie1Motor3 = cflie1Log.data(:,61);
% cflie1Motor4 = cflie1Log.data(:,63);
% cflie1RollRate = cflie1Log.data(:,65);
% cflie1PitchRate = cflie1Log.data(:,67);
% cflie1YawRate = cflie1Log.data(:,69);
% 
cflie2Time = cflie2Log.data(:,1);
cflie2Roll = cflie2Log.data(:,3);
cflie2Pitch = cflie2Log.data(:,5);
cflie2Yaw = cflie2Log.data(:,7);
cflie2X = cflie2Log.data(:,9);
cflie2Y = cflie2Log.data(:,11);
cflie2Z = cflie2Log.data(:,13);
cflie2CamYaw = cflie2Log.data(:,15);
cflie2CorrectedYaw = cflie2Log.data(:,17);
cflie2LoopTimeTotal = cflie2Log.data(:,19);
cflie2LoopTimeTotalDelta = cflie2Log.data(:,21);
cflie2LoopTime = cflie2Log.data(:,23);
cflie2LoopTimeDelta = cflie2Log.data(:,25);
cflie2VrpnPacketDelta = cflie2Log.data(:,27);
cflie2VrpnPacketTime = cflie2Log.data(:,29);
cflie2VrpnPacketBackup = cflie2Log.data(:,31);
% cflie2XSetpoint = cflie2Log.data(:,33);
% cflie2YSetpoint = cflie2Log.data(:,35);
% cflie2ZSetpoint = cflie2Log.data(:,37);
% cflie2YawSetpoint = cflie2Log.data(:,39);
% cflie2PitchDesired = cflie2Log.data(:,41);
% cflie2RollDesired = cflie2Log.data(:,43);
% cflie2YawDesired = cflie2Log.data(:,45);
% cflie2ThrustDesired = cflie2Log.data(:,47);
% cflie2RadioRSSI = cflie2Log.data(:,49);
% cflie2CamYawRad = cflie2Log.data(:,51);
% cflie2CamPitchRad = cflie2Log.data(:,53);
% cflie2CamRollRad = cflie2Log.data(:,55);
% cflie2Motor1 = cflie2Log.data(:,57);
% cflie2Motor2 = cflie2Log.data(:,59);
% cflie2Motor3 = cflie2Log.data(:,61);
% cflie2Motor4 = cflie2Log.data(:,63);
% cflie2RollRate = cflie2Log.data(:,65);
% cflie2PitchRate = cflie2Log.data(:,67);
% cflie2YawRate = cflie2Log.data(:,69);
% 
cflie3Time = cflie3Log.data(:,1);
cflie3Roll = cflie3Log.data(:,3);
cflie3Pitch = cflie3Log.data(:,5);
cflie3Yaw = cflie3Log.data(:,7);
cflie3X = cflie3Log.data(:,9);
cflie3Y = cflie3Log.data(:,11);
cflie3Z = cflie3Log.data(:,13);
cflie3CamYaw = cflie3Log.data(:,15);
cflie3CorrectedYaw = cflie3Log.data(:,17);
cflie3LoopTimeTotal = cflie3Log.data(:,19);
cflie3LoopTimeTotalDelta = cflie3Log.data(:,21);
cflie3LoopTime = cflie3Log.data(:,23);
cflie3LoopTimeDelta = cflie3Log.data(:,25);
cflie3VrpnPacketDelta = cflie3Log.data(:,27);
cflie3VrpnPacketTime = cflie3Log.data(:,29);
% cflie3VrpnPacketBackup = cflie3Log.data(:,31);
% cflie3XSetpoint = cflie3Log.data(:,33);
% cflie3YSetpoint = cflie3Log.data(:,35);
% cflie3ZSetpoint = cflie3Log.data(:,37);
% cflie3YawSetpoint = cflie3Log.data(:,39);
% cflie3PitchDesired = cflie3Log.data(:,41);
% cflie3RollDesired = cflie3Log.data(:,43);
% cflie3YawDesired = cflie3Log.data(:,45);
% cflie3ThrustDesired = cflie3Log.data(:,47);
% cflie3RadioRSSI = cflie1Log.data(:,49);
% cflie3CamYawRad = cflie3Log.data(:,51);
% cflie3CamPitchRad = cflie3Log.data(:,53);
% cflie3CamRollRad = cflie3Log.data(:,55);
% cflie3Motor1 = cflie3Log.data(:,57);
% cflie3Motor2 = cflie3Log.data(:,59);
% cflie3Motor3 = cflie3Log.data(:,61);
% cflie3Motor4 = cflie3Log.data(:,63);
% cflie3RollRate = cflie3Log.data(:,65);
% cflie3PitchRate = cflie3Log.data(:,67);
% cflie3YawRate = cflie3Log.data(:,69);
%
cflie4Time = cflie4Log.data(:,1);
cflie4Roll = cflie4Log.data(:,3);
cflie4Pitch = cflie4Log.data(:,5);
cflie4Yaw = cflie4Log.data(:,7);
cflie4X = cflie4Log.data(:,9);
cflie4Y = cflie4Log.data(:,11);
cflie4Z = cflie4Log.data(:,13);
cflie4CamYaw = cflie4Log.data(:,15);
cflie4CorrectedYaw = cflie4Log.data(:,17);
cflie4LoopTimeTotal = cflie4Log.data(:,19);
cflie4LoopTimeTotalDelta = cflie1Log.data(:,21);
cflie4LoopTime = cflie4Log.data(:,23);
cflie4LoopTimeDelta = cflie4Log.data(:,25);
cflie4VrpnPacketDelta = cflie4Log.data(:,27);
cflie4VrpnPacketTime = cflie4Log.data(:,29);
cflie4VrpnPacketBackup = cflie4Log.data(:,31);
% cflie4XSetpoint = cflie4Log.data(:,33);
% cflie4YSetpoint = cflie4Log.data(:,35);
% cflie4ZSetpoint = cflie4Log.data(:,37);
% cflie4YawSetpoint = cflie4Log.data(:,39);
% cflie4PitchDesired = cflie4Log.data(:,41);
% cflie4RollDesired = cflie4Log.data(:,43);
% cflie4YawDesired = cflie4Log.data(:,45);
% cflie4ThrustDesired = cflie4Log.data(:,47);
% cflie4RadioRSSI = cflie1Log.data(:,49);
% cflie4CamYawRad = cflie4Log.data(:,51);
% cflie4CamPitchRad = cflie4Log.data(:,53);
% cflie4CamRollRad = cflie4Log.data(:,55);
% cflie4Motor1 = cflie4Log.data(:,57);
% cflie4Motor2 = cflie4Log.data(:,59);
% cflie4Motor3 = cflie4Log.data(:,61);
% cflie4Motor4 = cflie4Log.data(:,63);
% cflie4RollRate = cflie4Log.data(:,65);
% cflie4PitchRate = cflie4Log.data(:,67);
% cflie4YawRate = cflie4Log.data(:,69);

% 'Done'
%% Using dataImport

% figure(1)
% hold off
% plot(cflie1Time, cflie1CamYaw, '-g')
% hold on
% plot(cflie1Time, cflie1Yaw, '-r')
% plot(cflie1Time, cflie1CorrectedYaw, '-b')
% title 'cflie1 Yaw'
% legend('CamYaw', 'Quad Yaw', 'CorrectedYaw');
%  ylim([-190,190]);
%  
% figure(2)
% hold off
% plot(cflie2Time, cflie2CamYaw, '-g')
% hold on
% plot(cflie2Time, cflie2Yaw, '-r')% hold off
% plot(cflie2Time, cflie2CorrectedYaw, '-b')
% title 'cflie2 Yaw'
% legend('CamYaw', 'Quad Yaw', 'CorrectedYaw');
%  ylim([-190,190]);
%  
%  figure(3)
% hold off
% plot(cflie3Time, cflie3CamYaw, '-g')
% hold on
% plot(cflie3Time, cflie3Yaw, '-r')
% plot(cflie3Time, cflie3CorrectedYaw, '-b')
% title 'cflie3 Yaw'
% legend('CamYaw', 'Quad Yaw', 'CorrectedYaw');
%  ylim([-190,190]);
%  
%   figure(4)
% hold off
% plot(cflie4Time, cflie4CamYaw, '-g')
% hold on
% plot(cflie4Time, cflie4Yaw, '-r')
% plot(cflie4Time, cflie4CorrectedYaw, '-b')
% title 'cflie4 Yaw'
% legend('CamYaw', 'Quad Yaw', 'CorrectedYaw');
%  ylim([-190,190]);
 
%% Plot X Using ImportData

figure(8)
hold off
plot(cflie1Time,cflie1X, '-b')
hold on
% plot(cflie1Time,cflie1XSetpoint, '-.b')
% 
plot(cflie2Time,cflie2X, '-g')
% plot(cflie2Time,cflie2XSetpoint, '-.g')
% 
plot(cflie3Time,cflie3X, '-k')
% plot(cflie3Time,cflie3XSetpoint, '-.k')
% 
plot(cflie4Time,cflie4X, '-y')
%
plot(handTime, handX, '-r')
% plot(cflie4Time,cflie4XSetpoint, '-.y')
title 'Crazyflie X-Positions'
legend('Crazyflie 1','Crazyflie 2','Crazyflie3', 'Crazyflie4', 'Hand')
xlabel 'Time (seconds)'
ylabel 'X-Position (m)'
grid on
% legend('Crazyflie 1','Setpoint', 'Crazyflie 2','Setpoint', 'Crazyflie3','Setpoint', 'Crazyflie4','Setpoint')
% 
% 
figure(9)
hold off
plot(cflie1Time,cflie1Y, '-b')
hold on
% plot(cflie1Time,cflie1YSetpoint, '-.b')
% 
plot(cflie2Time,cflie2Y, '-g')
% plot(cflie2Time,cflie2YSetpoint, '-.g')
% 
plot(cflie3Time,cflie3Y, '-k')
% plot(cflie3Time,cflie3YSetpoint, '-.k')
% 
plot(cflie4Time,cflie4Y, '-y')
% plot(cflie3Time,cflie3YSetpoint, '-.y')
%
plot(handTime, handY, '-r')
title 'Crazyflie Y-Positions'
legend('Crazyflie 1','Crazyflie 2','Crazyflie3', 'Crazyflie4', 'Hand')
% legend('Crazyflie 1', 'Setpoint','Crazyflie 2', 'Setpoint','Crazyflie3','Setpoint', 'Crazyflie4','Setpoint')
xlabel 'Time (seconds)'
ylabel 'Y-Position (m)'
grid on

%% Plot Yaw Using DataAnalysis.m
% 
% hold off
% plot(main.expData.Time.data, main.expData.CamYaw.data, '-g')
% hold on
% plot(main.expData.Time.data, main.expData.Yaw.data, '-r')
% plot(main.expData.Time.data, main.expData.CorrectedYaw.data, '-b')
% legend('CamYaw', 'Quad Yaw', 'CorrectedYaw');

% figure(2)
% hold off
% plot(main.expData.Time.data, main.expData.Pitch.data, '-g')
% hold on
% plot(main.expData.Time.data, main.expData.Roll.data, '-b')
% plot(main.expData.Time.data, main.expData.X.data, '-r')
% plot(main.expData.Time.data, main.expData.Y.data, '-k')
% legend('Pitch', 'Roll', 'X', 'Y')

%% Time Deltas (using importData)

% for i = 1:length(cflie1Log.data(:,1))-1
%     delta1(i) = cflie1Log.data(i+1,1)-cflie1Log.data(i,1);
% end
% 
% for j = 1:length(cflie2Log.data(:,1))-1
%     delta2(j) = cflie2Log.data(j+1,1)-cflie2Log.data(j,1);
% end
% 
% figure(3)
% hold off
% subplot(2,1,1)
% plot(cflie1Log.data(:,1),cflie1Log.data(:,11),'-g')
% hold on
% plot(cflie2Log.data(:,1),cflie2Log.data(:,11),'-b')
% 
% subplot(2,1,2)
% plot(cflie1Log.data(3:length(cflie1Log.data(:,1))-1,1),delta1(3:length(delta1)),'-g')
% hold on
% plot(cflie2Log.data(3:length(cflie2Log.data(:,1))-1,1),delta2(3:length(delta2)),'-b')
% 
figure(7)

subplot(3,1,1)
hold off
plot(cflie1Time,cflie1Z,'-b')
hold on
plot(cflie2Time,cflie2Z,'-g')
plot(cflie3Time,cflie3Z,'-k')
plot(cflie4Time,cflie4Z,'-y')
title 'Crazyflie Z (Height)'
legend('Crazyflie 1', 'Crazyflie2', 'Crazyflie3', 'Crazyflie4','Location','NorthWest')
ylabel 'Height (m)'
grid on

subplot(3,1,2)
hold off
plot(cflie1Time(4:end),cflie1LoopTime(4:end),'-b')
hold on
plot(cflie2Time(4:end),cflie2LoopTime(4:end),'-g')
plot(cflie3Time(4:end),cflie3LoopTime(4:end),'-k')
plot(cflie4Time(4:end),cflie4LoopTime(4:end),'-y')
title 'Loop Timing'
legend('Callback 1', 'Callback 2', 'Callback3', 'Callback4','Location','NorthWest')
ylabel 'Loop Time (seconds)'
grid on

subplot(3,1,3)
hold off
plot(cflie1Time(4:end),cflie1LoopTimeDelta(4:end),'-b')
hold on
plot(cflie2Time(4:end),cflie2LoopTimeDelta(4:end),'-g')
plot(cflie3Time(4:end),cflie3LoopTimeDelta(4:end),'-k')
plot(cflie4Time(4:end),cflie4LoopTimeDelta(4:end),'-y')
title 'Time Between Loop Calls'
legend('Callback 1', 'Callback 2', 'Callback3', 'Callback4','Location','NorthWest')
ylabel 'Loop Deltas (seconds)'
xlabel 'Time (seconds)'
grid on

%% dataImport Hand Plot

figure(12)

subplot(4,1,1)
hold off
plot(cflie1Time,cflie1X,'-b')
hold on
plot(cflie2Time,cflie2X,'-g')
plot(cflie3Time,cflie3X,'-k')
plot(cflie4Time,cflie4X,'-y')
plot(handTime, handX, '-r')
title 'X Position'
legend('Crazyflie 1', 'Crazyflie2', 'Crazyflie3', 'Crazyflie4','Hand')
ylabel 'X Position (m)'
grid on

subplot(4,1,2)
hold off
plot(cflie1Time,cflie1Y,'-b')
hold on
plot(cflie2Time,cflie2Y,'-g')
plot(cflie3Time,cflie3Y,'-k')
plot(cflie4Time,cflie4Y,'-y')
plot(handTime, handY, '-r')
title 'Y Position'
legend('Crazyflie 1', 'Crazyflie2', 'Crazyflie3', 'Crazyflie4','Hand')
ylabel 'Y Position (m)'
grid on

subplot(4,1,3)
hold off
plot(handTime, handZ, '-r')
title 'Hand Z Position'
legend('Hand Height')
ylabel 'Height (m)'
grid on

subplot(4,1,4)
hold off
plot(handTime, handPitch, '-r')
title 'Hand Pitch'
legend('Hand Pitch')
ylabel 'Angle (rad)'
grid on

%% Just Timing Data (dataImport)
figure(13)
subplot(4,1,1)
plot(cflie1Time(4:end),cflie1LoopTime(4:end),'-b')
title 'Loop Timing'
ylim([0,0.005])
grid on
subplot(4,1,2)
plot(cflie2Time(4:end),cflie2LoopTime(4:end),'-g')
ylim([0,0.005])
grid on
subplot(4,1,3)
plot(cflie3Time(4:end),cflie3LoopTime(4:end),'-k')
ylim([0,0.005])
grid on
subplot(4,1,4)
plot(cflie4Time(4:end),cflie4LoopTime(4:end),'-y')
ylim([0,0.005])
grid on

% legend('Callback 1', 'Callback 2', 'Callback3', 'Callback4','Location','NorthWest')
ylabel 'Loop Time (seconds)'
xlabel 'Time (seconds)'
grid on

figure(14)
subplot(4,1,1)
plot(cflie1Time(4:end),cflie1LoopTimeDelta(4:end),'-b')
title 'Time Between Loop Calls'
ylim([0,0.05])
grid on
subplot(4,1,2)
plot(cflie2Time(4:end),cflie2LoopTimeDelta(4:end),'-g')
ylim([0,0.05])
grid on
subplot(4,1,3)
plot(cflie3Time(4:end),cflie3LoopTimeDelta(4:end),'-k')
ylim([0,0.05])
grid on
subplot(4,1,4)
plot(cflie4Time(4:end),cflie4LoopTimeDelta(4:end),'-y')
ylim([0,0.05])
grid on

% legend('Callback 1', 'Callback 2', 'Callback3', 'Callback4','Location','NorthWest')
ylabel 'Loop Deltas (seconds)'
xlabel 'Time (seconds)'
grid on


%% Pitch Roll Yaw Performance from DataImport
% figure(10)
% 
% subplot(3,1,1)
% hold off
% plot(cflie1Time,cflie1Pitch, '-b')
% hold on
% plot(cflie1Time,cflie1PitchDesired, '-.b')
% 
% plot(cflie2Time,cflie2Pitch, '-g')
% plot(cflie2Time,cflie2PitchDesired, '-.g')
% plot(cflie3Time,cflie3Pitch, '-k')
% plot(cflie3Time,cflie3PitchDesired, '-.k')
% plot(cflie4Time,cflie4Pitch, '-y')
% plot(cflie4Time,cflie4PitchDesired, '-.y')
% title 'Crazyflie Pitch'
% legend('Crazyflie 1','', 'Crazyflie2','', 'Crazyflie3','', 'Crazyflie4')
% 
% subplot(3,1,2)
% hold off
% plot(cflie1Time,cflie1Roll, '-b')
% hold on
% plot(cflie1Time,cflie1RollDesired, '-.b')
% 
% plot(cflie2Time,cflie2Roll, '-g')
% plot(cflie2Time,cflie2RollDesired, '-.g')
% plot(cflie3Time,cflie3Roll, '-k')
% plot(cflie3Time,cflie3RollDesired, '-.k')
% plot(cflie4Time,cflie4Roll, '-y')
% plot(cflie4Time,cflie4RollDesired, '-.y')
% title 'Crazyflie Roll'
% 
% subplot(3,1,3)
% hold off
% plot(cflie1Time,cflie1Yaw, '-b')
% hold on
% plot(cflie1Time,cflie1YawDesired, '-.b')
% 
% plot(cflie2Time,cflie2Yaw, '-g')
% plot(cflie2Time,cflie2YawDesired, '-.g')
% plot(cflie3Time,cflie3Yaw, '-k')
% plot(cflie3Time,cflie3YawDesired, '-.k')
% plot(cflie4Time,cflie4Yaw, '-y')
% plot(cflie4Time,cflie4YawDesired, '-.y')
% title 'Crazyflie Yaw'


%% Time Deltas (using dataAnalysis.m)
% 
% for i = 1:length(main.expData.Time.data)-1
%     delta1(i) = main.expData.Time.data(i+1)-main.expData.Time.data(i);
% end
% 
% figure(3)
% hold off
% subplot(2,1,1)
% plot(main.expData.Time.data,main.expData.Z.data,'-g')
% title 'Crazyflie Z Data (Height)'
% 
% subplot(2,1,2)
% plot(main.expData.Time.data(3:length(main.expData.Time.data)-1),delta1(3:length(delta1)),'-g')
% title 'Crazyflie Time Deltas'
