%% simple pendulum using Euler's method
L = 1;
tf = 5;
dt = 0.1;
theta0 = 0.95 * pi;
omega0 = 0;
[theta, omega, t] = solvependulum(L, tf, dt, theta0, omega0);

n = length(theta);
writerObj = VideoWriter('pendulum1.avi');
open(writerObj);
clear F;
F(n) = struct('cdata',[],'colormap',[]);
for j = 1:n
    x = cos(theta(j) - pi/2);
    y = sin(theta(j) - pi/2);
    plot(x, y, 'o');
    line([0 x], [0 y]);
    axis([-1.1 1.1 -1.1 1.1])
    axis square
    F(j) = getframe;
    writeVideo(writerObj,F(j));
end
close(writerObj);


%% dt = 0.01
dt = 0.01;
[theta, omega, t] = solvependulum(L, tf, dt, theta0, omega0);

n = length(theta);
writerObj = VideoWriter('pendulum2.avi');
open(writerObj);
clear F;
F(n) = struct('cdata',[],'colormap',[]);
for j = 1:n
    x = cos(theta(j) - pi/2);
    y = sin(theta(j) - pi/2);
    plot(x, y, 'o');
    line([0 x], [0 y]);
    axis([-1.1 1.1 -1.1 1.1])
    axis square
    F(j) = getframe;
    writeVideo(writerObj,F(j));
end
close(writerObj);


%% dt = 0.001
dt = 0.001;
tf = 10;
[theta, omega, t] = solvependulum(L, tf, dt, theta0, omega0);

n = length(theta);
writerObj = VideoWriter('pendulum3.avi');
open(writerObj);
clear F;
F(n) = struct('cdata',[],'colormap',[]);
for j = 1:10:n
    x = cos(theta(j) - pi/2);
    y = sin(theta(j) - pi/2);
    plot(x, y, 'o');
    line([0 x], [0 y]);
    axis([-1.1 1.1 -1.1 1.1])
    axis square
    F(j) = getframe;
    writeVideo(writerObj,F(j));
end
close(writerObj);

%% pendulum solved using ode45
[t, Y] = ode45(@pendulumode, linspace(0, tf, 1000), [theta0; omega0]);
n = length(Y);
writerObj = VideoWriter('pendulum4.avi');
open(writerObj);
clear F;
F(n) = struct('cdata',[],'colormap',[]);
for j = 1:n
    theta = Y(j, 1);
    x = cos(theta - pi/2);
    y = sin(theta - pi/2);
    plot(x, y, 'o');
    line([0 x], [0 y]);
    axis([-1.1 1.1 -1.1 1.1])
    axis square
    F(j) = getframe;
    writeVideo(writerObj,F(j));
end
close(writerObj);

plot(t, Y(:, 1), 'b-', t, theta0 * cos(sqrt(9.81) * t), 'r--');
xlabel('time (s)');
ylabel('theta (radians)');
title('angular position vs time for simple pendulum');
legend('ode', 'small angle approximation')
axis([0 10 -4 4]);

