#include <string.h>

#include "esp_log.h"
#include "esp32-lora.h"
#include "lorcomm.h"
#include "main.h"

#define TAG "lorcomm"

#define MSG_ID_TRACK    (16)

typedef struct {
  uint8_t mac[3];
  uint8_t ids[MSG_ID_TRACK];
} id_cache_t;

uint8_t mac[6] = {0};
static uint8_t msg_id = 1;

id_cache_t id_cache[MSG_ID_TRACK] = { 0 };

uint8_t acmp(uint8_t a[], uint8_t b[], uint8_t len) {
  for(uint8_t i = 0; i < len; i++) {
    if(a[i] != b[i]) return 0;
  }

  return 1;
}

void lorcomm_handle_receive(uint8_t size) {
  printf("packet size: %d\n", size);

  uint8_t *buf = malloc(size);
  lora_packet *packet = malloc(sizeof(lora_packet));

  // get data from the lora driver
  lora32_read_data(&lora, buf);

  // copy buffer onto message headers
  memcpy(packet, buf, LORA_PACKET_HEADER_SIZE);
  ESP_LOGI(TAG, "[%02X:%02X:%02X] id: %02d type: %02X", packet->src[0], packet->src[1], packet->src[2], packet->id, packet->type);

  for(uint8_t i = 0; i < MSG_ID_TRACK; i++) {
    if(id_cache[i].mac[0]+id_cache[i].mac[1]+id_cache[i].mac[2] == 0) {
      memcpy(id_cache[i].mac, &packet->src, 3);

      // this is assumed to be empty
      id_cache[i].ids[0] = packet->id;
      ESP_LOGI(TAG, "adding new tracking entity");

      break;
    }

    for(uint8_t j = 0; j < MSG_ID_TRACK; j++) {
      if(id_cache[i].ids[j] == packet->id) {
        ESP_LOGI(TAG, "already seen packet, not relaying");
        goto receive_cleanup;
      } else if(id_cache[i].ids[j] == 0) {
        ESP_LOGI(TAG, "adding id to tracking");

        // add id to array
        id_cache[i].ids[i] = packet->id;
        // set next in queue to zero, rolling over as needed
        id_cache[i].ids[(i + 1) % MSG_ID_TRACK] = 0;

        goto break_outer;
      }
    }
  }

break_outer:

  // update local msg id to incoming +1
  // this will give a rolling local area id
  if(packet->id < msg_id) {
    msg_id = packet->id + 1;

    // 0 is an invalid id
    if(msg_id == 0) msg_id = 1;
  }

  if(memcmp(packet->src, &mac[3], 3) == 0) {
    ESP_LOGI(TAG, "received own packet, done");
    goto receive_cleanup;
  }

  if(packet->type == TEXT) {
    lora_msg *msg = malloc(sizeof(lora_msg));
    memcpy(msg, buf + LORA_PACKET_HEADER_SIZE, LORA_MSG_HEADER_SIZE);

    // allocate memory for message payload
    msg->msg = malloc(msg->length);

    memcpy(msg->msg, buf + LORA_PACKET_HEADER_SIZE + LORA_MSG_HEADER_SIZE, msg->length);

    // if the msg doesnt end in NULL it's probably already corrupt
    // just go ahead and terminate it
    //if(msg->msg[msg->length] != '\0')
      //msg->msg[msg->length] = '\0';

    ESP_LOGI(TAG, "msg (len: %d): %s", msg->length, msg->msg);

    free(msg->msg);
    msg->msg = NULL;
    free(msg);
  } else if(packet->type == ROUTING) {
    // future stuff
  } else {
    ESP_LOGW(TAG, "Unknown packet type: %02X", packet->type);
    ESP_LOGW(TAG, "msg: %s", (char*)packet);

    goto receive_cleanup;
  }

  vTaskDelay(3000/portTICK_PERIOD_MS);
  ESP_LOGI(TAG, "relaying message");

  lora32_send(&lora, buf, size);

receive_cleanup:
  free(buf);

  lora32_enable_continuous_rx(&lora);
}

void lorcomm_send_message(lora32_cfg_t *lora, char *text) {
  // allocate and zero out packet and msg payload
  uint8_t len = strlen(text) + 1;
  lora_packet packet = { 0 };
  lora_msg msg = { 0 };

  // set packet tracking id
  packet.id = msg_id;

  msg_id = msg_id + 1;
  if(msg_id == 0) msg_id = 1;

  // set src and dest (broadcast) addresses
  memcpy(packet.src, &mac[3], 3);
  memset(packet.dst, 0xFF, 3);

  // indicate message payload
  packet.type = TEXT;
  packet.payload = malloc(LORA_PACKET_HEADER_SIZE + len);

  msg.length = len;
  msg.msg = malloc(LORA_MSG_HEADER_SIZE + len);
  memcpy(msg.msg, text, len);

  uint8_t buf_len = LORA_PACKET_HEADER_SIZE + LORA_MSG_HEADER_SIZE + len;
  uint8_t *send_buf = malloc(buf_len);
  ESP_LOGI(TAG, "packet length: %d", buf_len);

  // set src, dst, length
  memcpy(send_buf, (uint8_t*)&packet, LORA_PACKET_HEADER_SIZE);
  memcpy((uint8_t*)(send_buf + LORA_PACKET_HEADER_SIZE), (void*)&msg, LORA_MSG_HEADER_SIZE);
  memcpy((uint8_t*)(send_buf + LORA_PACKET_HEADER_SIZE + LORA_MSG_HEADER_SIZE), (void*)msg.msg, len);

  ESP_LOGI(TAG, "id: %02d", packet.id);
  ESP_LOGI(TAG, "src: [%02X:%02X:%02X]", packet.src[0], packet.src[1], packet.src[2]);
  ESP_LOGI(TAG, "dst: [%02X:%02X:%02X]", packet.dst[0], packet.dst[1], packet.dst[2]);

  lora32_send(lora, send_buf, buf_len);

  free(packet.payload);
  packet.payload = NULL;

  free(msg.msg);
  msg.msg = NULL;

  free(send_buf);
}