#pragma once

#include <freertos/FreeRTOS.h>
#include <freertos/semphr.h>
#include <freertos/task.h>
#include <stdint.h>

namespace ugv {
namespace io {

enum GpsFixQual {
  GPS_FIX_INVALID = 0,    // invalid gps fix
  GPS_FIX_GPS = 1,        // GPS fix
  GPS_FIX_DGPS = 2,       // differential GPS fix
  GPS_FIX_PPS = 3,        // PPS fix
  GPS_FIX_RTK = 4,        // Real Time Kinematic fix
  GPS_FIX_FRTK = 5,       // Float Real Time Kinematic fix
  GPS_FIX_ESTIMATED = 6,  // Estimated fix
  GPS_FIX_MANUAL = 7,     // Manual fix
  GPS_FIX_SIMULATED = 8,  // Simulated fix
};

struct GpsData {
  TickType_t last_update;
  bool valid;
  GpsFixQual fix_quality;
  uint8_t num_satellites;

  float latitude;   // degrees +/-
  float longitude;  // degrees +/-
  float altitude;   // meters
  float speed;      // knots
  float course;     // degrees clockwise of north
  float pdop;
  float hdop;
  float vdop;
};

class UART_GPS {
 public:
  explicit UART_GPS();
  ~UART_GPS();

  void Init();

  void GetData(GpsData& data);

 private:
  TaskHandle_t task_;
  SemaphoreHandle_t mutex_;
  QueueHandle_t uart_queue_;
  GpsData data_;
  uint8_t* buffer_;

  esp_err_t WriteCommand(const char* cmd, size_t len);
  void HandleUartPattern();
  void ProcessLine(const char* line, size_t len);

  void DoTask();
  static void TaskEntry(void* arg);
};

}  // namespace io
}  // namespace ugv