/****************** Task execution scheduler ***********************
 *  for Arduino impl, derived from schip's USBPIC code
 *  converted to run entirely in "user" space
 *   even though installing decrScheduler() in the tick interrupt is better...
 **/

// our local stuff
#include "schip_scheduler.h"

#ifdef SCHEDULERLIB_H // only include all this if using it...

// get SREG and interrupt definitions
#include <avr/interrupt.h>
// get the memset prototype
#include <string.h>

// the list of task items to be executed
Sitem Tasks[USE_NumTasks];
// last tick count that we checked
uint16_t _Ticks;

/** initialize scheduler system
 **/
void initScheduler(void)
{
	uint8_t i;
	// clear all the task structure
	// then init the tick signal value to NADA
	memset( Tasks, 0, sizeof(Tasks) );
	for( i=0; i<USE_NumTasks; ++i )
		Tasks[i].ticks = NOENTRY;

	// save current ticks so decrScheduler() knows what to what
	//  we really don't care about anything but small change
	//  so don't bother keeping upper bytes of millis()
	_Ticks = (millis() & 0xffff);

	return;
}

/** decrement tick settings as needed
 *  this version is not called from timer interrupt
 **/
void decrScheduler(void)
{
	uint8_t i;	// index into Task table
	uint8_t t;	// current low byte of milli-Ticks
	uint8_t l;	// last low byte of milli-Ticks

	// figure out if a tick (or more) has elapsed
	t = (millis() & 0xffff);
	if( t != _Ticks )
	{
		// timer tick(s) have occured
		// save current ticks for next time
		// note: because of the byte sized variables here
		//  we depend on the idea that this gets done at least 
		//  every 255 ticks, or better, every tick...
		l = _Ticks;
		_Ticks = t;
		// get number of ticks that elapsed since last change
		t -= l;

		// subtract elapsed tick count from all Task entries that need it
		for( i=0; i<USE_NumTasks; ++i )
		{
			// shut off interrupts to check ticks value
			// because tasks can be posted from interrupts
			uint8_t oldSREG = SREG;
			cli();

			if( (Tasks[i].ticks != NOENTRY) &&
				(Tasks[i].ticks != RUNENTRY) &&
				(Tasks[i].ticks != 0) )
			{
				// if 't' would cause us to underflow, just set=0
				// otherwise subtract from current value
				if( Tasks[i].ticks <= t )
					Tasks[i].ticks = 0;
				else
					Tasks[i].ticks -= t;
			}

			// re-enable interrupts
			SREG = oldSREG; 
		}
	}

	return;
}

/** run the scheduler,
 *    decrement the Task tick counts when timer says so
 *    do one pass through the task list,
 *    exec anything with a 0 tick count
 *    (make sure you've posted something to do)
 *    runs in user loop() context
 **/
uint8_t _ti;	// current index in task list, for repost_task() use
void scheduler(void)
{
	PFV func;
	uint16_t arg;

	// decrement task tick counts as needed
	decrScheduler();

	// look for stuff to execute
	for( _ti = 0; _ti < USE_NumTasks; ++_ti )
	{
		// if it's time, do the wine
		// doesn't need to shut off interrupts...
		// because only empty entries are affected by posting from interrupt
		if( Tasks[_ti].ticks == 0 )
		{
			//uint8_t oldSREG = SREG;
			//cli();
			Tasks[_ti].ticks = RUNENTRY;
			//SREG = oldSREG;   

			func = Tasks[_ti].function;
			arg = Tasks[_ti].arg;

			// execute this task
			(*func)( arg );
			
			// clear slot if task wasn't reposted
			// I think the same proviso on empty entries holds here too
			//oldSREG = SREG;
			//cli();
			if( Tasks[_ti].ticks == RUNENTRY )
				Tasks[_ti].ticks = NOENTRY;
			//SREG = oldSREG;   
		}
	}

	return;
}

/** Insert task into list
 *  returns index in task list or -1 on fail
 **/
int postTask( uint16_t ticks,	// ticks until exec -- 0==now, -1==empty
			   PFV func,		// pointer to function to execute
			   uint16_t arg )	// function argument
{
	uint8_t i;

	for( i=0; i<USE_NumTasks; ++i )
	{
		// find an empty slot and set it
		// shut off interrupts to check ticks value
		// because tasks can be posted from interrupts
		uint8_t oldSREG = SREG;
		cli();

		if( Tasks[i].ticks == NOENTRY )
		{
			// grabs entry by setting tick count
			Tasks[i].ticks = ticks;

			// re-enable interrupts
			SREG = oldSREG; 

			Tasks[i].function = func;
			Tasks[i].arg = arg;

			return i;
		}

		// re-enable interrupts
		SREG = oldSREG; 
	}

	return -1;
}

/** re-Insert current task into same place in list
 *  should only be called from task executed by scheduler loop
 *  Note1: fails silently and may fubark everything if misused
 **/
void repostTask( uint16_t ticks,	// ticks until exec
				 uint16_t arg )		// new argument value
{
	Tasks[_ti].ticks = ticks;
	Tasks[_ti].arg = arg;
}

#endif  // SCHEDULERLIB_H --  only include all this if using it...