#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include <stdio.h>

#define QUEUE_LENGTH 5
#define ITEM_SIZE sizeof(int)

// Declare the queues and the queue set.
QueueHandle_t queue1, queue2;
QueueSetHandle_t queueSet;

void senderTask1(void *pvParameters) {
    int data = 10;
    while (1) {
        // Send data to queue1.
        xQueueSend(queue1, &data, portMAX_DELAY);
        printf("Task 1 sent: %d\n", data);
        vTaskDelay(pdMS_TO_TICKS(1000)); // Wait 1 second.
    }
}

void senderTask2(void *pvParameters) {
    int data = 20;
    while (1) {
        // Send data to queue2.
        xQueueSend(queue2, &data, portMAX_DELAY);
        printf("Task 2 sent: %d\n", data);
        vTaskDelay(pdMS_TO_TICKS(2000)); // Wait 2 seconds.
    }
}

void receiverTask(void *pvParameters) {
    QueueHandle_t activatedQueue;
    int receivedData;

    while (1) {
        // Block until a queue in the queue set has data available.
        activatedQueue = (QueueHandle_t)xQueueSelectFromSet(queueSet, portMAX_DELAY);

        // Check which queue was activated and read data from it.
        if (activatedQueue == queue1) {
            xQueueReceive(queue1, &receivedData, 0);
            printf("Receiver Task got from Queue 1: %d\n", receivedData);
        } else if (activatedQueue == queue2) {
            xQueueReceive(queue2, &receivedData, 0);
            printf("Receiver Task got from Queue 2: %d\n", receivedData);
        }
    }
}

void app_main(void) {
    // Create two queues.
    queue1 = xQueueCreate(QUEUE_LENGTH, ITEM_SIZE);
    queue2 = xQueueCreate(QUEUE_LENGTH, ITEM_SIZE);

    // Create a queue set to hold up to 2 queues.
    queueSet = xQueueCreateSet(QUEUE_LENGTH * 2);

    // Add the queues to the queue set.
    xQueueAddToSet(queue1, queueSet);
    xQueueAddToSet(queue2, queueSet);

    // Create the sender and receiver tasks.
    xTaskCreate(senderTask1, "Sender Task 1", 2048, NULL, 1, NULL);
    xTaskCreate(senderTask2, "Sender Task 2", 2048, NULL, 1, NULL);
    xTaskCreate(receiverTask, "Receiver Task", 2048, NULL, 1, NULL);
}