#include <Arduino_FreeRTOS.h>
#include <queue.h>
#include <Arduino.h>
#include <LiquidCrystal.h>

typedef struct {
  char text[30];
  int message_number;
} message_t;

void TaskRead(void *queue);
void TaskPerform(void *queue);

QueueHandle_t queue;
message_t message;
const int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

void setup() {
    pinMode(8, OUTPUT);
    pinMode(9, OUTPUT);

    lcd.begin(16, 2);
    lcd.print("Hello, World!");

    Serial.begin(115200);
    Serial.setTimeout(2000);

    queue = xQueueCreate(5, sizeof(message_t));

    xTaskCreate(TaskRead, "UART Read Task", 256, queue, 2, NULL);
    xTaskCreate(TaskPerform, "Perform Queue Task", 256, queue, 2, NULL);
}

void TaskRead(void *queue){
  message_t message;

  while(1){
    if(Serial.available() > 0){
      size_t num_read = Serial.readBytesUntil('\r', message.text, sizeof(message.text) - 1);
      message.text[num_read] = '\0';
      xQueueSend(queue, &message, portMAX_DELAY);
      message.message_number++;

      Serial.println("Sent message to taskPerform()");
      vTaskDelay(pdMS_TO_TICKS(3000));
    }
  }
}

void TaskPerform(void *queue){
    message_t message;
    while(1){
        xQueueReceive(queue, &message, portMAX_DELAY);
        Serial.println("Received messsage from taskRead()");
        if(strcmp(message.text, "set led 1 on") == 0) {
          digitalWrite(8, HIGH);
        } else if(strcmp(message.text, "set led 1 off") == 0) {
          digitalWrite(8, LOW);
        } else if(strcmp(message.text, "set led 2 on") == 0) {
          digitalWrite(9, HIGH);
        } else if(strcmp(message.text, "set led 2 off") == 0) {
          digitalWrite(9, LOW);
        } else if(strcmp(message.text, "get pot") == 0) {
          Serial.println(analogRead(A0));
        } else if(strstr(message.text, "set lcd") != NULL) {
          lcd.clear();
          char buffer[40] = { 0 };
          int index = 0;

          for(int i = 8; i < strlen(message.text); i++) {
              buffer[index] = message.text[i];
              index++;
          }

          lcd.print(buffer);
        } else if(strstr(message.text, "set piezo") != NULL) {
          char buffer[40] = { 0 };
          int index = 0;

          for(int i = 10; i < 13; i++) {
            buffer[index] = message.text[i];
            index++;
          }

          int frequency;
          sscanf(buffer, "%d", &frequency);
          tone(7, frequency, 200);
        }
    }
}

void loop()
{
}