#include <iostream>

#include <stdlib.h>
#include <time.h>
#include <string.h>
#include <assert.h>
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "freertos/timers.h"
#include "nvs_flash.h"
#include "esp_event.h"
#include "esp_netif.h"
#include "esp_wifi.h"
#include "esp_log.h"
#include "esp_system.h"
#include "esp_now.h"
#include "esp_crc.h"
#include "espnow_example.h"
#define LED (GPIO_NUM_2)
static const char *TAG = "espnow_example";
static xQueueHandle s_example_espnow_queue;
#ifndef ESPNOW_EXAMPLE_H
#define ESPNOW_EXAMPLE_H

#if CONFIG_ESPNOW_WIFI_MODE_STATION
#define ESPNOW_WIFI_MODE WIFI_MODE_STA
#define ESPNOW_WIFI_IF ESP_IF_WIFI_STA
#else
#define ESPNOW_WIFI_MODE WIFI_MODE_AP
#define ESPNOW_WIFI_IF WIFI_IF_STA
#endif
#define ESPNOW_QUEUE_SIZE 6

#define IS_BROADCAST_ADDR(addr) (memcmp(addr, s_example_broadcast_mac, ESP_NOW_ETH_ALEN) == 0)

typedef enum
{
  EXAMPLE_ESPNOW_SEND_CB,
  EXAMPLE_ESPNOW_RECV_CB,
} example_espnow_event_id_t;
enum
{
  EXAMPLE_ESPNOW_DATA_BROADCAST,
  EXAMPLE_ESPNOW_DATA_UNICAST,
  EXAMPLE_ESPNOW_DATA_MAX,
};
/* User defined field of ESPNOW data in this example. */
typedef struct
{
  uint8_t type;     //Broadcast or unicast ESPNOW data.
  uint8_t state;    //Indicate that if has received broadcast ESPNOW data or not.
  uint16_t seq_num; //Sequence number of ESPNOW data.
  uint16_t crc;     //CRC16 value of ESPNOW data.
  uint32_t magic;   //Magic number which is used to determine which device to send unicast ESPNOW data.
  char payload[0];  //Real payload of ESPNOW data.
} __attribute__((packed)) example_espnow_data_t;

/* Parameters of sending ESPNOW data. */
typedef struct
{
  uint8_t mac_addr[ESP_NOW_ETH_ALEN];
  esp_now_send_status_t status;
} example_espnow_event_send_cb_t;
typedef struct
{
  uint8_t mac_addr[ESP_NOW_ETH_ALEN];
  uint8_t *data;
  int data_len;
} example_espnow_event_recv_cb_t;
typedef union
{
  example_espnow_event_send_cb_t send_cb;
  example_espnow_event_recv_cb_t recv_cb;
} example_espnow_event_info_t;

/* When ESPNOW sending or receiving callback function is called, post event to ESPNOW task. */
typedef struct
{
  example_espnow_event_id_t id;
  example_espnow_event_info_t info;
} example_espnow_event_t;
typedef struct
{
  bool unicast;                       //Send unicast ESPNOW data.
  bool broadcast;                     //Send broadcast ESPNOW data.
  uint8_t state;                      //Indicate that if has received broadcast ESPNOW data or not.
  uint32_t magic;                     //Magic number which is used to determine which device to send unicast ESPNOW data.
  uint16_t count;                     //Total count of unicast ESPNOW data to be sent.
  uint16_t delay;                     //Delay between sending two ESPNOW data, unit: ms.
  int len;                            //Length of ESPNOW data to be sent, unit: byte.
  uint8_t *buffer;                    //Buffer pointing to ESPNOW data.
  uint8_t dest_mac[ESP_NOW_ETH_ALEN]; //MAC address of destination device.
} example_espnow_send_param_t;

#endif

static uint8_t s_example_broadcast_mac[ESP_NOW_ETH_ALEN] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
static uint16_t s_example_espnow_seq[EXAMPLE_ESPNOW_DATA_MAX] = {0, 0};

extern "C" void example_espnow_deinit(example_espnow_send_param_t *send_param);

/* WiFi should start before using ESPNOW */
extern "C" void example_wifi_init(void)
{
  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(ESPNOW_WIFI_MODE));
  ESP_ERROR_CHECK(esp_wifi_start());

