% Program 6.3 Animation program for pendulum
% Inputs: time interval inter,
%  initial values ic = [y(1,1) y(1,2)], number of steps n
% Calls a one-step method such as trapstep.m
% Example usage: pend([0 10],[pi/2 0],200,19)
%A is the forcing strength
%ic is the initial value and velocity
%inter is the time interval
%n is the number of steps
function print = pendosc(inter,ic,n,A)
print = 1;
h=(inter(2)-inter(1))/n;  % plot n points in total
y(1,:)=ic;                % enter initial conds in y
t(1)=inter(1);
set(gca,'xlim',[-1.2 1.2],'ylim',[-1.2 1.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',[]);
for k=1:n
  t(k+1)=t(k)+h;
  y(k+1,:)=trapstep(t(k),y(k,:),h,A);
  xbob = sin(y(k+1,1)); ybob = -cos(y(k+1,1));
  xrod = [0 xbob]; yrod = [0 ybob];
  set(rod,'xdata',xrod,'ydata',yrod)
  set(bob,'xdata',xbob,'ydata',ybob)
  if abs(xbob)<0.05 & ybob<0
      print = 0;
      break
  end
  %drawnow; pause(h)
end
function y = trapstep(t,x,h,A)
%one step of the Trapezoid Method
z1=ydot(t,x,A);
g=x+h*z1;
z2=ydot(t+h,g,A);
y=x+h*(z1+z2)/2;

function z=ydot(t,y,A)
g=9.81;length=2.5; d=0.1;
z(1) = y(2);
z(2) = -d*y(2)-(g/length+A*cos(2*pi*t))*sin(y(1));

%Function PendAFind
for i=1:100
    print = pendosc([0 10],[3.1 0],200,i);
    %k = waitforbuttonpress;
    if print ==1
        i
        break
    end
end

