#include <stdio.h>
#include <stdint.h>
#include <inttypes.h>
#include <stdlib.h>

#define MAX_TASKS 100

struct Task {
  uint32_t due;
  uint32_t intervall;
  // Funktionspointer etc.
  // Arguments
  // Debug:
  int pid;
} tasks[MAX_TASKS];
int taskcount=0;

// Zwei Elemente vertauschen.
// für größere sizeof(Task) macht ein Umstellen des Arrays auf Pointer Sinn.
void swap(int a, int b) {
  struct Task tmp=tasks[a];
  tasks[a]=tasks[b];
  tasks[b]=tmp;
}

#define LEFT(i) ((i)*2+1)
#define RIGHT(i) ((i)*2+2)
#define PARENT(i) ((i-1)/2)
// An Knoten "i" die Heap-Bedingung herstellen
// Laufzeit O(log n)
void heapify(int i) {
  // Wir suchen unter diesem und direkten Kindern den kleinsten Knoten
  int kleinster=i;
  int left=LEFT(i);
  int right=RIGHT(i);
  if (left < taskcount && tasks[left].due < tasks[kleinster].due)
    kleinster=left;
  if (right < taskcount && tasks[right].due < tasks[kleinster].due)
    kleinster=right;
  if (kleinster == i) return; // Passt schon
  // Ansonsten tauschen, kleinsten Knoten Richtung Wurzel wandern lassen.
  swap(i, kleinster);
  // Tail-rekursion
  heapify(kleinster);
}

// Neuen Heap aus unsortierten Daten erstellen, dafür
// beginnend von den Blättern "heapify" aufrufen.
// Gesamt-Laufzeit ist trotzem nur O(n), 
// was nicht ganz offensichtlich ist.
void build_heap() {
  if (!taskcount) return;
  for (int i = taskcount/2-1; i>=0; --i)
    heapify(i);
}

// Übungsaufgaben für den geneigten Leser:
// - Funktion um (dynamisch) einen Task hinzuzufügen, solange taskcount<MAX_TASKS
//   Laufzeit O(log n): in tasks[taskcount] schreiben, taskcount++, und dann solange nötig swap mit jeweiligem PARENT
// - Funktion um den gerade ausgeführten Task nicht wieder zu re-schedulen, für nicht-periodische one-shots.
//   Laufzeit O(log n): letzten Task an Index 0 kopieren, taskcount--, heapify(0)


// Dummy-Tasks erstellen
void setup() {
  // Generate random tasks
  for (; taskcount<42; taskcount++) {
    tasks[taskcount].due=rand() % 10000;
    tasks[taskcount].intervall=rand()%1000+5;
    tasks[taskcount].pid=taskcount;
  }
  // einmal vor-sortieren ("Binary Heap" herstellen)
  build_heap();
}

// fake "millis()", "systick()", "time()" etc.
uint32_t systick() {
  static uint32_t dummy=0;
  return ++dummy;
}

void loop() {
  // Alle fälligen Tasks ausführen:
  while (taskcount && (int32_t)(tasks[0].due - systick()) < 0) {
     // Execute task...
     printf("at time %" PRIu32 ": Executing task with pid %i, ival=%" PRIu32 ", delay=%i\n",systick(),tasks[0].pid, tasks[0].intervall, (int32_t)(tasks[0].due - systick()));
     // Reschedule:
     tasks[0].due += tasks[0].intervall;
     // Task hochschieben, falls andere früher dran kommen.
     heapify(0);
  }
}


int main() {
  // Make it look like Arduino :)
  setup();
  while (systick() < 100000) loop();
  return 0;
}