#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"

namespace
{

using namespace Networking;

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

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);
}

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);
}

}

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(1, 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(nullptr));

  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);
    s_tx_inflight = true;
  }
}

}// namespace App