MetronomeAnimation1(t,x,u,flag,pausetime)

function [sys,x0,str,ts] = MetronomeAnimation1(t,x,u,flag,pausetime)
%=====================================================================
%
% MetronomeAnimation S-function.
% Modified by Zeng xianfa, 2008.10.11 
%
%=====================================================================
switch flag,
%================%
% Initialization %
%================%
  case 0,
    [sys,x0,str,ts]=mdlInitializeSizes;
    
%========%
% Update %
%========%
  case 2,
    sys=mdlUpdate(t,x,u,pausetime);
    
%=========%
% Not use %
%=========%
  case { 1, 3, 4, 9 },
    sys = [];
    
%==================%
% Unexpected flags %
%==================% 
  otherwise
    error(['Unhandled flag = ',num2str(flag)]);
end

%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 0;
sizes.NumInputs      = 6;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);

%
% initialize the initial conditions
%
x0  = [];

%
% str is always an empty matrix
%
str = [];

%
% initialize the array of sample times, for the pendulum demo,
% the animation is updated every 0.1 seconds
%
ts  = [0.1 0];

%
% create the figure, if necessary
%
LocalPendInit;

% end mdlInitializeSizes 

%=============================================================================
% mdlUpdate
% Update the pendulum animation.
%=============================================================================
function sys=mdlUpdate(t,x,u,pausetime)
fig = get_param(gcbh,'UserData');
if ishandle(fig),
  if strcmp(get(fig,'Visible'),'on'),
    ud = get(fig,'UserData');
    LocalPendSets(t,ud,u,pausetime);
  end
end;
sys = [];
% end mdlUpdate

%=============================================================================
% LocalPendSets
% Local function to set the position of the graphics objects in the
% inverted pendulum animation window.
%=============================================================================
function LocalPendSets(time,ud,u,pausetime)
u = u([1 2 3 4 5 6]);
XDelta   = 15;
PDelta   = 7;
XPendTop = u(1) + 10*sin(u(2));
YPendTop = -10*cos(u(2));
X1PendTop = u(1) + 10*sin(u(3)) - PDelta;
Y1PendTop = -10*cos(u(3));
X2PendTop = u(1) + 10*sin(u(4)) - PDelta*2;
Y2PendTop = -10*cos(u(3));
X3PendTop = u(1) + 10*sin(u(5)) + PDelta;
Y3PendTop = -10*cos(u(5));
X4PendTop = u(1) + 10*sin(u(6)) + PDelta*2;
Y4PendTop = -10*cos(u(6));
set(ud.Cart,...
  'XData',ones(2,1)*[u(1)-XDelta u(1)+XDelta]);
set(ud.Pend,...
    'XData',[u(1),XPendTop],...
    'YData',[1,YPendTop]);
set(ud.Pend1,...
    'XData',[u(1)-PDelta,X1PendTop],...
    'YData',[1,Y1PendTop]);
set(ud.Pend2,...
    'XData',[u(1)-PDelta*2,X2PendTop],...
    'YData',[1,Y2PendTop]);
set(ud.Pend3,...
    'XData',[u(1)+PDelta,X3PendTop],...
    'YData',[1,Y3PendTop]);
set(ud.Pend4,...
    'XData',[u(1)+PDelta*2,X4PendTop],...
    'YData',[1,Y4PendTop]);

set(ud.Head,...
    'XData',XPendTop,...
    'YData',YPendTop);
set(ud.Head1,...
    'XData',X1PendTop,...
    'YData',Y1PendTop);
set(ud.Head2,...
    'XData',X2PendTop,...
    'YData',Y2PendTop);
set(ud.Head3,...
    'XData',X3PendTop,...
    'YData',Y3PendTop);
set(ud.Head4,...
    'XData',X4PendTop,...
    'YData',Y4PendTop);

set(ud.TimeField,...
  'String',num2str(time));

% Force plot to be drawn
pause(pausetime);
drawnow;
% end LocalPendSets

