function main
% A&S demo of velocity and acceleration calculations
% given a data file of time and location data

clear all %OK in main function
close all

srss = inline('sqrt(a.*a + b.*b)','a','b');

%read data from file
m = csvread('drpgps.csv'); %gets nx3 matrix with time in column 1, x in column 2, y in column 3
mflip = m'; %transpose (flip rows and columns) now time in row 1, etc.

%extract data into their own row vectors
t = mflip(1,:); 
x = mflip(2,:);
y = mflip(3,:);

%plot path
figure(1);
plot(x,y,'*-');
title('Dr P. GPS data plot');
xlabel('X position (m)');
ylabel('Y position (m)');
set(gcf,'position',[42 313 480 420]);
for ii=1:1:length(t)
    text(x(ii),y(ii),[' ' num2str(t(ii))]);
end
text(40,-10,{'Numbers shown are','time in seconds'});
fprintf('Press a key to continue . . .')
pause

%calculate distance travelled (position along the path)
%hints - the distance is the cumulative sum of the distances
%      - position at time=0 is 0
dx = diff(x);
dy = diff(y);
ds = srss(dx,dy);
s = [ 0 cumsum(ds) ];

% plot the position data
figure(2);
set(gcf,'position',[532 51 449 676]);
initplot(3,1,1,'Distance (m)');
plot(t,s,'-');

% calc and plot velocity data
[tv v] = numder(t,s);
initplot(3,1,2,'Velocity (m/s)');
plot(tv,v,'-');

% calc and plot tangential acceleration data
[tat at] = numder(tv,v);
initplot(3,1,3,'Acceleration (m/s^2)');
plot(tat,at,'-');

% calc velocity data a second way (by components)
[tvx,vx] = numder(t,x);
[tvy,vy] = numder(t,y);
  
% plot on top of existing velocity curve to show they are the same
v2 = srss(vx,vy);
subplot(3,1,2);
plot(tvx,v2,'r*');
legend('ds/dt','vel by components');

% calc total acceleration by components 
[tax,ax] = numder(tvx,vx);
[tay,ay] = numder(tvy,vy);
 
% plot on top of existing accel curve to show they are different
atot = srss(ax,ay);
subplot(3,1,3)
plot(tax,atot,'r-');
legend('a_{tan}','a_{tot}');

% display velocity and acceleration vectors on position graph
% tweak a little because we have velocity at time midpoints
% and acceleration at at points 2 thru n-1
vxx = vx(1:length(vx)-1) + diff(vx);
vyy = vy(1:length(vy)-1) + diff(vy);
xx = x(2:length(x)-1);
yy = y(2:length(y)-1);

close(1)
figure(1)
set(gcf,'position',[42 313 480 420]);
fprintf('Press a key to step through the display of velocity and acceleration vectors\n');
for ii=1:length(tax)
    clf reset;
    hold on;
    axis equal;
    axis([-120 120 -120 80]);
    plot(x,y,':'); %the entire curve
    vvec = [vx(ii) vy(ii)];
    plotvec(xx(ii),yy(ii),vvec(1),vvec(2),'b-',2); %velocity
    vdir = vvec./norm(vvec); %unit vector
    avec = [ax(ii) ay(ii)]; %total acceleration
    atvec = sum(vdir.*avec).*vdir; %(dot product)*(unit vector)
    anvec = avec - atvec; %vector subtraction
    plotvec(xx(ii),yy(ii),atvec(1),atvec(2),'r-',4);
    plotvec(xx(ii),yy(ii),anvec(1),anvec(2),'g-',4);
    legend('path','velocity','a_t','a_n',4);
    pause
end
close(1)

return

function plotvec(x,y,dx,dy,style,scale)
%plot a vector from x,y with direction dx,dy and length scaled by scale
plot([x x+dx.*scale], [y y+dy.*scale], style);
return

function [rx,ry] = numint(x,y,c)
% return data points representing numerical integration
% of lists x and y with integration constant c
rx = x;
ry = cumtrapz(x,y);
ry = ry + c;
return

function [rx,ry] = numder(x,y)
% return data points representing numerical differentiation
% of lists x and y
% rx is returned as midpoints of adjacent x values
dx = diff(x);
dy = diff(y);
rx = x(1:length(x)-1);
rx = rx + dx./2;
ry = dy./dx;
return

function initplot(nr,nc,pos,ylab)
% initialize a subplot for the current figure
subplot(nr,nc,pos);
hold on;
%axis(lim); %allow Matlab to do automatic scaling
xlabel('Time (sec)');
ylabel(ylab);
return