// Scheduler.cpp
#define ledPin 13
int initcount; // startvalue for interrupt counter

#define MAX_TASKS 100
void (*taskPtr[MAX_TASKS])(); // declare function pointer array
int numOfTasks;
int taskCnt;

void setup()
{
  pinMode(ledPin, OUTPUT);  // LED as output
  
  // Timer 1
  noInterrupts();           // Alle Interrupts temporär abschalten
  TCCR1A = 0;
  TCCR1B = 0;
  // initcount = maxcount - deltaT * cpufreq / prescale 
  //           = 65536    - deltaT * 16000000/256
  initcount =62411; 
  TCNT1 = initcount;            // Timer nach obiger Rechnung vorbelegen
  TCCR1B |= (1 << CS12);        // 256 als Prescale-Wert spezifizieren
  TIMSK1 |= (1 << TOIE1);       // Timer Overflow Interrupt aktivieren
  interrupts();                 // alle Interrupts scharf schalten

  numOfTasks = 0;
  taskCnt=0;
  for (int i=0;i<MAX_TASKS;i++){
    taskPtr[i] = NULL;
  }
  Serial.begin(9600);
}
// --- Interrupt routine -----------------
// ---------------------------------------
ISR(TIMER1_OVF_vect)        
{
  TCNT1 = initcount;             // Zähler erneut vorbelegen
  digitalWrite(ledPin, digitalRead(ledPin) ^ 1); // LED ein und aus
  
  if (taskCnt>=numOfTasks){
    taskCnt=0;
  }
  if (taskPtr[taskCnt]!=NULL)
    taskPtr[taskCnt](); // call task via Function pointer
  
  taskCnt++; // next Task
}
// --- simplest form of adding a Task to be scheduled ---
// ------------------------------------------------------
void addTask(void (*taskFkt)()){
  if (numOfTasks<MAX_TASKS){
    taskPtr[numOfTasks++] = taskFkt;
  } 
}
// --- Task A ----------------------------
// ---------------------------------------
void taskA(){
  Serial.print("TaskA,");
}
// --- Task B ----------------------------
// ---------------------------------------
void taskB(){
  Serial.print("TaskB,");
}
// --- Task C ----------------------------
// ---------------------------------------
void taskC(){
  Serial.print("TaskC\n");
}
// --- main loop ----------------------------
// ---------------------------------------
void loop()
{
  addTask(taskA);
  addTask(taskB);
  addTask(taskC);
  
 do{
  Serial.print("oooooooooooooo\n");
 }while(1); // wait endlessly
 
}