#include <STM32FreeRTOS.h>
#include <queue.h>

// ---- The fixed memory bank: N identical buffers, allocated ONCE at build time.
// No malloc in the hot path -> deterministic timing, no heap fragmentation.
#define POOL_SIZE  4
#define FRAME_LEN  32
static uint8_t bufferBank[POOL_SIZE][FRAME_LEN];   // the actual RAM, reserved up front

// Two queues of buffer POINTERS turn that RAM into a managed pool:
QueueHandle_t freePool;    // buffers ready to be filled   (starts FULL: all N)
QueueHandle_t readyQueue;  // buffers filled, awaiting work (starts EMPTY)

static int inUse() { return POOL_SIZE - uxQueueMessagesWaiting(freePool); }

// PRODUCER: a fast sensor task capturing frames.
void producer(void *pv) {
  int frame = 0;
  for (;;) {
    uint8_t *buf;
    // Claim a free buffer. BLOCKS here if every buffer is in flight (pool empty).
    xQueueReceive(freePool, &buf, portMAX_DELAY);

    memset(buf, ++frame & 0xFF, FRAME_LEN);        // "capture" a frame into our buffer
    Serial.print("produced frame "); Serial.print(frame);
    Serial.print("   -> buffers in use = "); Serial.println(inUse());

    xQueueSend(readyQueue, &buf, portMAX_DELAY);   // hand the filled buffer downstream
    vTaskDelay(pdMS_TO_TICKS(30));                  // fast: a new frame every ~30 ms
  }
}

// CONSUMER: a slow processing task (the bottleneck).
void consumer(void *pv) {
  for (;;) {
    uint8_t *buf;
    xQueueReceive(readyQueue, &buf, portMAX_DELAY);  // BLOCKS if nothing is ready

    vTaskDelay(pdMS_TO_TICKS(100));                   // slow: 100 ms to process a frame

    xQueueSend(freePool, &buf, portMAX_DELAY);        // return the buffer to the pool
    Serial.print("   consumed frame    -> buffers in use = "); Serial.println(inUse());
  }
}

void setup() {
  Serial.begin(115200);
  freePool   = xQueueCreate(POOL_SIZE, sizeof(uint8_t *));
  readyQueue = xQueueCreate(POOL_SIZE, sizeof(uint8_t *));

  // Seed the pool: every buffer starts free.
  for (int i = 0; i < POOL_SIZE; i++) {
    uint8_t *p = bufferBank[i];
    xQueueSend(freePool, &p, 0);
  }

  xTaskCreate(producer, "P", 256, NULL, 1, NULL);
  xTaskCreate(consumer, "C", 256, NULL, 1, NULL);
  vTaskStartScheduler();
}
void loop() {}