#pragma once

#include <memory.h>
#include <QObject>

#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>

#include "az_mission.h"
#include "az_mission_controller.h"

using namespace mavsdk;

typedef enum {
    AZ_DRONE_STATE_DISCONNECTED,
    AZ_DRONE_STATE_CONNECTED,
    AZ_DRONE_STATE_GOT_SYSTEM,
    AZ_DRONE_STATE_GOT_TELEMETRY_MODULE,
    AZ_DRONE_STATE_GOT_ACTION_MODULE,
    AZ_DRONE_STATE_HEALTH_OK,
    AZ_DRONE_STATE_ARMED,
    AZ_DRONE_STATE_TAKE_OFF,
    AZ_DRONE_STATE_AUTO_MODE_ACTIVATED,
    AZ_DRONE_STATE_FLY_MISSION,
    AZ_DRONE_STATE_LAND,
    AZ_DRONE_STATE_END,
} AzDroneState;

class AzDroneController : public QObject
{
    Q_OBJECT
public:
    explicit AzDroneController(AzMission &mission, QObject *parent = nullptr);

    // Starts the autopilot.
    void start();

protected:
    // Specify the methods that will be called one at a time to initialise the drone and fly the mission.
    bool stateConnect(void);
    bool stateGetSystem(void);
    bool stateGetTelemetryModule(void);
    bool stateGetActionModule(void);
    bool stateHealthOk(void);
    bool stateArm(void);
    bool stateTakeoff(void);
    bool stateFlyMission(void);

    // Slots that are called by the emitted Qt signals.
protected slots:
    // Launches a state machine that progressively initialises the drone without blocking.
    virtual void droneStateMachineSlot(void);

    // ArduPilot seems to require some delay between calls. This method uses QTimer internally
    // to call each new state method with some delay without blocking the main thread.
    void delayedStateCallSlot(int ms);

    // Slot that is called when the newPosition() signal below is emitted.
    void newPositionSlot(Telemetry::Position);

    // A mission file contains several action points. This is called
    // when the action point is reached. Indexing starts at 1.
    void missionIndexChangedSlot(int currentIndex, int totalIndexes);

    // Called at the end of the mission. Lands and disarms the drone.
    void missionFinishedSlot(void);

    // Gets Telemetry::Health information in prearming.
    void newHealthInfoSlot(Telemetry::Health health);

signals:
    // Signal is emitted when Ardupilot sends a new position from the
    // MAVSDK thread. Signal goes through the main event loop and is
    // captured in the main thread to avoid threading issues.
    void newPosition(Telemetry::Position);

    // If Telemetry::health_all_ok() fails, then autopilot subscripes for the healt updates to
    // see exactly what is wrong. This signal is emitted to catch updates in the main thread.
    void newHealthInfo(Telemetry::Health);

protected:
    Mavsdk mMavsdk;
    AzDroneState mDroneState;
    shared_ptr<System> mSystem;
    Telemetry *mTelemetry;
    Action *mAction;
    Telemetry::Position mFirstPosition;
    Telemetry::Position mCurrentPosition;
    AzMissionController *mMissionController;
};