#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;

void ugv_comms_init() {
  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;

  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(ugv_comms_task, "ugv_comms", 2 * 1024, NULL, 2,
              &ugv_comms_task_hndl);
}

void ugv_comms_task(void *params) {
  TickType_t send_period  = pdMS_TO_TICKS(2000);
  TickType_t current_time = xTaskGetTickCount();
  TickType_t next_send    = current_time + send_period;

  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_time;
    vTaskDelay(delay_ticks);
    current_time = xTaskGetTickCount();
    if (current_time < next_send) {
      continue;
    }
    packet_num++;
    esp_err_t ret =
        sx127x_send_packet_pb(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_time = xTaskGetTickCount();
    next_send    = current_time + send_period;
  }
}