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

QueueHandle_t queue1, queue2;         // Separate queues for Task 1 and Task 2
QueueSetHandle_t queueSet;            // Queue set to monitor both queues

// Task 1: Sends "Waiting for user input" to queue1 every 1 second
void Task1_SendMessage(void *pvParameters) {
    const char *message = "Waiting for user input";

    while (true) {
        xQueueSend(queue1, &message, portMAX_DELAY);
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

// Task 2: Reads user input from the serial monitor and sends it to queue2
void Task2_ReadUserInput(void *pvParameters) {
    while (true) {
        if (Serial.available()) {
            String userInput = Serial.readStringUntil('\n');
            char *inputMessage = (char *)pvPortMalloc(userInput.length() + 1);
            if (inputMessage != NULL) {
                strcpy(inputMessage, userInput.c_str());
                xQueueSend(queue2, &inputMessage, portMAX_DELAY);
            }
        }
        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

// Task 3: Monitors the queue set and prints messages from Task 1 or Task 2
void Task3_PrintMessages(void *pvParameters) {
    char *receivedMessage;

    while (true) {
        // Wait for either queue1 or queue2 to have data
        QueueHandle_t activeQueue = (QueueHandle_t)xQueueSelectFromSet(queueSet, portMAX_DELAY);

        if (activeQueue == queue1) {
            if (xQueueReceive(queue1, &receivedMessage, 0) == pdPASS) {
                Serial.print("Received from Task 1: ");
                Serial.println(receivedMessage);
            }
        } else if (activeQueue == queue2) {
            if (xQueueReceive(queue2, &receivedMessage, 0) == pdPASS) {
                Serial.print("Received from Task 2: ");
                Serial.println(receivedMessage);
                vPortFree(receivedMessage); // Free memory after use
            }
        }
    }
}

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

    // Create the queues
    queue1 = xQueueCreate(5, sizeof(char *));
    queue2 = xQueueCreate(5, sizeof(char *));

    // Create a queue set to manage both queues
    queueSet = xQueueCreateSet(10); // Total size should be at least the sum of queue1 and queue2 sizes

    if (queue1 != NULL && queue2 != NULL && queueSet != NULL) {
        // Add queues to the queue set
        xQueueAddToSet(queue1, queueSet);
        xQueueAddToSet(queue2, queueSet);

        // Create tasks with reduced stack sizes
        xTaskCreate(Task1_SendMessage, "Task1", 64, NULL, 1, NULL);
        xTaskCreate(Task2_ReadUserInput, "Task2", 128, NULL, 1, NULL);
        xTaskCreate(Task3_PrintMessages, "Task3", 128, NULL, 1, NULL);

        // Start the FreeRTOS scheduler
        vTaskStartScheduler();
    } else {
        Serial.println("Queue or Queue Set creation failed!");
    }
}

void loop() {
    // Empty. All work is done in tasks.
}