// Task Scheduling, Message Queues, Timers Demo for Wokwi

#include <ESP32Servo.h>       // Required for controlling the servo motor
#include <freertos/FreeRTOS.h>  // FreeRTOS base definitions
#include <freertos/task.h>      // FreeRTOS task definitions
#include <freertos/queue.h>     // FreeRTOS message queue definitions
#include <freertos/timers.h>    // FreeRTOS software timer definitions

// Pin Definitions (referencing your diagram.json)
const int RED_LED_PIN = 7;
const int GREEN_LED_PIN = 6;
const int YELLOW_LED_PIN = 5;
const int PUSH_BUTTON_PIN = 8; // Not used in this specific demo
const int SERVO_PIN = 4;

Servo servoMotor; // Global servo object

// --- FreeRTOS Handles ---
// Handle for the message queue used to send servo angles
QueueHandle_t servoAngleQueue;

// Handles for FreeRTOS software timers
TimerHandle_t xOneShotTimer = NULL;     // For a single event after delay
TimerHandle_t xAutoReloadTimer = NULL;  // For periodic events

// --- FreeRTOS Task Functions ---

// Task 1: Angle Producer Task
// This task calculates new servo angles and sends them to the queue.
void angleProducerTask(void *parameter) {
  int angle = 0;
  bool increasing = true; // True for increasing angle, false for decreasing

  for (;;) { // Infinite loop for the task
    // Calculate next angle
    if (increasing) {
      angle += 10;
      if (angle >= 180) {
        angle = 180;
        increasing = false;
      }
    } else {
      angle -= 10;
      if (angle <= 0) {
        angle = 0;
        increasing = true;
      }
    }

    // Send the calculated angle to the message queue.
    // portMAX_DELAY means the task will block indefinitely if the queue is full,
    // waiting for space to become available.
    if (xQueueSend(servoAngleQueue, &angle, portMAX_DELAY) != pdPASS) {
      Serial.println("Angle Producer: FAILED to send angle to queue!");
    } else {
      Serial.print("Angle Producer: Sent angle ");
      Serial.println(angle);
    }

    vTaskDelay(pdMS_TO_TICKS(100)); // Delay for 100ms before sending next angle
  }
}

// Task 2: Servo Consumer Task
// This task waits for angles from the queue and controls the servo.
void servoConsumerTask(void *parameter) {
  servoMotor.attach(SERVO_PIN); // Attach the servo object to its pin

  int receivedAngle;
  for (;;) { // Infinite loop for the task
    // Wait to receive an angle from the queue.
    // portMAX_DELAY means the task will block indefinitely if the queue is empty,
    // waiting for data to become available.
    if (xQueueReceive(servoAngleQueue, &receivedAngle, portMAX_DELAY) == pdPASS) {
      servoMotor.write(receivedAngle); // Control the servo
      Serial.print("Servo Consumer: Set servo to angle ");
      Serial.println(receivedAngle);
    } else {
      Serial.println("Servo Consumer: FAILED to receive angle from queue!");
    }
  }
}

// Task 3: LED Blink Task (General activity indicator)
// This task blinks the Red LED to show general system activity.
void ledBlinkTask(void *parameter) {
  pinMode(RED_LED_PIN, OUTPUT);
  for (;;) { // Infinite loop for the task
    digitalWrite(RED_LED_PIN, HIGH);
    vTaskDelay(pdMS_TO_TICKS(200));
    digitalWrite(RED_LED_PIN, LOW);
    vTaskDelay(pdMS_TO_TICKS(800));
    Serial.println("LED Task: Blinking Red LED...");
  }
}

// --- FreeRTOS Software Timer Callbacks ---
// These functions are executed when their respective timers expire.
// They run in the FreeRTOS Timer Service Task context.

// Callback for the one-shot timer
void vOneShotTimerCallback(TimerHandle_t xTimer) {
  // This will fire only once after it's started
  Serial.println("One-Shot Timer Fired! (Yellow LED ON briefly)");
  digitalWrite(YELLOW_LED_PIN, HIGH);
  vTaskDelay(pdMS_TO_TICKS(500)); // Flash for 500ms
  digitalWrite(YELLOW_LED_PIN, LOW);
}

// Callback for the auto-reload timer
void vAutoReloadTimerCallback(TimerHandle_t xTimer) {
  // This will fire repeatedly at its set period
  Serial.println("Auto-Reload Timer Fired! (Green LED toggled)");
  digitalWrite(GREEN_LED_PIN, !digitalRead(GREEN_LED_PIN)); // Toggle Green LED state
}

