#include <Arduino.h>

const int green1Pin = 17;   // Green light for direction 1
const int yellow1Pin = 16;  // Yellow light for direction 1
const int red1Pin = 4;      // Red light for direction 1

const int green2Pin = 18;   // Green light for direction 2
const int yellow2Pin = 19;  // Yellow light for direction 2
const int red2Pin = 21;     // Red light for direction 2

const int green3Pin = 22;   // Green light for direction 3
const int yellow3Pin = 23;  // Yellow light for direction 3
const int red3Pin = 5;      // Red light for direction 3

const int green4Pin = 25;   // Green light for direction 4
const int yellow4Pin = 26;  // Yellow light for direction 4
const int red4Pin = 27;     // Red light for direction 4

const TickType_t greenDelay = 3000 / portTICK_PERIOD_MS;
const TickType_t yellowDelay = 1000 / portTICK_PERIOD_MS;

TaskHandle_t taskHandleDirection1 = NULL;
TaskHandle_t taskHandleDirection2 = NULL;
TaskHandle_t taskHandleDirection3 = NULL;
TaskHandle_t taskHandleDirection4 = NULL;

// 0-2: Direction 1, 3-5: Direction 2, 6-8: Direction 3, 9-11: Direction 4
int status[12];  

SemaphoreHandle_t xMutex;

void updateLEDsDirection1(void *pvParameters) {
  while (true) {
    if (xSemaphoreTake(xMutex, portMAX_DELAY)) {  // Acquire mutex before accessing the status array
      Serial.println("D1");
      digitalWrite(green1Pin, status[0]);
      digitalWrite(yellow1Pin, status[1]);
      digitalWrite(red1Pin, status[2]);
      xSemaphoreGive(xMutex);  // Release the mutex after updating LEDs
    }
    vTaskDelay(200 / portTICK_PERIOD_MS);
  }
}

void updateLEDsDirection2(void *pvParameters) {
  while (true) {
    if (xSemaphoreTake(xMutex, portMAX_DELAY)) {  // Acquire mutex before accessing the status array
      Serial.println("D2");
      digitalWrite(green2Pin, status[3]);
      digitalWrite(yellow2Pin, status[4]);
      digitalWrite(red2Pin, status[5]);
      xSemaphoreGive(xMutex);  // Release the mutex after updating LEDs
    }
    vTaskDelay(200 / portTICK_PERIOD_MS);
  }
}

void updateLEDsDirection3(void *pvParameters) {
  while (true) {
    if (xSemaphoreTake(xMutex, portMAX_DELAY)) {  // Acquire mutex before accessing the status array
      Serial.println("D3");
      digitalWrite(green3Pin, status[6]);
      digitalWrite(yellow3Pin, status[7]);
      digitalWrite(red3Pin, status[8]);
      xSemaphoreGive(xMutex);  // Release the mutex after updating LEDs
    }
    vTaskDelay(200 / portTICK_PERIOD_MS);
  }
}

void updateLEDsDirection4(void *pvParameters) {
  while (true) {
    if (xSemaphoreTake(xMutex, portMAX_DELAY)) {  // Acquire mutex before accessing the status array
      Serial.println("D4");
      digitalWrite(green4Pin, status[9]);
      digitalWrite(yellow4Pin, status[10]);
      digitalWrite(red4Pin, status[11]);
      xSemaphoreGive(xMutex);  // Release the mutex after updating LEDs
    }
    vTaskDelay(200 / portTICK_PERIOD_MS);
  }
}

