#include "ugv_comms.h"
#include "ugv_config.h"

#include <esp_log.h>
#include <pb_decode.h>
#include <pb_encode.h>

#include "messages.pb.h"

static const char *TAG = "ugv_comms";

static void     ugv_comms_task(void *arg);
static uint16_t packet_num;

static void ugv_comms_task(void *arg);
static void ugv_comms_handle_packet(ugv_comms_state_t * st,
                                    sx127x_rx_packet_t *rx_packet);
static void ugv_comms_handle_command(ugv_comms_state_t *    st,
                                     uas_ugv_GroundCommand *command);

void ugv_comms_init() {
  ugv_comms_state_t *st = &ugv_comms_state;

  st->mutex = xSemaphoreCreateMutex();

  sx127x_config_t lora_config  = SX127X_CONFIG_DEFAULT;
  lora_config.sck_io_num       = LORA_SCK;
  lora_config.miso_io_num      = LORA_MISO;
  lora_config.mosi_io_num      = LORA_MOSI;
  lora_config.cs_io_num        = LORA_CS;
  lora_config.rst_io_num       = LORA_RST;
  lora_config.irq_io_num       = LORA_IRQ;
  lora_config.frequency        = LORA_FREQ;
  lora_config.tx_power         = 17;
  lora_config.spreading_factor = 12;
  lora_config.signal_bandwidth = 10E3;
  lora_config.sync_word        = 0x34;
  lora_config.crc              = SX127X_CRC_ENABLED;

  esp_err_t        ret;
  uas_ugv_Location loc = uas_ugv_Location_init_default;
  memcpy(&st->location, &loc, sizeof(loc));
  st->ugv_state        = uas_ugv_UGV_State_IDLE;
  st->last_packet_tick = 0;
  st->last_packet_rssi = INT32_MIN;
  st->last_packet_snr  = INT8_MIN;

  ret = sx127x_init(&lora_config, &st->lora);
  if (ret != ESP_OK) {
    const char *err_name = esp_err_to_name(ret);
    ESP_LOGE(TAG, "LoRa init failed: %s", err_name);
    return;
  }
  ESP_LOGI(TAG, "LoRa initialized");
  ret = sx127x_start(st->lora);
  if (ret != ESP_OK) {
    ESP_LOGI(TAG, "LoRa start failed: %d", ret);
    return;
  }

  packet_num = 0;

  xTaskCreate(ugv_comms_task, "ugv_comms", 2 * 1024, st, 2, &st->task_handle);
}

static void ugv_comms_task(void *param) {
  ugv_comms_state_t *st = (ugv_comms_state_t *)param;

  TickType_t send_period  = pdMS_TO_TICKS(2000);
  TickType_t current_tick = xTaskGetTickCount();
  TickType_t next_send    = current_tick + send_period;

  esp_err_t          ret;
  sx127x_rx_packet_t rx_packet;

  uas_ugv_UGV_Message ugv_message = uas_ugv_UGV_Message_init_default;
  ugv_message.which_ugv_message   = uas_ugv_UGV_Message_status_tag;
  ugv_message.ugv_message.status.location.fix_quality = 0;
  ugv_message.ugv_message.status.location.latitude    = 43.65;
  ugv_message.ugv_message.status.location.longitude   = -116.20;
  ugv_message.ugv_message.status.location.altitude    = 2730;
  ugv_message.ugv_message.status.state                = uas_ugv_UGV_State_IDLE;

  while (true) {
    TickType_t delay_ticks = next_send - current_tick;
    ret = sx127x_recv_packet(st->lora, &rx_packet, delay_ticks);

    current_tick = xTaskGetTickCount();
    if (ret == ESP_OK) {
      ugv_comms_handle_packet(st, &rx_packet);

      sx127x_packet_rx_free(&rx_packet);
    }

    if (current_tick < next_send) {
      continue;
    }

    packet_num++;
    ret = sx127x_send_packet_pb(st->lora, uas_ugv_UGV_Message_fields,
                                &ugv_message,
                                0);  // 0 means error if queue full
    if (ret != ESP_OK) {
      ESP_LOGE(TAG, "error sending packet: %d", ret);
    } else {
      ESP_LOGI(TAG, "lora wrote UGV_Message packet");
    }

    current_tick = xTaskGetTickCount();
    next_send    = current_tick + send_period;
  }
}

static void ugv_comms_handle_packet(ugv_comms_state_t * st,
                                    sx127x_rx_packet_t *rx_packet) {
  ESP_LOGI(TAG, "lora received packet (len %d, rssi: %d, snr: %f): %.*s\n",
           rx_packet->data_len, rx_packet->rssi, rx_packet->snr * 0.25f,
           rx_packet->data_len, rx_packet->data);

  xSemaphoreTake(st->mutex, portMAX_DELAY);
  st->last_packet_tick = xTaskGetTickCount();
  st->last_packet_rssi = rx_packet->rssi;
  st->last_packet_snr  = rx_packet->snr;
  xSemaphoreGive(st->mutex);

  uas_ugv_GroundMessage ground_message = uas_ugv_GroundMessage_init_default;
  pb_istream_t          istream;
  bool                  pb_ret;

  istream =
      pb_istream_from_buffer((pb_byte_t *)rx_packet->data, rx_packet->data_len);
  pb_ret = pb_decode(&istream, uas_ugv_GroundMessage_fields, &ground_message);
  if (!pb_ret) {
    ESP_LOGE(TAG, "rx invalid protobuf");
    return;
  }

  switch (ground_message.which_ground_message) {
    case uas_ugv_GroundMessage_command_tag:
      ugv_comms_handle_command(st, &ground_message.ground_message.command);
      break;
    default:
      ESP_LOGE(TAG, "invalid ground message: %d",
               ground_message.which_ground_message);
      break;
  }
}

static void ugv_comms_handle_command(ugv_comms_state_t *    st,
                                     uas_ugv_GroundCommand *command) {
  ESP_LOGI(TAG, "rx command id %d type %d", command->id, command->type);
  // TODO: handle command

  uas_ugv_UGV_Message ugv_message     = uas_ugv_UGV_Message_init_default;
  ugv_message.which_ugv_message       = uas_ugv_UGV_Message_command_ack_tag;
  ugv_message.ugv_message.command_ack = command->id;

  sx127x_send_packet_pb(st->lora, uas_ugv_UGV_Message_fields, &ugv_message, 0);
}