#pragma once

#include "ugv_io_gps.hh"
#include "ugv_io_mpu.hh"

namespace ugv {
namespace io {

using mpud::float_axes_t;

struct Inputs {
  MpuData mpu;
  GpsData gps;
};

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:
  UART_GPS gps_;
  MPU mpu_;

  void InitMotors();
};

}  // namespace io
}  // namespace ugv