function HW2_2022
close all

%% Read and plot the dataset

data = csvread('Midge_Data_Ob17.csv');
id = data(:,1); pos = data(:,2:4); time=data(:,5); vel = data(:,6:8); acc = data(:,9:11);

% This solution set uses vector masking to extract data for a particular
% midge.   Vector masking is not covered in the MATLAB tutorial - the
% problem set suggests extracting the data for the midge of interest using a
% loop and a conditional statement.   For example
% [nrows,ncols] = size(data);
% count = 1;
% for i=1:nrows
%    if (data(i,1)==id)
%       count = count + 1;
%       pos(count,1:3) = data(i,2:4);
%       time(count) = data(i,5);
%       vel(count,1:3) = data(i,6:8);
%       acc(count,1:3) = data(i,9:11);
%     end
% end
mask = id==4;

% Plot trajectory
figure
plot3(pos(mask,1),pos(mask,2),pos(mask,3),'Linewidth',2,'Color',[1,0,0]);
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on','ZGrid','on');
title('Trajectory (fixed orientation)')
xlabel('x (mm)'); ylabel('y (mm)');  zlabel('z (mm)');
% Plot velocity
figure
p1 = plot(time(mask),vel(mask,1),'Linewidth',1,'Color',[0,0,1]);
hold on
p2 = plot(time(mask),vel(mask,2),'Linewidth',1,'Color',[1,0,0]);
p3 = plot(time(mask),vel(mask,3),'Linewidth',1,'Color',[0,1,0]);
xlabel('time (s)')
ylabel('Velocity (mm/s)')
title('Midge velocity ')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p2,p3],'v_x','v_y','v_z');
set(legend1,...
    'Position',[0.41665983606557377 0.208333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);
% 
% Plot acceleration
figure
p1 = plot(time(mask),acc(mask,1),'Linewidth',1,'Color',[0,0,1]);
hold on
p2 = plot(time(mask),acc(mask,2),'Linewidth',1,'Color',[1,0,0]);
p3 = plot(time(mask),acc(mask,3),'Linewidth',1,'Color',[0,1,0]);
xlabel('time (s)')
ylabel('Acceleration (mm/s^2)')
title('Midge acceleration')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p2,p3],'a_x','a_y','a_z');
set(legend1,...
    'Position',[0.41665983606557377 0.208333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);
%%
% Integrate acceleration
integratedaccel = cumtrapz(time(mask),acc(mask,1:3));
figure
p1 = plot(time(mask),integratedaccel(:,1),'Linewidth',1,'Color',[0,0,1]);
ylim([-1000,2500]);
hold on
p2 = plot(time(mask),integratedaccel(:,2),'Linewidth',1,'Color',[1,0,0]);
p3 = plot(time(mask),integratedaccel(:,3),'Linewidth',1,'Color',[0,1,0]);
p4 = plot(time(mask),vel(mask,1),'Linewidth',1,'Color',[0,0,1],'LineStyle','--');
p5 = plot(time(mask),vel(mask,2),'Linewidth',1,'Color',[1,0,0],'LineStyle','--');
p6 = plot(time(mask),vel(mask,3),'Linewidth',1,'Color',[0,1,0],'LineStyle','--');
xlabel('time (s)')
ylabel('Velocity (mm/s)')
title('Midge velocity (uncorrected)')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p2,p3,p4,p5,p6],'integrated a_x','integrated a_y','integrated a_z','measured v_x',' measured v_y',' measured v_z');
set(legend1,...
    'Position',[0.126243172319535 0.5843447808423 0.297999994158745 0.250999992847443],...
    'FontAngle','italic',...
    'FontSize',14);
%% 
% Integrate acceleration
v = vel(mask,1:3); % We need to extract the velocity into a new vector
initialv = v(1,1:3);
integratedaccel = cumtrapz(time(mask),acc(mask,1:3)) + initialv;
figure
p1 = plot(time(mask),integratedaccel(:,1),'Linewidth',1,'Color',[0,0,1]);
ylim([-1000,2500]);
hold on
p2 = plot(time(mask),integratedaccel(:,2),'Linewidth',1,'Color',[1,0,0]);
p3 = plot(time(mask),integratedaccel(:,3),'Linewidth',1,'Color',[0,1,0]);
p4 = plot(time(mask),vel(mask,1),'Linewidth',1,'Color',[0,0,1],'LineStyle','--');
p5 = plot(time(mask),vel(mask,2),'Linewidth',1,'Color',[1,0,0],'LineStyle','--');
p6 = plot(time(mask),vel(mask,3),'Linewidth',1,'Color',[0,1,0],'LineStyle','--');
xlabel('time (s)')
ylabel('Velocity (mm/s)')
title('Midge velocity')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p2,p3,p4,p5,p6],'integrated a_x','integrated a_y','integrated a_z','measured v_x',' measured v_y',' measured v_z');
set(legend1,...
    'Position',[0.126243172319535 0.5843447808423 0.297999994158745 0.250999992847443],...
    'FontAngle','italic',...
    'FontSize',14);

%% Find the speed

for i=1:length(time(mask))
    speed(i) = sqrt( v(i,1)^2 + v(i,2)^2 + v(i,3)^2 );
end
figure
plot(time(mask),speed,'Linewidth',1,'Color',[0,0,1]);
xlabel('time (s)')
ylabel('speed (mm/s)')
title('Midge Speed')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');

%% Calculate the tangential acceleration
t = time(mask);  % Need to extract time into a new vector
for i=1:length(t)-1
    at(i) = (speed(i+1)-speed(i))/(t(i+1)-t(i));
    tmidpt(i) = (t(i)+t(i+1))/2;
end
figure
plot(tmidpt,at,'Linewidth',1,'Color',[0,0,1]);
xlabel('time (s)')
ylabel('acceleration (mm/s^2)')
title('Midge tangential acceleration')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');

%% Calculate the tangential acceleration as the dot product of tangent vector with acceleration vector
%
a = acc(mask,1:3);
for i = 1:length(t)
    at_direct(i) = dot(a(i,1:3),v(i,1:3))/speed(i);
end
figure
plot(t,at_direct,'Linewidth',1,'Color',[0,0,1]);
xlabel('time (s)')
ylabel('acceleration (mm/s^2)')
title('Midge tangential acceleration')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');


end