#include "PID.hpp"
#include <memory>
#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]
};

struct PidSettings {
    float p;
    float i;
    float d;
};

class FlightController {
  public:
    FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
                     PIDController *pid3, bool unlimitedBattery = false);
    ~FlightController();
    ReportedDeviceState currentDeviceState() const;
    void startTakeoff();
    void startBoarding();
    void startImuCalibration();
    void tick();
    [[deprecated]] void moveToFlightHeight(float newFlightHeight);  //[cm]
    [[deprecated]] void newFlightHeightStickPosition(int16_t newFlightHeightStickPostition);
    [[deprecated]] void newYawStickPosition(int16_t newYawStickPostition);
    [[deprecated]] void newPitchStickPosition(int16_t newPitchStickPostition);
    void newRotor1Duty(uint32_t rotor1Duty);
    void newRotor2Duty(uint32_t rotor2Duty);
    void newRotor3Duty(uint32_t rotor3Duty);
    void stopAllRotors();
    void setPid1Params(float p, float i, float d);
    void setPid2Params(float p, float i, float d);
    void setPid3Params(float p, float i, float d);
    PidSettings *pidSettings() const;
  private:
    void _updateBatteryCharge();
    [[deprecated]] void _takeoff();
    [[deprecated]] 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
};