#include <Arduino_FreeRTOS.h>
#include <task.h>
#include <semphr.h>  // add the FreeRTOS functions for Semaphores (or Flags).
#include <FreeRTOSConfig.h>


/* Define the traceTASK_SWITCHED_IN() and *_OUT() macros become HIGH when the tasks is switched in, and low w
when switched out. This has to be done in the RTOS Library. */

//#define traceTASK_SWITCHED_IN() digitalWrite( (int)pxCurrentTCB->pxTaskTag, HIGH )
//#define traceTASK_SWITCHED_OUT() digitalWrite((int)pxCurrentTCB->pxTaskTag, LOW )

/* Define the pin on which the tasks is being monitored (to be put in each task) */ /*modified by Markus Pfeil*/
/* vTaskSetApplicationTaskTag( NULL, ( void * )data.debug_pin); */ 

#include <Arduino_FreeRTOS.h>
#include <task.h>

// Task Handles für die Benachrichtigungen aus der ISR
TaskHandle_t TaskLED_Handles[3];
TaskHandle_t TaskPrio2_Handle;

// Pin-Definitionen
const int ledTasks[3] = {4, 5, 6}; // Die drei Prio 3 Tasks
const int ledPrio2 = 7;            // Der Prio 2 Task

void setup() {
  Serial.begin(115200);

  // Pins als Ausgänge definieren
  pinMode(ledPrio2, OUTPUT);
  for (int i = 0; i < 3; i++) {
    pinMode(ledTasks[i], OUTPUT);
  }

  // 1. Erstelle den Prio 2 Task (läuft im Hintergrund, wenn Prio 3 blockiert ist)
  xTaskCreate(TaskPrio2_Func, "Task2", 128, NULL, 2, &TaskPrio2_Handle);

  // 2. Erstelle die drei LED-Tasks (Prio 3). 
  // Wir übergeben den Index (0, 1, 2) als Parameter.
  xTaskCreate(TaskLED_Func, "LED_0", 128, (void*)0, 3, &TaskLED_Handles[0]);
  xTaskCreate(TaskLED_Func, "LED_1", 128, (void*)1, 3, &TaskLED_Handles[1]);
  xTaskCreate(TaskLED_Func, "LED_2", 128, (void*)2, 3, &TaskLED_Handles[2]);

  // 3. Hardware-Timer (Timer1) für 5 ms Interrupts konfigurieren
  noInterrupts();
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;
  
  // 16 MHz / 64 Prescaler = 250 kHz (1 Tick = 4 us)
  // 5 ms = 5000 us -> 5000 / 4 = 1250 Ticks. OCR1A = 1249
  OCR1A = 1249; 
  
  TCCR1B |= (1 << WGM12);               // CTC Mode (Clear Timer on Compare Match)
  TCCR1B |= (1 << CS11) | (1 << CS10);  // Prescaler 64
  TIMSK1 |= (1 << OCIE1A);              // Compare Match Interrupt aktivieren
  interrupts();
}

void loop() {
  // Bleibt leer, FreeRTOS übernimmt die Kontrolle
}

// --- Task Funktionen ---

void TaskPrio2_Func(void *pvParameters) {
  (void) pvParameters;

  vTaskSetApplicationTaskTag( NULL, ( void * ) 13);
  
  for (;;) {
    // Dieser Task läuft immer dann, wenn die ISR keinen Prio 3 Task aufgeweckt hat.
    digitalWrite(ledPrio2, HIGH);
    
    // Simuliere Arbeit, um den Task auszulasten. 
    // Wir nutzen hier Absicht kein vTaskDelay, damit er rechnet und 
    // von den Prio 3 Tasks unterbrochen (preempted) werden MUSS.
    delayMicroseconds(500); 
    
    digitalWrite(ledPrio2, LOW);
    delayMicroseconds(500); 
  }
}

void TaskLED_Func(void *pvParameters) {
  int taskIndex = (int)pvParameters;
  int pin = ledTasks[taskIndex];
  vTaskSetApplicationTaskTag( NULL, ( void * ) (pin+6));
  //Setzt den Tracer Pin auf 4/5/6 + 6 = 10/11/12
  for (;;) {
    // 1. Blockieren und auf die Benachrichtigung der ISR warten
    // (Ersetzt dein "geht danach in vTaskDelay")
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

    // 2. Wenn wir hier ankommen, hat die ISR uns aufgeweckt. 
    // Wir haben Prio 3 und verdrängen den Prio 2 Task sofort.
    digitalWrite(pin, HIGH);
    
    // Kurze Simulation von Arbeit (damit man die LED sieht)
    delayMicroseconds(200); 
    
    digitalWrite(pin, LOW);
    
    // Am Ende des Loops wartet der Task wieder auf ulTaskNotifyTake 
    // -> Er blockiert, Prio 2 darf wieder laufen.
  }
}

// --- Timer ISR (Alle 5 ms) ---

ISR(TIMER1_COMPA_vect) {
  static int currentTaskIndex = 0;
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;

  // Sende ein Signal an den aktuellen Task im Round-Robin-Verfahren
  vTaskNotifyGiveFromISR(TaskLED_Handles[currentTaskIndex], &xHigherPriorityTaskWoken);

  // Index für das nächste Mal hochzählen (0, 1, 2)
  currentTaskIndex++;
  if (currentTaskIndex >= 3) {
    currentTaskIndex = 0;
  }

  // Wenn ein Task mit höherer Prio (3) durch das Signal aufgeweckt wurde (was der Fall ist),
  // erzwinge einen sofortigen Context Switch. Der Scheduler setzt ihn sofort ein.
  if (xHigherPriorityTaskWoken == pdTRUE) {
    taskYIELD(); 
  }
}