//Input variables (Assumptions: massless pivot, no energy loss)
bob_mass=10;
g=-9.81;
L=2;
theta0=-%pi/6;
v0=0;
t0=0;

//No. of steps
steps=300;

//Setting deltaT or duration (comment either of the lines below)
//deltaT=0.1; t_max=t0+deltaT*steps;
t_max=5; deltaT=(t_max-t0)/steps;

if t_max<=t0 then
    error("Check duration (t0 and t_f), number of steps and deltaT.");
end

//Initial position
not_a_pendulum=%F;
t=zeros(1,steps); t(1)=t0;                      //time
theta=zeros(1,steps); theta(1)=theta0;          //angle
F=zeros(1,steps); F(1)=bob_mass*g*sin(theta0);  //force
A=zeros(1,steps); A(1)=F(1)/bob_mass;           //acceleration
V=zeros(1,steps); V(1)=v0;                      //linear speed
W=zeros(1,steps); W(1)=v0/L;                    //angular speed

for i=2:steps
    t(i)=t(i-1)+deltaT;
    V(i)=A(i-1)*deltaT+V(i-1);
    W(i)=V(i)/L;
    theta(i)=theta(i-1)+W(i)*deltaT;
    F(i)=bob_mass*g*sin(theta(i));
    A(i)=F(i)/bob_mass;
    if (abs(theta(i))>=%pi | (abs(theta(i))==0 & V(i)==0)) & ~not_a_pendulum  then
        disp("Initial conditions do not describe a pendulum.");
        not_a_pendulum = %T;
    end
end
clear i

//Ploting the pendulum
bob_r=0.08*L;
bob_shape=bob_r*exp(%i.*linspace(0,360,20)/180*%pi);

bob_pos=zeros(20,steps);
rod_pos=zeros(1,steps);
for i=1:steps
    rod_pos(i)=L*exp(%i*(-%pi/2+theta(i)));
    bob_pos(:,i)=bob_shape'+rod_pos(i);
end
clear i

scf(0); clf(); xname("Simple gravity pendulum");
plot2d(real([0 rod_pos(1)]),imag([0 rod_pos(1)]));
axes=gca();
axes.isoview="on";
axes.children(1).children.mark_style=3;
axes.children(1).children.mark_size=1;
axes.children(1).children.thickness=3;

plot2d(real(bob_pos(:,1)),imag(bob_pos(:,1)));
axes=gca();
axes.children(1).children.fill_mode="on";
axes.children(1).children.foreground=2;
axes.children(1).children.background=2;

if max(imag(bob_pos))>0 then
    axes.data_bounds=[-L-bob_r,-L-1.01*bob_r;L+bob_r,max(imag(bob_pos))];
else
    axes.data_bounds=[-L-bob_r,-L-1.01*bob_r;L+bob_r,bob_r];
end



//Animating the plot
disp("Duration: "+string(max(t)+deltaT-t0)+"s.");
sleep(850);
for i=2:steps
    axes.children(1).children.data=[real(bob_pos(:,i)), imag(bob_pos(:,i))];
    axes.children(2).children.data=[0, 0; real(rod_pos(i)), imag(rod_pos(i))];
    sleep(deltaT*1000)
end
clear i