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

#include <esp_log.h>

#include "messages.pb.h"
#include "sx127x_registers.h"

namespace ugv {
namespace comms {

CommsClass Comms;

static const char *TAG = "ugv_comms";

CommsClass::CommsClass()
    : location(),
      ugv_state(messages::IDLE),
      last_packet_tick(0),
      last_packet_rssi(INT32_MIN),
      last_packet_snr(INT8_MIN),
      packet_num(0) {
  mutex = xSemaphoreCreateMutex();
}

void CommsClass::Init() {
  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;

  ret = sx127x_init(&lora_config, &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(lora);
  if (ret != ESP_OK) {
    ESP_LOGI(TAG, "LoRa start failed: %d", ret);
    return;
  }

  packet_num = 0;

  xTaskCreate(CommsClass::CommsTask, "ugv_comms", 2 * 1024, this, 2,
              &task_handle);
}

void CommsClass::Lock(TickType_t ticks_to_wait) {
  xSemaphoreTake(mutex, ticks_to_wait);
}

void CommsClass::Unlock() { xSemaphoreGive(mutex); }

int32_t CommsClass::ReadRssi() {
  int32_t rssi;
  sx127x_read_rssi(lora, &rssi);
  return rssi;
}

uint8_t CommsClass::ReadLnaGain() {
  uint8_t lna_gain;
  sx127x_read_lna_gain(lora, &lna_gain);
  return lna_gain;
}

void CommsClass::CommsTask(void *arg) { ((CommsClass *)arg)->RunTask(); }

void CommsClass::RunTask() {
  using messages::Location;
  using messages::UGV_Message;
  using messages::UGV_State;
  using messages::UGV_Status;

  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;

  UGV_Message ugv_message;
  UGV_Status *status   = ugv_message.mutable_status();
  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(UGV_State::IDLE);

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

    current_tick = xTaskGetTickCount();
    if (ret == ESP_OK) {
      HandlePacket(&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(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;
  }
}

void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) {
  using messages::GroundMessage;

  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(mutex, portMAX_DELAY);
  last_packet_tick = xTaskGetTickCount();
  last_packet_rssi = rx_packet->rssi;
  last_packet_snr  = rx_packet->snr;
  xSemaphoreGive(mutex);

  GroundMessage ground_message;

  bool 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 GroundMessage::kCommand:
      HandleCommand(ground_message.command());
      break;
    default:
      ESP_LOGE(TAG, "invalid ground message: %d",
               ground_message.ground_message_case());
      break;
  }
}

void CommsClass::HandleCommand(const messages::GroundCommand &command) {
  ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
  // TODO: handle command

  messages::UGV_Message ugv_message;
  ugv_message.set_command_ack(command.id());

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

}  // namespace comms
}  // namespace ugv