#include <QCoreApplication>
#include <QDebug>
#include "az_drone_controller_plane.h"

AzDroneControllerPlane::AzDroneControllerPlane(AzMission &mission, QObject *parent) :
    AzDroneController(mission, parent)
{}


bool AzDroneControllerPlane::stateWaitAutoSwitch(void)
{
    Telemetry::FlightMode flight_mode = mTelemetry->flight_mode();
    cout << "[CONTROLLER] Current flight mode: " << flight_mode << endl;
    return flight_mode == Telemetry::FlightMode::Mission;
}


void AzDroneControllerPlane::droneStateMachineSlot(void)
{
    using MethodPtr = bool (AzDroneControllerPlane::*)();

    struct MethodInfo
    {
        AzDroneState currentState;
        MethodPtr nextStateMethodPtr;
        string description;
        int nextStateDelay;
    };

    // Plane type will pass arming and taking off states and wait for auto mode from the RC controller instead.
    static const MethodInfo stateMethods[]
        = {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneControllerPlane::stateConnect, "Connecting to autopilot", 1000},
           {AZ_DRONE_STATE_CONNECTED, &AzDroneControllerPlane::stateGetSystem, "Getting MAVSDK system", 1000},
           {AZ_DRONE_STATE_GOT_SYSTEM, &AzDroneControllerPlane::stateGetTelemetryModule, "Getting telemetry module", 1000},
           {AZ_DRONE_STATE_GOT_TELEMETRY_MODULE, &AzDroneControllerPlane::stateGetActionModule, "Getting action module", 1000},
           {AZ_DRONE_STATE_GOT_ACTION_MODULE, &AzDroneControllerPlane::stateHealthOk, "Drone health OK", 1000},
           {AZ_DRONE_STATE_HEALTH_OK, &AzDroneControllerPlane::stateWaitAutoSwitch, "User switched to AUTO mode", 1000},
           {AZ_DRONE_STATE_AUTO_MODE_ACTIVATED, &AzDroneControllerPlane::stateFlyMission, "Starting the mission", 1000},
           {AZ_DRONE_STATE_FLY_MISSION, nullptr, "Mission started", 0}};

    // Iterate through states. If the state function fails, it's repeated until it succeeds.
    for (uint i = 0; i < sizeof(stateMethods) / sizeof(MethodInfo); i++) {
        if (stateMethods[i].currentState == mDroneState) {
            if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
                cout << "[CONTROLLER] Waiting signals from the mission controller." << endl;
                return;
            }

            bool result = (this->*stateMethods[i].nextStateMethodPtr)();

            if (result) {
                cout << "[CONTROLLER] " << stateMethods[i].description << " succeeded." << endl;
                mDroneState = stateMethods[i + 1].currentState;
            }
            else {
                cout << "[CONTROLLER] " << stateMethods[i].description << " failed. Trying again." << endl;
            }

            delayedStateCallSlot(stateMethods[i].nextStateDelay);

            break;
        }
    }
}