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

#include "lwip/netdb.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"

#include "esp_netif_now.h"

#define TAG "esp_netif_now"

static uint8_t broadcast_addr[ESP_NOW_ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF  };
esp_netif_t* netif;
void esp_netif_now_io_free_rx_buffer(void *h, void *buffer);
static esp_err_t esp_netif_now_io_transmit(void *h, void *buffer, size_t len);
static esp_err_t esp_netif_now_io_transmit_wrap(void *h, void *buffer, size_t len, void *netstack_buf);
static esp_err_t esp_netif_now_io_attach(esp_netif_t * esp_netif, void * args);

const esp_netif_driver_ifconfig_t esp_netif_now_ifconfig = {
  .driver_free_rx_buffer = esp_netif_now_io_free_rx_buffer,
  .transmit = esp_netif_now_io_transmit,
  .transmit_wrap = esp_netif_now_io_transmit_wrap,
  .handle = "esp-netif-now"
};

const esp_netif_driver_base_t esp_netif_now_driver_base = {
  .post_attach = esp_netif_now_io_attach
};

void esp_netif_now_io_free_rx_buffer(void *h, void *buffer) {
  // TODO
}

esp_err_t esp_netif_now_add_peer(uint8_t *mac) {
  esp_now_peer_info_t *peer = malloc(sizeof(esp_now_peer_info_t));
  if (peer == NULL) {
    return -1;
  }

  memset(peer, 0, sizeof(esp_now_peer_info_t));
  peer->channel = 1;
  peer->ifidx = ESP_IF_WIFI_STA;
  memcpy(peer->peer_addr, mac, ESP_NOW_ETH_ALEN);

  // check for some conditions
  esp_err_t ret = esp_now_add_peer(peer);

  free(peer);

  return ret;
}

static esp_err_t esp_netif_now_io_transmit(void *h, void *buffer, size_t len) {
  uint8_t *buf = buffer;

  uint8_t peer[6];
  memcpy(&peer, buf, 6);

  ESP_LOGI(TAG, "sending %d bytes to: %02X:%02X:%02X:%02X:%02X:%02X", len, peer[0], peer[1], peer[2], peer[3], peer[4], peer[5]);
  for(uint8_t i = 0; i < len; i++) {
    printf("0x%02X ", buf[i]);
    if(i % 16 == 15) printf("\n");
  }
  printf("\n");

  esp_err_t ret = ESP_OK;

  ret = esp_now_send(peer, buffer, len);

  if(ret == ESP_ERR_ESPNOW_NOT_FOUND) {
    ESP_LOGE("esp-netif-now", "ESP-Now peer not found");
  } else if(ret == ESP_ERR_ESPNOW_INTERNAL) {
    ESP_LOGE("esp-netif-now", "ESP-Now experienced an internal error");
  } else if(ret != ESP_OK) {
    ESP_ERROR_CHECK(ret);
  }

  return ret;
}

static esp_err_t esp_netif_now_io_transmit_wrap(void *h, void *buffer, size_t len, void *netstack_buf) {
  return  esp_netif_now_io_transmit(h, buffer, len);
}

static esp_err_t esp_netif_now_io_attach(esp_netif_t * esp_netif, void * args) {
  ESP_ERROR_CHECK(esp_netif_set_driver_config(esp_netif, &esp_netif_now_ifconfig));

  return ESP_OK;
}

void * esp_netif_now_new_handler(void) {
  return (void*)&esp_netif_now_driver_base;
}

static void esp_netif_now_recv_cb(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len) {
  // TODO
  // * Handle MAC related, peering, broadcast, etc
  // * Pipe received data into LwIP

  uint8_t peer[6];
  memcpy(&peer, recv_info->src_addr, 6);

  ESP_LOGI(TAG, "receiving %d bytes from: %02X:%02X:%02X:%02X:%02X:%02X", len, peer[0], peer[1], peer[2], peer[3], peer[4], peer[5]);

  esp_netif_now_add_peer((uint8_t*)&peer);

  for(uint8_t i = 0; i < len; i++) {
    printf("0x%02X ", data[i]);
    if(i % 16 == 15) printf("\n");
  }

  printf("\n");

  esp_netif_receive(netif, (void*)data, len, NULL);
}

static void esp_netif_now_send_cb(const uint8_t *mac_addr, esp_now_send_status_t status) {
  // TODO free any memory allocated during io_transmit
}


esp_err_t esp_netif_now_init(esp_netif_now_config_t *netif_now_config) {
  esp_netif_inherent_config_t netif_common_config = {
    .flags = ESP_NETIF_FLAG_AUTOUP,
    .ip_info = &netif_now_config->ip_info,
    .if_key = "TEST",
    .if_desc = "net_test_if"
  };

  esp_netif_config_t config = {
    .base = &netif_common_config,                 // use specific behaviour configuration
    .stack = ESP_NETIF_NETSTACK_DEFAULT_WIFI_STA, // use default WIFI-like network stack configuration
  };

  ESP_LOGI(TAG, "esp_now_init()");
  ESP_ERROR_CHECK(esp_now_init());
  ESP_ERROR_CHECK(esp_now_register_recv_cb(esp_netif_now_recv_cb));
  ESP_ERROR_CHECK(esp_now_register_send_cb(esp_netif_now_send_cb));
  esp_netif_now_add_peer(broadcast_addr);

  // Netif creation and configuration
  //
  netif = esp_netif_new(&config);

  esp_netif_attach(netif, esp_netif_now_new_handler());

  // Start the netif in a manual way, no need for events
  //
  esp_netif_set_mac(netif, netif_now_config->mac);
  esp_netif_action_start(netif, NULL, 0, NULL);

  return ESP_OK;
}