#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 = 25;
static constexpr int GPS_UART_RX_PIN = 26;
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;

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_gps";

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_RX_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, 8, &this->uart_queue_, 0);
  if (ret != ESP_OK) {
    ESP_LOGE(TAG, "uart_driver_install: %d", ret);
    return;
  }
  uart_enable_pattern_det_intr(GPS_UART, '\n', 1, 10000, 10, 10);
  uart_pattern_queue_reset(GPS_UART, 8);
  uart_flush(GPS_UART);
  ESP_LOGI(TAG, "gps uart configured");

  buffer_ = new uint8_t[GPS_BUF_SIZE];

  BaseType_t xRet = xTaskCreate(UART_GPS::TaskEntry, "ugv_io_gps", 8 * 1024,
                                this, 3, &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) {
  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_tocoord(&rmc.latitude);
      data_.longitude = minmea_tocoord(&rmc.longitude);
      data_.speed = minmea_tofloat(&rmc.speed);
      data_.course = minmea_tofloat(&rmc.course);
      data_.last_update = xTaskGetTickCount();
      ESP_LOGV(TAG, "RMC: %s, (%f,%f), speed=%f, course=%f",
               data_.valid ? "valid" : "invalid", data_.latitude,
               data_.longitude, data_.speed, data_.course);
      xSemaphoreGive(mutex_);
      break;
    }
    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_tocoord(&gga.latitude);
      data_.longitude = minmea_tocoord(&gga.longitude);
      if (gga.fix_quality != 0 && gga.altitude_units != 'M') {
        ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units);
      }
      data_.altitude = minmea_tofloat(&gga.altitude);
      data_.last_update = xTaskGetTickCount();
      ESP_LOGV(TAG, "GGA: qual: %d, num_sat=%d, (%f,%f), alt=%f",
               data_.fix_quality, data_.num_satellites, data_.latitude,
               data_.longitude, data_.altitude);
      xSemaphoreGive(mutex_);
      break;
    }
    case MINMEA_SENTENCE_GSA: {
      minmea_sentence_gsa gsa;
      bool parse_success;
      parse_success = minmea_parse_gsa(&gsa, line);
      if (!parse_success) {
        goto invalid;
      }
      xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
      data_.pdop = minmea_tofloat(&gsa.pdop);
      data_.hdop = minmea_tofloat(&gsa.hdop);
      data_.vdop = minmea_tofloat(&gsa.vdop);
      ESP_LOGV(TAG, "GSA: pdop=%f, hdop=%f, vdop=%f", data_.pdop, data_.hdop,
               data_.vdop);
      xSemaphoreGive(mutex_);
      break;
    }
    default: ESP_LOGV(TAG, "unsupported nmea sentence: %s", line);
  }
}

void UART_GPS::HandleUartPattern() {
  int pos = uart_pattern_pop_pos(GPS_UART);
  if (pos < 0) {
    ESP_LOGE(TAG, "uart_pattern_pop_pos: %d", pos);
    return;
  }
  int read_bytes =
      uart_read_bytes(GPS_UART, this->buffer_, pos + 1, pdMS_TO_TICKS(100));
  if (read_bytes <= 0) {
    ESP_LOGW(TAG, "uart_read_bytes error: %d", read_bytes);
    return;
  }
  ESP_LOGV(TAG, "GPS bytes received: %.*s", read_bytes, buffer_);
  buffer_[read_bytes] = '\0';
  this->ProcessLine((const char *)buffer_,
                    read_bytes + 1);  // process that line
}

void UART_GPS::DoTask() {
  ESP_LOGI(TAG, "gps_task start");
  uart_event_t uart_event;

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

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

  while (true) {
    BaseType_t pdRet = xQueueReceive(uart_queue_, &uart_event, portMAX_DELAY);
    if (!pdRet) {
      continue;
    }
    switch (uart_event.type) {
      case UART_DATA: break;
      case UART_PATTERN_DET: this->HandleUartPattern(); break;
      default:
        ESP_LOGW(TAG, "Unhandled UART event type: %d", uart_event.type);
        break;
    }
  }
}

void UART_GPS::TaskEntry(void *arg) {
  UART_GPS *gps = (UART_GPS *)arg;
  gps->DoTask();
  vTaskDelete(NULL);
}

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