#include "PID.hpp"
#include "Sensors/Sensors.hpp"

enum DeviceStatus {
    Idle = 0,
    IsPreparingForTakeoff,
    IsFlying,
    IsBoarding,
    IsImuCalibration,
    ChargeRequired
};

struct ReportedDeviceState {
    int batteryCharge;
    float flightHeight;
    DeviceStatus status;
    MpuData mpuState;
};

struct FlightParams {
    float height;         // [cm]
    float yaw;      // [degrees]
    float pitch;  // [degrees]
};

class FlightController {
  public:
    FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2, PIDController *pid3, bool unlimitedBattery = false);
    ~FlightController();
    ReportedDeviceState currentDeviceState() const;
    void startTakeoff(OnMeasuringFinishedCb onTakeoffStarted);
    void startBoarding();
    void startImuCalibration();
    void tick();
  private:
    void _updateBatteryCharge();
    void _takeoff();
    void _boarding();

    Sensors *_sensors = nullptr;
    PIDController *_pid1 = nullptr;
    PIDController *_pid2 = nullptr;
    PIDController *_pid3 = nullptr;
    DeviceStatus _status;
    OnMeasuringFinishedCb _onTakeoffStarted = nullptr;

    static constexpr const char *_tag = "FlightController";
    static constexpr uint8_t _batteryCriticalCharge = 15;  // [%]
    static constexpr float _defaultFlightHeight = 30.f;    // [cm]
    static constexpr float _defaultHorizontAngle = 90.f;   // [degrees]
    static constexpr float _defaultStopMotorHeight = 5;    // [cm]

    bool _unlimitedBattery = false; // for debug purposes
};