#include "blk_box_drivers/bbnow_old.hpp"
#include "esp_now.h"
#include "esp_log.h"
#include "esp_mac.h"
#include <memory.h>
#include <cstring>
#include <span>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"

// TODO: remove this file and the hpp file associated with it.

const static char TAG[] = "bbnow";

#define BBNOW_QUEUE_LEN   16
#define BBNOW_TASK_STACK  (4096)
#define BBNOW_TASK_PRIO   5

static QueueHandle_t g_bbnow_queue = NULL;
static TaskHandle_t  g_bbnow_task_handle = NULL;

// Wire-format bytes of the pub magic number (big-endian), used to classify
// received packets without depending on host byte order of the uint32_t.
static const uint8_t k_pub_magic_bytes[4] = {
    static_cast<uint8_t>((BBNowPubPacket::PUB_MAGIC_NUMBER >> 24) & 0xFF),
    static_cast<uint8_t>((BBNowPubPacket::PUB_MAGIC_NUMBER >> 16) & 0xFF),
    static_cast<uint8_t>((BBNowPubPacket::PUB_MAGIC_NUMBER >> 8)  & 0xFF),
    static_cast<uint8_t>((BBNowPubPacket::PUB_MAGIC_NUMBER >> 0)  & 0xFF),
};

static void espnow_send_cb(const esp_now_send_info_t *tx_info, esp_now_send_status_t status)
{
    if (tx_info == NULL) {
        ESP_LOGE(TAG, "Send cb arg error");
        return;
    }
    ESP_LOGI(TAG, "sending espnow packet of size: %d to " MACSTR, tx_info->data_len, MAC2STR(tx_info->des_addr));
}

static void espnow_recv_cb(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len)
{
    uint8_t * src_addr = recv_info->src_addr;
    uint8_t * des_addr = recv_info->des_addr;

    if (src_addr == NULL || data == NULL || len <= 0) {
        ESP_LOGE(TAG, "Receive cb arg error");
        return;
    }

    ESP_LOGI(TAG, "got espnow packet of size: %d from " MACSTR " to " MACSTR, len, MAC2STR(src_addr), MAC2STR(des_addr));

    // Hand the raw bytes to the queue; the bbnow task will classify and
    // deserialize them. If the queue isn't up yet, just drop it.
    if (g_bbnow_queue != NULL) {
        bbnow_enqueue_received(src_addr, data, static_cast<size_t>(len));
    }
}

esp_err_t bbnow_enqueue_received(const uint8_t* src, const uint8_t* data, size_t len) {
    if (g_bbnow_queue == NULL) {
        return ESP_ERR_INVALID_STATE;
    }
    if (data == NULL || len == 0 || len > ESP_NOW_MAX_DATA_LEN) {
        return ESP_ERR_INVALID_ARG;
    }

    BBNowQueueItem item{};
    item.type = BBNowQueueItemType::RX;
    item.len = len;
    memcpy(item.payload, data, len);
    if (src != NULL) {
        memcpy(item.addr, src, ESP_NOW_ETH_ALEN);
    } else {
        memcpy(item.addr, BROADCAST_MAC, ESP_NOW_ETH_ALEN);
    }

    // Classify by sniffing the leading magic number so the handler knows
    // whether to treat the payload as a pub or (future) priv packet.
    if (len >= sizeof(k_pub_magic_bytes) &&
        memcmp(data, k_pub_magic_bytes, sizeof(k_pub_magic_bytes)) == 0) {
        item.kind = BBNowPacketKind::PUB;
    } else {
        item.kind = BBNowPacketKind::PRIV;
    }

    if (xQueueSend(g_bbnow_queue, &item, pdMS_TO_TICKS(100)) != pdTRUE) {
        ESP_LOGE(TAG, "failed to enqueue received packet");
        return ESP_FAIL;
    }
    return ESP_OK;
}

esp_err_t bbnow_send_pub(const BBNowPubPacket& pkt, const uint8_t* dest) {
    if (dest == NULL) {
        dest = BROADCAST_MAC;
    }
    if (g_bbnow_queue == NULL) {
        ESP_LOGE(TAG, "bbnow not ready; call bbnow_start_task() first");
        return ESP_ERR_INVALID_STATE;
    }

    auto buf = pkt.serialize();
    if (buf.empty()) {
        ESP_LOGE(TAG, "pub packet failed to serialize");
        return ESP_ERR_INVALID_ARG;
    }
    if (buf.size() > ESP_NOW_MAX_DATA_LEN) {
        ESP_LOGE(TAG, "pub packet too large for esp_now (%u > %d)", buf.size(), ESP_NOW_MAX_DATA_LEN);
        return ESP_ERR_INVALID_SIZE;
    }

    BBNowQueueItem item{};
    item.type = BBNowQueueItemType::TX;
    item.kind = BBNowPacketKind::PUB;
    item.len = buf.size();
    memcpy(item.payload, buf.data(), buf.size());
    memcpy(item.addr, dest, ESP_NOW_ETH_ALEN);

    if (xQueueSend(g_bbnow_queue, &item, pdMS_TO_TICKS(100)) != pdTRUE) {
        ESP_LOGE(TAG, "failed to enqueue pub packet for tx");
        return ESP_FAIL;
    }
    return ESP_OK;
}

