%%
% Planar pendulum
clear all
close all
clc

f=@(q)(-sin(q))

q0=30*pi/180;
v0=0;

tin=0; tfin=12*pi; N=500;
t=linspace(tin,tfin,N); dt=t(2)-t(1);

q(1)=q0; v(1)=v0;
for i=1:N-1
   qc=q(i)+v(i)*dt/2;
   vc=v(i)+f(q(i))*dt/2;
   q(i+1)=q(i)+vc*dt;
   v(i+1)=v(i)+f(qc)*dt;
end

qdegree=q*180/pi;
vdegree=v*180/pi;

subplot(2,1,1)
hold on
plot(t,qdegree,'r','Linewidth',2)
plot(t,vdegree,'b')
legend('\theta [degrees]', 'd \theta/dt [degrees]')
xlabel('t [dimensionless]','FontSize',16)
hold off
subplot(2,1,2)
hold on
plot(qdegree,vdegree,'r','Linewidth',2)
xlabel('\theta [degrees]','FontSize',16)
ylabel('d\theta/dt [degrees]','FontSize',16)
hold off
%%
clear all
close all

qex=@(t,q0)(q0*cos(t));
vex=@(t,q0)(-q0*sin(t))

f=@(q)(-sin(q))

q0=30*pi/180;
v0=0;

tin=0; tfin=12*pi; N=500;
t=linspace(tin,tfin,N); dt=t(2)-t(1);

q(1)=q0; v(1)=v0;
for i=1:N-1
   qc=q(i)+v(i)*dt/2;
   vc=v(i)+f(q(i))*dt/2;
   q(i+1)=q(i)+vc*dt;
   v(i+1)=v(i)+f(qc)*dt;
end

subplot(2,1,1)
hold on
plot(t,q,'r','Linewidth',2)
plot(t,v,'b')
plot(t,qex(t,q0),'r--')
plot(t,vex(t,q0),'b--')
legend('\theta', 'd\theta/dt','\theta HO','d\theta/dt HO')
xlabel('t [dimensionless]','FontSize',16)
hold off
subplot(2,1,2)
hold on
plot(q,v,'r','Linewidth',2)
plot(qex(t,q0),vex(t,q0),'r--')
legend('RK','HO')
axis('equal')
xlabel('\theta','FontSize',16)
ylabel('d\theta/dt','FontSize',16)
hold off
%%
% different initial condition
clear all
close all

f=@(q)(-sin(q))

q0=pi-0.05;
v0=0;

tin=0; tfin=12*pi; N=500;

[t, q, v]=RK_pendulo(tin, tfin, N, q0, v0, f)

subplot(2,1,1)
hold on
plot(t,q,'r','Linewidth',2)
plot(t,v,'b')
legend('\theta', 'd\theta/dt')
xlabel('t [dimensionless]','FontSize',16)
hold off
subplot(2,1,2)
hold on
plot(q,v,'r','Linewidth',2)
legend('RK')
axis('equal')
xlabel('\theta','FontSize',16)
ylabel('d\theta/dt','FontSize',16)
hold off
%%
% different initial conditions: phase space
clear all
close all

f=@(q)(-sin(q))

tin=0; tfin=12*pi; N=500;

hold on
q0=0.1*pi;
v0=0;
[t, q, v]=RK_pendulo(tin, tfin, N, q0, v0, f)
plot(q,v,'r')

q0=0.2*pi;
v0=0;
[t, q, v]=RK_pendulo(tin, tfin, N, q0, v0, f)
plot(q,v,'b')

q0=0.5*pi;
v0=0;
[t, q, v]=RK_pendulo(tin, tfin, N, q0, v0, f)
plot(q,v,'c')

q0=pi-0.1;
v0=0;
[t, q, v]=RK_pendulo(tin, tfin, N, q0, v0, f)
plot(q,v,'g')

q0=pi-0.1;
v0=-1;
[t, q, v]=RK_pendulo(tin, tfin, N, q0, v0, f)
plot(q,v,'m')

q0=-pi-0.1;
v0=+1;
[t, q, v]=RK_pendulo(tin, tfin, N, q0, v0, f)
plot(q,v,'rx')

legend('\theta_0=0.1*\pi, v_0=0','\theta_0=0.2*\pi, v_0=0',...
    '\theta_0=0.5*\pi, v_0=0','\theta_0=\pi, v_0=0',...
    '\theta_0=\pi, v_0=-1','\theta_0=\pi, v_0=1')
%axis('equal')
axis([-3.5, 3.5, -3.5, 3.5])
xlabel('\theta','FontSize',16)
ylabel('d\theta/dt','FontSize',16)
hold off

