#include "ugv_io_gps.hh"

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

#include "minmea.h"

namespace ugv {
namespace io {

static constexpr size_t      GPS_BUF_SIZE         = 1024;
static constexpr uart_port_t GPS_UART             = UART_NUM_1;
static constexpr int         GPS_UART_TX_PIN      = 17;
static constexpr int         GPS_UART_RX_PIN      = 23;
static constexpr int         GPS_UART_BAUD        = 9600;
static constexpr size_t      GPS_UART_RX_BUF_SIZE = 1024;
static constexpr size_t      GPS_UART_TX_BUF_SIZE = 0;
static constexpr size_t      GPS_UART_QUEUE_SIZE  = 8;

const char NMEA_OUTPUT_RMCONLY[] =
    "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29";
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";
const char NMEA_OUTPUT_ALL[] =
    "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28";
const char NMEA_OUTPUT_OFF[] =
    "$PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28";

const char NMEA_UPDATE_1HZ[]  = "$PMTK220,1000*1F";
const char NMEA_UPDATE_5HZ[]  = "$PMTK220,200*2C";
const char NMEA_UPDATE_10HZ[] = "$PMTK220,100*2F";

const char NMEA_Q_RELEASE[] = "$PMTK605*31";

const char NMEA_END_CMD[] = "\r\n";

std::string st;

static const char *TAG = "ugv_io";

UART_GPS::UART_GPS() {}

UART_GPS::~UART_GPS() {}

void UART_GPS::Init() {
  esp_err_t ret;

  mutex_ = xSemaphoreCreateMutex();
  data_  = GpsData{};

  uart_config_t gps_uart_config;
  gps_uart_config.baud_rate           = GPS_UART_BAUD;
  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, GPS_UART_TX_PIN, GPS_UART_TX_PIN, UART_PIN_NO_CHANGE,
               UART_PIN_NO_CHANGE);
  ret = uart_driver_install(GPS_UART, GPS_UART_RX_BUF_SIZE,
                            GPS_UART_TX_BUF_SIZE, GPS_UART_QUEUE_SIZE, NULL, 0);
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "uart_driver_install: %d", ret);
    return;
  }
  ESP_LOGI(TAG, "gps uart configured");

  BaseType_t xRet = xTaskCreate(UART_GPS::GPS_Task, "ugv_io_gps", 2 * 1024,
                                this, 1, &this->task_);
  if (xRet != pdTRUE) {
    ESP_LOGE(TAG, "error creating GPS task");
    return;
  }
}

void UART_GPS::GetData(GpsData &data) {
  xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
  data = GpsData(data_);
  xSemaphoreGive(mutex_);
}

esp_err_t UART_GPS::WriteCommand(const char *cmd, size_t len) {
  esp_err_t ret;
  uart_write_bytes(GPS_UART, cmd, len);
  uart_write_bytes(GPS_UART, NMEA_END_CMD, sizeof(NMEA_END_CMD));
  ret = uart_wait_tx_done(GPS_UART, 1000);
  if (ret != ESP_OK) goto err;

  return ESP_OK;

err:
  const char *error_name = esp_err_to_name(ret);
  ESP_LOGE(TAG, "WriteCommand error: %s (%d)", error_name, ret);
  return ret;
}

void UART_GPS::ProcessLine(const char *line, size_t len) {
  ESP_LOGI(TAG, "gps data: %.*s", len, line);
  enum minmea_sentence_id id = minmea_sentence_id(line, false);
  switch (id) {
    case MINMEA_INVALID:
    invalid:
      ESP_LOGE(TAG, "invalid nmea sentence: %s", line);
      return;
    case MINMEA_SENTENCE_RMC: {
      minmea_sentence_rmc rmc;
      bool                parse_success;
      parse_success = minmea_parse_rmc(&rmc, line);
      if (!parse_success) {
        goto invalid;
      }
      xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
      data_.valid       = rmc.valid;
      data_.latitude    = minmea_tofloat(&rmc.latitude);
      data_.longitude   = minmea_tofloat(&rmc.longitude);
      data_.speed       = minmea_tofloat(&rmc.speed);
      data_.course      = minmea_tofloat(&rmc.course);
      data_.last_update = xTaskGetTickCount();
      xSemaphoreGive(mutex_);
    }
    case MINMEA_SENTENCE_GGA: {
      minmea_sentence_gga gga;
      bool                parse_success;
      parse_success = minmea_parse_gga(&gga, line);
      if (!parse_success) {
        goto invalid;
      }
      xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
      data_.fix_quality    = (GpsFixQual)gga.fix_quality;
      data_.num_satellites = gga.satellites_tracked;
      data_.latitude       = minmea_tofloat(&gga.latitude);
      data_.longitude      = minmea_tofloat(&gga.longitude);
      if (gga.altitude_units != 'M') {
        ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units);
      }
      data_.altitude    = minmea_tofloat(&gga.altitude);
      data_.last_update = xTaskGetTickCount();
      xSemaphoreGive(mutex_);
    }
    default: ESP_LOGW(TAG, "unsupported nmea sentence: %s", line);
  }
}

void UART_GPS::GPS_Task(void *arg) {
  UART_GPS *gps = (UART_GPS *)arg;
  (void)gps;

  ESP_LOGI(TAG, "gps_task start");
  char * buf1    = (char *)malloc(GPS_BUF_SIZE);
  char * buf2    = (char *)malloc(GPS_BUF_SIZE);
  char * buf_end = buf1;
  size_t read_bytes;

  esp_err_t ret;
  ret = gps->WriteCommand(NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA));
  ESP_LOGI(TAG, "sent output rmc and gga: %d", ret);
  ret = gps->WriteCommand(NMEA_UPDATE_1HZ, sizeof(NMEA_UPDATE_1HZ));
  ESP_LOGI(TAG, "sent update 1hz: %d", ret);

  vTaskDelay(pdMS_TO_TICKS(100));
  ret = gps->WriteCommand(NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE));
  ESP_LOGI(TAG, "sent q release: %d", ret);

  while (true) {
    size_t buf_remaining = GPS_BUF_SIZE - (buf_end - buf1);
    read_bytes = uart_read_bytes(GPS_UART, (uint8_t *)buf_end, buf_remaining,
                                 portMAX_DELAY);
    if (read_bytes <= 0) {
      ESP_LOGW(TAG, "GPS error: %d", read_bytes);
      continue;
    }
    ESP_LOGW(TAG, "GPS bytes received: %.*s", read_bytes, buf_end);
    buf_end += read_bytes;
    int lines_received = 0;
    for (char *c = buf1; c < buf_end; c++) {
      if (*c != '\n') {  // wait for \n, if not found loop again
        continue;
      }
      lines_received++;
      c++;  // now c is one past the end of the whole string including \n

      size_t remaining = buf_end - c;
      memcpy(buf2, c, remaining);  // copy remaining text from buf1 to buf2
      buf_end = buf2 + remaining;  // update buf_end to point to the end of the
                                   // remaining text in buf2

      char * str_ptr = buf1;
      size_t str_len = c - buf1;
      str_ptr[str_len] =
          '\0';  // append the NULL byte (safe because old data was copied)
      gps->ProcessLine(str_ptr, str_len);  // process that line

      std::swap(buf1, buf2);  // swap buffers
      c = buf1;  // start over on text which was copied to buf2 (now buf1)
    }
    if (lines_received == 0) {
      ESP_LOGW(TAG, "no lines received, waiting for more data");
    }
  }
  free(buf1);
  free(buf2);
  vTaskDelete(NULL);
}

};  // namespace io
};  // namespace ugv