/*
          file: qtask.c
       version: 1.02
   description: Queued Task Manager/Scheduler
           (C): (C) michael bradley

      Note: this driver currently is using timer1, so don't use it!
            you can change, and modify if you wish

   Changelog:
               12/12/2009  mqBeginSlice() removed, no longer needed
               12/07/2009  ISR is only called when needed vs ever x uSeconds
                           Code added to aid in creating state machines, called slices
               12/05/2009  Overall, faster code, smaller footprint
                           now working in ticks, instead of uSec
                           defined pri levels
                           changed flags internally
                           changed counter to count down
*/

#define QTASK_DEBUG_PIN       PIN_C0 // i use this for monitoring timing

#DEFINE QTPRI_HIGHEST      0
#DEFINE QTPRI_HIGH         1
#DEFINE QTPRI_NORMAL       2
#DEFINE QTPRI_LOW          3
#DEFINE QTPRI_LOWEST       4

#define QTFLAG_RUNNING     0
#define QTFLAG_ENABLED     1
#define QTFLAG_QUED        2

#define QTFLAG_MANAGETASK  0b00000010  // basically enabled bit only

// a few macros
#define mqTaskSetPri(id,n)    qTasks[id].priority = n;
#define mqTaskSetTmr(id,t)    qTasks[id].threshold = t; qTasks[id].counter = t;
#define mqTaskEnable(id)      bit_set(qTasks[id].flags,QTFLAG_ENABLED);
#define mqTaskDisable(id)     bit_clear(qTasks[id].flags,QTFLAG_ENABLED);
#define mqTSR(id)             bit_set(qTasks[id].flags,QTFLAG_RUNNING);
#define mqTCR(id)             bit_clear(qTasks[id].flags,QTFLAG_RUNNING);
#define mqTaskManager()       switch (qTaskGet())
#define mqTaskFunction(id,fn) case id: { mqTSR(id); fn; mqTCR(id); break; }

#define mqTaskSchedulerEnable()  enable_interrupts(INT_TIMER1);
#define mqTaskSchedulerDisable() disable_interrupts(INT_TIMER1);
#define mqTaskTimeOut()          set_timer1(65535-(tocks*10));
#define mqTaskRest()             set_timer1(0);
#define mqTaskTimerCur()         get_timer1();

#define qDefinePie int8
#define mqSliceManager(slice)    switch (slice)
#define mqOrderSlice(sid,fn)     case sid: { fn; break; }
#define mqPieIni(slice)          slice = 1;
#define mqLastSlice(slice,num)   slice++; if (slice > num) { slice = 1; }

// total tasks
#ifndef TASKS_MAX             // define this on your own if you wish
   #define TASKS_MAX 16       // max number of tasks
   #endif

#if TASKS_MAX > 32
   #error Too many tasks, must be <= 32
#endif

unsigned int8 qTaskFiFo[TASKS_MAX];
unsigned int8 qTaskPointer;
unsigned int16 qTaskTocDelta;


struct stTaskItem
   {
   int8 flags;
   int8 priority;
   unsigned int16 threshold;
   unsigned int16 counter;
   } qTasks[TASKS_MAX];


unsigned int8 qTaskGet(void)
{
   unsigned int8 i,res;

   res = 0;

   if (qTaskPointer != 0)
     {
     res = qTaskFiFo[0];
     for (i=0; i<qTaskPointer; i++)
       {
       qTaskFiFo[i] = qTaskFiFo[i+1];
       }
     qTaskPointer--;
     bit_clear(qTasks[res].flags,QTFLAG_QUED);
     }

   return res;
}

void qTaskQue(unsigned int8 id)
{
   int i,pa,pb;
   unsigned int8 tid;
   bit_set(qTasks[id].flags,QTFLAG_QUED);
   qTasks[id].counter = qTasks[id].threshold;

   qTaskFiFo[qTaskPointer] = id;

   for (i=qTaskPointer; i>1; i--)
     {
     if ( qTasks[qTaskFiFo[i]].priority < qTasks[qTaskFiFo[i-1]].priority )
       {
       tid = qTaskFiFo[i-1];
       qTaskFiFo[i-1] = qTaskFiFo[i];
       qTaskFiFo[i] = tid;
       }
     }

   qTaskPointer++;
   //if (qTaskPointer == TASKS_MAX) { qTaskPointer--; }
}

void qTaskInitialize()
{
   unsigned int8 i;
   unsigned int16 tocks;

   // used to set qtask ticking
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);
   tocks = QTASK_TOCK;
   qTaskTocDelta = 1;
   mqTaskTimeOut();

   qTaskPointer = 0;
   for (i=0; i<TASKS_MAX; i++)
     {
     qTaskFiFo[i] = 0;
     qTasks[i].flags = 0;
     qTasks[i].priority = QTPRI_NORMAL;
     }
}

// we use the timer 1 interrupt for scheduling
// this should be high, not 1us!!!
#int_TIMER1
void  isr_scheduler(void)
{
   unsigned int8 i;
   unsigned int16 tocks;

   //mqTaskRest();  // set timer to 0 so we can time how long we took in this isr
   tocks = qTaskTocDelta;
   #ifdef QTASK_DEBUG_PIN
      output_high(QTASK_DEBUG_PIN);
      #endif
   for (i=1; i<TASKS_MAX; i++)
     {
     // manage task if its enabled only, not running, not queued
     if ( qTasks[i].flags == QTFLAG_MANAGETASK )
       {
       qTasks[i].counter = qTasks[i].counter - tocks;
       if ( qTasks[i].counter == 0 )
         {
         qTaskQue(i);
         }
       else
         {
         if ( qTasks[i].counter < qTaskTocDelta ) { qTaskTocDelta = qTasks[i].counter; }
         }
       }
     }

   // adjust for tock resolution, and reduce by time spend in this isr
   // (at least try to reduce some, hey, were not an RTOS!)
   //tocks = (qTaskTocDelta * QTASK_TOCK) - mqTaskTimerCur();
   tocks = (qTaskTocDelta * QTASK_TOCK);

   #ifdef QTASK_DEBUG_PIN
      output_low(QTASK_DEBUG_PIN);
      #endif
   mqTaskTimeOut();     // set the next isr entry time
}