#include <driver/uart.h>
#include <esp_log.h>
#include <string.h>
#include <u8g2.h>

#include "sx127x_driver.h"
#include "sx127x_registers.h"
#include "u8g2_esp32_hal.h"
#include "ugv_comms.h"
#include "ugv_config.h"
#include "U8g2lib.hh"

static const char *TAG = "ugv_main";

U8G2 *oled;

void setup_oled(void) {
  oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, 16, 15, 4);
  oled->initDisplay();
  oled->clearDisplay();
  oled->setPowerSave(false);
}

const size_t GPS_BUF_SIZE = 1024;

const char NMEA_OUTPUT_RMCGGA[] =
    "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
const char NMEA_SET_UPDATE_1HZ[] = "$PMTK220,1000*1F\r\n";
const char NMEA_Q_RELEASE[]      = "$PMTK605*31\r\n";

static void gps_task(void *arg) {
  ESP_LOGI(TAG, "gps_task start");
  uint8_t * data = (uint8_t*) malloc(GPS_BUF_SIZE);
  size_t    read_bytes;
  esp_err_t ret;
  uart_write_bytes(GPS_UART, NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA));
  ret = uart_wait_tx_done(GPS_UART, 1000);
  ESP_LOGI(TAG, "sent output rmc and gga: %d", ret);
  uart_write_bytes(GPS_UART, NMEA_SET_UPDATE_1HZ, sizeof(NMEA_SET_UPDATE_1HZ));
  ret = uart_wait_tx_done(GPS_UART, 1000);
  ESP_LOGI(TAG, "sent update 1hz: %d", ret);
  vTaskDelay(pdMS_TO_TICKS(100));
  uart_write_bytes(GPS_UART, NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE));
  ret = uart_wait_tx_done(GPS_UART, 1000);
  ESP_LOGI(TAG, "sent q release: %d", ret);
  while (true) {
    read_bytes =
        uart_read_bytes(GPS_UART, data, GPS_BUF_SIZE, pdMS_TO_TICKS(100));
    if (read_bytes > 0) {
      ESP_LOGI(TAG, "gps data: %.*s", read_bytes, data);
    }
  }
}

void setup_gps(void) {
  uart_config_t gps_uart_config;
  esp_err_t     ret;
  // gps_uart_config.baud_rate = 9600;
  gps_uart_config.baud_rate           = 57600;
  gps_uart_config.data_bits           = UART_DATA_8_BITS;
  gps_uart_config.parity              = UART_PARITY_DISABLE;
  gps_uart_config.stop_bits           = UART_STOP_BITS_1;
  gps_uart_config.flow_ctrl           = UART_HW_FLOWCTRL_DISABLE;
  gps_uart_config.rx_flow_ctrl_thresh = 122;
  gps_uart_config.use_ref_tick        = false;
  ret = uart_param_config(GPS_UART, &gps_uart_config);
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "uart_param_config: %d", ret);
    return;
  }
  uart_set_pin(GPS_UART, 17, 23, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
  ret = uart_driver_install(GPS_UART, 1024, 0, 8, NULL, 0);
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "uart_driver_install: %d", ret);
    return;
  }
  ESP_LOGI(TAG, "gps uart configured");
  xTaskCreate(&gps_task, "gps_task", 2 * 1024, NULL, 1, NULL);
}

void setup(void) {
  ESP_LOGI(TAG, "setup");

  setup_gps();
  setup_oled();
  ugv_comms_init();
}

#define BUF_SZ 32

void loop(void) {
  static int32_t    lora_rssi;
  static uint8_t    lora_lna_gain;
  static TickType_t last_packet_tick;
  static int32_t    last_packet_rssi;
  static int8_t     last_packet_snr;

  static char buf[BUF_SZ];

  oled->firstPage();
  sx127x_read_rssi(ugv_comms_state.lora, &lora_rssi);
  sx127x_read_lna_gain(ugv_comms_state.lora, &lora_lna_gain);

  xSemaphoreTake(ugv_comms_state.mutex, portMAX_DELAY);
  last_packet_tick = ugv_comms_state.last_packet_tick;
  last_packet_rssi = ugv_comms_state.last_packet_rssi;
  last_packet_snr  = ugv_comms_state.last_packet_snr;
  xSemaphoreGive(ugv_comms_state.mutex);
  do {
    oled->drawRFrame(0, 0, OLED_W, OLED_H, 4);

    multi_heap_info_t heap_info;
    heap_caps_get_info(&heap_info, MALLOC_CAP_DEFAULT);
    oled->setFont(u8g2_font_4x6_mr);
    oled->drawStr(4, 8, "=====UAS UGV=====");
    snprintf(buf, BUF_SZ, "heap allc/free %d/%d",
             heap_info.total_allocated_bytes, heap_info.total_free_bytes);
    oled->drawStr(4, 2 * 8, buf);

    snprintf(buf, BUF_SZ, "rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
    oled->drawStr(4, 3 * 8, buf);

    if (last_packet_tick > 0) {
      double time_since_last_packet =
          1000.0f /
          ((xTaskGetTickCount() - last_packet_tick) * portTICK_RATE_MS);
      snprintf(buf, BUF_SZ, "last pkt rx %f s ago", time_since_last_packet);
      oled->drawStr(4, 4 * 8, buf);
      snprintf(buf, BUF_SZ, "pkt rssi: %d, snr: %f", last_packet_rssi,
               last_packet_snr * 0.25f);
      oled->drawStr(4, 5 * 8, buf);
    } else {
      oled->drawStr(4, 4 * 8, "no pkt rx");
    }
  } while (oled->nextPage());
  vTaskDelay(pdMS_TO_TICKS(1000));
}

void loopTask(void *pvUser) {
  setup();
  while (1) {
    loop();
  }
}

extern "C" void app_main() {
  xTaskCreatePinnedToCore(loopTask, "loopTask", 8192, NULL, 1, NULL, 1);
}