% Part 8 variant
function pend8(inter, ic1, ic2, n)
h=(inter(2)-inter(1))/n;  % plot n points in total
y1(1,:)=ic1;                % enter initial conds in y1
y3(1,:)=ic2;                % enter initial conds in y3
set(gca,'xlim',[-2.2 2.2],'ylim',[-2.2 2.2], ...
  'XTick',[-1 0 1],'YTick',[-1 0 1], ...
  'Drawmode','fast','Visible','on','NextPlot','add');
cla;         
axis square               % make aspect ratio 1 - 1
bob = line('color','r','Marker','.','markersize',40,...
    'erase','xor','xdata',[],'ydata',[]);
rod = line('color','b','LineStyle','-','LineWidth',3,...
    'erase','xor','xdata',[],'ydata',[]);
bob2 = line('color','g','Marker','.','markersize',40,...
    'erase','xor','xdata',[],'ydata',[]);
rod2 = line('color','b','LineStyle','-','LineWidth',3,...
    'erase','xor','xdata',[],'ydata',[]);
for k=1:n
  [y1(k+1,:), y3(k+1,:)] = trapstep(y1(k,:), y3(k,:), h);
  xbob = sin(y1(k+1,1)); ybob = -cos(y1(k+1,1));
  xrod = [0 xbob]; yrod = [0 ybob];
  xbob2 = xbob + sin(y3(k+1,1)); ybob2 = ybob - cos(y3(k+1,1));
  xrod2 = [xbob xbob2]; yrod2 = [ybob ybob2];
  set(rod,'xdata',xrod,'ydata',yrod)
  set(bob,'xdata',xbob,'ydata',ybob)
  set(rod2,'xdata',xrod2,'ydata',yrod2)
  set(bob2,'xdata',xbob2,'ydata',ybob2)
  drawnow; pause(h)
end

function [y1, y3] = trapstep(x1, x2, h)
%one step of the Trapezoid Method
[z1, z2] =ydot(x1,x2);
g1 = x1 + z1*h;
g2 = x2 + z2*h;
[z3, z4] =ydot(g1,g2);
y1 = x1 + (h/2)*(z1+z3); 
y3 = x2 + (h/2)*(z2+z4);

%[z3, z4] =ydot(x1 + z1*h/2,	x2 + z2*h/2);
%[z5, z6] =ydot(x1 + z3*h/2,	x2 + z4*h/2);
%[z7, z8] =ydot(x1 + z5*h,		x2 + z6*h);

%y1=x1+(h/6)*(z1+2*z3+2*z5+z7);
%y3=x2+(h/6)*(z2+2*z4+2*z6+z8);

function [y2, y4]=ydot(y1,y3)
g=9.81;d=0;
y2(1) = y1(2);
y4(1) = y3(2);

a = 2*sin(y1(1) - y3(1));
b = 3 - cos(2*y1(1) - 2*y3(1));
c = 3*g*sin(y1(1));
i = g*sin(y1(1) - 2*y3(1));
%e = (y4(1))^2 - cos(y1(1) - y3(1))*(y2(1))^2; Typo?????
e = (y4(1))^2 + cos(y1(1) - y3(1))*(y2(1))^2;
f = 2*(y2(1))^2 + 2*g*cos(y1(1)) + cos(y1(1) - y3(1))*(y4(1))^2;

y2(2) = (-(c + i + a*e)/b) - d*y2(1);
y4(2) = (a*f)/b;

%y2(2) = ((-3*g*sin(y1(1)) - g*sin(y1(1) - 2*y3(1)) - 2*sin(y1(1) - y3(1))*(y4(1)^2 - (y2(1)^2)*cos(y1(1) - y3(1))))/(3 - cos(2*y1(1) - 2*y3(1)))) - d*y2(1);
%y4(2) = (2*sin(y1(1) -y3(1))*(2*y2(1)^2 + 2*g*cos(y1(1)) + (y4(1)^2)*cos(y1(1) -y3(1))))/(3 - cos(2*y1(1) - 2*y3(1)));