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

// Định nghĩa queue handle
QueueHandle_t xQueue;

// Task 1: Tạo queue và thêm các phần tử vào queue
void vTask1(void *pvParameters)
{
    char msg1[] = "message 1";
    char msg2[] = "message 2";
    char msg3[] = "message 3";
    char msg4[] = "message 4";

    // Tạo queue có 3 phần tử, mỗi phần tử là một chuỗi tối đa 15 kí tự
    xQueue = xQueueCreate(3, sizeof(char[15]));

    if (xQueue != NULL)
    {
        // Gửi message 1 và message 2 vào queue
        xQueueSend(xQueue, &msg1, portMAX_DELAY);
        xQueueSend(xQueue, &msg2, portMAX_DELAY);

        // Đọc và ghi đè message 3 và message 4 vào queue
        xQueueReceive(xQueue, &msg1, portMAX_DELAY); // Remove msg1
        xQueueSend(xQueue, &msg3, portMAX_DELAY); // Add msg3
        xQueueReceive(xQueue, &msg2, portMAX_DELAY); // Remove msg2
        xQueueSend(xQueue, &msg4, portMAX_DELAY); // Add msg4
    }

    // Tạm dừng task để không chiếm CPU
    vTaskSuspend(NULL);
}

// Task 2: Đọc hết dữ liệu từ queue và in ra màn hình
void vTask2(void *pvParameters)
{
    char msg[15];

    if (xQueue != NULL)
    {
        // Đọc các phần tử từ queue và in ra màn hình
        while (1)
        {
            if (xQueueReceive(xQueue, &msg, portMAX_DELAY) == pdPASS)
            {
                Serial.println(msg);
            }
            else
            {
                // Hàng đợi trống, tạm dừng một chút trước khi kiểm tra lại
                vTaskDelay(1000 / portTICK_PERIOD_MS);
            }
        }
    }

    // Tạm dừng task để không chiếm CPU
    vTaskSuspend(NULL);
}

void setup()
{
    // Khởi tạo Serial Monitor
    Serial.begin(9600);

    // Tạo task 1
    xTaskCreate(vTask1, "Task 1", 128, NULL, 1, NULL);

    // Tạo task 2
    xTaskCreate(vTask2, "Task 2", 128, NULL, 1, NULL);

    // Bắt đầu scheduler
    vTaskStartScheduler();
}

void loop()
{
    // Chương trình không bao giờ đến đây
}