close all; clear all; clc;

%Define parameters
p = struct('m',1,'g',1);

%Define tarray
tf = 15; %integration ends at 15 seconds
n = 101;
tarray = linspace(0,tf,n);

%Define initial conditions
x0 = 0;
y0 = 3;
v0 = 10;
theta = pi/3;
xd0 = v0*cos(theta);
yd0 = v0*sin(theta);

z0 = [x0,y0,xd0,yd0]; %Initial conditions

%Options
opts = odeset('AbsTol',1e-6,'RelTol',1e-6);

%overwrite myrhs as an anonymous function
f = @(t,z) myrhs(t,z,p);

%Solve numerically
%[tarray zarray] = ode45(f,tarray,z0,opts);
z = ode45(f,tarray,z0,opts);
zarray = deval(z,tarray);

%Unpack zarray
x = zarray(1,:);
y = zarray(2,:);
xdot = zarray(3,:);
ydot = zarray(4,:);

%Plot Trajectory
figure
plot(x,y)
xlabel('X (m)')
ylabel('Y (m)')
title('Trajectory')
axis equal
grid on

%Plot Velocities
figure
hold on
plot(tarray,xdot)
plot(tarray,ydot)
xlabel('Time (s)')
ylabel('Velocity (m/s)')
legend('V_x','V_y')
grid on

%Animate Trajectory
figure
p = plot(x0,y0,'kh');
a = animatedline(x0,y0,'Color',[0,.4,.4]);
axis equal
axis([0 100 -20 50])

t = 0;
timeScale = 2;
tic
while t<tf
    znow = deval(z,t);
    x = znow(1);
    y = znow(2);
    p.XData = x; p.YData = y;
    addpoints(a,x,y)
    
    drawnow
    t = toc*timeScale;
end