function [sys,x0,str,ts] = s_ball_and_beam_an(t,x,u,flag,RefBlock,Lbeam,Rball,init,pauseTime,Ibeam,Mball,DrawingScale,Km)
% s_ball_and_beam_an S-function for making 'ball and beam' animation.
%
%   See also: ball_and_beam_demo.mdl
%
%   Created: October 15, 2004
%   Last Modified: October 29, 2004

% Plots every major integration step, but has no states of its own

switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
    [sys,x0,str,ts]=mdlInitializeSizes(RefBlock,Lbeam,Rball,init);

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2,
    sys=mdlUpdate(t,x,u,Lbeam,Rball,pauseTime,Ibeam,Mball,Km);

    %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  case 3,
    sys=mdlOutputs;

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9,
    sys=mdlTerminate(t,x,u);
    
  %%%%%%%%%%%%%%%%
  % Unused flags %
  %%%%%%%%%%%%%%%%
  case { 1, 4},
    sys = [];
    
  %%%%%%%%%%%%%%%
  % DeleteBlock %
  %%%%%%%%%%%%%%%
  case 'DeleteBlock',
    LocalDeleteBlock
    
  %%%%%%%%%%%%%%%
  % DeleteFigure %
  %%%%%%%%%%%%%%%
  case 'DeleteFigure',
    LocalDeleteFigure
  
  %%%%%%%%%%
  % Slider %
  %%%%%%%%%%
  case 'Slider',
    LocalSlider
  
  %%%%%%%%%
  % Close %
  %%%%%%%%%
  case 'Close',
    LocalClose
  
  %%%%%%%%%%%%
  % Playback %
  %%%%%%%%%%%%
  case 'Playback',
    LocalPlayback
   
  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    error(['Unhandled flag = ',num2str(flag)]);
end
% flag
% disp('end main...')
% end pendan

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts]=mdlInitializeSizes(RefBlock,Lbeam,Rball,init)

%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
sizes = simsizes;

sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 1;   % output to terminate simulation if ball rolls off beam
sizes.NumInputs      = 6;   % u(1) = reference signal,
                            % u(2) = r, linear position of ball along beam
                            % u(3) = theta, incline angle of beam
                            % u(4) = r_dot, linear velocity of ball along beam
                            % u(5) = theta_dot, angular velocity of beam
                            % u(6) = u, control signal into plant
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 'ball and beam' demo,
% the animation is updated every 0.1 seconds
%
% ts  = [0.1 0];    % fixed time step
ts  = [0 0];        % inherit sampling time

%
% create the figure, if necessary
%
LocalBeamInit(RefBlock,Lbeam,Rball,init);

% global variables for trajectory control
global sBB_Flight_isFlight      % variable that determines whether the ball is in flight or not
                                           %   1 = is in flight, so animator plots trajectory
                                           %   0 = not in flight, so animator plots ball on beam
sBB_Flight_isFlight = 0;
                                           
global sBB_Flight_init_pos_r         % initial r of flight
global sBB_Flight_init_pos_theta     % initial velocity of flight
global sBB_Flight_init_vel_r     % initial angle of flight
global sBB_Flight_init_vel_theta % initial angular velocity of flight 
global sBB_Flight_init_time      % initial time of flight

% global variable for terminating simulation if ball rolls off beam
global sBB_Terminate 
sBB_Terminate= 0;

return;
% end mdlInitializeSizes


%
%=============================================================================
% mdlUpdate
% Update the 'ball and beam' animation.
%=============================================================================
%
function sys=mdlUpdate(t,x,u,Lbeam,Rball,pauseTime,Ibeam,Mball,Km)

% global variables for trajectory control
global sBB_Flight_isFlight      % variable that determines whether the ball is in flight or not
                                           %   1 = is in flight, so animator plots trajectory
                                           %   0 = not in flight, so animator plots ball on beam                                      

global sBB_Flight_init_pos_r         % initial r of flight
global sBB_Flight_init_pos_theta     % initial velocity of flight
global sBB_Flight_init_vel_r     % initial angle of flight
global sBB_Flight_init_vel_theta % initial angular velocity of flight 
global sBB_Flight_init_time      % initial time of flight

