#include <stdlib.h>

#define COMMAND_MAX_LENGTH 8
#define SERIAL_OUTPUT_MAX_LENGTH 128

// Declare Command structure
struct Command {
  char body[COMMAND_MAX_LENGTH];
};
// Declare Serial Output structure
struct SerialOutput {
  char body[SERIAL_OUTPUT_MAX_LENGTH];
};

// Setup command queue and serial output queue
QueueHandle_t commandQueue;
QueueHandle_t serialOutputQueue;

// Check if the command is valid ("led on" or "led off")
bool isCommandValid(const char* cmd) {
  return (strcmp(cmd, "led on") == 0 || strcmp(cmd, "led off") == 0);
}

void vPortFreeWrapper(void *ptr) {
  // Wrapper function to call vPortFree to free allocated memory.
  vPortFree(ptr);
}

// Task 1. Input Serial Monitor
void taskInputSerialMonitor(void *parameter) {
  Command receivedCommand;
  SerialOutput outputMessage;
  char lastValidCommand[COMMAND_MAX_LENGTH] = "";

  while (1) {
    // Read input from Serial Monitor
    String input = Serial.readStringUntil('\n');
    input.trim(); // Remove leading/trailing whitespace

    if (!input.isEmpty()) {
      // Copy the received command into the Command structure
      strncpy(receivedCommand.body, input.c_str(), COMMAND_MAX_LENGTH);

      // Check if the received command is valid
      if (isCommandValid(receivedCommand.body)) {
        if (strcmp(receivedCommand.body, lastValidCommand) == 0) {
          // If the command is a repetition, send an error message
          snprintf(outputMessage.body, SERIAL_OUTPUT_MAX_LENGTH, "Error: Command '%s' repeated", receivedCommand.body);
          xQueueSend(serialOutputQueue, &outputMessage, portMAX_DELAY);
        } else {
          // Send a message indicating the valid command is received
          snprintf(outputMessage.body, SERIAL_OUTPUT_MAX_LENGTH, "Command '%s' received and queued.", receivedCommand.body);
          xQueueSend(serialOutputQueue, &outputMessage, portMAX_DELAY);

          // Allocate memory using vPortMalloc
          char *allocatedCommand = (char *)pvPortMalloc(COMMAND_MAX_LENGTH);
          // Error handling for failed memory allocation
          if (allocatedCommand == NULL) {
            snprintf(outputMessage.body, SERIAL_OUTPUT_MAX_LENGTH, "Error: Memory allocation failed");
            xQueueSend(serialOutputQueue, &outputMessage, portMAX_DELAY);
          } else {
            // Send the allocated memory with the command to the command queue
            strncpy(allocatedCommand, receivedCommand.body, COMMAND_MAX_LENGTH);

            // Error handling for failed post to command queue
            if (xQueueSend(commandQueue, &allocatedCommand, portMAX_DELAY) != pdPASS) {
              snprintf(outputMessage.body, SERIAL_OUTPUT_MAX_LENGTH, "Failed to post to command queue");
              xQueueSend(serialOutputQueue, &outputMessage, portMAX_DELAY);
              // Free the memory in case of failure
              vPortFreeWrapper(allocatedCommand);
            } else {
              // Update last valid command
              strncpy(lastValidCommand, receivedCommand.body, COMMAND_MAX_LENGTH);
            }
          }
        }
      } else {
        // Send error message for invalid input
        snprintf(outputMessage.body, SERIAL_OUTPUT_MAX_LENGTH, "Error: Invalid command");
        xQueueSend(serialOutputQueue, &outputMessage, portMAX_DELAY);
      }
    }
  }
}

// Task 2. Processing Command from Task 1
void taskProcessCommand(void *parameter) {
  char *currentCommand;
  bool ledState = false;
  SerialOutput outputMessage;

  while (1) {
    // Fetch command from command queue
    if (xQueueReceive(commandQueue, &currentCommand, portMAX_DELAY) == pdPASS) {
      // If command is "led on", turn on LED
      if (strcmp(currentCommand, "led on") == 0) {
        ledState = true;
        digitalWrite(LED_BUILTIN, HIGH); // Turn on LED
        snprintf(outputMessage.body, SERIAL_OUTPUT_MAX_LENGTH, "LED is now on");
      } 
      // If command is "led off", turn off LED 
      else if (strcmp(currentCommand, "led off") == 0) {
        ledState = false;
        digitalWrite(LED_BUILTIN, LOW); // Turn off LED
        snprintf(outputMessage.body, SERIAL_OUTPUT_MAX_LENGTH, "LED is now off");
      }
      // Send message indicating LED status
      xQueueSend(serialOutputQueue, &outputMessage, portMAX_DELAY);

      // Free the memory allocated using vPortMalloc
      vPortFreeWrapper(currentCommand);
    }
  }
}

// Task 3. Print message from Task 2
void taskOutputSerialMonitor(void *parameter) {
  SerialOutput outputMessage;
  while (1) {
    // Receive message from serial output queue and print to serial monitor
    if (xQueueReceive(serialOutputQueue, &outputMessage, portMAX_DELAY) == pdPASS) {
      Serial.println(outputMessage.body);
    }
  }
}

void setup() {
  Serial.begin(115200);
  // Create both command and serial output queues
  commandQueue = xQueueCreate(10, sizeof(Command));
  serialOutputQueue = xQueueCreate(10, sizeof(SerialOutput));

  pinMode(LED_BUILTIN, OUTPUT); // Set LED pin as output

  // Create all three main tasks with varying priority
  xTaskCreatePinnedToCore(taskInputSerialMonitor, "InputSerial", 4096, NULL, 1, NULL, 0);
  xTaskCreatePinnedToCore(taskProcessCommand, "ProcessCommand", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskOutputSerialMonitor, "OutputSerial", 4096, NULL, 3, NULL, 0);

  // Queue error handling
  if (commandQueue == NULL || serialOutputQueue == NULL) {
    Serial.println("Error creating queues");
  }

  // Delete setup task
  vTaskDelete(NULL);
}

void loop() {
  // Empty loop
}