#include <Arduino.h>
// #include <freertos/FreeRTOS.h>
// #include <freertos/task.h>
// #include <freertos/queue.h>

#define LED_BUILTIN 2

static const uint8_t cmd_max = 8;

struct Command {
  char body[8];
};

struct SerialOutput {
  char body[128];
};

QueueHandle_t commandQueue;
QueueHandle_t serialOutputQueue;

bool ledState = false;

void taskInputSerialMonitor(void *pvParameters) {
  char c;
  uint8_t idx = 0;
  char buf[cmd_max];

  while (true) {
    Command *cmd = (Command *)pvPortMalloc(sizeof(Command));
    SerialOutput *output = (SerialOutput *)pvPortMalloc(sizeof(SerialOutput));

    if (Serial.available() > 0) {
      c = Serial.read();

      if (c == '\n') {
        Serial.println(buf);
        if (strcmp(buf, "led on") == 0 || strcmp(buf, "led off") == 0) {
          strcpy(cmd->body, buf);
          xQueueSend(commandQueue, cmd, portMAX_DELAY);
          strcpy(output->body, "Command received successfully.");
        } else {
          strcpy(output->body, "Invalid command.");
        }
        memset(buf, 0, cmd_max);
        idx = 0;
      } else {
        if (idx < cmd_max - 1) {
          buf[idx] = c;
          idx++;
        }
      }
    }
  }
}

void taskProcessCommand(void *pvParameters) {
  while (true) {
    Command cmd;
    if (xQueueReceive(commandQueue, &cmd, portMAX_DELAY) == pdPASS) {
      SerialOutput *output = (SerialOutput *)pvPortMalloc(sizeof(SerialOutput));

      bool newLedState = (strcmp(cmd.body, "led on") == 0);
      if (newLedState != ledState) {
        ledState = newLedState;
        digitalWrite(LED_BUILTIN, ledState ? HIGH : LOW);
        strcpy(output->body, "LED state changed successfully.");
      } else {
        strcpy(output->body, "LED state is already as requested.");
      }

      xQueueSend(serialOutputQueue, output, portMAX_DELAY);
      vPortFree(output);
    }
  }
}

void taskOutputSerialMonitor(void *pvParameters) {
  while (true) {
    SerialOutput output;
    if (xQueueReceive(serialOutputQueue, &output, portMAX_DELAY) == pdPASS) {
      Serial.println(output.body);
    }
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("Enter command :");
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);

  commandQueue = xQueueCreate(10, sizeof(Command));
  serialOutputQueue = xQueueCreate(10, sizeof(SerialOutput));

  xTaskCreatePinnedToCore(taskInputSerialMonitor, "InputSerialMonitor", 2048, NULL, 1, NULL, 1);
  xTaskCreatePinnedToCore(taskProcessCommand, "ProcessCommand", 2048, NULL, 1, NULL, 2);
  xTaskCreatePinnedToCore(taskOutputSerialMonitor, "OutputSerialMonitor", 2048, NULL, 1, NULL, 3);
}

void loop() {
  // Empty loop as tasks are running on FreeRTOS
}