#include <QDebug>

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

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


AzMissionController::AzMissionController(AzMission &mission, QObject *parent)
    : QObject(parent)
    , mMission(mission)
    , mAction(nullptr)
    , mMissionStarted(false)
    , mAbsoluteAltitude(-10000)
{}

void AzMissionController::setAction(Action *action)
{
    mAction = action;
}

bool AzMissionController::startMissions(float absoluteAltitude)
{
    mCurrentMissionIndex = -1;
    mMissionStarted      = true;
    mAbsoluteAltitude    = absoluteAltitude; // TODO!! Use altitudes from the JSON file.
    return flyToNextMissionItem();
}

void AzMissionController::stopMissions(void)
{
    // TODO!! Needs to be improved. At least send signal to AzDroneController.
    mMissionStarted = false;
}

bool AzMissionController::flyToNextMissionItem(void)
{
    mCurrentMissionIndex++;

    if (mCurrentMissionIndex >= (int) mMission.getActionPoints().size()) {
        qDebug() << "AzMissionController::flyToNextMissionItem() All mission action points are handled.";
        mMissionStarted = false;
        emit finished();
        return true;
    }

    qDebug() << "AzMissionController::flyToNextMissionItem() flying to the item" << mCurrentMissionIndex + 1 << "/"
             << mMission.getActionPoints().size();

    const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
    qDebug() << "AzMissionController::flyToNextMissionItem() navigating to point" << coordinate.latitude
             << coordinate.longitude;

    qDebug() << "AzMissionController::flyToNextMissionItem() MAVSDK::Action::goto_location() starts";
    Action::Result result = mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f);
    qDebug() << "AzMissionController::flyToNextMissionItem() MAVSDK::Action::goto_location() ends";

    // TODO!! Check return value and print warnings and errors.
    if (result == Action::Result::Success) {
        emit missionProgressChanged(mCurrentMissionIndex + 1, mMission.getActionPoints().size());
        return true;
    }

    return false;
}

void AzMissionController::newPosition(Telemetry::Position pos)
{
    if (mMissionStarted == false) {
        return;
    }

    const AzCoordinate &target = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
    float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, target.latitude, target.longitude);

    qDebug() << "AzMissionController::newPosition() distance to target: " << distance;

    // TODO!! In final application we need to use the camera to find the target.
    if (distance <= 0.01) {
        qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item.";
        flyToNextMissionItem();
    }
}