#pragma once

#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);

  Vec3f operator+(const Vec3f& a) const { return {x + a.x, y + a.y, z + a.z}; }

  Vec3f& operator+=(const Vec3f& a) {
    x += a.x;
    y += a.y;
    z += a.z;
    return *this;
  }
};

struct Mat3f {
  float xx, xy, xz;
  float yx, yy, yz;
  float zx, zy, zz;

  Vec3f operator*(const Vec3f& v) const {
    return {xx * v.x + xy * v.y + xz + v.z,
            yx * v.x + yy * v.y + yz * v.z,
            zx * v.x + zy * v.y + zz * v.z};
  }
};

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 Calibrate();

  void GetData(MpuData& data);

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

}  // namespace io
}  // namespace ugv