#include "blk_box_drivers/bbnow.hpp"
#include "esp_now.h"
#include "esp_log.h"
#include "esp_mac.h"
#include <memory.h>

const static char TAG[] = "bbnow";

static void example_espnow_send_cb(const esp_now_send_info_t *tx_info, esp_now_send_status_t status);
static void example_espnow_recv_cb(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len);

/* ESPNOW sending or receiving callback function is called in WiFi task.
 * Users should not do lengthy operations from this task. Instead, post
 * necessary data to a queue and handle it from a lower priority task. */
static void example_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 example_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, MAC2STR(src_addr), MAC2STR(des_addr));

    // if (IS_BROADCAST_ADDR(des_addr)) {
    //     /* If added a peer with encryption before, the receive packets may be
    //      * encrypted as peer-to-peer message or unencrypted over the broadcast channel.
    //      * Users can check the destination address to distinguish it.
    //      */
    //     ESP_LOGD(TAG, "Receive broadcast ESPNOW data");
    // } else {
    //     ESP_LOGD(TAG, "Receive unicast ESPNOW data");
    // }
}


// TODO: add "initializing _____..."
// and "____ initialized!" logs to all driver init functions.
void init_espnow(void) {
    ESP_ERROR_CHECK( esp_now_init() );
    ESP_ERROR_CHECK( esp_now_register_send_cb(example_espnow_send_cb) );
    ESP_ERROR_CHECK( esp_now_register_recv_cb(example_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 = {0};
    peer.channel = BBNOW_DEFAULT_CHANNEL;
    peer.ifidx = WIFI_IF_AP;
    peer.encrypt = false;
    memcpy(peer.peer_addr, BROADCAST_MAC, ESP_NOW_ETH_ALEN);
    ESP_ERROR_CHECK( esp_now_add_peer(&peer) );
}