/* This code demonstrates a simple task scheduler for Ard Mega.
* Function pointers for our tasks management, implements a 
* sleep mechanism to control tasks execution frequency.
* We have two LEDs, run at different frequencies, one on 13 and one on 10
*/

// Define our constants
#define LED_INTERNAL 13
#define LED_EXTERNAL 10
#define LEDPORT PORTB // port registers for controlling LED states
#define LEDDDR DDRB // data direction registerfor setting LED pin as output
#define L_I_BIT 1<<7 // Bit mask for LED Bit 7
#define L_E_BIT 1<<4 // Bit mask for LED Bit 4
#define NTASKS 3

// Global variables
void (*tasks [NTASKS])();
int delays[NTASKS] = {0};
int task_active = 0; // index

// We need a sleep function
void sleep(int sleepTime) {
  delays[task_active] = sleepTime;

  // Using a global variable hee allows us to track the state across multiple function calls
}

void setup() {
  LEDDDR |= (L_I_BIT | L_E_BIT);

  // Initialize the tasks
  tasks[0] = taskA;
  tasks[1] = taskB;
  tasks[2] = NULL;
}

void loop() {

  for(task_active = 0; task_active < NTASKS; task_active++) {
    if ((tasks[task_active] != NULL) && (delays[task_active] <= 0)) {
      (*tasks[task_active])(); // execute the task
    }
    else if (tasks[task_active] != NULL) {
      delays[task_active]--; // decrement delay if the task is not ready yet
    }
  }
  delay(1);
}

// Define taskA
void taskA() {
  static int flag = 0; // because it is a static variable it retains 
                       // its value between calls to this function
  flag ^= 1; // toggle the flag state
  sleep(1000); // set taskA to sleep for 1000ms

  if (flag) {
    LEDPORT |= L_I_BIT;
  }
  else {
    LEDPORT &= ~L_I_BIT;
  }
}

// Define taskB
void taskB() {
  static int time = 0;

  time++;

  if (time == 500) {
    LEDPORT |= L_E_BIT;
  }
  if (time == 1000) {
    LEDPORT &= ~L_E_BIT;
    time = 0;
  }
}