% values of current integration step
r = u(2);
theta = u(3);
r_dot = u(4);
theta_dot = u(5);
u_torque = u(6);
g = 9.78;

% values of previous integration step
r_ = sBB_Flight_init_pos_r;
theta_ = sBB_Flight_init_pos_theta;
r_dot_ = sBB_Flight_init_vel_r;
theta_dot_ = sBB_Flight_init_vel_theta;

% check if ball is starting flight
if sBB_Flight_isFlight == 0
%     cos(theta)*r*(u_torque-r*Mball*g*cos(theta))/Ibeam 
%     if cos(theta)*r*(u_torque-r*Mball*g*cos(theta))/Ibeam <-(g+5)
    cos(theta)*r*(Km*u_torque-Mball*g*r*cos(theta))/Ibeam;
%     % ball is catapulted
%     if cos(theta)*r*(Km*u_torque-Mball*g*r*cos(theta))/Ibeam < -10*g & false
% 
%         disp('Catapult!');
%         sBB_Flight_isFlight = 1;     % enable flight flag
% 
%         % compute initial flight data
%         sBB_Flight_init_pos_r = r
%         sBB_Flight_init_pos_theta = theta
%         sBB_Flight_init_vel_theta = theta + pi/2
%         sBB_Flight_init_vel_r = r*theta_dot
%         sBB_Flight_init_time = t;
%         
%     else
if abs(r) > Lbeam/2     % ball rolls off end of beam

        disp('Rolling off...');
        sBB_Flight_isFlight = 1;

        % compute initial trajectory data
        sBB_Flight_init_pos_r = r;
        sBB_Flight_init_pos_theta = theta;
        sBB_Flight_init_vel_r = r_dot_;
        sBB_Flight_init_vel_theta = theta;
        sBB_Flight_init_time = t;
    end
end

fig = get_param(gcbh,'UserData');
if ishandle(fig),
  if strcmp(get(fig,'Visible'),'on'),
    ud = get(fig,'UserData');

    if sBB_Flight_isFlight == 1
        % draw ball's flight trajectory
        LocalTrajectorySets(t,ud,u,Lbeam,Rball);
    else
        % store data of current integration step
		sBB_Flight_init_pos_r = r;
		sBB_Flight_init_pos_theta = theta;
		sBB_Flight_init_vel_r = r_dot;
		sBB_Flight_init_vel_theta = theta_dot;
        
		sBB_Flight_init_time = t;

        % draw beam
        LocalBeamSets(t,ud,u,Lbeam,Rball);
    end
    
    % Force plot to be drawn and wait for 'pauseTime' in between frames
	if isempty(pauseTime)
        pause
	else
        pause(pauseTime);
	end
	drawnow
    
  end
end;
 
sys = [];

return;
% end mdlUpdate


%
%=============================================================================
% mdlOutputs
% Output from the 'ball and beam' animation to a 'STOP' simulink block
%=============================================================================
%
function sys=mdlOutputs

global sBB_Terminate

% output 1 will terminate simulation, 0 will continue
sys = sBB_Terminate;     


%
%=============================================================================
% LocalDeleteBlock
% The animation block is being deleted, delete the associated figure.
%=============================================================================
%
function LocalDeleteBlock

fig = get_param(gcbh,'UserData');
if ishandle(fig),
  delete(fig);
  set_param(gcbh,'UserData',-1)
end

% end LocalDeleteBlock

%
%=============================================================================
% LocalDeleteFigure
% The animation figure is being deleted, set the S-function UserData to -1.
%=============================================================================
%
function LocalDeleteFigure

ud = get(gcbf,'UserData');
set_param(ud.Block,'UserData',-1);
  
% end LocalDeleteFigure

%
%=============================================================================
% LocalSlider
% The callback function for the animation window slider uicontrol.  Change
% the reference block's value.
%=============================================================================
%
function LocalSlider

ud = get(gcbf,'UserData');
set_param(ud.RefBlock,'Value',num2str(get(gcbo,'Value')));

% end LocalSlider

%
%=============================================================================
% LocalClose
% The callback function for the animation window close button.  Delete
% the animation figure window.
%=============================================================================
%
function LocalClose

delete(gcbf)

% end LocalClose

%
%=============================================================================
% LocalPlayback
% The callback function for the animation window playback button.  Playback
% the animation.
%=============================================================================
%
function LocalPlayback

