#define	schedVer		1
#define schedRev		0

/****************************************************************************
*
*   Copyright (c) 2008 Matt Bateman
*   Robotics4Fun http://www.laughingsky.com/hobbies/robotics/
*   (please link back if you use this code!)
*
*   This program is free software; you can redistribute it and/or modify
*   it under the terms of the GNU General Public License version 2 as
*   published by the Free Software Foundation.
*
*   Alternatively, this software may be distributed under the terms of BSD
*   license.
*
****************************************************************************/

/****************************************************************************
*   Version Notes
*	
*   1.0   10/28/2008  Initial release
****************************************************************************/

//#define schedDebug 			1				// Turn on debug output
#define schedShowInitStatus	1				// Display init status and version info


typedef void (*PFV)(void);					// Pointer to void function

#define maxSched		16					// Max number of scheduled functions

struct {									// Structure for scheduled function info
	uint16_t			period;					// Period between runs in mS
	uint16_t			last;					// Time stamp of last run
	PFV					proc;					// Pointer to function
	uint16_t			max;					// Max time function took to run in mS
} sched[maxSched];

#ifndef getNow()
	// *******************************************************
	// Timer functions using timer2. By default on the Axon,
	// the overflow of timer2 is ~1ms. Be sure to init timer2
	// somewhere prior to calling any of the avrCam functions.
	//
	// timer2Init();
	//
	// These defines are in a misc module, but are included
	// here in case that module is not loaded. These are used
	// to handle timeouts when communicating with the camera.
	// *******************************************************
	#define getNow()				timer2GetOverflowCount()
	#define getTimeDiff(now,last)	((last>now)?(65535-last+now):(now-last))
#endif

void schedDel (PFV proc) {
	// *******************************************************
	// Delete function from schedule list
	// *******************************************************
	for (register uint8_t i=0;i<maxSched;i++) {	// Loop though schedule slots
		if (sched[i].proc==proc) {					// Found it
			sched[i].period=0;							// Delete it
			sched[i].last=0;
			sched[i].proc=NULL;
			return;
		}
	}
}

void schedAdd (PFV proc, unsigned int period) {
	// ********************************************************
	// Add a function to the scheduler. Period is the min time
	// between runs in mS. Due to length of other functions,
	// it could be longer between runs.
	// ********************************************************
	schedDel(proc);									// If it is already there remove the old one
	if (period) {
		for (register uint8_t i=0;i<maxSched;i++) {	// Loop through slots
			if (sched[i].period==0) {					// Found empty slot
				sched[i].period=period;						// Set the period
				sched[i].last=0;							// Clear last run
				sched[i].proc=proc;							// Save function pointer
#ifdef schedDebug
				rprintf("proc %d:",i);
				rprintf("(%d)\t",period);
#endif
				return;
			}
		}
#ifdef schedDebug
		rprintf("ERROR:	Too many scheduled processes\r\n");
#endif
	}
}



void schedRun (void) {
	// ***************************************************************
	// Check all the scheduled functions and run any that are due
	// This should be called as often as possible from you main loop
	// ***************************************************************
	uint16_t	now,diff;
#ifdef schedDebug
	uint32_t	tmpPeriod;
#endif
	for (register uint8_t i=0;i<maxSched;i++) {		// Loop through slots
		if (sched[i].period) {							// Don't run if period is 0
			now=getNow();								// Get currrent time
			diff=getTimeDiff(now,sched[i].last);		// Calculate how long since last run
			if (diff>=sched[i].period) {				// Needs to run
#ifdef schedDebug											// Calculate if time is over more than 40% too long
															// since last run. If so and debugging is turned on,
															// print debug message with time info.
				tmpPeriod=sched[i].period;		
				tmpPeriod*=140;
				tmpPeriod/=100;								// Starvation is 40% over requested
				if (diff>tmpPeriod) {						// Check for starvation
					rprintf("ERROR:	Starvation on process %d ",i);
					rprintf("(%d):\t",sched[i].period);
					rprintf("Time = %d\r\n",diff);
				}
#endif
				sched[i].last=now;							// Save current run timestamp
				sched[i].proc();							// Run the function
				diff=getTimeDiff(getNow(),sched[i].last);	// Calculate how long it took to run
				sched[i].max=(sched[i].max<diff)?diff:sched[i].max;	// Store in max if longer than previous max
			}
		}
	}
}

void schedPrintTimes (void) {
	// ****************************************************
	// Print out max times for each scheduled function.
	// This can be useful for debugging starvation issues.
	// It only references slot numbers, so you will need
	// to know the order that you scheduled them.
	// ****************************************************
	rprintf("\r\n");
	for (register uint8_t i=0;i<maxSched;i++) {		// Loop through slots
		if (sched[i].period) {							// Slot in use
			rprintf("Proc (%d):\t",i);					// Print slot number
			rprintf("%d mS\r\n",sched[i].max);			// Print max time
		}
	}
}

void schedInit(void) {
	// ********************************
	// Initialize the scheduler module
	// ********************************
#ifdef schedShowInitStatus
	rprintf("Scheduler:\t%d.%d\t",schedVer,schedRev);
#endif
	for (register uint8_t i=0;i<maxSched;i++) {		// Clear all slots
		sched[i].period=0;
		sched[i].last=0;
		sched[i].proc=NULL;
	}
#ifdef schedShowInitStatus
	rprintf("done\r\n");
#endif
}