#include "app_networking.hpp"

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

#include "esp_event.h"
#include "esp_mac.h"
#include "esp_netif.h"
#include "esp_now.h"
#include "esp_wifi.h"
#include "esp_log.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, CONFIG_ESPTNL_BUFFER_SIZE> s_rx_buffer;
std::variant<MacAddress, std::monostate> s_peer;

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

constexpr MacAddress s_getBroadcastAddress()
{
  return MacAddress{ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
}

esp_err_t s_addPeer(const MacAddress& peer_address)
{
  esp_now_peer_info_t broadcast_peer;
  std::memset(&broadcast_peer, 0, sizeof(broadcast_peer));

  broadcast_peer.channel = CONFIG_ESPTNL_CHANNEL;
  broadcast_peer.ifidx = WIFI_IF_STA;
  broadcast_peer.encrypt = false;
  std::memcpy(broadcast_peer.lmk, CONFIG_ESPTNL_LMK, ESP_NOW_KEY_LEN);
  std::memcpy(broadcast_peer.peer_addr, peer_address.data(), peer_address.size());

  return esp_now_add_peer(&broadcast_peer);
}

void s_cbEspNowSendComplete(const std::uint8_t* peer_mac, esp_now_send_status_t status)
{
  EspNowEvent event = { EspNowEvent::MSG_SEND_COMPLETE, 0 };
  xQueueSend(s_esp_now_queue, &event, 0);
  ESP_LOGD(LOG_TAG, "Send complete.");
}

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(LOG_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));

  const auto broadcast_mac = s_getBroadcastAddress();
  s_peer = broadcast_mac;

  ESP_ERROR_CHECK(s_addPeer(broadcast_mac));

  return s_esp_now_queue;
}

void sendHello()
{
  MacAddress mac;
  ESP_ERROR_CHECK(esp_read_mac(mac.data(), ESP_MAC_WIFI_STA));

  char buffer[100];
  const std::size_t length = std::sprintf(buffer, "Hello from %02X:%02X:%02X:%02X:%02X:%02X.\n\r",
    mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);

  const auto broadcast_mac = s_getBroadcastAddress();
  ESP_ERROR_CHECK(esp_now_send(broadcast_mac.data(), (uint8_t*)buffer, length));
}

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

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

    ESP_LOGD(LOG_TAG, "Message send started. Length: %u", length);
  }
  else
  {
    ESP_LOGW(LOG_TAG, "Dropped message.");
  }
}

}// namespace App