function th = ani_pend(Theta, Mag, Mag2)
% ani_pend(Theta, Mag, Mag2) or ani_pend([Theta, Mag, Mag2])
%	Animated pendulum block
%	Theta: angle (in radians) from the vertical (right == positive)
%	Mag: direction vector
%	th = output... copies Theta through
%
% Peter C. Jones
% March 1997
%
% Derived from the SIMULINK demo:
%    penddemo.m, and it's associated files
%
% Copyright 1997 by Peter C. Jones and Scott D. Tepavich.
% All Rights Reserved.


if nargin<1, error('Not enough arguments.'), break; end

if nargin<2
  n=length(Theta);
  if n<3, Mag2 = 0; else Mag2=Theta(3); end
  if n<2, Mag = 0; else, Mag=Theta(2); end
  Theta=Theta(1);
elseif nargin<3
  Mag2 = 0;
end

th = Theta;

PDelta = 0.2;
FigureName = 'Pendulum Graphic';
XTop = 10*sin(Theta);
YTop = 10*cos(Theta);
PDcosT = PDelta*cos(Theta);
PDsinT = -PDelta*sin(Theta);

PXD = [XTop-PDcosT XTop+PDcosT; -PDcosT PDcosT];
PYD = [YTop-PDsinT YTop+PDsinT; -PDsinT PDsinT];

m = abs(Mag);
sm = sign(Mag);
AXD = sm*([0 m m m+2*PDelta; 0 m m m+2*PDelta] + 2);
AYD = [ PDelta  PDelta  2*PDelta 0; -PDelta -PDelta -2*PDelta 0]/2-1.5;

m = abs(Mag2);
sm = sign(Mag2);
AX2 = sm*([0 m m m+2*PDelta; 0 m m m+2*PDelta] + 2);
AY2 = [ PDelta  PDelta  2*PDelta 0; -PDelta -PDelta -2*PDelta 0]/2-0.5;

Figures = get(0,'Chil');
for INDEX=1:length(Figures),
   if strcmp(get(Figures(INDEX),'Type'),'figure'),
      if strcmp(get(Figures(INDEX),'Name'),FigureName),
         Fig = Figures(INDEX);
         MyUD = get(Fig,'UserData');
         Pend = MyUD(1);
         Arrow = MyUD(2);
         A2 = MyUD(3);
         set(Pend, 'XData', PXD, 'YData', PYD);
         set(Arrow, 'XData', AXD);
         set(Arrow, 'YData', AYD);
         set(A2, 'XData', AX2);
         set(A2, 'YData', AY2);
         drawnow
         % Time to exit the routine
         return
      end % if right name
   end % if ifigure
end % for INDEX

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Due to *return* above, rest of code only executed if creating animation
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Fig = figure('Unit','pixel','Pos',[10 10 300 200],'Name',FigureName);
axes('Unit','pixel','Pos',[15 15 285 185],'CLim',[1 64], ...
     'Xlim',[-6 6],'Ylim',[-2 10],'Vis','off');
Tri = surface('XData',[-PDelta PDelta; -2 2],...
              'YData',[0 0; -2 -2],...
              'ZData',[0 0; 0 0],...
              'CData',[44 44; 44 44],...
              'Erase','xor');
Pend = surface('XData', PXD, 'YData', PYD,...
               'ZData',zeros(2),...
               'CData',12*ones(2),...
               'Erase','xor');
Arrow = surface('XData', AXD, 'YData', AYD,...
                'ZData', zeros(2,4),...
                'CData', ones(2,4),...
                'Erase','xor');
A2 = surface('XData', AX2, 'YData', AY2,...
             'ZData', zeros(2,4),...
             'CData', 23*ones(2,4),...
             'Erase','xor');
set(Fig,'UserData',[Pend Arrow A2]);

text(-6,-0.5,'Control','Color',[0,1,0]);
text(-6,-1.5,'Disturbance','Color',[1,0,0]);

drawnow

% end pend0