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

// ---- Frame Pipeline Design Notes (v2).
// Frames are allocated on demand,
// so there's no POOL_SIZE to tune, frames can be any length, and the heap
// gives us plenty of room to buffer bursts. .

#define READY_DEPTH  64          // generous backlog depth -- the heap can back it

// A frame.
typedef struct {
  uint16_t len;
  uint8_t *data;
} Frame;

QueueHandle_t readyQueue;        // filled frames awaiting work

// --- cycle-accurate stopwatch (Cortex-M DWT) so we can watch per-call cost ---
static inline void dwtInit() {
  CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
  DWT->CYCCNT = 0;
  DWT->CTRL  |= DWT_CTRL_CYCCNTENA_Msk;
}
static inline uint32_t cycles() { return DWT->CYCCNT; }

static uint32_t allocMin = 0xFFFFFFFF, allocMax = 0;

// PRODUCER: a fast sensor task capturing variable-length frames.
void producer(void *pv) {
  int frame = 0;
  for (;;) {
    ++frame;
    // Variable frame size, as if capturing compressed/variable data.
    uint16_t len = 32 + ((frame * 40) % 480);      // 32..511 bytes

    // ---- grab this frame's buffer right here, in the capture loop ----
    uint32_t t0   = cycles();
    uint8_t *data = (uint8_t *) pvPortMalloc(len);
    uint32_t dt   = cycles() - 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);
  dwtInit();
  readyQueue = xQueueCreate(READY_DEPTH, sizeof(Frame));

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