#include <Arduino_FreeRTOS.h>

// Pin definitions
#define LEFT_LED 10
#define MIDDLE_LED 11
#define RIGHT_LED 12
#define LEFT_SWITCH 5
#define RIGHT_SWITCH 6

// Task handles
TaskHandle_t LeftLEDTaskHandle, MiddleLEDTaskHandle, RightLEDTaskHandle;

// Function prototypes
void LeftLEDTask(void *pvParameters);
void MiddleLEDTask(void *pvParameters);
void RightLEDTask(void *pvParameters);
void LeftSwitchTask(void *pvParameters);
void RightSwitchTask(void *pvParameters);

volatile int leftSwitchPressCount = 0; // To track left switch press count

void setup() {
  // Initialize LEDs as output
  pinMode(LEFT_LED, OUTPUT);
  pinMode(MIDDLE_LED, OUTPUT);
  pinMode(RIGHT_LED, OUTPUT);

  // Initialize switches as input with pull-down resistors
  pinMode(LEFT_SWITCH, INPUT);
  pinMode(RIGHT_SWITCH, INPUT);

  // Create LED tasks
  xTaskCreate(LeftLEDTask, "Left LED Task", 128, NULL, 1, &LeftLEDTaskHandle);
  xTaskCreate(MiddleLEDTask, "Middle LED Task", 128, NULL, 1, &MiddleLEDTaskHandle);
  xTaskCreate(RightLEDTask, "Right LED Task", 128, NULL, 1, &RightLEDTaskHandle);

  // Create switch tasks
  xTaskCreate(LeftSwitchTask, "Left Switch Task", 128, NULL, 2, NULL);
  xTaskCreate(RightSwitchTask, "Right Switch Task", 128, NULL, 2, NULL);
}

void loop() {
  // Do nothing, everything is handled in tasks
}

// LED Task: Blink left LED 2 times
void LeftLEDTask(void *pvParameters) {
  for (;;) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Wait for notification
    for (int i = 0; i < 2; i++) {
      digitalWrite(LEFT_LED, HIGH);
      vTaskDelay(pdMS_TO_TICKS(500));
      digitalWrite(LEFT_LED, LOW);
      vTaskDelay(pdMS_TO_TICKS(500));
    }
  }
}

// LED Task: Blink middle LED 4 times
void MiddleLEDTask(void *pvParameters) {
  for (;;) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Wait for notification
    for (int i = 0; i < 4; i++) {
      digitalWrite(MIDDLE_LED, HIGH);
      vTaskDelay(pdMS_TO_TICKS(500));
      digitalWrite(MIDDLE_LED, LOW);
      vTaskDelay(pdMS_TO_TICKS(500));
    }
  }
}

// LED Task: Blink right LED 6 times
void RightLEDTask(void *pvParameters) {
  for (;;) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // Wait for notification
    for (int i = 0; i < 6; i++) {
      digitalWrite(RIGHT_LED, HIGH);
      vTaskDelay(pdMS_TO_TICKS(500));
      digitalWrite(RIGHT_LED, LOW);
      vTaskDelay(pdMS_TO_TICKS(500));
    }
  }
}

// Switch Task: Handle left switch logic
void LeftSwitchTask(void *pvParameters) {
  for (;;) {
    if (digitalRead(LEFT_SWITCH) == HIGH) {
      vTaskDelay(pdMS_TO_TICKS(50)); // Debounce delay
      if (digitalRead(LEFT_SWITCH) == HIGH) {
        leftSwitchPressCount = (leftSwitchPressCount % 3) + 1; // Cycle through 1, 2, 3
        switch (leftSwitchPressCount) {
          case 1: xTaskNotifyGive(LeftLEDTaskHandle); break; // Trigger Left LED
          case 2: xTaskNotifyGive(MiddleLEDTaskHandle); break; // Trigger Middle LED
          case 3: xTaskNotifyGive(RightLEDTaskHandle); break; // Trigger Right LED
        }
        vTaskDelay(pdMS_TO_TICKS(500)); // Wait for release
      }
    }
    vTaskDelay(pdMS_TO_TICKS(10)); // Polling delay
  }
}

// Switch Task: Handle right switch logic
void RightSwitchTask(void *pvParameters) {
  for (;;) {
    if (digitalRead(RIGHT_SWITCH) == HIGH) {
      vTaskDelay(pdMS_TO_TICKS(50)); // Debounce delay
      if (digitalRead(RIGHT_SWITCH) == HIGH) {
        // Trigger all LEDs simultaneously
        xTaskNotifyGive(LeftLEDTaskHandle);
        xTaskNotifyGive(MiddleLEDTaskHandle);
        xTaskNotifyGive(RightLEDTaskHandle);
        vTaskDelay(pdMS_TO_TICKS(500)); // Wait for release
      }
    }
    vTaskDelay(pdMS_TO_TICKS(10)); // Polling delay
  }
}