#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 gravity;
    float zInertial;
};

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

    ~Sensors();

    void measureBaseAltitudeSync(void);
    void measureBaseAltitudeAsync(void);
    void onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback);
    float rawFlightHeight(void) const;
    float flightHeight(void) const;

    void startMpuCalibration();
    void onMpuCalibrationFinished(OnCalibrationFinishedCb callback);

    MpuData mpuData(void) const;

    int batteryCharge(void) const;  // [%]

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


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