esp_err_t bbnow_send_priv(const uint8_t* dest) {
    // SCAFFOLD: BBNOW private packets are not implemented yet.
    //
    // Planned implementation mirrors `bbnow_send_pub`: serialize a
    // `BBNowPrivPacket` (once defined in blk_box_common) into `item.payload`,
    // set `item.kind = BBNowPacketKind::PRIV`, and enqueue a TX item. The
    // task and RX handler below already route `PRIV` items correctly, so only
    // this function and the `BBNowPacketKind::PRIV` branch of
    // `bbnow_handle_received` need filling in.
    (void)dest;
    ESP_LOGE(TAG, "bbnow_send_priv: not implemented");
    return ESP_ERR_NOT_SUPPORTED;
}

/// Handles a packet that arrived over esp_now. Routing on `item.kind` already
/// accounts for private packets even though their handling is still scaffolded.
static void bbnow_handle_received(const BBNowQueueItem& item) {
    switch (item.kind) {
        case BBNowPacketKind::PUB: {
            auto pkt = BBNowPubPacket::deserialize(std::span<const uint8_t>(item.payload, item.len));
            if (!pkt.has_value()) {
                ESP_LOGW(TAG, "received pub packet failed to deserialize");
                return;
            }
            ESP_LOGI(TAG, "received PUB packet: type=%d", static_cast<int>(pkt->packet_type));
            if (pkt->packet_type == BBNowPubPacketType::DISCOVERY) {
                ESP_LOGI(TAG, "  DISCOVERY expantion=%d", static_cast<int>(pkt->packet_data.discovery.expantion_type));
            }
            break;
        }
        case BBNowPacketKind::PRIV:
            // SCAFFOLD: private packet handling not implemented yet.
            ESP_LOGW(TAG, "received PRIV packet (%u bytes) -- handling not implemented", item.len);
            break;
    }
}

static void bbnow_task(void* pv) {
    (void)pv;
    BBNowQueueItem item;
    for (;;) {
        if (xQueueReceive(g_bbnow_queue, &item, portMAX_DELAY) != pdTRUE) {
            continue;
        }
        switch (item.type) {
            case BBNowQueueItemType::TX: {
                esp_err_t err = esp_now_send(item.addr, item.payload, item.len);
                if (err != ESP_OK) {
                    ESP_LOGE(TAG, "esp_now_send failed: %s", esp_err_to_name(err));
                }
                break;
            }
            case BBNowQueueItemType::RX:
                bbnow_handle_received(item);
                break;
        }
    }
}

void bbnow_start_task() {
    if (g_bbnow_queue == NULL) {
        g_bbnow_queue = xQueueCreate(BBNOW_QUEUE_LEN, sizeof(BBNowQueueItem));
        if (g_bbnow_queue == NULL) {
            ESP_LOGE(TAG, "failed to create bbnow queue");
            return;
        }
    }
    if (g_bbnow_task_handle == NULL) {
        BaseType_t rc = xTaskCreate(bbnow_task, "bbnow_task", BBNOW_TASK_STACK, NULL, BBNOW_TASK_PRIO, &g_bbnow_task_handle);
        if (rc != pdPASS) {
            ESP_LOGE(TAG, "failed to create bbnow task");
        } else {
            ESP_LOGI(TAG, "bbnow task started");
        }
    }
}

void init_espnow(void) {
    ESP_LOGI(TAG, "Intializing espnow...");
    ESP_ERROR_CHECK( esp_now_init() );
    ESP_ERROR_CHECK( esp_now_register_send_cb(espnow_send_cb) );
    ESP_ERROR_CHECK( esp_now_register_recv_cb(espnow_recv_cb) );

    /* Set primary master key. */
    // TODO: add provisions for encryption
    // ESP_ERROR_CHECK( esp_now_set_pmk(ESPNOW_PMK));

    // Add broadcast peer information to peer list, allowing us to send to it
    esp_now_peer_info_t peer = {
        .peer_addr = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF },
        .lmk = {},
        .channel = 0,
        .ifidx = WIFI_IF_AP,
        .encrypt = false,
        .priv = NULL,
    };
    ESP_ERROR_CHECK( esp_now_add_peer(&peer) );

    // Create the queue used to hand packets between callers, the esp_now
    // callbacks, and the bbnow task. The task itself is started separately
    // (see bbnow_start_task) so its lifecycle is explicit.
    if (g_bbnow_queue == NULL) {
        g_bbnow_queue = xQueueCreate(BBNOW_QUEUE_LEN, sizeof(BBNowQueueItem));
        if (g_bbnow_queue == NULL) {
            ESP_LOGE(TAG, "failed to create bbnow queue");
        }
    }
}