#define U8X8_USE_PINS
#include <driver/uart.h>
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#include <string.h>
#include <u8g2.h>

#include "sx127x_driver.h"
#include "sx127x_registers.h"
#include "u8g2_esp32_hal.h"

const char *TAG = "uas-ugv";

u8g2_t u8g2;

#define LORA_SCK 5
#define LORA_MISO 19
#define LORA_MOSI 27
#define LORA_CS 18
#define LORA_RST 14
#define LORA_IRQ 26

#define LORA_FREQ (433E6)
#define LORA_BUF_LEN 64

#define OLED_H 64
#define OLED_W 128

uint16_t packet_num;

sx127x_hndl lora;

void loraOnReceive(int packetSize);
void loraTask(void *params);

TaskHandle_t lora_task_hndl;

sx127x_rx_packet_t rx_packet;

void setup_oled(void) {
  u8g2_esp32_hal_t u8g2_hal_config = {
      .scl   = 15,
      .sda   = 4,
      .reset = 16,
  };
  u8g2_esp32_hal_init(u8g2_hal_config);

  u8g2_Setup_ssd1306_i2c_128x64_noname_f(&u8g2, U8G2_R0, u8g2_esp32_i2c_byte_cb,
                                         u8g2_esp32_gpio_and_delay_cb);
  u8g2_InitDisplay(&u8g2);
  u8g2_ClearDisplay(&u8g2);
  u8g2_SetPowerSave(&u8g2, false);
}

#define GPS_UART UART_NUM_1

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 = 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;
  // 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");
  esp_err_t ret;

  setup_gps();
  setup_oled();

  sx127x_config_t lora_config  = SX127X_CONFIG_DEFAULT;
  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;

  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);
  } else {
    ESP_LOGI(TAG, "LoRa initialized");
    ret = sx127x_start(lora);
    if (ret != ESP_OK) {
      ESP_LOGI(TAG, "LoRa start failed: %d", ret);
    }
  }
  memset(&rx_packet, 0, sizeof(rx_packet));

  packet_num = 0;

  xTaskCreate(loraTask, "loraTask", 1024 * 10, NULL, 2, &lora_task_hndl);
}

#define XO 10

void loraTask(void *params) {
  char         tx_buf[20];
  const size_t tx_buf_len   = (sizeof(tx_buf) / sizeof(tx_buf[0]));
  TickType_t   send_period  = pdMS_TO_TICKS(2000);
  TickType_t   current_time = xTaskGetTickCount();
  TickType_t   next_send    = current_time + send_period;
  while (true) {
    TickType_t delay_ticks = next_send - current_time;
    vTaskDelay(delay_ticks);
    current_time = xTaskGetTickCount();
    if (current_time < next_send) {
      continue;
    }
    int written_bytes =
        snprintf(tx_buf, tx_buf_len, "hello world %d", packet_num);
    if (written_bytes < 0) {
      ESP_LOGE(TAG, "snprintf error: %d", written_bytes);
      continue;
    }
    packet_num++;
    esp_err_t ret = sx127x_send_packet(lora, tx_buf, written_bytes,
                                       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 %d bytes", written_bytes);
    }

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

int32_t lora_rssi;
uint8_t lora_lna_gain;

void loop(void) {
  ESP_LOGI(TAG, "loop");
  u8g2_FirstPage(&u8g2);
  sx127x_recv_packet(lora, &rx_packet, 0);
  sx127x_read_rssi(lora, &lora_rssi);
  sx127x_read_lna_gain(lora, &lora_lna_gain);
  do {
    u8g2_DrawRFrame(&u8g2, 0, 0, OLED_W, OLED_H, 4);

    multi_heap_info_t heap_info;
    heap_caps_get_info(&heap_info, MALLOC_CAP_DEFAULT);
    u8g2_SetFont(&u8g2, u8g2_font_4x6_tr);
    u8g2_DrawStr(&u8g2, 4, 8, "=====UAS UGV=====");
    char buf[32];
    memset(buf, 0, 32);
    snprintf(buf, 32, "heap allc/free %d/%d", heap_info.total_allocated_bytes,
             heap_info.total_free_bytes);
    u8g2_DrawStr(&u8g2, 4, 2 * 8, buf);

    if (rx_packet.data_len) {
      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);
      u8g2_SetFont(&u8g2, u8g2_font_4x6_tr);
      snprintf(buf, 40, "lora pkt(rssi: %d, snr: %f)", rx_packet.rssi,
               rx_packet.snr * 0.25f);
      u8g2_DrawStr(&u8g2, 4, 3 * 8, buf);
      snprintf(buf, 40, "%.*s", rx_packet.data_len, rx_packet.data);
      u8g2_DrawStr(&u8g2, 4, 4 * 8, buf);
    }
    snprintf(buf, 40, "rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
    u8g2_DrawStr(&u8g2, 4, 5 * 8, buf);
  } while (u8g2_NextPage(&u8g2));
  vTaskDelay(pdMS_TO_TICKS(1000));
}

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

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