% rsched - regular state threading scheduler
%
% rsched ::
%    timed_action({nonneg,S},{S}) ~'timed action from current period and state to state',
%    S      ~'first state',
%    nonneg ~'timer period',
%    time   ~'start time',
%    options {
%       its     :: natural/inf ~'iteration limit';
%       defer   :: bool/0 ~ 'if true, do not start timer';
%       error   :: real/0 ~ 'estimate of timing error on timer start';
%       final   :: bool/0 ~ 'make final state available?';
%
%       onstart :: (
%             S       ~'current state', 
%             real    ~'scheduled start time',
%             datenum ~'actual start date-stamp'
%          => void)/@nop ~ 'called on timer start';
%
%       onstop  :: (
%             S       ~'current state',
%             datenum ~'actual start date-stamp'
%          => void)/@nop ~ 'called on timer stop';
%
%       onfinish:: (
%             datenum ~'actual start date-stamp'
%          => void)/@nop ~ 'called if state seq finishes';
%
%       busy_mode :: {'queue','drop'};
%       exec_mode :: {'fixedRate','fixedDelay','fixedSpacing'}
%    }
% => struct {
%       timer    :: timer ~'timer being used';
%       startat  :: (double => void)  ~'start timer at given time';
%       start    :: (void   => void)  ~'start timer asap';
%       stop     :: (void   => void)  ~'stop timer';
%       rewind   :: (void   => void)  ~'rewind to first state';
%       trim     :: (double => double)~'trim timing errors';
%       dispose  :: (void   => void)  ~'stop timer';
%       getstate :: (void   => S)     ~'current state';
%       setstate :: (S      => void)  ~'set state (only when stopped)';
%       isrunning:: (void   => bool)  ~'true if scheduler is running';
%       wait     :: (void   => void)  ~'wait for sched to stop'
%    }.
%
% NB: switching on final state availability option 'final' can make
% timer callbacks much slower due to requirement for copying state.

function api=rsched(schedfn,S0,dt,T0,varargin)

	warning('off','MATLAB:TIMER:STARTDELAYPRECISION');
	warning('off','MATLAB:TIMER:RATEPRECISION');

	opts=prefs('defer',0,'its',inf,'final',0, ...
			'onstart',@nop,'onstop',@nop,'onfinish',@nop, ...
			'busy_mode','queue','exec_mode','fixedRate',varargin{:});

	% Implementation note: it's MUCH faster to use a local variable
	% as mutable state instead of using the timer getter and setter.
	State=S0; SchedStart=T0; FinalState=[];
	STARTERR=getparam(opts,'error',0); 
	DT=dt;

	if opts.final, timfn=@adv; else timfn=@adv2; end
	Timer=timer('ExecutionMode',opts.exec_mode,'BusyMode',opts.busy_mode, ...
			'StartFcn',@startfn,'StopFcn',@stopfn,'TimerFcn',timfn, ...
			'Period',DT,'TasksToExecute',opts.its);

	api = struct(...
		'dispose',	@()delete(Timer), ...
		'isrunning',@()isrunning(Timer), ...
		'startat',	@startat, ...
		'start',  	@startnow, ...
		'stop',   	@()stop(Timer), ...
		'rewind',	@()setstate(S0), ...
		'getstate',	@getstate, ...
		'setstate',	@setstate, ...
		'trim',   	@trim, ...
		'timer',    @()Timer, ...
		'wait',     @sched_wait ...
	);
	
	if opts.final, api.finalstate=@()FinalState; end
	if ~opts.defer, startat(T0); end

	function check(msg), if isrunning(Timer), error(msg); end; end
	function err=trim(derr), STARTERR=STARTERR+derr; err=STARTERR; end
	function s=getstate, s=State; end
	function setstate(s), 
		check('Cannot set state of running scheduler'); 
		State=s; 
	end

	function startat(t0) % !! what if timer is already running?
		check('Timer is already running');
		SchedStart=t0;
		start_delay = t0-nows-STARTERR
		if start_delay<0
			fprintf('\n| WARNING: start delay=%f, starting now.',start_delay);
			start_delay=0;
		end
		set(Timer,'StartDelay',start_delay);
		start(Timer);
	end

	function startnow % !! what if timer is already running?
		check('Timer is already running');
		SchedStart=nows+STARTERR;
		set(Timer,'StartDelay',0);
		start(Timer);
	end

	function stopfn(o,e), 
		opts.onstop(State,e.Data.time); 
		if isempty(State), opts.onfinish(e.Data.time); end
	end

	function startfn(o,e)
		DT=get(o,'Period'); % use current period in timer callback
		opts.onstart(State,SchedStart,e.Data.time); 
	end

	% if final state not required we can use faster assign in place
	function adv2(o,e)
		[err,State]=schedfn(SchedStart,DT,State);
		if isempty(State), stop(o); end
		SchedStart=SchedStart+DT;
	end

	% final state required
	function adv(o,e)
		[err,s1]=schedfn(SchedStart,DT,State);
		if isempty(s1), FinalState=State; stop(o); end
		SchedStart=SchedStart+DT;
		State=s1;
	end

	function sched_wait
		while Timer.isrunning(); pause(0.05); end
	end
end
