#pragma once

#include <freertos/FreeRTOS.h>
#include <stdint.h>
#include <MPU.hpp>

#include "ugv_io_gps.hh"

namespace ugv {
namespace io {

using mpud::float_axes_t;

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 IOClass {
 public:
  explicit IOClass();
  ~IOClass();

  void Init();

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

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

  UART_GPS *gps_;

  void InitMotors();
  void InitMPU();
};

extern IOClass IO;

}  // namespace io
}  // namespace ugv