#if CONFIG_ESPNOW_ENABLE_LONG_RANGE
  ESP_ERROR_CHECK(esp_wifi_set_protocol(ESPNOW_WIFI_IF, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR));
#endif
}

/* 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. */
extern "C" void example_espnow_send_cb(const uint8_t *mac_addr, esp_now_send_status_t status)
{
  example_espnow_event_t evt;
  example_espnow_event_send_cb_t *send_cb = &evt.info.send_cb;

  if (mac_addr == NULL)
  {
    ESP_LOGE(TAG, "Send cb arg error");
    return;
  }

  evt.id = EXAMPLE_ESPNOW_SEND_CB;
  memcpy(send_cb->mac_addr, mac_addr, ESP_NOW_ETH_ALEN);
  send_cb->status = status;
  if (xQueueSend(s_example_espnow_queue, &evt, ESPNOW_MAXDELAY) != pdTRUE)
  {
    ESP_LOGW(TAG, "Send send queue fail");
  }
}

extern "C" void example_espnow_recv_cb(const uint8_t *mac_addr, const uint8_t *data, int len)
{
  example_espnow_event_t evt;
  example_espnow_event_recv_cb_t *recv_cb = &evt.info.recv_cb;

  if (mac_addr == NULL || data == NULL || len <= 0)
  {
    ESP_LOGE(TAG, "Receive cb arg error");
    return;
  }

  evt.id = EXAMPLE_ESPNOW_RECV_CB;
  memcpy(recv_cb->mac_addr, mac_addr, ESP_NOW_ETH_ALEN);
  recv_cb->data = (uint8_t *)malloc(len);
  if (recv_cb->data == NULL)
  {
    ESP_LOGE(TAG, "Malloc receive data fail");
    return;
  }
  //ESP_LOGE(TAG, "This is data:%u", *data);
  memcpy(recv_cb->data, data, len);

  recv_cb->data_len = len;
  if (xQueueSend(s_example_espnow_queue, &evt, ESPNOW_MAXDELAY) != pdTRUE)
  {
    ESP_LOGW(TAG, "Send receive queue fail");
    free(recv_cb->data);
  }
}

/* Parse received ESPNOW data. */
extern "C" int example_espnow_data_parse(uint8_t *data, uint16_t data_len, uint8_t *state, uint16_t *seq, int *magic)
{
  example_espnow_data_t *buf = (example_espnow_data_t *)data;
  uint16_t crc, crc_cal = 0;

  if (data_len < sizeof(example_espnow_data_t))
  {
    ESP_LOGE(TAG, "Receive ESPNOW data too short, len:%d", data_len);
    return -1;
  }

  *state = buf->state;
  *seq = buf->seq_num;
  *magic = buf->magic;

  crc = buf->crc;
  buf->crc = 0;
  crc_cal = esp_crc16_le(UINT16_MAX, (uint8_t const *)buf, data_len);
  ESP_LOGE(TAG, "This is seq_num:%d", buf->seq_num);
  ESP_LOGE(TAG, "This is data:%c", buf->payload[1]);
  ESP_LOGE(TAG, "This is data:%u", sizeof(buf->payload[4]));
  ESP_LOGE(TAG, "This is magic:%d", buf->magic);
  if (crc_cal == crc)
  {
    return buf->type;
  }

  return -1;
}

/* Prepare ESPNOW data to be sent. */
extern "C" void example_espnow_data_prepare(example_espnow_send_param_t *send_param)
{
  example_espnow_data_t *buf = (example_espnow_data_t *)send_param->buffer;

  assert(send_param->len >= sizeof(example_espnow_data_t));

  buf->type = IS_BROADCAST_ADDR(send_param->dest_mac) ? EXAMPLE_ESPNOW_DATA_BROADCAST : EXAMPLE_ESPNOW_DATA_UNICAST;
  buf->state = send_param->state;
  buf->seq_num = s_example_espnow_seq[buf->type]++;
  buf->crc = 0;
  buf->magic = send_param->magic;
  buf->payload[0] = 'a';
  buf->payload[1] = 'b';

  /* Fill all remaining bytes after the data with random values */
  //ESP_LOGI(TAG, "This is len%d", sizeof(example_espnow_data_t));
  //esp_fill_random(buf->payload, send_param->len - sizeof(example_espnow_data_t));

  buf->crc = esp_crc16_le(UINT16_MAX, (uint8_t const *)buf, send_param->len);
}

