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

// Khai báo queue
QueueHandle_t xQueue;

// Kích thước của các phần tử trong queue
const int queueItemSize = 16;

void task1(void *pvParameters) {
  // Dữ liệu ban đầu cho vào queue
  const char* message1 = "message 1";
  const char* message2 = "message 2";
  const char* message3 = "message 3";
  const char* message4 = "message 4";

  // Thêm hai phần tử đầu tiên vào queue
  xQueueSend(xQueue, message1, portMAX_DELAY);
  xQueueSend(xQueue, message2, portMAX_DELAY);

  vTaskDelay(pdMS_TO_TICKS(2000)); // Tạm dừng 2 giây

  // Thêm hai phần tử tiếp theo vào queue, ghi đè lên các phần tử cũ
  xQueueSend(xQueue, message3, portMAX_DELAY);
  xQueueSend(xQueue, message4, portMAX_DELAY);

  // Xóa task sau khi hoàn thành
  vTaskDelete(NULL);
}

void task2(void *pvParameters) {
  char buffer[queueItemSize];

  while (1) {
    // Đọc hết dữ liệu từ queue
    while (xQueuePeek(xQueue, buffer, portMAX_DELAY) == pdTRUE) {
      Serial.println(buffer);
      xQueueReceive(xQueue, buffer, portMAX_DELAY); // Loại bỏ phần tử đã đọc
    }
    vTaskDelay(pdMS_TO_TICKS(1000)); // Tạm dừng 1 giây trước khi kiểm tra lại
  }
}

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

  // Tạo queue với 3 phần tử, mỗi phần tử là một chuỗi ký tự tối đa 15 ký tự
  xQueue = xQueueCreate(3, queueItemSize);

  // Kiểm tra queue đã được tạo thành công chưa
  if (xQueue != NULL) {
    // Tạo các task
    xTaskCreate(task1, "Task 1", 256, NULL, 1, NULL);
    xTaskCreate(task2, "Task 2", 256, NULL, 1, NULL);

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

void loop() {
  // Không cần làm gì trong loop vì FreeRTOS sẽ quản lý các task
}