#include <STM32FreeRTOS.h>

#define SENSOR_COUNT 3

typedef struct {
    uint16_t values[SENSOR_COUNT];
    uint32_t timestamp;
} SensorData_t;

static QueueHandle_t xSensorQueue;

void vProducerTask(void *pvParameters)
{
    SensorData_t sample;

    for (;;)
    {
        sample.values[0] = analogRead(A0);
        sample.values[1] = analogRead(A1);
        sample.values[2] = analogRead(A2);
        sample.timestamp = xTaskGetTickCount();

        if (xQueueSend(xSensorQueue, &sample, 0) != pdPASS)
        {
            Serial.println("Queue full, sample dropped");
        }

        vTaskDelay(pdMS_TO_TICKS(400));
    }
}


void vConsumerTask(void *pvParameters)
{
    SensorData_t received;

    for (;;)
    {
        if (xQueueReceive(xSensorQueue, &received, portMAX_DELAY) == pdPASS)
        {
            uint32_t sum = 0;
            for (int i = 0; i < SENSOR_COUNT; i++)
                sum += received.values[i];

            float avg = (float)sum / SENSOR_COUNT;

            Serial.print("t=");
            Serial.print(received.timestamp);
            Serial.print(" Avg=");
            Serial.print(avg);
            Serial.print(" raw: ");
            Serial.print(received.values[0]); Serial.print(" ");
            Serial.print(received.values[1]); Serial.print(" ");
            Serial.println(received.values[2]);
        }
    }
}


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

    xSensorQueue = xQueueCreate(5, sizeof(SensorData_t));

    xTaskCreate(vProducerTask, "Producer", configMINIMAL_STACK_SIZE + 128,
                NULL, tskIDLE_PRIORITY + 2, NULL);

    xTaskCreate(vConsumerTask, "Consumer", configMINIMAL_STACK_SIZE + 128,
                NULL, tskIDLE_PRIORITY + 1, NULL);

    vTaskStartScheduler();
}


void loop()
{
    // Scheduler has taken over; this is never reached.
    // Must exist so vApplicationIdleHook() links correctly.
}