void controlDirection(void *pvParameters) {
  Serial.println("Control");
  while (true) {
    // Loop to go through each traffic light (0, 1, 2, 3)
    for (int i = 0; i < 4; i++) {
      int baseIndex = i * 3;  // Base index for each set of LEDs (green, yellow, red)
      
      // Set the priority of the current task based on the iteration index
      switch (i) {
        case 0:
          vTaskPrioritySet(taskHandleDirection1, 3); // Set priority 2 for updateLEDsDirection1
          break;
        case 1:
          vTaskPrioritySet(taskHandleDirection2, 3); // Set priority 2 for updateLEDsDirection2
          break;
        case 2:
          vTaskPrioritySet(taskHandleDirection3, 3); // Set priority 2 for updateLEDsDirection3
          break;
        case 3:
          vTaskPrioritySet(taskHandleDirection4, 3); // Set priority 2 for updateLEDsDirection4
          break;
      }

      // Green state
      if (xSemaphoreTake(xMutex, portMAX_DELAY)) {
        Serial.println("g");
        status[baseIndex] = HIGH;      // Green on
        status[baseIndex + 1] = LOW;   // Yellow off
        status[baseIndex + 2] = LOW;   // Red off
        xSemaphoreGive(xMutex);
      }
      vTaskDelay(greenDelay);  // Green for 3 seconds

      // Yellow state
      if (xSemaphoreTake(xMutex, portMAX_DELAY)) {
        Serial.println("y");
        status[baseIndex] = LOW;       // Green off
        status[baseIndex + 1] = HIGH;  // Yellow on
        status[baseIndex + 2] = LOW;   // Red off
        xSemaphoreGive(xMutex);
      }
      vTaskDelay(yellowDelay);  // Yellow for 1 second

      // Red state
      if (xSemaphoreTake(xMutex, portMAX_DELAY)) {
        Serial.println("r");
        status[baseIndex] = LOW;       // Green off
        status[baseIndex + 1] = LOW;   // Yellow off
        status[baseIndex + 2] = HIGH;  // Red on
        xSemaphoreGive(xMutex);
      }
      vTaskDelay(300 / portTICK_PERIOD_MS);  // Small delay before next cycle

      vTaskPrioritySet(taskHandleDirection1, 1);
      vTaskPrioritySet(taskHandleDirection2, 1);
      vTaskPrioritySet(taskHandleDirection3, 1);
      vTaskPrioritySet(taskHandleDirection4, 1);
    }
  }
}

void setup() {
  Serial.begin(115200);  // Initialize serial communication
  
  // Initialize the pin modes for LEDs
  pinMode(green1Pin, OUTPUT);
  pinMode(yellow1Pin, OUTPUT);
  pinMode(red1Pin, OUTPUT);

  pinMode(green2Pin, OUTPUT);
  pinMode(yellow2Pin, OUTPUT);
  pinMode(red2Pin, OUTPUT);

  pinMode(green3Pin, OUTPUT);
  pinMode(yellow3Pin, OUTPUT);
  pinMode(red3Pin, OUTPUT);

  pinMode(green4Pin, OUTPUT);
  pinMode(yellow4Pin, OUTPUT);
  pinMode(red4Pin, OUTPUT);

  // Initialize all LEDs as OFF, except red LEDs which are set to HIGH
  for (int i = 0; i < 12; i++) {
    if ((i + 1) % 3 == 0) {  // Indexes 2, 5, 8, and 11 correspond to red LEDs
      status[i] = HIGH;  // Red LEDs on
    } else {
      status[i] = LOW;   // Green and yellow LEDs off
    }
  }

  // Create the mutex
  xMutex = xSemaphoreCreateMutex();
  if (xMutex == NULL) {
    Serial.println("Failed to create mutex.");
    return;
  }
  Serial.println("Mutex created.");

  if (xTaskCreate(updateLEDsDirection1, "Update LEDs Direction 1", configMINIMAL_STACK_SIZE * 2, NULL, 1, &taskHandleDirection1) != pdPASS) {
    Serial.println("Failed to create Update LEDs Direction 1 task");
  } else {
    Serial.println("Update LEDs Direction 1 task created.");
  }

  if (xTaskCreate(updateLEDsDirection2, "Update LEDs Direction 2", configMINIMAL_STACK_SIZE * 2, NULL, 1, &taskHandleDirection2) != pdPASS) {
    Serial.println("Failed to create Update LEDs Direction 2 task");
  } else {
    Serial.println("Update LEDs Direction 2 task created.");
  }

  if (xTaskCreate(updateLEDsDirection3, "Update LEDs Direction 3", configMINIMAL_STACK_SIZE * 2, NULL, 1, &taskHandleDirection3) != pdPASS) {
    Serial.println("Failed to create Update LEDs Direction 3 task");
  } else {
    Serial.println("Update LEDs Direction 3 task created.");
  }

  if (xTaskCreate(updateLEDsDirection4, "Update LEDs Direction 4", configMINIMAL_STACK_SIZE * 2, NULL, 1, &taskHandleDirection4) != pdPASS) {
    Serial.println("Failed to create Update LEDs Direction 4 task");
  } else {
    Serial.println("Update LEDs Direction 4 task created.");
  }

  if (xTaskCreate(controlDirection, "Control Direction", configMINIMAL_STACK_SIZE * 2, NULL, 3, NULL) != pdPASS) {
    Serial.println("Failed to create Control Direction task");
  } else {
    Serial.println("Control Direction task created.");
  }

}

void loop() {
  // Empty, as we are using FreeRTOS
}