#include <Arduino_FreeRTOS.h>
#include <semphr.h>
#include <event_groups.h>

// ==============================================================================
// KONFIGURATIONS-BLOCK (Für die Vorlesung)
// ==============================================================================

// --- NEU: Priority Inversion Demo ---
// 0 = Normales Verhalten (wie vorher)
// 1 = Aktiviert das Priority Inversion Szenario (Task 2 blockiert Task 1)
#define CONF_DEMO_PRIO_INVERSION 0

// 1. Prioritäten (Wird für die Inversion Demo automatisch auf 1 überschrieben!)
// 0 = Alle Tasks haben Prio 2
// 1 = Task 1 (Prio 3), Task 2 (Prio 2), Task 3 (Prio 1)
#define CONF_PRIO_DIFFERENT 0

// 2. Delay-Typ im normalen Modus
// 0 = Blocking Delay (vTaskDelay)
// 1 = Busy-Wait (Active Polling)
#define CONF_BUSY_WAIT 1

// 3. Interrupt-Trigger (z.B. Taster an Pin 2)
// 0 = Tasks laufen kontinuierlich durch (Für Prio Inversion auf 0 lassen!)
// 1 = Binäre Semaphore 
// 2 = Event Group 
#define CONF_ISR_TRIGGER 0

// 4. Schutz der Serial-Ausgabe / Shared Resource
// 0 = Ungeschützt
// 1 = Mutex -> LÖSUNG für Prio Inversion (Besitzt Priority Inheritance!)
// 2 = Binäre Semaphore -> PROBLEM (Erzeugt Priority Inversion!)
#define CONF_SERIAL_PROTECT 0

// 5. TaskYield am Ende des Tasks oder nicht
// 0 = Kein Task Yield, Task läuft immer sofort  wieder von vorne
// 1 = Task gibt am Ende die Prio an den nächsten Task ab - bei gleicher Prio läuft jeder Task nur einmal, dann der nächste
#define CONF_TASK_YIELD 0

// ==============================================================================

#define BUTTON_PIN 2

SemaphoreHandle_t xSerialMutex = NULL;
SemaphoreHandle_t xSerialSemaphore = NULL;
SemaphoreHandle_t xIsrSemaphore = NULL;
EventGroupHandle_t xIsrEventGroup = NULL;

#define ISR_EVENT_BIT (1 << 0)

// Hilfsfunktion: Simuliert CPU-Last ohne den Scheduler aufzurufen
void simulateBusyWait(uint32_t ms) {
  uint32_t start = micros();
  while ((micros() - start) < (ms * 1000UL)) {
    __asm__ __volatile__("nop");
  }
}

// Sichere Ressourcen-Anforderung (mit Timeout-Rückgabe)
bool takeResource() {
#if CONF_SERIAL_PROTECT == 1
  return (xSemaphoreTake(xSerialMutex, portMAX_DELAY) == pdTRUE);
#elif CONF_SERIAL_PROTECT == 2
  return (xSemaphoreTake(xSerialSemaphore, portMAX_DELAY) == pdTRUE);
#else
  return true;
#endif
}

// Sichere Ressourcen-Freigabe
void giveResource() {
#if CONF_SERIAL_PROTECT == 1
  xSemaphoreGive(xSerialMutex);
#elif CONF_SERIAL_PROTECT == 2
  xSemaphoreGive(xSerialSemaphore);
#endif
}

