#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*(float s) const { return {s * x, s * y, s * z}; }

  Vec3f& operator*=(float s) {
    x *= s;
    y *= s;
    z *= s;
    return *this;
  }

  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;
  }

  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;
  }

  float dot(const Vec3f& v) const { return x * v.x + y * v.y + z * v.z; }

  float norm2() const { return x * x + y * y + z * z; }
};

struct Mat3f {
  union {
    Vec3f rx;
    float xx, xy, xz;
  };
  union {
    Vec3f ry;
    float yx, yy, yz;
  };
  union {
    Vec3f rz;
    float zx, zy, zz;
  };

  Mat3f(Vec3f rx_, Vec3f ry_, Vec3f rz_) : rx(rx_), ry(ry_), rz(rz_) {}

  Mat3f(float xx, float xy, float xz, float yx, float yy, float yz, float zx,
        float zy, float zz)
      : rx(xx, xy, xz), ry(yx, yy, yz), rz(zx, zy, zz) {}

  Vec3f operator*(const Vec3f& v) const {
    return {rx.dot(v), ry.dot(v), rz.dot(v)};
  }

  Mat3f transpose() const { return {xx, yx, zx, xy, yy, zy, xz, yz, zz}; }

  Mat3f operator*(const Mat3f& m) const {
    Mat3f mt = m.transpose();
    return {mt * rx, mt * ry, mt * rz};
  }
};

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_;
  TaskHandle_t task_;
  SemaphoreHandle_t mutex_;
  MpuData last_data_;

  void DoTask();
  bool Initialize();
  static void TaskEntry(void* arg);
};

}  // namespace io
}  // namespace ugv