%
%=============================================================================
% LocalPendInit
% Local function to initialize the pendulum animation.  If the animation
% window already exists, it is brought to the front.  Otherwise, a new
% figure window is created.
%=============================================================================
function LocalPendInit
sys = get_param(gcs,'Parent');
TimeClock = 0;
RefSignal = 0;
XCart     = 0;
Theta     = 0;
XDelta    = 15;
PDelta    = 7;
XPendTop  = XCart + 10*sin(Theta); 
YPendTop  = -10*cos(Theta);
%
% The animation figure handle is stored in the pendulum block's UserData.
% If it exists, initialize the reference mark, time, cart, and pendulum
% positions/strings/etc.
%
Fig = get_param(gcbh,'UserData');
if ishandle(Fig),
  FigUD = get(Fig,'UserData');
  set(FigUD.TimeField,...
      'String',num2str(TimeClock));
  set(FigUD.Cart,...
      'XData',ones(2,1)*[XCart-XDelta XCart+XDelta]);  
  
  % bring it to the front
  figure(Fig);
  return
end
%
% the animation figure doesn't exist, create a new one and store its
% handle in the animation block's UserData
%
FigureName = 'Metronome';
Fig = figure(...
  'Units',           'pixel',...
  'Position',        [100 100 500 300],...
  'Name',            FigureName,...
  'NumberTitle',     'off',...
  'IntegerHandle',   'off',...
  'Resize',          'off');

AxesH = axes(...
  'Parent',  Fig,...
  'Units',   'pixel',...
  'Position',[50 50 400 200],...
  'CLim',    [1 64], ...
  'Xlim',    [-22 22],...
  'Ylim',    [-10 2],...
  'Visible', 'off');

Cart = surface(...
  'Parent',   AxesH,...
  'XData',    ones(2,1)*[XCart-XDelta XCart+XDelta],...
  'YData',    [0 0; 2 2],...
  'ZData',    zeros(2),...
  'CData',    ones(2),...
  'EraseMode','xor');

line([-20,20],[0,0],'linewidth',3);

Pend = line([XCart,XPendTop],[1,YPendTop],'color','b',...
    'linestyle','-','linewidth',2,'erasemode','xor');
Pend1 = line([XCart-PDelta,XPendTop-PDelta],[1,YPendTop],...
    'color','b','linestyle','-','linewidth',2,'erasemode','xor');
Pend2 = line([XCart-PDelta*2,XPendTop-PDelta*2],[1,YPendTop],...
    'color','b','linestyle','-','linewidth',2,'erasemode','xor');
Pend3 = line([XCart+PDelta,XPendTop+PDelta],[1,YPendTop],...
    'color','b','linestyle','-','linewidth',2,'erasemode','xor');
Pend4 = line([XCart+PDelta*2,XPendTop+PDelta*2],[1,YPendTop],...
    'color','b','linestyle','-','linewidth',2,'erasemode','xor');

Head = line(XPendTop,YPendTop,'color','r','linestyle','.',...
    'erasemode','xor','markersize',40);
Head1 = line(XPendTop-PDelta,YPendTop,'color','r','linestyle','.',...
    'erasemode','xor','markersize',40);
Head2 = line(XPendTop-PDelta*2,YPendTop,'color','r','linestyle','.',...
    'erasemode','xor','markersize',40);
Head3 = line(XPendTop+PDelta,YPendTop,'color','r','linestyle','.',...
    'erasemode','xor','markersize',40);
Head4 = line(XPendTop+PDelta*2,YPendTop,'color','r','linestyle','.',...
    'erasemode','xor','markersize',40);

uicontrol(...
  'Parent',             Fig,...
  'Style',              'text',...
  'Units',              'pixel',...
  'Position',           [150 0 100 25], ...
  'HorizontalAlignment','right',...
  'String',             'Time: ');

TimeField = uicontrol(...
  'Parent',             Fig,...
  'Style',              'text',...
  'Units',              'pixel', ...
  'Position',           [250 0 100 25],...
  'HorizontalAlignment','left',...
  'String',             num2str(TimeClock));

FigUD.Cart         = Cart;
FigUD.Pend         = Pend;
FigUD.Pend1        = Pend1;
FigUD.Pend2        = Pend2;
FigUD.Pend3        = Pend3;
FigUD.Pend4        = Pend4;
FigUD.TimeField    = TimeField;
FigUD.Head         = Head;
FigUD.Head1        = Head1;
FigUD.Head2        = Head2;
FigUD.Head3        = Head3;
FigUD.Head4        = Head4;

FigUD.Block        = get_param(gcbh,'Handle');
set(Fig,'UserData',FigUD);
drawnow
% store the figure handle in the animation block's UserData
set_param(gcbh,'UserData',Fig);
% end LocalPendInit