extern "C" void example_espnow_task(void *pvParameter)
{
  example_espnow_event_t evt;
  uint8_t recv_state = 0;
  uint16_t recv_seq = 0;
  int recv_magic = 0;
  bool is_broadcast = false;
  int ret;

  vTaskDelay(5000 / portTICK_RATE_MS);
  ESP_LOGI(TAG, "Start sending broadcast data");

  /* Start sending broadcast ESPNOW data. */
  example_espnow_send_param_t *send_param = (example_espnow_send_param_t *)pvParameter;
  ESP_LOGI(TAG, "MAC : %d", *send_param->dest_mac);
  ESP_LOGI(TAG, "MAC : %d", *send_param->buffer);
  ESP_LOGI(TAG, "MAC : %d", send_param->len);
  ESP_LOGI(TAG, "MAC : %d", esp_now_send(send_param->dest_mac, send_param->buffer, send_param->len));
  if (esp_now_send(send_param->dest_mac, send_param->buffer, send_param->len) != ESP_OK)
  {
    ESP_LOGE(TAG, "Send error");
    example_espnow_deinit(send_param);
    vTaskDelete(NULL);
  }

  while (xQueueReceive(s_example_espnow_queue, &evt, portMAX_DELAY) == pdTRUE)
  {
    switch (evt.id)
    {
    case EXAMPLE_ESPNOW_SEND_CB:
    {
      example_espnow_event_send_cb_t *send_cb = &evt.info.send_cb;
      is_broadcast = IS_BROADCAST_ADDR(send_cb->mac_addr);

      ESP_LOGD(TAG, "Send data to " MACSTR ", status1: %d", MAC2STR(send_cb->mac_addr), send_cb->status);

      if (is_broadcast && (send_param->broadcast == false))
      {
        break;
      }

      if (!is_broadcast)
      {
        send_param->count--;
        if (send_param->count == 0)
        {
          ESP_LOGI(TAG, "Send done");
          example_espnow_deinit(send_param);
          vTaskDelete(NULL);
        }
      }

      /* Delay a while before sending the next data. */
      if (send_param->delay > 0)
      {
        vTaskDelay(send_param->delay / portTICK_RATE_MS);
      }

      ESP_LOGI(TAG, "send data to " MACSTR "", MAC2STR(send_cb->mac_addr));

      memcpy(send_param->dest_mac, send_cb->mac_addr, ESP_NOW_ETH_ALEN);
      example_espnow_data_prepare(send_param);

      /* Send the next data after the previous data is sent. */
      if (esp_now_send(send_param->dest_mac, send_param->buffer, send_param->len) != ESP_OK)
      {
        ESP_LOGE(TAG, "Send error");
        example_espnow_deinit(send_param);
        vTaskDelete(NULL);
      }
      break;
    }
    case EXAMPLE_ESPNOW_RECV_CB:
    {
      example_espnow_event_recv_cb_t *recv_cb = &evt.info.recv_cb;

      ret = example_espnow_data_parse(recv_cb->data, recv_cb->data_len, &recv_state, &recv_seq, &recv_magic);

      free(recv_cb->data);
      if (ret == EXAMPLE_ESPNOW_DATA_BROADCAST)
      {
        ESP_LOGI(TAG, "Receive %dth broadcast data from: " MACSTR ", len: %d", recv_seq, MAC2STR(recv_cb->mac_addr), recv_cb->data_len);

        /* If MAC address does not exist in peer list, add it to peer list. */
        if (esp_now_is_peer_exist(recv_cb->mac_addr) == false)
        {
          esp_now_peer_info_t *peer = (esp_now_peer_info_t *)malloc(sizeof(esp_now_peer_info_t));
          if (peer == NULL)
          {
            ESP_LOGE(TAG, "Malloc peer information fail");
            example_espnow_deinit(send_param);
            vTaskDelete(NULL);
          }
          memset(peer, 0, sizeof(esp_now_peer_info_t));
          peer->channel = CONFIG_ESPNOW_CHANNEL;
          peer->ifidx = ESPNOW_WIFI_IF;
          peer->encrypt = true;
          memcpy(peer->lmk, CONFIG_ESPNOW_LMK, ESP_NOW_KEY_LEN);
          memcpy(peer->peer_addr, recv_cb->mac_addr, ESP_NOW_ETH_ALEN);
          ESP_ERROR_CHECK(esp_now_add_peer(peer));
          free(peer);
        }

        /* Indicates that the device has received broadcast ESPNOW data. */
        if (send_param->state == 0)
        {
          send_param->state = 1;
        }
      }
      else if (ret == EXAMPLE_ESPNOW_DATA_UNICAST)
      {
        ESP_LOGI(TAG, "Receive %dth unicast data from: " MACSTR ", len: %d", recv_seq, MAC2STR(recv_cb->mac_addr), recv_cb->data_len);

        /* If receive unicast ESPNOW data, also stop sending broadcast ESPNOW data. */
        send_param->broadcast = false;
      }
      else
      {
        ESP_LOGI(TAG, "Receive error data from: " MACSTR "", MAC2STR(recv_cb->mac_addr));
      }
      break;
    }
    default:
      ESP_LOGE(TAG, "Callback type error: %d", evt.id);
      break;
    }
  }
}