%
% first find the animation data in the base workspace, issue an error
% if the information isn't there
%
t = evalin('base','t','[]');
y = evalin('base','y','[]');

if isempty(t) | isempty(y),
  errordlg(...
    ['You must first run the simulation before '...
     'playing back the animation.'],...
    'Animation Playback Error');
end

%
% playback the animation, note that the playback is wrapped in a try-catch
% because is it is possible for the figure and it's children to be deleted
% during the drawnow in LocalPendSets
%
try
  ud = get(gcbf,'UserData');
  for i=1:length(t),
    LocalBeamSets(t(i),ud,y(i,:));
  end
end

% end LocalPlayback

%
%=============================================================================
% LocalBeamSets
% Local function to set the position of the graphics objects in the
% 'ball and beam' animation window.
%=============================================================================
%
function LocalBeamSets(time,ud,u,Lbeam,Rball)

reference = u(1);       % tracking reference
XBall = u(2);           % current position of ball
Theta = u(3);           % angle of beam

BeamDelta = 0.005*Lbeam;    % thickness of beam
Wmarker = 0.01*Lbeam;       % width of marker

% draw beam
DrawBeam(ud,Theta,Lbeam,BeamDelta);

% draw ball and ball marker
DrawBallandMarker(ud,XBall,Theta,[],Rball,BeamDelta);

% draw reference marker
DrawRefMarker(ud,reference,Wmarker);

%%%%% update time/reference signal strings
set(ud.TimeField,'String',num2str(time));
set(ud.ReferenceField,'String',num2str(reference));
set(ud.PositionField,'String',num2str(XBall));

% end LocalBeamSets

%
%=============================================================================
% LocalTrajectorySets
% Local function to set the position of the graphics objects in the
% 'ball and beam' animation window.
%=============================================================================
%
function LocalTrajectorySets(time,ud,u,Lbeam,Rball)

% trajectory control variable
global sBB_Flight_isFlight      % variable that determines whether the ball is in flight or not
                                           %   1 = is in flight, so animator plots trajectory
                                           %   0 = no in flight, so animator plots ball on beam

global sBB_Flight_init_pos_r         % initial r of flight
global sBB_Flight_init_pos_theta     % initial velocity of flight
global sBB_Flight_init_vel_r     % initial angle of flight
global sBB_Flight_init_vel_theta % initial angular velocity of flight 
global sBB_Flight_init_pos_theta_dot      % initial time of flight
global sBB_Flight_init_time      % initial time of flight

BDelta = 0.005*Lbeam;

cos_pT = cos(sBB_Flight_init_pos_theta);
sin_pT = sin(sBB_Flight_init_pos_theta);

cos_vT = cos(sBB_Flight_init_vel_theta);
sin_vT = sin(sBB_Flight_init_vel_theta);

% compute the initial (x,y) position, velocity, and acceleration
x = sBB_Flight_init_pos_r * cos_pT  - (BDelta+Rball)*sin_pT;
x_vi = sBB_Flight_init_vel_r * cos_vT;
x_a = 0;

y = sBB_Flight_init_pos_r * sin_pT + (BDelta+Rball)*cos_pT;
y_vi = sBB_Flight_init_vel_r * sin_vT;
y_a = -9.78;   % gravitational acceleration

% time of flight
flightTime = time - sBB_Flight_init_time;

%%%%% draw ball
BRadius = Rball;    % radius of ball

% compute the (x,y) position of trajectory
x_ballCenter = x + x_vi*flightTime;
y_ballCenter = y + y_vi*flightTime + 0.5*y_a*flightTime^2;

% compute the ball rotation
temp = sign(sBB_Flight_init_pos_r)*Lbeam/2/(2*pi*BRadius);
ballRotation_start = -(temp-floor(temp))*2*pi;
ballAngVel = -sBB_Flight_init_vel_r/BRadius;

ballRotation = ballRotation_start + ballAngVel*flightTime;
% XBall = sign(sBB_Flight_init_pos_r)*sqrt(x_ballCenter^2 + y_ballCenter^2)
XBall = sqrt(x_ballCenter^2 + y_ballCenter^2);
ThetaBall = atan2(y_ballCenter,x_ballCenter);

