// Implementing and Managing Threads (FreeRTOS Tasks) Demo for Wokwi - FIXED APP_CPU_NUM ERROR

#include <ESP32Servo.h> // Required for controlling the servo motor
#include <freertos/FreeRTOS.h> // Include FreeRTOS base definitions
#include <freertos/task.h>     // Include FreeRTOS task 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; // Create a servo object globally

// --- FreeRTOS Task Functions ---

// Task 1: LED Blinking Task
// This task will independently blink the Red and Green LEDs at different rates.
void ledBlinkTask(void *parameter) {
  // Initialize LED pins as outputs within the task, or in setup()
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(YELLOW_LED_PIN, OUTPUT); // Yellow LED also initialized for potential future use

  // Set initial state for all LEDs to OFF
  digitalWrite(RED_LED_PIN, LOW);
  digitalWrite(GREEN_LED_PIN, LOW);
  digitalWrite(YELLOW_LED_PIN, LOW);

  // The task runs in an infinite loop
  for (;;) {
    // Toggle Red LED
    digitalWrite(RED_LED_PIN, HIGH);
    vTaskDelay(pdMS_TO_TICKS(100)); // Delay for 100ms
    digitalWrite(RED_LED_PIN, LOW);
    vTaskDelay(pdMS_TO_TICKS(900)); // Delay for 900ms (total 1000ms cycle for Red)

    // Toggle Green LED
    digitalWrite(GREEN_LED_PIN, HIGH);
    vTaskDelay(pdMS_TO_TICKS(50));
    digitalWrite(GREEN_LED_PIN, LOW);
    vTaskDelay(pdMS_TO_TICKS(150)); // total 200ms cycle for Green

    // Print a message to confirm this task is running
    Serial.println("LED Task: Blinking LEDs...");

    // vTaskDelay() is crucial: it yields CPU time to other tasks.
    // Without it, this task would hog the CPU.
  }
}

// Task 2: Servo Control Task
// This task will independently sweep the servo motor back and forth.
void servoControlTask(void *parameter) {
  servoMotor.attach(SERVO_PIN); // Attach the servo object to the physical pin

  int angle = 0;
  bool increasing = true; // Flag to control sweep direction

  // The task runs in an infinite loop
  for (;;) {
    servoMotor.write(angle); // Set the servo to the current angle
    Serial.print("Servo Task: Angle = ");
    Serial.println(angle);

    // Update angle for sweeping motion
    if (increasing) {
      angle += 5;
      if (angle >= 180) {
        angle = 180;
        increasing = false; // Change direction
      }
    } else {
      angle -= 5;
      if (angle <= 0) {
        angle = 0;
        increasing = true; // Change direction
      }
    }
    
    vTaskDelay(pdMS_TO_TICKS(50)); // Small delay for smooth movement and to yield CPU
  }
}


void setup() {
  Serial.begin(115200); // Initialize serial communication
  Serial.println("--- Demo 3: FreeRTOS Tasks for Concurrent Execution ---");

  // Create the LED Blinking Task
  // xTaskCreatePinnedToCore( TaskFunction, TaskName, StackSize, Parameters, Priority, TaskHandle, CoreID )
  xTaskCreatePinnedToCore(
    ledBlinkTask,        // Task function to be executed
    "LED Blink Task",    // Name of the task (for debugging)
    2048,                // Stack size in words (not bytes for ESP32 FreeRTOS)
    NULL,                // Parameter to be passed to the task (none here)
    1,                   // Priority of the task (0 is lowest, configMAX_PRIORITIES-1 is highest)
    NULL,                // Task handle (optional, can be NULL if not needed for control later)
    PRO_CPU_NUM          // Core to run the task on. For ESP32-C3 (single-core), use PRO_CPU_NUM.
  );

  // Create the Servo Control Task
  xTaskCreatePinnedToCore(
    servoControlTask,    // Task function
    "Servo Control Task",// Name of the task
    2048,                // Stack size
    NULL,                // Parameter
    2,                   // Priority (higher than LED task to ensure smooth servo)
    NULL,                // Task handle
    PRO_CPU_NUM          // Core. For ESP32-C3 (single-core), use PRO_CPU_NUM.
  );

  // The main loop (Arduino loop()) continues to run, but often at a lower priority.
  // In many FreeRTOS applications, most logic moves into dedicated tasks.
}

void loop() {
  // This loop will also be running as a low-priority FreeRTOS task.
  // We can add some minimal activity here, or just let it spin.
  // It's good practice to have a small delay if it's doing nothing, to yield CPU.
  Serial.println("Main loop: Still running (low priority)...");
  vTaskDelay(pdMS_TO_TICKS(2000)); // Delay for 2 seconds to slow down main loop messages
}