samer@2: % rsched - regular state threading scheduler
samer@0: %
samer@0: % rsched ::
samer@2: %    timed_action({nonneg,S},{S}) ~'timed action from current period and state to state',
samer@0: %    S      ~'first state',
samer@2: %    nonneg ~'timer period',
samer@2: %    time   ~'start time',
samer@0: %    options {
samer@0: %       its     :: natural/inf ~'iteration limit';
samer@0: %       defer   :: bool/0 ~ 'if true, do not start timer';
samer@0: %       error   :: real/0 ~ 'estimate of timing error on timer start';
samer@0: %       final   :: bool/0 ~ 'make final state available?';
samer@0: %
samer@0: %       onstart :: (
samer@0: %             S       ~'current state', 
samer@0: %             real    ~'scheduled start time'
samer@0: %             datenum ~'actual time'
samer@2: %          => void)/@nop ~ 'called on timer start';
samer@0: %
samer@0: %       onstop  :: (
samer@0: %             S       ~'current state',
samer@2: %             datenum ~'actual time' %          => void)/@nop ~ 'called on timer stop';
samer@0: %
samer@0: %       onfinish:: (
samer@0: %             S       ~'final state', 
samer@0: %             datanum ~'actual time'
samer@2: %          => void)/@nop ~ 'called if state seq finishes';
samer@0: %
samer@0: %       busy_mode :: {'queue','drop'};
samer@0: %       exec_mode :: {'fixedRate','fixedDelay','fixedSpacing'}
samer@0: %    }
samer@2: % => struct {
samer@0: %       timer    :: timer ~'timer being used';
samer@2: %       startat  :: (double => void)  ~'start timer at given time';
samer@2: %       start    :: (void   => void)  ~'start timer asap';
samer@2: %       stop     :: (void   => void)  ~'stop timer';
samer@2: %       rewind   :: (void   => void)  ~'rewind to first state';
samer@2: %       trim     :: (double => double)~'trim timing errors';
samer@2: %       dispose  :: (void   => void)  ~'stop timer';
samer@2: %       getstate :: (void   => S)     ~'current state';
samer@2: %       setstate :: (S      => void)  ~'set state (only when stopped)';
samer@2: %       isrunning:: (void   => bool)  ~'true if scheduler is running';
samer@2: %       wait     :: (void   => void)  ~'wait for sched to stop'
samer@0: %    }.
samer@0: %
samer@0: % NB: switching on final state availability option 'final' can make
samer@0: % timer callbacks much slower due to requirement for copying state.
samer@0: 
samer@2: function api=rsched(schedfn,S0,dt,T0,varargin)
samer@0: 
samer@0: 	warning('off','MATLAB:TIMER:STARTDELAYPRECISION');
samer@0: 	warning('off','MATLAB:TIMER:RATEPRECISION');
samer@0: 
samer@0: 	opts=prefs('defer',0,'its',inf,'final',0, ...
samer@0: 			'onstart',@nop,'onstop',@nop,'onfinish',@nop, ...
samer@0: 			'busy_mode','queue','exec_mode','fixedRate',varargin{:});
samer@0: 
samer@0: 	% Implementation note: it's MUCH faster to use a local variable
samer@0: 	% as mutable state instead of using the timer getter and setter.
samer@2: 	State=S0; SchedStart=T0; FinalState=[];
samer@0: 	STARTERR=getparam(opts,'error',0); 
samer@0: 	DT=dt;
samer@0: 
samer@0: 	if opts.final, timfn=@adv; else timfn=@adv2; end
samer@0: 	Timer=timer('ExecutionMode',opts.exec_mode,'BusyMode',opts.busy_mode, ...
samer@0: 			'StartFcn',@startfn,'StopFcn',@stopfn,'TimerFcn',timfn, ...
samer@0: 			'Period',DT,'TasksToExecute',opts.its);
samer@0: 
samer@0: 	api = struct(...
samer@0: 		'dispose',	@()delete(Timer), ...
samer@0: 		'isrunning',@()isrunning(Timer), ...
samer@0: 		'startat',	@startat, ...
samer@0: 		'start',  	@startnow, ...
samer@0: 		'stop',   	@()stop(Timer), ...
samer@0: 		'rewind',	@()setstate(S0), ...
samer@0: 		'getstate',	@getstate, ...
samer@0: 		'setstate',	@setstate, ...
samer@0: 		'trim',   	@trim, ...
samer@0: 		'timer',    @()Timer ...
samer@2: 		'wait',     @sched_wait
samer@0: 	);
samer@0: 	
samer@2: 	if opts.final, api.finalstate=@()FinalState; end
samer@0: 	if ~opts.defer, startat(T0); end
samer@0: 
samer@0: 	function check(msg), if isrunning(Timer), error(msg); end; end
samer@0: 	function err=trim(derr), STARTERR=STARTERR+derr; err=STARTERR; end
samer@2: 	function s=getstate, s=State; end
samer@0: 	function setstate(s), 
samer@0: 		check('Cannot set state of running scheduler'); 
samer@2: 		State=s; 
samer@0: 	end
samer@0: 
samer@0: 	function startat(t0) % !! what if timer is already running?
samer@0: 		check('Timer is already running');
samer@2: 		SchedStart=t0;
samer@0: 		start_delay = t0-nows-STARTERR
samer@0: 		if start_delay<0
samer@0: 			fprintf('\n| WARNING: start delay=%f, starting now.',start_delay);
samer@0: 			start_delay=0;
samer@0: 		end
samer@0: 		set(Timer,'StartDelay',start_delay);
samer@0: 		start(Timer);
samer@0: 	end
samer@0: 
samer@0: 	function startnow % !! what if timer is already running?
samer@0: 		check('Timer is already running');
samer@2: 		SchedStart=nows+STARTERR;
samer@0: 		set(Timer,'StartDelay',0);
samer@0: 		start(Timer);
samer@0: 	end
samer@0: 
samer@0: 	function stopfn(o,e), 
samer@2: 		opts.onstop(State,e.Data.time); 
samer@2: 		if isempty(State), 
samer@2: 			opts.onfinish(State,e.Data.time); 
samer@0: 		end
samer@0: 	end
samer@0: 
samer@0: 	function startfn(o,e)
samer@0: 		DT=get(o,'Period'); % use current period in timer callback
samer@2: 		opts.onstart(State,SchedStart,e.Data.time); 
samer@0: 	end
samer@0: 
samer@0: 	% if final state not required we can use faster assign in place
samer@0: 	function adv2(o,e)
samer@2: 		[err,State]=schedfn(SchedStart,DT,State);
samer@2: 		if isempty(State), stop(o); end
samer@2: 		SchedStart=SchedStart+DT;
samer@0: 	end
samer@0: 
samer@0: 	% final state required
samer@0: 	function adv(o,e)
samer@2: 		[err,s1]=schedfn(SchedStart,DT,State);
samer@2: 		if isempty(s1), FinalState=State; stop(o); end
samer@2: 		SchedStart=SchedStart+DT;
samer@2: 		State=s1;
samer@2: 	end
samer@0: 
samer@2: 	function sched_wait
samer@2: 		while Timer.isrunning(); pause(0.05); end
samer@0: 	end