#pragma once

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

namespace ugv {
namespace io {

struct Vec3f {
  float x;
  float y;
  float z;

  Vec3f();
  Vec3f(float x, float y, float z);
  Vec3f(const mpud::float_axes_t& axes);
};

struct MpuData {
  // G's
  Vec3f accel;
  // degrees/s
  Vec3f gyro_rate;
  // flux density uT
  Vec3f mag;
};

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

  void Init();

  void GetData(MpuData &data);

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

  TaskHandle_t task_;
  SemaphoreHandle_t mutex_;
  MpuData mpu_data_;

  void DoTask();

  static void MPU_Task(void *arg);
};

}  // namespace io
}  // namespace ugv