% draw ball and ball marker
% NOTE: parameter BDelta = -BRadius so that (x,y) is correct
DrawBallandMarker(ud,XBall,ThetaBall,ballRotation,BRadius,-BRadius);

% draw beam
ThetaBeam = u(3);
DrawBeam(ud,ThetaBeam,Lbeam,BDelta);

% determine if ball is off the figure, if so, then terminate the simulation

% max (X,Y) coordinates of figure
maxX = Lbeam/2+0.3*Lbeam/2;
minX = -maxX;
maxY = maxX*3/4;
minY = -maxX/4;
global sBB_Terminate
if x_ballCenter<minX | x_ballCenter>maxX | y_ballCenter<minY | y_ballCenter>maxY
    sBB_Terminate = 1;
end

% ball is rolling off the beam; hence controller has failed
set(ud.MessageField,'String','Controller failed...');

return;
% end of LocalTrajectorySets



%
%=============================================================================
% LocalBeamInit
% Local function to initialize the 'ball and beam' animation.  If the animation
% window already exists, it is brought to the front.  Otherwise, a new
% figure window is created.
%=============================================================================
%
function LocalBeamInit(RefBlock,Lbeam,Rball,init)

%
% The name of the reference is derived from the name of the
% subsystem block that owns the 'ball and beam' animation S-function block.
% This subsystem is the current system and is assumed to be the same
% layer at which the reference block resides.
%
sys = get_param(gcs,'Parent');
sys = get_param(sys, 'Parent');

TimeClock = 0;
RefSignal = str2num(get_param([sys '/' RefBlock],'Value'));

XBall = init(1);
Theta = init(2);

%%%%% draw beam
BeamDelta = 0.005*Lbeam;    % thickness of beam
Wmarker = 0.01*Lbeam;       % thickness of reference marker

% draw beam
[xDataBeam,yDataBeam]=DrawBeam([],Theta,Lbeam,BeamDelta);

% draw ball and ball marker
[xDataBall,yDataBall,xDataBallMarker,yDataBallMarker] = DrawBallandMarker([],XBall,Theta,[],Rball,BeamDelta);

% draw reference marker
[xDataRefMarker,yDataRefMarker]=DrawRefMarker([],RefSignal,Wmarker);


%
% The animation figure handle is stored in the 'ball and beam' block's UserData.
% If it exists, initialize the reference mark, time, beam, and ball
% positions/strings/etc.
%
Fig = get_param(gcbh,'UserData');
if ishandle(Fig),
  FigUD = get(Fig,'UserData');
  

  set(FigUD.TimeField,...
      'String',num2str(TimeClock));
  set(FigUD.ReferenceField,...
      'String',num2str(RefSignal));  
  set(FigUD.MessageField,...
      'String','Tracking...');

  % draw graphic objects
  set(FigUD.RefMark,'XData',xDataRefMarker,'YData',yDataRefMarker);
  set(FigUD.Beam,'XData',xDataBeam,'YData',yDataBeam);
  set(FigUD.Ball,'XData',xDataBall,'YData',yDataBall);
  set(FigUD.BallMarker,'XData',xDataBallMarker,'YData',yDataBallMarker);
  
 
  %
  % 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 = 'ECE410F - Lab 3: Ball and Beam Experiment';
Fig = figure(...
  'Units',           'pixel',...
  'Position',        [100 100 700 400],...
  'Name',            FigureName,...
  'NumberTitle',     'off',...
  'IntegerHandle',   'off',...
  'HandleVisibility','callback',...
  'Resize',          'on',...
  'DeleteFcn',       'pendan([],[],[],''DeleteFigure'')',...
  'CloseRequestFcn', 'pendan([],[],[],''Close'');');

% double buffer to eliminate flickering
set(Fig,'Renderer','painter');      
set(Fig,'DoubleBuffer','on'); 

AxesH = axes(...
  'Parent',  Fig,...
  'Units',   'pixel',...
  'Position',[50 50 600 300],...
  'CLim',    [1 64], ...
  'Xlim',    [-(Lbeam/2+0.3*Lbeam/2) (Lbeam/2+0.3*Lbeam/2)],...
  'Ylim',    [-(Lbeam/2+0.3*Lbeam/2)/4 (Lbeam/2+0.3*Lbeam/2)*3/4],...
  'Visible', 'off');
