#pragma once

#include <stdint.h>

typedef enum gps_fix_qual_e {
  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
} gps_fix_qual_t;

typedef struct ugv_inputs_s {
  float gyro_x;   // degrees
  float gyro_y;   // degrees
  float gyro_z;   // degrees
  float accel_x;  // gs
  float accel_y;  // gs
  float accel_z;  // gs
  float mag_x;    // flux density uT
  float mag_y;    // flux density uT
  float mag_z;    // flux density uT

  gps_fix_qual_t gps_fix_quality;
  uint8_t        gps_num_satellites;

  double gps_lat;       // degrees +/-
  double gps_long;      // degrees +/-
  double gps_altitude;  // meters
} ugv_inputs_t;

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

void ugv_io_init();

void ugv_io_read_inputs(ugv_inputs_t *inputs);
void ugv_io_write_outputs(ugv_outputs_t *outputs);