#pragma once

#include <stdint.h>
#include <MPU.hpp>

namespace ugv {
namespace io {

using mpud::float_axes_t;

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 GpsInfo {
  GpsFixQual fix_quality;
  uint8_t    num_satellites;

  double latitude;   // degrees +/-
  double longitude;  // degrees +/-
  double altitude;   // meters
};

struct Inputs {
  // G's
  float_axes_t accel;
  // degrees/s
  float_axes_t gyro_rate;
  // flux density uT
  float_axes_t mag;

  GpsInfo gps_info;
};

struct Outputs {
  float left_motor;   // left motor power -1.0 to +1.0
  float right_motor;  // right motor power -1.0 to +1.0
};

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

  void Init();

  void ReadInputs(Inputs &inputs);
  void WriteOutputs(const Outputs &outputs);

 private:
  mpud::MPU *      mpu_;
  mpud::raw_axes_t accel_, gyro_, mag_;
};

extern IO io;

}  // namespace io
}  // namespace ugv