Beam = surface(...
  'Parent',   AxesH,...
  'XData',    xDataBeam,...
  'YData',    yDataBeam,...
  'ZData',    zeros(2),...
  'CData',    ones(2),...
  'EraseMode','normal');
Ball = patch(...
  'Parent',   AxesH,...
  'XData',    xDataBall,...
  'YData',    yDataBall,...
  'CData',    10000,...
  'EraseMode','normal');
BallMarker = line(...
  'Parent',   AxesH,...
  'XData',    xDataBallMarker,...
  'YData',    yDataBallMarker,...
  'EraseMode','xor');
RefMark = patch(...
  'Parent',   AxesH,...
  'XData',    xDataRefMarker,...
  'YData',    yDataRefMarker,...
  'CData',    22);
Pivot = patch(...
  'Parent',   AxesH,...
  'XData',    [-Lbeam/20 0 Lbeam/20],...
  'YData',    [-Lbeam/10 0 -Lbeam/10],...
  'CData',    22);
uicontrol(...       % gray patch
  'Parent',  Fig,...
  'Style',   'text',...
  'Units',   'pixel',...
  'Position',[0 0 700 50]);
uicontrol(...
  'Parent',             Fig,...
  'Style',              'text',...
  'Units',              'pixel',...
  'Position',           [50 0 100 25], ...
  'HorizontalAlignment','right',...
  'String',             'Time: ');
TimeField = uicontrol(...
  'Parent',             Fig,...
  'Style',              'text',...
  'Units',              'pixel', ...
  'Position',           [150 0 100 25],...
  'HorizontalAlignment','left',...
  'String',             num2str(TimeClock));
uicontrol(...
  'Parent',             Fig,...
  'Style',              'text',...
  'Units',              'pixel',...
  'Position',           [200 0 100 25], ...
  'HorizontalAlignment','right',...
  'String',             'Reference: ');
ReferenceField = uicontrol(...
  'Parent',             Fig,...
  'Style',              'text',...
  'Units',              'pixel', ...
  'Position',           [300 0 100 25],...
  'HorizontalAlignment','left',...
  'String',             num2str(RefSignal));
uicontrol(...
  'Parent',             Fig,...
  'Style',              'text',...
  'Units',              'pixel',...
  'Position',           [350 0 100 25], ...
  'HorizontalAlignment','right',...
  'String',             'Current Position: ');
PositionField = uicontrol(...
  'Parent',             Fig,...
  'Style',              'text',...
  'Units',              'pixel', ...
  'Position',           [450 0 100 25],...
  'HorizontalAlignment','left',...
  'String',             num2str(XBall));
MessageField = uicontrol(...
  'Parent',             Fig,...
  'Style',              'text',...
  'Units',              'pixel', ...
  'BackgroundColor',    [0.8 0.8 0.8],...
  'Position',           [5 375 200 20],...
  'HorizontalAlignment','left',...
  'String',             'Tracking...');
SlideControl = uicontrol(...
  'Parent',   Fig,...
  'Style',    'slider',...
  'Units',    'pixel', ...
  'Position', [100 25 500 22],...
  'Min',      -Lbeam/2,...
  'Max',      Lbeam/2,...
  'Value',    RefSignal,...
  'Callback', 'pendan([],[],[],''Slider'');');
uicontrol(...
  'Parent',  Fig,...
  'Style',   'pushbutton',...
  'Position',[610 15 70 20],...
  'String',  'Close', ...
  'Callback','pendan([],[],[],''Close'');');
uicontrol(...
  'Parent',  Fig,...
  'Style',   'pushbutton',...
  'Position',[15 15 70 20],...
  'String',  'Playback', ...
  'Callback','pendan([],[],[],''Playback'');',...
  'Interruptible','off',...
  'BusyAction','cancel');
set(RefMark,'EraseMode','normal');

%
% all the HG objects are created, store them into the Figure's UserData
%
FigUD.Beam         = Beam;  
FigUD.Ball         = Ball;
FigUD.BallMarker   = BallMarker;
FigUD.Pivot        = Pivot;

