%  This function solves homework problem [H8.7]
%  --------------------------------------------
%
echo on
%
g = 9.81;
m = 10;
l = 1;
prephi = pi/4;
prephidot = 0;
phi2dot = 0;
y0 = sqrt(2)/2;
h = 0.01;
tvec = zeros(1,1001);
phivec = zeros(1,1001);
Efvec = zeros(1,1001);
echo off
for i=0:1000,
    t = h*i;
    f = ones(2,1);
    nf = 0;
    while abs(norm(f) - nf) > 1.0e-6,
        nf = norm(f);
        phidot = prephidot + h*phi2dot;
        pd2 = phidot*phidot;
        phi = prephi + h*phidot;
        sp = sin(phi);
        cp = cos(phi);
        x = l*sp;
        y = l*cp;
        dvx1 = x*l*pd2;
        dvx2 = x*cp*g;
        dvx3 = x*sp;
        dvx4 = y*cp;
        dvx5 = dvx1 + dvx2;
        dvx6 = dvx3 + dvx4;
        dvx = -dvx5/dvx6;
        np2d1 = l*cp;
        np2d2 = sp/cp;
        np2d3 = dvx/np2d1;
        np2d4 = np2d2*pd2;
        newphi2dot = np2d3 + np2d4;
        vy = -l*sp*phidot;
        vx = l*cp*phidot;
        Ek = m*vx^2/2 + m*vy^2/2;
        Ep = m*g*(y0 - y);
        Ef = Ep + Ek;
        f = [ newphi2dot - phi2dot ; Ef ];
        Dphidot = h;
        Dpd2 = 2*phidot*Dphidot;
        Dphi = h*Dphidot;
        Dsp = cos(phi)*Dphi;
        Dcp = -sin(phi)*Dphi;
        Dx = l*Dsp;
        Dy = l*Dcp;
        Ddvx1 = Dx*l*pd2 + x*l*Dpd2;
        Ddvx2 = Dx*cp*g + x*Dcp*g;
        Ddvx3 = Dx*sp + x*Dsp;
        Ddvx4 = Dy*cp + y*Dcp;
        Ddvx5 = Ddvx1 + Ddvx2;
        Ddvx6 = Ddvx3 + Ddvx4;
        Ddvx = - (Ddvx5*dvx6 - dvx5*Ddvx6)/(dvx6*dvx6);
        Dnp2d1 = l*Dcp;
        Dnp2d2 = (Dsp*cp - sp*Dcp)/(cp*cp);
        Dnp2d3 = (Ddvx*np2d1 - dvx*Dnp2d1)/(np2d1*np2d1);
        Dnp2d4 = Dnp2d2*pd2 + np2d2*Dpd2;
        Dnewphi2dot = Dnp2d3 + Dnp2d4;
        Dvy = -l*(Dsp*phidot + sp*Dphidot);
        Dvx = l*(Dcp*phidot + cp*Dphidot);
        DEk = m*vx*Dvx + m*vy*Dvy;
        DEp = -m*g*Dy;
        DEf = DEp + DEk;
        H = [ Dnewphi2dot - 1 ; DEf ];
        phi2dot = phi2dot - H\f;
    end,
    dvy = -l*sp*phi2dot - l*cp*phidot^2;
    F = (m*g*l)/y - (m*l*dvy)/y;
    tvec(i+1) = t;
    phivec(i+1) = phi;
    Efvec(i+1) = Ef;
    prephi = phi;
    prephidot = phidot;
end
echo on
%
subplot(2,1,1)
plot(tvec,phivec,'k-')
grid on;
title('\tex{Stabilized Backward Euler Simulation of Pendulum}')
xlabel('\tex{$time$}')
ylabel('\tex{$\varphi$}')
%
subplot(2,1,2)
plot(tvec,Efvec,'k-')
grid on;
xlabel('\tex{$time$}')
ylabel('\tex{$E_f$}')
%
print -deps nsds_hw8_7.eps
%
return
