#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

// Pin Definitions
#define LED_RED 18
#define LED_YELLOW 17
#define LED_GREEN 16

// Task handles initialization
TaskHandle_t Task1_Handle = NULL;
TaskHandle_t Task2_Handle = NULL;
TaskHandle_t Task3_Handle = NULL;

QueueHandle_t myQueue = xQueueCreate(3, sizeof(int));

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

  xTaskCreate(Task_Red, "RedLight", 1024, NULL, 1, &Task1_Handle);
  xTaskCreate(Task_Yellow, "YellowLight", 1024, NULL, 1, &Task2_Handle);
  xTaskCreate(Task_Green, "GreenLight", 1024, NULL, 1, &Task3_Handle);

  int redState = 0;
  int yellowState = 1;
  int greenState = 2;

  while(1) {
    vTaskDelay(pdMS_TO_TICKS(1)); // delay sending order
    xQueueSend(myQueue, &redState, portMAX_DELAY);
    vTaskDelay(pdMS_TO_TICKS(2999)); // delay sending order
    xQueueSend(myQueue, &yellowState, portMAX_DELAY);
    vTaskDelay(pdMS_TO_TICKS(2999)); // delay sending order
    xQueueSend(myQueue, &greenState, portMAX_DELAY);
    vTaskDelay(pdMS_TO_TICKS(2999)); // delay sending order
  }

}

void loop() {

}



// Task function for the Fixed Blinking LED
void Task_Red (void *pvParameter) {
  pinMode(LED_RED, OUTPUT);
  int receivedState;

  while(1) {
    if (xQueueReceive(myQueue, &receivedState, portMAX_DELAY) == pdTRUE) {
      Serial.println("Red Light");
      digitalWrite(LED_RED, HIGH);
      vTaskDelay(pdMS_TO_TICKS(3000));
      digitalWrite(LED_RED, LOW);
    }
  }
}

void Task_Yellow (void *pvParameter) {
  pinMode(LED_YELLOW, OUTPUT);
  int receivedState;

  while(1) {
    if (xQueueReceive(myQueue, &receivedState, portMAX_DELAY) == pdTRUE) {
      Serial.println("Yellow Light");
      digitalWrite(LED_YELLOW, HIGH);
      vTaskDelay(pdMS_TO_TICKS(3000));
      digitalWrite(LED_YELLOW, LOW);
    }
  }
}

void Task_Green (void *pvParameter) {
  pinMode(LED_GREEN, OUTPUT);
  int receivedState;

  while(1) {
    if (xQueueReceive(myQueue, &receivedState, portMAX_DELAY) == pdTRUE) {
      Serial.println("Green Light");
      digitalWrite(LED_GREEN, HIGH);
      vTaskDelay(pdMS_TO_TICKS(3000));
      digitalWrite(LED_GREEN, LOW);
    }
  }
}

/*
// Pin definitions
#define LED_FIXED 4
#define LED_VARIABLE 2
#define POT_PIN 5
#define BUTTON_FIXED 14
#define BUTTON_VARIABLE 19

// Task handles initialization
TaskHandle_t Task1_Handle = NULL;
TaskHandle_t Task2_Handle = NULL;
// TaskHandle_t Task3_Handle = NULL;

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

  xTaskCreate(Task_FixedBlink, "FixedBlink", 1024, NULL, 1, &Task1_Handle);
  xTaskCreate(Task_VariableBlink, "VariableBlink", 1024, NULL, 1, &Task2_Handle);
  xTaskCreate(Task_ButtonMonitor, "ButtonMonitor", 1024, NULL, 1, NULL);
}

void loop() {

}

// Task function for the Fixed Blinking LED
void Task_FixedBlink (void *pvParameter) {
  pinMode(LED_FIXED, OUTPUT);

  while (1) {
    digitalWrite(LED_FIXED, HIGH);
    vTaskDelay(pdMS_TO_TICKS(500));
    digitalWrite(LED_FIXED, LOW);
    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

void Task_VariableBlink (void *pvParameter) {
  pinMode(LED_VARIABLE, OUTPUT);
  pinMode(POT_PIN, INPUT);
  analogReadResolution(12); // Sets ADC resolution to 12-bit (0-4095)

  while (1) {
    int potValue = analogRead(POT_PIN);
    int delayTime = map(potValue, 0, 4095, 100, 1000);

    digitalWrite(LED_VARIABLE, HIGH);
    vTaskDelay(pdMS_TO_TICKS(delayTime));
    digitalWrite(LED_VARIABLE, LOW);
    vTaskDelay(pdMS_TO_TICKS(delayTime));

  }
}

void Task_ButtonMonitor (void *pvParameter) {
  pinMode(BUTTON_FIXED, INPUT_PULLUP); // Set the button to input with a pull-up resistor
  pinMode(BUTTON_VARIABLE, INPUT_PULLUP); // Set the button to input with a pull-up resistor
  
  bool isFixedPaused = false;
  bool isVariablePaused = false;

  while (1) {
    // When the button is pushed, the input goes to LOW
    if (digitalRead(BUTTON_FIXED) == LOW) {
      vTaskDelay(pdMS_TO_TICKS(50));
      while (digitalRead(BUTTON_FIXED) == LOW) {
        vTaskDelay(pdMS_TO_TICKS(10));
      }

      if (Task1_Handle != NULL) {
        if (isFixedPaused) {
          vTaskResume(Task1_Handle);
          Serial.println("Resuming Fixed LED Task");
        } else {
          vTaskSuspend(Task1_Handle);
          Serial.println("Pausing Fixed LED Task");
        }
        isFixedPaused = !isFixedPaused;
      }
    }

    if (digitalRead(BUTTON_VARIABLE) == LOW) {
      vTaskDelay(pdMS_TO_TICKS(50));
      while (digitalRead(BUTTON_VARIABLE) == LOW) {
        vTaskDelay(pdMS_TO_TICKS(10));
      }

      if (Task2_Handle != NULL) {
        if (isVariablePaused) {
          vTaskResume(Task2_Handle);
          Serial.println("Resuming Variable LED Task");
        } else {
          vTaskSuspend(Task2_Handle);
          Serial.println("Pausing Variable LED Task");
        }
        isVariablePaused = !isVariablePaused;
      }
    }
  }
}
*/