% Calculate and plot the 1-d spring pendulum energy and trajectory
% Isaac Lenton
% August 2017

mass = 1.0;             % Mass of pendulum (kg)
acceleration = 0.0;     % Gravitational acceleration (m/s/s)
springConst = 1.0;      % Spring constant (kg/s/s)
x0 = -1.0;              % Initial position (m)

dt = 1.0e-3;            % Simulation step size
outputRate = 100;       % Number of steps between output cycles

potential = @(x) -mass*acceleration*x + 0.5*springConst*x.^2;
eqmotion = @(x) mass*acceleration - springConst*x;
kinetic = @(v) 0.5*mass*v.^2;

tt = 0.0;
xt = x0;
vt = 0.0;
pt = potential(x0);
kt = kinetic(vt);

h = figure;

% plot the potential, initial energy and show the current value
subplot(1, 2, 1);
xp = linspace(-2, 2, 100);
hleft = plot(xp, potential(xp), 'b', ...
    xp, potential(x0)*ones(size(xp)), 'g', xt, potential(xt), 'ro');
xlabel('Position (m)');
ylabel('Potential Energy (J)');

% Plot the kinetic and potential energy
subplot(1, 2, 2);
tmax = 10;
hright = plot(tt, pt, tt, kt);
axis([0, tmax, 0, 0.5]);
legend('Potential', 'Kinetic');
xlabel('Time (s)');
ylabel('Energy (J)');

while ishandle(h)
    % Evolve the particle state (first order, inacurate at long times)
    xt = [xt xt(end)];
    vt = [vt vt(end)];
    for n = 1:outputRate
        oldvt = vt(end);
        vt(end) = vt(end) + eqmotion(xt(end))/mass*dt;
        xt(end) = xt(end) + oldvt*dt;
    end
    pt = [pt potential(xt(end))];
    kt = [kt kinetic(vt(end))];
    tt = [tt tt(end)+dt*outputRate];
    
    % Update the plots (could also plot vt and xt too)
    set(hleft(3), 'xdata', xt(end), 'ydata', pt(end));
    set(hright(1), 'xdata', tt, 'ydata', pt);
    set(hright(2), 'xdata', tt, 'ydata', kt);
    
    pause(0.05);
    drawnow;
end