extern "C" esp_err_t example_espnow_init(void)
{
  example_espnow_send_param_t *send_param;

  s_example_espnow_queue = xQueueCreate(ESPNOW_QUEUE_SIZE, sizeof(example_espnow_event_t));
  if (s_example_espnow_queue == NULL)
  {
    ESP_LOGE(TAG, "Create mutex fail");
    return ESP_FAIL;
  }

  /* Initialize ESPNOW and register sending and receiving callback function. */
  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. */
  ESP_ERROR_CHECK(esp_now_set_pmk((uint8_t *)CONFIG_ESPNOW_PMK));

  /* Add broadcast peer information to peer list. */
  esp_now_peer_info_t *peer = (esp_now_peer_info_t *)malloc(sizeof(esp_now_peer_info_t));
  if (peer == NULL)
  {
    ESP_LOGE(TAG, "Malloc peer information fail");
    vSemaphoreDelete(s_example_espnow_queue);
    esp_now_deinit();
    return ESP_FAIL;
  }
  memset(peer, 0, sizeof(esp_now_peer_info_t));
  peer->channel = CONFIG_ESPNOW_CHANNEL;
  peer->ifidx = ESPNOW_WIFI_IF;
  peer->encrypt = false;
  memcpy(peer->peer_addr, s_example_broadcast_mac, ESP_NOW_ETH_ALEN);
  ESP_ERROR_CHECK(esp_now_add_peer(peer));
  free(peer);

  /* Initialize sending parameters. */
  send_param = (example_espnow_send_param_t *)malloc(sizeof(example_espnow_send_param_t));
  memset(send_param, 0, sizeof(example_espnow_send_param_t));
  if (send_param == NULL)
  {
    ESP_LOGE(TAG, "Malloc send parameter fail");
    vSemaphoreDelete(s_example_espnow_queue);
    esp_now_deinit();
    return ESP_FAIL;
  }
  send_param->unicast = false;
  send_param->broadcast = true;
  send_param->state = 0;
  send_param->magic = esp_random();
  send_param->count = CONFIG_ESPNOW_SEND_COUNT;
  send_param->delay = CONFIG_ESPNOW_SEND_DELAY;
  send_param->len = 13; //CONFIG_ESPNOW_SEND_LEN;
  send_param->buffer = (uint8_t *)malloc(13);
  if (send_param->buffer == NULL)
  {
    ESP_LOGE(TAG, "Malloc send buffer fail");
    free(send_param);
    vSemaphoreDelete(s_example_espnow_queue);
    esp_now_deinit();
    return ESP_FAIL;
  }
  memcpy(send_param->dest_mac, s_example_broadcast_mac, ESP_NOW_ETH_ALEN);
  example_espnow_data_prepare(send_param);

  xTaskCreate(example_espnow_task, "example_espnow_task", 2048, send_param, 4, NULL);

  return ESP_OK;
}

extern "C" void example_espnow_deinit(example_espnow_send_param_t *send_param)
{
  free(send_param->buffer);
  free(send_param);
  vSemaphoreDelete(s_example_espnow_queue);
  esp_now_deinit();
}

extern "C" void app_main(void)
{
  // Initialize NVS
  esp_err_t ret = nvs_flash_init();
  if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND)
  {
    ESP_ERROR_CHECK(nvs_flash_erase());
    ret = nvs_flash_init();
  }
  ESP_ERROR_CHECK(ret);

  example_wifi_init();
  example_espnow_init();
}