FigUD.TimeField    = TimeField;
FigUD.PositionField = PositionField;
FigUD.ReferenceField    = ReferenceField;
FigUD.MessageField = MessageField;
FigUD.SlideControl = SlideControl;
FigUD.RefMark      = RefMark;
FigUD.Block        = get_param(gcbh,'Handle');
FigUD.RefBlock     = get_param([sys '/' RefBlock],'Handle');

set(Fig,'UserData',FigUD);

drawnow

%
% store the figure handle in the animation block's UserData
%
set_param(gcbh,'UserData',Fig);

% end LocalBeamInit


%
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
function sys=mdlTerminate(t,x,u)

sys = [];

% delete global trajectory control variable
clear global sBB_Flight_isFlight      % variable that determines whether the ball is in flight or not
                                                 %   1 = is in flight, so animator plots trajectory
                                                 %   0 = no in flight, so animator plots ball on beam

clear global sBB_Flight_init_pos_r         % initial r of flight
clear global sBB_Flight_init_pos_theta     % initial velocity of flight
clear global sBB_Flight_init_vel_r     % initial angle of flight
clear global sBB_Flight_init_vel_theta % initial angular velocity of flight 
clear global sBB_Flight_init_time      % initial time of flight

clear global sBB_Terminate
% end mdlTerminate


%
%=============================================================================
% Draw Beam
%=============================================================================
%
function [xData, yData]=DrawBeam(ud,theta,BeamLen,BDelta)

% % % Theta = u(3);       % angle of beam

cosT = cos(theta);  
sinT = sin(theta);

%%%%% draw beam
BDelta = 0.005*BeamLen;       % thickness of beam

% define points of beam
x_beamLeft = -BeamLen/2*cosT;
y_beamLeft = -BeamLen/2*sinT;
x_beamRight = BeamLen/2*cosT;
y_beamRight = BeamLen/2*sinT;
BDcosT = BDelta*cosT;
BDsinT = BDelta*sinT;

xData = [x_beamLeft-BDsinT x_beamRight-BDsinT; x_beamLeft+BDsinT x_beamRight+BDsinT];
yData = [y_beamLeft+BDcosT y_beamRight+BDcosT; y_beamLeft-BDcosT y_beamRight-BDcosT];

if ~isempty(ud)
	set(ud.Beam,'XData',xData,'YData',yData);
end

return;
% end of Draw Beam


%
%=============================================================================
% Draw Ball and Marker
%=============================================================================
%
function [xDataBall, yDataBall, xDataBallMarker, yDataBallMarker]=...
                 DrawBallandMarker(ud,XBall,Theta,ballRotation,BRadius,BDelta)

%%%%% draw ball

cosT = cos(Theta);
sinT = sin(Theta);

% define points of ball
x_ballCenter = XBall*cosT - (BRadius+BDelta)*sinT;
y_ballCenter = XBall*sinT + (BRadius+BDelta)*cosT;
pts=linspace(0,2*pi,100);
xDataBall = BRadius*cos(pts) + x_ballCenter;
yDataBall = BRadius*sin(pts) + y_ballCenter;

% compute the angle of rotation of the ball
if isempty(ballRotation)
	temp = XBall/(2*pi*BRadius);
	ballRotation = -(temp-floor(temp))*2*pi;
end

xDataBallMarker = [x_ballCenter BRadius*cos(ballRotation)+x_ballCenter];
yDataBallMarker = [y_ballCenter BRadius*sin(ballRotation)+y_ballCenter];

if ~isempty(ud)
	set(ud.Ball,'XData',xDataBall,'YData',yDataBall);     % update ball 
	set(ud.BallMarker,'XData',xDataBallMarker,'YData',yDataBallMarker); % update marker
end

return;
% end of Draw Ball and Marker


%=============================================================================
% Draw Reference Marker
%=============================================================================
%
function [xData, yData]=DrawRefMarker(ud,ref,Wmarker)

% define points of ref markers
Lbeam = Wmarker/0.01;           % length of beam
y = (Lbeam/2+0.3*Lbeam/2)*0.7;
Hmarker = 7.5*Wmarker;
xData = ref + [-Wmarker 0 Wmarker];
yData = [ y y-Hmarker y ];

if ~isempty(ud)
	set(ud.RefMark,'XData',xData,'YData',yData);
end

return;
% end of DrawRefMarker