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

#define READY_DEPTH  64          

typedef struct {
  uint16_t len;
  uint8_t *data;
} Frame;

QueueHandle_t readyQueue;       

static inline uint32_t tickSnapshot() { return SysTick->VAL; }
static inline uint32_t cyclesSince(uint32_t start) {
  uint32_t now  = SysTick->VAL;
  uint32_t load = SysTick->LOAD;
  return (start >= now) ? (start - now) : (start + (load - now) + 1);
}

static uint32_t allocMin = 0xFFFFFFFF, allocMax = 0;


void producer(void *pv) {
  int frame = 0;
  for (;;) {
    ++frame;
   
    uint16_t len = 32 + ((frame * 40) % 480);      // 32..511 bytes fake data generator

    uint32_t t0   = tickSnapshot();
    uint8_t *data = (uint8_t *) pvPortMalloc(len);
    uint32_t dt   = cyclesSince(t0);               // cost of THIS allocation

    if (!data) {
      Serial.print("!! malloc FAILED for frame "); Serial.print(frame);
      Serial.print("   free heap = "); Serial.println(xPortGetFreeHeapSize());
      vTaskDelay(pdMS_TO_TICKS(30));
      continue;
    }

    if (dt < allocMin) allocMin = dt;
    if (dt > allocMax) allocMax = dt;
    memset(data, frame & 0xFF, len);               // "capture" the frame

    Frame f = { len, data };
    xQueueSend(readyQueue, &f, portMAX_DELAY);      // hand it downstream

    uint32_t us = dt / (SystemCoreClock / 1000000UL);
    Serial.print("produced frame "); Serial.print(frame);
    Serial.print("  len ");     Serial.print(len);
    Serial.print("  malloc ");  Serial.print(dt);  Serial.print(" cyc (~");
    Serial.print(us);           Serial.print(" us)  [min ");
    Serial.print(allocMin);     Serial.print(" / max ");
    Serial.print(allocMax);     Serial.print("]  freeHeap ");
    Serial.println(xPortGetFreeHeapSize());

    vTaskDelay(pdMS_TO_TICKS(30));                  // fast: a new frame every ~30 ms
  }
}

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

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

    vPortFree(f.data);                             // release the buffer
    Serial.print("   consumed frame   backlog ");
    Serial.print(uxQueueMessagesWaiting(readyQueue));
    Serial.print("   freeHeap ");
    Serial.println(xPortGetFreeHeapSize());
  }
}

void setup() {
  Serial.begin(115200);
  readyQueue = xQueueCreate(READY_DEPTH, sizeof(Frame));

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