// Die Main Task-Schleife
void vGenericTask(void *pvParameters) {
  int taskNum = (int)pvParameters;
  int tracePin = (taskNum == 1) ? 8 : (taskNum == 2) ? 9 : 10;
  
  pinMode(tracePin, OUTPUT);
  vTaskSetApplicationTaskTag(NULL, (TaskHookFunction_t)tracePin);

  for (;;) {
#if CONF_DEMO_PRIO_INVERSION == 1
    // =====================================================================
    // SPEZIELLES SZENARIO: PRIORITY INVERSION
    // =====================================================================
    if (taskNum == 1) { // High Prio
      vTaskDelay(pdMS_TO_TICKS(50)); // Warte kurz, lass Task 3 starten
      Serial.println("Task 1 (High) will Ressource...");
      if (takeResource()) {
        Serial.println("Task 1 (High) HAT Ressource! (Inversion beendet)");
        simulateBusyWait(50);
        giveResource();
      }
      vTaskDelay(pdMS_TO_TICKS(1000)); // Reset für nächsten Durchlauf
      
    } else if (taskNum == 2) { // Medium Prio
      vTaskDelay(pdMS_TO_TICKS(100)); // Warte, bis Task 1 an Task 3 blockiert
      Serial.println("Task 2 (Med) preemptiert Task 3 und macht Last!");
      simulateBusyWait(500); // BÖSE: Blockiert die CPU für 500ms!
      Serial.println("Task 2 (Med) fertig mit Last.");
      vTaskDelay(pdMS_TO_TICKS(1000)); // Reset für nächsten Durchlauf
      
    } else if (taskNum == 3) { // Low Prio
      if (takeResource()) {
        Serial.println("Task 3 (Low) hat Ressource gesperrt.");
        // Hier sollte Task 3 unterbrochen werden von Task 1, dann Task 2
        simulateBusyWait(300); 
        Serial.println("Task 3 (Low) gibt Ressource frei.");
        giveResource();
      }
      vTaskDelay(pdMS_TO_TICKS(1000)); // Reset für nächsten Durchlauf
    }

#else
    // =====================================================================
    // NORMALES SZENARIO (Dein ursprünglicher Wunsch)
    // =====================================================================
#if CONF_ISR_TRIGGER == 1
    xSemaphoreTake(xIsrSemaphore, portMAX_DELAY);
#elif CONF_ISR_TRIGGER == 2
    xEventGroupWaitBits(xIsrEventGroup, ISR_EVENT_BIT, pdTRUE, pdFALSE, portMAX_DELAY);
#endif

    if (takeResource()) {
      Serial.print("Task "); Serial.print(taskNum); Serial.println(" laeuft.");
      giveResource();
    }

#if CONF_BUSY_WAIT == 1
    simulateBusyWait(100);
#else
    vTaskDelay(pdMS_TO_TICKS(100));
#endif

#if CONF_TASK_YIELD == 1
taskYIELD();
#endif

#endif
  }
}

// Interrupt Service Routine (ISR)
void isrTrigger() {
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;

#if CONF_ISR_TRIGGER == 1
  xSemaphoreGiveFromISR(xIsrSemaphore, &xHigherPriorityTaskWoken);
#elif CONF_ISR_TRIGGER == 2
  xEventGroupSetBitsFromISR(xIsrEventGroup, ISR_EVENT_BIT, &xHigherPriorityTaskWoken);
#endif

  // FIX FÜR ARDUINO UNO (AVR Portierung):
  // Auf ARM-Systemen nutzt man portYIELD_FROM_ISR().
  // Auf dem Uno erzwingen wir den Context Switch mit taskYIELD().
  if (xHigherPriorityTaskWoken == pdTRUE) {
    taskYIELD(); 
  }
}

void setup() {
  delay(100);
  Serial.begin(115200);
  while (!Serial) { ; }

  pinMode(BUTTON_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), isrTrigger, FALLING);

#if CONF_SERIAL_PROTECT == 1
  xSerialMutex = xSemaphoreCreateMutex();
#elif CONF_SERIAL_PROTECT == 2
  xSerialSemaphore = xSemaphoreCreateBinary();
  xSemaphoreGive(xSerialSemaphore); 
#endif

#if CONF_ISR_TRIGGER == 1
  xIsrSemaphore = xSemaphoreCreateBinary();
#elif CONF_ISR_TRIGGER == 2
  xIsrEventGroup = xEventGroupCreate();
#endif

  UBaseType_t p1 = 3, p2 = 2, p3 = 1; // Default Inversion Priorities
#if CONF_PRIO_DIFFERENT == 0 && CONF_DEMO_PRIO_INVERSION == 0
  p1 = 2; p2 = 2; p3 = 2;
#endif

  xTaskCreate(vGenericTask, "Task1", 128, (void *)1, p1, NULL);
  xTaskCreate(vGenericTask, "Task2", 128, (void *)2, p2, NULL);
  xTaskCreate(vGenericTask, "Task3", 128, (void *)3, p3, NULL);
}

void loop() {}