%{
  Solution of the non-linear pendulum equation by a
  specific, implicit Newmark method
%}
clear all
clc
close all

T = 100;

t = 0;
q = pi/4;
u = 0;

h = 0.1;
toll = 1e-12;
ns = 0;
count = 0;
stored = zeros(0,3);

hf = figure;
while 1

    ns = ns+1;
    t = t+h;

    qn = q;
    un = u;

    while 1

        R = [q-qn-un*h+(h^2/6)*(2*sin(qn)+sin(q));
            u-un+0.5*h*(sin(qn)+sin(q))];

        J = [1+(h^2/6)*cos(q) , 0;0.5*h*cos(q) , 1];

        dq = -J\R;
        q = q + dq(1);
        u = u + dq(2);

        err = norm([R(1);h*R(2)])/norm([q-qn;h*(u-un)]);
        if (err<toll)
            break
        end

    end

    if mod(ns,5)==0
        count = count+1;
        stored(count,:) = [t,q,u];
    end

    if t>=T
        break;
    end

end


figure(hf);
subplot(1,2,1)
hq=plot(stored(:,1),stored(:,2),'b-');
grid on

subplot(1,2,2)
hq=plot(stored(:,1),stored(:,3),'r-');
grid on