#include "EspNowCrsf.h"
#include <WiFi.h>
#include <esp_wifi.h>
#include <esp_now.h>

namespace EspNowCrsf {

// ---- Ring buffer dei frame CRSF ----------------------------
static const uint8_t  RING_SLOTS = 8;
static const uint16_t SLOT_SIZE  = 64;

struct Slot { uint8_t data[SLOT_SIZE]; uint16_t len; };
static Slot         ring[RING_SLOTS];
static volatile uint8_t  head = 0;   // scritto dalla callback
static volatile uint8_t  tail = 0;   // letto dal loop
static portMUX_TYPE      mux  = portMUX_INITIALIZER_UNLOCKED;

static volatile uint32_t s_received = 0;
static volatile uint32_t s_dropped  = 0;

static uint8_t s_uid[6];

// ---- Callback ricezione (firma arduino-esp32 core 2.x) -----
static void onRecv(const uint8_t* mac, const uint8_t* in, int len) {
    if (len <= 8 || in == nullptr) return;
    // keep-alive: type/payload a zero subito dopo l'header di 8 byte
    // (len > 9 prima degli accessi: con len==9 leggere in[9] sarebbe OOB)
    if (len > 9 && in[8] == 0 && in[9] == 0) return;

    uint16_t crsfLen = len - 8;
    if (crsfLen > SLOT_SIZE) crsfLen = SLOT_SIZE;

    portENTER_CRITICAL_ISR(&mux);
    uint8_t next = (head + 1) % RING_SLOTS;
    if (next == tail) {              // pieno -> scarta il piu' vecchio
        tail = (tail + 1) % RING_SLOTS;
        s_dropped++;
    }
    memcpy(ring[head].data, in + 8, crsfLen);
    ring[head].len = crsfLen;
    head = next;
    s_received++;
    portEXIT_CRITICAL_ISR(&mux);
}

void begin(const uint8_t uid[6]) {
    memcpy(s_uid, uid, 6);
    s_uid[0] &= ~0x01;               // MAC unicast

    WiFi.mode(WIFI_STA);
    WiFi.disconnect();
    esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B);
    esp_wifi_set_mac(WIFI_IF_STA, s_uid);
    esp_wifi_start();
    esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE);

    if (esp_now_init() != ESP_OK) return;
    esp_now_register_recv_cb(onRecv);
}

bool poll(uint8_t* out, uint16_t* outLen, uint16_t maxLen) {
    bool got = false;
    portENTER_CRITICAL(&mux);
    if (tail != head) {
        uint16_t n = ring[tail].len;
        if (n > maxLen) n = maxLen;
        memcpy(out, ring[tail].data, n);
        *outLen = n;
        tail = (tail + 1) % RING_SLOTS;
        got = true;
    }
    portEXIT_CRITICAL(&mux);
    return got;
}

uint32_t framesReceived() { return s_received; }
uint32_t framesDropped()  { return s_dropped; }

#ifdef UNIT_TEST
void test_injectRaw(const uint8_t* data, uint16_t len) { onRecv(nullptr, data, len); }

void test_reset() {
    portENTER_CRITICAL(&mux);
    head = 0;
    tail = 0;
    s_received = 0;
    s_dropped = 0;
    portEXIT_CRITICAL(&mux);
}
#endif

} // namespace EspNowCrsf