#include "Barometer.hpp"
#include "BatteryController.hpp"
#include "Filters/Kalman2DFilter.hpp"
#include "MPU.hpp"
#include "esp_log.h"

/* Sensors module provides cached and filtered sensors values */

struct MpuData {
    float ax, ay, az;
    float gx, gy, gz;
    float yaw, pitch, roll;
    float gravityZ;
};

class Sensors {
  public:
    Sensors(Barometer &barometer, MPU &mpu, Kalman2DFilter &filter, BatteryController &battery, bool loop_on_fail = true);

    ~Sensors();

    void measureBaseAltitudeSync(void);
    void measureBaseAltitudeAsync(void);
    float rawFlightHeight(void);
    float flightHeight(void);

    MpuData mpuData(void);

    int batteryCharge(void);  // [%]

    bool tick(void);

  private:
    Barometer *_barometer = nullptr;
    MPU *_mpu = nullptr;
    BatteryController *_battery = nullptr;
    Kalman2DFilter *_2d_filter = nullptr;

    /* cached filtered values */
    float _flightHeightFromBarometer = NAN;
    float _zVelocityAltitude = NAN;
    MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN };


    static constexpr const char *_tag = "Sensors";
};