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

#include <esp_log.h>

#include "messages.pb.h"

ugv_comms_state_t ugv_comms_state;

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,
                                     const 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       = (gpio_num_t)LORA_SCK;
  lora_config.miso_io_num      = (gpio_num_t)LORA_MISO;
  lora_config.mosi_io_num      = (gpio_num_t)LORA_MOSI;
  lora_config.cs_io_num        = (gpio_num_t)LORA_CS;
  lora_config.rst_io_num       = (gpio_num_t)LORA_RST;
  lora_config.irq_io_num       = (gpio_num_t)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;
  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_Status *status   = ugv_message.mutable_status();
  uas::ugv::Location *  location = status->mutable_location();
  location->set_fix_quality(0);
  location->set_latitude(43.65);
  location->set_longitude(-116.20);
  location->set_altitude(2730);
  status->set_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++;
    auto str = ugv_message.SerializeAsString();
    ret      = sx127x_send_packet(st->lora, str.c_str(), str.size(),
                             0);  // 0 means error if queue full
    // 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;
  bool                    pb_ret;

  pb_ret = ground_message.ParseFromArray(rx_packet->data, rx_packet->data_len);
  if (!pb_ret) {
    ESP_LOGE(TAG, "rx invalid protobuf");
    return;
  }

  switch (ground_message.ground_message_case()) {
    case uas::ugv::GroundMessage::kCommand:
      ugv_comms_handle_command(st, &ground_message.command());
      break;
    default:
      ESP_LOGE(TAG, "invalid ground message: %d",
               ground_message.ground_message_case());
      break;
  }
}

static void ugv_comms_handle_command(ugv_comms_state_t *            st,
                                     const 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;
  ugv_message.set_command_ack(command->id());

  std::string str = ugv_message.SerializeAsString();
  sx127x_send_packet(st->lora, str.c_str(), str.size(), 0);
}