#include "app_networking.hpp"

#include <algorithm>
#include <cstring>
#include <variant>

#include "esp_event.h"
#include "esp_mac.h"
#include "esp_netif.h"
#include "esp_now.h"
#include "esp_wifi.h"
#include "esp_log.h"

namespace
{

using namespace Networking;

static const char* TAG = "Networking";

static_assert(std::is_standard_layout_v<EspNowEvent> && std::is_trivial_v<EspNowEvent>,
        "EspNowEvent is not compatible with a FreeRTOS queue.");

template<std::size_t N>
constexpr auto STR_LEN(const char (&s)[N])
{
  return N;
}

static_assert(STR_LEN(CONFIG_ESPTNL_PMK) == 16 + 1, "CONFIG_ESPTNL_PMK must be of length 16 bytes + 1 null terminator.");
static_assert(STR_LEN(CONFIG_ESPTNL_LMK) == 16 + 1, "CONFIG_ESPTNL_LMK must be of length 16 bytes + 1 null terminator.");

QueueHandle_t s_esp_now_queue = nullptr;
std::array<std::uint8_t, 128> s_rx_buffer;
std::variant<MacAddress, std::monostate> s_peer;
bool s_tx_inflight = false;

bool s_isBroadcastAddress(const std::uint8_t* mac)
{
  constexpr MacAddress broadcast_mac{ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF  };
  return std::memcmp(mac, broadcast_mac.begin(), ESP_NOW_ETH_ALEN) == 0;
}

bool s_isBroadcastAddress(const esp_now_recv_info_t* sender)
{
  return s_isBroadcastAddress(sender->src_addr);
}

void s_cbEspNowSendComplete(const std::uint8_t* peer_mac, esp_now_send_status_t status)
{
  s_tx_inflight = false;

  EspNowEvent event = { EspNowEvent::MSG_SEND_COMPLETE, 0 };
  xQueueSend(s_esp_now_queue, &event, 0);
  ESP_LOGD(TAG, "Send complete. s_tx_inflight = false.");
}

void s_cbEspNowReceiveComplete(const esp_now_recv_info_t* recv_info, const std::uint8_t* data, int len)
{
  const std::size_t rx_len = std::min<std::size_t>(len, s_rx_buffer.size());
  std::copy_n(data, rx_len, s_rx_buffer.begin());

  EspNowEvent event = { EspNowEvent::MSG_RECEIVED, rx_len };
  xQueueSend(s_esp_now_queue, &event, 0);
  ESP_LOGD(TAG, "Received message. Length: %d.", len);
}

}

namespace Networking
{

void setupWifi()
{
  ESP_ERROR_CHECK(esp_netif_init());
  ESP_ERROR_CHECK(esp_event_loop_create_default());

  wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();

  ESP_ERROR_CHECK(esp_wifi_init(&cfg));
  ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM));
  ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
  ESP_ERROR_CHECK(esp_wifi_start());
  ESP_ERROR_CHECK(esp_wifi_set_channel(CONFIG_ESPTNL_CHANNEL, WIFI_SECOND_CHAN_NONE));
}

QueueHandle_t setupEspNow()
{
  s_esp_now_queue = xQueueCreate(4, sizeof(EspNowEvent));
  ESP_ERROR_CHECK(s_esp_now_queue == nullptr);

  ESP_ERROR_CHECK(esp_now_init());
  ESP_ERROR_CHECK(esp_now_set_pmk((const std::uint8_t*)CONFIG_ESPTNL_PMK));

  ESP_ERROR_CHECK(esp_now_register_send_cb(s_cbEspNowSendComplete));
  ESP_ERROR_CHECK(esp_now_register_recv_cb(s_cbEspNowReceiveComplete));

  s_peer = MacAddress{ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };

  return s_esp_now_queue;
}

const std::array<std::uint8_t, 128>& getRxBuffer()
{
  return s_rx_buffer;
}

void sendData(const std::array<std::uint8_t, 128>& buffer, const std::size_t length)
{
  if (const MacAddress* peer_mac = std::get_if<MacAddress>(&s_peer);
      !s_tx_inflight && peer_mac != nullptr)
  {
    esp_now_send(peer_mac->data(), buffer.data(), length);
    if (!s_isBroadcastAddress(peer_mac->data()))
      s_tx_inflight = true;

    ESP_LOGD(TAG, "Message send started. s_tx_inflight = %d.", s_tx_inflight);
  }
  else
  {
    ESP_LOGW(TAG, "Dropped message.");
  }
}

}// namespace App