void setup() {
  Serial.begin(115200); // Initialize serial communication
  Serial.println("--- Demo 5: Task Scheduling, Message Queues, Timers ---");

  // Initialize LED pins
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(YELLOW_LED_PIN, OUTPUT);
  digitalWrite(RED_LED_PIN, LOW);
  digitalWrite(GREEN_LED_PIN, LOW);
  digitalWrite(YELLOW_LED_PIN, LOW);

  // 1. Create a FreeRTOS Message Queue
  // xQueueCreate(QueueLength, ItemSize)
  // Here, a queue that can hold up to 10 integer values (sizeof(int))
  servoAngleQueue = xQueueCreate(10, sizeof(int));
  if (servoAngleQueue == NULL) {
    Serial.println("ERROR: Failed to create servo angle queue!");
    // You might want to handle this error more robustly in a real application
  } else {
    Serial.println("Message Queue created successfully.");
  }

  // 2. Create FreeRTOS Tasks
  // xTaskCreatePinnedToCore( TaskFunction, TaskName, StackSize, Parameters, Priority, TaskHandle, CoreID )
  // Remember: For ESP32-C3 (single-core), use PRO_CPU_NUM (or 0) for CoreID.

  // Task for producing angles (lower priority)
  xTaskCreatePinnedToCore(
    angleProducerTask,    // Task function
    "Angle Producer",     // Name of task
    2048,                 // Stack size in words (not bytes)
    NULL,                 // Parameter to task (none)
    1,                    // Priority (0 is lowest)
    NULL,                 // Task handle (not needed for this demo)
    PRO_CPU_NUM           // Core to run on (the only core for ESP32-C3)
  );

  // Task for consuming angles and controlling servo (higher priority)
  xTaskCreatePinnedToCore(
    servoConsumerTask,    // Task function
    "Servo Consumer",     // Name of task
    2048,                 // Stack size
    NULL,                 // Parameter
    2,                    // Priority (higher than producer to ensure responsiveness)
    NULL,                 // Task handle
    PRO_CPU_NUM           // Core
  );

  // Task for blinking an LED (medium priority)
  xTaskCreatePinnedToCore(
    ledBlinkTask,         // Task function
    "LED Blink Task",     // Name of task
    1024,                 // Stack size
    NULL,                 // Parameter
    1,                    // Priority (same as producer, scheduler will interleave)
    NULL,                 // Task handle
    PRO_CPU_NUM           // Core
  );

  // 3. Create FreeRTOS Software Timers
  // xTimerCreate( TimerName, PeriodInTicks, AutoReload, TimerID, CallbackFunction )

  // One-shot timer: will fire once after 5 seconds
  xOneShotTimer = xTimerCreate(
    "One-Shot Timer",             // Name of timer
    pdMS_TO_TICKS(5000),          // Period: 5000 milliseconds
    pdFALSE,                      // Auto-reload: pdFALSE for one-shot
    (void *)0,                    // Timer ID (can be anything, 0 here)
    vOneShotTimerCallback         // Function to call when timer expires
  );

  // Auto-reload timer: will fire every 2 seconds
  xAutoReloadTimer = xTimerCreate(
    "Auto-Reload Timer",          // Name of timer
    pdMS_TO_TICKS(2000),          // Period: 2000 milliseconds
    pdTRUE,                       // Auto-reload: pdTRUE for periodic
    (void *)1,                    // Timer ID
    vAutoReloadTimerCallback      // Function to call when timer expires
  );

  // Start the timers
  if (xOneShotTimer != NULL) {
    xTimerStart(xOneShotTimer, 0); // Start immediately (0 tick delay)
  } else {
    Serial.println("ERROR: Failed to create One-Shot Timer!");
  }

  if (xAutoReloadTimer != NULL) {
    xTimerStart(xAutoReloadTimer, 0); // Start immediately
  } else {
    Serial.println("ERROR: Failed to create Auto-Reload Timer!");
  }

  // The main Arduino loop() function will also run as a low-priority FreeRTOS task.
  // Most core logic is now in dedicated tasks.
}

void loop() {
  // This loop serves as a very low-priority background task.
  // It's good practice to have a small delay if it's doing nothing,
  // to yield CPU time to other tasks.
  Serial.println("Main loop: Running (very low priority)...");
  vTaskDelay(pdMS_TO_TICKS(5000)); // Delay for 5 seconds to reduce its messages
}