MissionController.cc

/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/


#include "MissionController.h"
#include "MultiVehicleManager.h"
#include "MissionManager.h"
#include "CoordinateVector.h"
#include "FirmwarePlugin.h"
#include "QGCApplication.h"
#include "SimpleMissionItem.h"
#include "SurveyMissionItem.h"
#include "JsonHelper.h"
#include "ParameterManager.h"
#include "QGroundControlQmlGlobal.h"

#ifndef __mobile__
#include "QGCFileDialog.h"
#endif

QGC_LOGGING_CATEGORY(MissionControllerLog, "MissionControllerLog")

const char* MissionController::jsonSimpleItemsKey = "items";

const char* MissionController::_settingsGroup =                 "MissionController";
const char* MissionController::_jsonMavAutopilotKey =           "MAV_AUTOPILOT";
const char* MissionController::_jsonComplexItemsKey =           "complexItems";
const char* MissionController::_jsonPlannedHomePositionKey =    "plannedHomePosition";

MissionController::MissionController(QObject *parent)
    : PlanElementController(parent)
    , _visualItems(NULL)
    , _complexItems(NULL)
    , _firstItemsFromVehicle(false)
    , _missionItemsRequested(false)
    , _queuedSend(false)
    , _missionDistance(0.0)
    , _missionMaxTelemetry(0.0)
    , _cruiseDistance(0.0)
    , _hoverDistance(0.0)
{

}

MissionController::~MissionController()
{

}

void MissionController::start(bool editMode)
{
    qCDebug(MissionControllerLog) << "start editMode" << editMode;

    PlanElementController::start(editMode);

    // We start with an empty mission
    _visualItems = new QmlObjectListModel(this);
    _addPlannedHomePosition(_visualItems, false /* addToCenter */);
    _initAllVisualItems();
}

// Called when new mission items have completed downloading from Vehicle
void MissionController::_newMissionItemsAvailableFromVehicle(void)
{
    qCDebug(MissionControllerLog) << "_newMissionItemsAvailableFromVehicle";

    if (!_editMode || _missionItemsRequested || _visualItems->count() == 1) {
        // Fly Mode:
        //      - Always accepts new items from the vehicle so Fly view is kept up to date
        // Edit Mode:
        //      - Either a load from vehicle was manually requested or
        //      - The initial automatic load from a vehicle completed and the current editor is empty

        QmlObjectListModel* newControllerMissionItems = new QmlObjectListModel(this);
        const QList<MissionItem*>& newMissionItems = _activeVehicle->missionManager()->missionItems();

        qCDebug(MissionControllerLog) << "loading from vehicle: count"<< _visualItems->count();
        foreach(const MissionItem* missionItem, newMissionItems) {
            newControllerMissionItems->append(new SimpleMissionItem(_activeVehicle, *missionItem, this));
        }

        _deinitAllVisualItems();

        _visualItems->deleteListAndContents();
        _visualItems = newControllerMissionItems;

        if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _visualItems->count() == 0) {
            _addPlannedHomePosition(_visualItems,true /* addToCenter */);
        }

        _missionItemsRequested = false;

        _initAllVisualItems();
        emit newItemsFromVehicle();
    }
}

void MissionController::loadFromVehicle(void)
{
    Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();

    if (activeVehicle) {
        _missionItemsRequested = true;
        activeVehicle->missionManager()->requestMissionItems();
    }
}

void MissionController::sendToVehicle(void)
{
    if (_activeVehicle) {
        // Convert to MissionItems so we can send to vehicle
        QList<MissionItem*> missionItems;

        for (int i=0; i<_visualItems->count(); i++) {
            VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
            if (visualItem->isSimpleItem()) {
                missionItems.append(new MissionItem(qobject_cast<SimpleMissionItem*>(visualItem)->missionItem()));
            } else {
                ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
                QmlObjectListModel* complexMissionItems = complexItem->getMissionItems();
                for (int j=0; j<complexMissionItems->count(); j++) {
                    missionItems.append(new MissionItem(*qobject_cast<MissionItem*>(complexMissionItems->get(j))));
                }
                delete complexMissionItems;
            }
        }

        _activeVehicle->missionManager()->writeMissionItems(missionItems);
        _visualItems->setDirty(false);

        for (int i=0; i<missionItems.count(); i++) {
            delete missionItems[i];
        }
    }
}

int MissionController::_nextSequenceNumber(void)
{
    if (_visualItems->count() == 0) {
        qWarning() << "Internal error: Empty visual item list";
        return 0;
    } else {
        VisualMissionItem* lastItem = qobject_cast<VisualMissionItem*>(_visualItems->get(_visualItems->count() - 1));

        if (lastItem->isSimpleItem()) {
            return lastItem->sequenceNumber() + 1;
        } else {
            return qobject_cast<ComplexMissionItem*>(lastItem)->lastSequenceNumber() + 1;
        }
    }
}

int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
{
    int sequenceNumber = _nextSequenceNumber();
    SimpleMissionItem * newItem = new SimpleMissionItem(_activeVehicle, this);
    newItem->setSequenceNumber(sequenceNumber);
    newItem->setCoordinate(coordinate);
    newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT);
    _initVisualItem(newItem);
    if (_visualItems->count() == 1) {
        newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF);
    }
    newItem->setDefaultsForCommand();
    if ((MAV_CMD)newItem->command() == MAV_CMD_NAV_WAYPOINT) {
        double lastValue;
        MAV_FRAME lastFrame;

        if (_findLastAcceptanceRadius(&lastValue)) {
            newItem->missionItem().setParam2(lastValue);
        }
        if (_findLastAltitude(&lastValue, &lastFrame)) {
            newItem->missionItem().setFrame(lastFrame);
            newItem->missionItem().setParam7(lastValue);
        }
    }
    _visualItems->insert(i, newItem);

    _recalcAll();

    return newItem->sequenceNumber();
}

int MissionController::insertComplexMissionItem(QGeoCoordinate coordinate, int i)
{
    int sequenceNumber = _nextSequenceNumber();
    SurveyMissionItem* newItem = new SurveyMissionItem(_activeVehicle, this);
    newItem->setSequenceNumber(sequenceNumber);
    newItem->setCoordinate(coordinate);
    _initVisualItem(newItem);

    _visualItems->insert(i, newItem);
    _complexItems->append(newItem);

    _recalcAll();

    return newItem->sequenceNumber();
}

void MissionController::removeMissionItem(int index)
{
    VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->removeAt(index));

    _deinitVisualItem(item);
    if (!item->isSimpleItem()) {
        ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(_complexItems->removeOne(item));
        if (!complexItem) {
            qWarning() << "Complex item missing";
        }
    }
    item->deleteLater();

    _recalcAll();

    // Set the new current item

    if (index >= _visualItems->count()) {
        index--;
    }
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* item =  qobject_cast<VisualMissionItem*>(_visualItems->get(i));
        item->setIsCurrentItem(i == index);
    }
    _visualItems->setDirty(true);
}

void MissionController::removeAll(void)
{
    if (_visualItems) {
        _deinitAllVisualItems();
        _visualItems->deleteListAndContents();
        _visualItems = new QmlObjectListModel(this);
        _addPlannedHomePosition(_visualItems, false /* addToCenter */);
        _initAllVisualItems();
        _visualItems->setDirty(true);
    }
}

bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectListModel* visualItems, QmlObjectListModel* complexItems, QString& errorString)
{
    QJsonParseError jsonParseError;
    QJsonDocument   jsonDoc(QJsonDocument::fromJson(bytes, &jsonParseError));

    if (jsonParseError.error != QJsonParseError::NoError) {
        errorString = jsonParseError.errorString();
        return false;
    }

    QJsonObject json = jsonDoc.object();

    // Check for required keys
    QStringList requiredKeys;
    requiredKeys << JsonHelper::jsonVersionKey << _jsonPlannedHomePositionKey;
    if (!JsonHelper::validateRequiredKeys(json, requiredKeys, errorString)) {
        return false;
    }

    // Validate base key types
    QStringList             keyList;
    QList<QJsonValue::Type> typeList;
    keyList << jsonSimpleItemsKey << JsonHelper::jsonVersionKey << JsonHelper::jsonGroundStationKey << _jsonMavAutopilotKey << _jsonComplexItemsKey << _jsonPlannedHomePositionKey;
    typeList << QJsonValue::Array << QJsonValue::String << QJsonValue::String << QJsonValue::Double << QJsonValue::Array << QJsonValue::Object;
    if (!JsonHelper::validateKeyTypes(json, keyList, typeList, errorString)) {
        return false;
    }

    // Version check
    if (json[JsonHelper::jsonVersionKey].toString() != "1.0") {
        errorString = QStringLiteral("QGroundControl does not support this file version");
        return false;
    }

    // Read complex items
    QJsonArray complexArray(json[_jsonComplexItemsKey].toArray());
    qCDebug(MissionControllerLog) << "Json load: complex item count" << complexArray.count();
    for (int i=0; i<complexArray.count(); i++) {
        const QJsonValue& itemValue = complexArray[i];

        if (!itemValue.isObject()) {
            errorString = QStringLiteral("Mission item is not an object");
            return false;
        }

        SurveyMissionItem* item = new SurveyMissionItem(_activeVehicle, this);
        if (item->load(itemValue.toObject(), errorString)) {
            qCDebug(MissionControllerLog) << "Json load: complex item start:stop" << item->sequenceNumber() << item->lastSequenceNumber();
            complexItems->append(item);
        } else {
            return false;
        }
    }

    // Read simple items, interspersing complex items into the full list

    int nextSimpleItemIndex= 0;
    int nextComplexItemIndex= 0;
    int nextSequenceNumber = 1; // Start with 1 since home is in 0
    QJsonArray itemArray(json[jsonSimpleItemsKey].toArray());

    qCDebug(MissionControllerLog) << "Json load: simple item loop start simpleItemCount:ComplexItemCount" << itemArray.count() << complexItems->count();
    do {
        qCDebug(MissionControllerLog) << "Json load: simple item loop nextSimpleItemIndex:nextComplexItemIndex:nextSequenceNumber" << nextSimpleItemIndex << nextComplexItemIndex << nextSequenceNumber;

        // If there is a complex item that should be next in sequence add it in
        if (nextComplexItemIndex < complexItems->count()) {
            SurveyMissionItem* complexItem = qobject_cast<SurveyMissionItem*>(complexItems->get(nextComplexItemIndex));

            if (complexItem->sequenceNumber() == nextSequenceNumber) {
                qCDebug(MissionControllerLog) << "Json load: injecting complex item expectedSequence:actualSequence:" << nextSequenceNumber << complexItem->sequenceNumber();
                visualItems->append(complexItem);
                nextSequenceNumber = complexItem->lastSequenceNumber() + 1;
                nextComplexItemIndex++;
                continue;
            }
        }

        // Add the next available simple item
        if (nextSimpleItemIndex < itemArray.count()) {
            const QJsonValue& itemValue = itemArray[nextSimpleItemIndex++];

            if (!itemValue.isObject()) {
                errorString = QStringLiteral("Mission item is not an object");
                return false;
            }

            SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this);
            if (item->load(itemValue.toObject(), errorString)) {
                qCDebug(MissionControllerLog) << "Json load: adding simple item expectedSequence:actualSequence" << nextSequenceNumber << item->sequenceNumber();
                visualItems->append(item);
            } else {
                return false;
            }

            nextSequenceNumber++;
        }
    } while (nextSimpleItemIndex < itemArray.count() || nextComplexItemIndex < complexItems->count());

    if (json.contains(_jsonPlannedHomePositionKey)) {
        SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this);

        if (item->load(json[_jsonPlannedHomePositionKey].toObject(), errorString)) {
            visualItems->insert(0, item);
        } else {
            return false;
        }
    } else {
        _addPlannedHomePosition(visualItems, true /* addToCenter */);
    }

    return true;
}

bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString)
{
    bool addPlannedHomePosition = false;

    QString firstLine = stream.readLine();
    const QStringList& version = firstLine.split(" ");

    bool versionOk = false;
    if (version.size() == 3 && version[0] == "QGC" && version[1] == "WPL") {
        if (version[2] == "110") {
            // ArduPilot file, planned home position is already in position 0
            versionOk = true;
        } else if (version[2] == "120") {
            // Old QGC file, no planned home position
            versionOk = true;
            addPlannedHomePosition = true;
        }
    }

    if (versionOk) {
        while (!stream.atEnd()) {
            SimpleMissionItem* item = new SimpleMissionItem(_activeVehicle, this);

            if (item->load(stream)) {
                visualItems->append(item);
            } else {
                errorString = QStringLiteral("The mission file is corrupted.");
                return false;
            }
        }
    } else {
        errorString = QStringLiteral("The mission file is not compatible with this version of QGroundControl.");
        return false;
    }

    if (addPlannedHomePosition || visualItems->count() == 0) {
        _addPlannedHomePosition(visualItems, true /* addToCenter */);

        // Update sequence numbers in DO_JUMP commands to take into account added home position in index 0
        for (int i=1; i<visualItems->count(); i++) {
            SimpleMissionItem* item = qobject_cast<SimpleMissionItem*>(visualItems->get(i));
            if (item && item->command() == MavlinkQmlSingleton::MAV_CMD_DO_JUMP) {
                item->missionItem().setParam1((int)item->missionItem().param1() + 1);
            }
        }
    }

    return true;
}

void MissionController::loadFromFile(const QString& filename)
{
    QString errorString;

    if (filename.isEmpty()) {
        return;
    }

    QmlObjectListModel* newVisualItems = new QmlObjectListModel(this);
    QmlObjectListModel* newComplexItems = new QmlObjectListModel(this);

    QFile file(filename);

    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        errorString = file.errorString();
    } else {
        QByteArray  bytes = file.readAll();
        QTextStream stream(&bytes);

        QString firstLine = stream.readLine();
        if (firstLine.contains(QRegExp("QGC.*WPL"))) {
            stream.seek(0);
            _loadTextMissionFile(stream, newVisualItems, errorString);
        } else {
            _loadJsonMissionFile(bytes, newVisualItems, newComplexItems, errorString);
        }
    }

    if (!errorString.isEmpty()) {
        for (int i=0; i<newVisualItems->count(); i++) {
            newVisualItems->get(i)->deleteLater();
        }
        for (int i=0; i<newComplexItems->count(); i++) {
            newComplexItems->get(i)->deleteLater();
        }
        delete newVisualItems;
        delete newComplexItems;

        qgcApp()->showMessage(errorString);
        return;
    }

    if (_visualItems) {
        _deinitAllVisualItems();
        _visualItems->deleteListAndContents();
    }
    if (_complexItems) {
        _complexItems->deleteLater();
    }

    _visualItems = newVisualItems;
    _complexItems = newComplexItems;

    if (_visualItems->count() == 0) {
        _addPlannedHomePosition(_visualItems, true /* addToCenter */);
    }

    _initAllVisualItems();
}

void MissionController::loadFromFilePicker(void)
{
#ifndef __mobile__
    QString filename = QGCFileDialog::getOpenFileName(NULL, "Select Mission File to load", QString(), "Mission file (*.mission);;All Files (*.*)");

    if (filename.isEmpty()) {
        return;
    }
    loadFromFile(filename);
#endif
}

void MissionController::saveToFile(const QString& filename)
{
    qDebug() << filename;

    if (filename.isEmpty()) {
        return;
    }

    QString missionFilename = filename;
    if (!QFileInfo(filename).fileName().contains(".")) {
        missionFilename += QString(".%1").arg(QGCApplication::missionFileExtension);
    }

    QFile file(missionFilename);

    if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
        qgcApp()->showMessage(file.errorString());
    } else {
        QJsonObject missionFileObject;      // top level json object
        QJsonArray  simpleItemsObject;
        QJsonArray  complexItemsObject;

        missionFileObject[JsonHelper::jsonVersionKey] =         "1.0";
        missionFileObject[JsonHelper::jsonGroundStationKey] =   JsonHelper::jsonGroundStationValue;

        MAV_AUTOPILOT firmwareType = MAV_AUTOPILOT_GENERIC;
        if (_activeVehicle) {
            firmwareType = _activeVehicle->firmwareType();
        } else {
            // FIXME: Hack duplicated code from QGroundControlQmlGlobal. Had to do this for now since
            // QGroundControlQmlGlobal is not available from C++ side.

            QSettings settings;
            firmwareType = (MAV_AUTOPILOT)settings.value("OfflineEditingFirmwareType", MAV_AUTOPILOT_ARDUPILOTMEGA).toInt();
        }
        missionFileObject[_jsonMavAutopilotKey] = firmwareType;

        // Save planned home position
        QJsonObject homePositionObject;
        SimpleMissionItem* homeItem = qobject_cast<SimpleMissionItem*>(_visualItems->get(0));
        if (homeItem) {
            homeItem->missionItem().save(homePositionObject);
        } else {
            qgcApp()->showMessage(QStringLiteral("Internal error: VisualMissionItem at index 0 not SimpleMissionItem"));
            return;
        }
        missionFileObject[_jsonPlannedHomePositionKey] = homePositionObject;

        // Save the visual items
        for (int i=1; i<_visualItems->count(); i++) {
            QJsonObject itemObject;

            VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
            visualItem->save(itemObject);

            if (visualItem->isSimpleItem()) {
                simpleItemsObject.append(itemObject);
            } else {
                complexItemsObject.append(itemObject);
            }
        }

        missionFileObject[jsonSimpleItemsKey] = simpleItemsObject;
        missionFileObject[_jsonComplexItemsKey] = complexItemsObject;

        QJsonDocument saveDoc(missionFileObject);
        file.write(saveDoc.toJson());
    }

    _visualItems->setDirty(false);
}

void MissionController::saveToFilePicker(void)
{
#ifndef __mobile__
    QString filename = QGCFileDialog::getSaveFileName(NULL, "Select file to save mission to", QString(), "Mission file (*.mission);;All Files (*.*)");

    if (filename.isEmpty()) {
        return;
    }
    saveToFile(filename);
#endif
}

void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference)
{
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
    QGeoCoordinate  prevCoord =     prevItem->exitCoordinate();
    bool            distanceOk =    false;

    // Convert to fixed altitudes

    qCDebug(MissionControllerLog) << homeAlt
                                  << currentItem->coordinateHasRelativeAltitude() << currentItem->coordinate().altitude()
                                  << prevItem->exitCoordinateHasRelativeAltitude() << prevItem->exitCoordinate().altitude();

    distanceOk = true;
    if (currentItem->coordinateHasRelativeAltitude()) {
        currentCoord.setAltitude(homeAlt + currentCoord.altitude());
    }
    if (prevItem->exitCoordinateHasRelativeAltitude()) {
        prevCoord.setAltitude(homeAlt + prevCoord.altitude());
    }

    qCDebug(MissionControllerLog) << "distanceOk" << distanceOk;

    if (distanceOk) {
        *altDifference = currentCoord.altitude() - prevCoord.altitude();
        *distance = prevCoord.distanceTo(currentCoord);
        *azimuth = prevCoord.azimuthTo(currentCoord);
    } else {
        *altDifference = 0.0;
        *azimuth = 0.0;
        *distance = 0.0;
    }
}

void MissionController::_calcHomeDist(VisualMissionItem* currentItem, VisualMissionItem* homeItem, double* distance)
{
    QGeoCoordinate  currentCoord =  currentItem->coordinate();
    QGeoCoordinate  homeCoord =     homeItem->exitCoordinate();
    bool            distanceOk =    false;

    distanceOk = true;

    qCDebug(MissionControllerLog) << "distanceOk" << distanceOk;

    if (distanceOk) {
        *distance = homeCoord.distanceTo(currentCoord);
    } else {
        *distance = 0.0;
    }
}

void MissionController::_recalcWaypointLines(void)
{
    bool                firstCoordinateItem =   true;
    VisualMissionItem*  lastCoordinateItem =    qobject_cast<VisualMissionItem*>(_visualItems->get(0));

    SimpleMissionItem*  homeItem = qobject_cast<SimpleMissionItem*>(lastCoordinateItem);

    if (!homeItem) {
        qWarning() << "Home item is not SimpleMissionItem";
    }

    bool    showHomePosition =  homeItem->showHomePosition();

    qCDebug(MissionControllerLog) << "_recalcWaypointLines";

    CoordVectHashTable old_table = _linesTable;
    _linesTable.clear();
    _waypointLines.clear();

    bool linkBackToHome = false;
    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));


        // If we still haven't found the first coordinate item and we hit a a takeoff command link back to home
        if (firstCoordinateItem &&
                item->isSimpleItem() &&
                (qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF ||
                 qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF)) {
            linkBackToHome = true;
        }

        if (item->specifiesCoordinate()) {
            if (!item->isStandaloneCoordinate()) {
                firstCoordinateItem = false;
                VisualItemPair pair(lastCoordinateItem, item);
                if (lastCoordinateItem != homeItem || (showHomePosition && linkBackToHome)) {
                    if (old_table.contains(pair)) {
                        // Do nothing, this segment already exists and is wired up
                        _linesTable[pair] = old_table.take(pair);
                    } else {
                        // Create a new segment and wire update notifiers
                        auto linevect       = new CoordinateVector(lastCoordinateItem->isSimpleItem() ? lastCoordinateItem->coordinate() : lastCoordinateItem->exitCoordinate(), item->coordinate(), this);
                        auto originNotifier = lastCoordinateItem->isSimpleItem() ? &VisualMissionItem::coordinateChanged : &VisualMissionItem::exitCoordinateChanged,
                             endNotifier    = &VisualMissionItem::coordinateChanged;
                        // Use signals/slots to update the coordinate endpoints
                        connect(lastCoordinateItem, originNotifier, linevect, &CoordinateVector::setCoordinate1);
                        connect(item,               endNotifier,    linevect, &CoordinateVector::setCoordinate2);

                        // FIXME: We should ideally have signals for 2D position change, alt change, and 3D position change
                        // Not optimal, but still pretty fast, do a full update of range/bearing/altitudes
                        connect(item, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcAltitudeRangeBearing);
                        _linesTable[pair] = linevect;
                    }
                }
                lastCoordinateItem = item;
            }
        }
    }


    {
        // Create a temporary QObjectList and replace the model data
        QObjectList objs;
        objs.reserve(_linesTable.count());
        foreach(CoordinateVector *vect, _linesTable.values()) {
            objs.append(vect);
        }

        // We don't delete here because many links may still be valid
        _waypointLines.swapObjectList(objs);
    }


    // Anything left in the old table is an obsolete line object that can go
    qDeleteAll(old_table);

    _recalcAltitudeRangeBearing();

    emit waypointLinesChanged();
}

void MissionController::_recalcAltitudeRangeBearing()
{
    if (!_visualItems->count())
        return;

    bool                firstCoordinateItem =   true;
    VisualMissionItem*  lastCoordinateItem =    qobject_cast<VisualMissionItem*>(_visualItems->get(0));
    SimpleMissionItem*  homeItem = qobject_cast<SimpleMissionItem*>(lastCoordinateItem);

    if (!homeItem) {
        qWarning() << "Home item is not SimpleMissionItem";
    }

    bool    showHomePosition =  homeItem->showHomePosition();

    qCDebug(MissionControllerLog) << "_recalcAltitudeRangeBearing";

    // If home position is valid we can calculate distances between all waypoints.
    // If home position is not valid we can only calculate distances between waypoints which are
    // both relative altitude.

    // No values for first item
    lastCoordinateItem->setAltDifference(0.0);
    lastCoordinateItem->setAzimuth(0.0);
    lastCoordinateItem->setDistance(0.0);

    double minAltSeen = 0.0;
    double maxAltSeen = 0.0;
    const double homePositionAltitude = homeItem->coordinate().altitude();
    minAltSeen = maxAltSeen = homeItem->coordinate().altitude();

    double missionDistance = 0.0;
    double missionMaxTelemetry = 0.0;

    bool vtolCalc = (QGroundControlQmlGlobal::offlineEditingVehicleType()->enumStringValue() == "VTOL" || (_activeVehicle && _activeVehicle->vtol())) ? true : false ;
    double cruiseDistance = 0.0;
    double hoverDistance = 0.0;
    bool hoverDistanceCalc = false;
    bool hoverTransition = false;
    bool cruiseTransition = false;
    bool hoverDistanceReset = false;

    bool linkBackToHome = false;


    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
        // Assume the worst
        item->setAzimuth(0.0);
        item->setDistance(0.0);

        // If we still haven't found the first coordinate item and we hit a takeoff command link back to home
        if (firstCoordinateItem &&
                item->isSimpleItem() &&
                qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
            linkBackToHome = true;
            hoverDistanceCalc = true;
        }

        if (item->isSimpleItem() && vtolCalc) {
            SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(item);
            if (simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_DO_VTOL_TRANSITION){  //transition waypoint value
                if (simpleItem->missionItem().param1() == 3){ //hover mode value
                    hoverDistanceCalc = true;
                    hoverTransition = true;
                }
                else if (simpleItem->missionItem().param1() == 4){
                    hoverDistanceCalc = false;
                    cruiseTransition = true;
                }
            }
            if(!hoverTransition && cruiseTransition && !hoverDistanceReset && !linkBackToHome){
                hoverDistance = missionDistance;
                hoverDistanceReset = true;
            }
        }

        if (item->specifiesCoordinate()) {
            // Keep track of the min/max altitude for all waypoints so we can show altitudes as a percentage

            double absoluteAltitude = item->coordinate().altitude();
            if (item->coordinateHasRelativeAltitude()) {
                absoluteAltitude += homePositionAltitude;
            }
            minAltSeen = std::min(minAltSeen, absoluteAltitude);
            maxAltSeen = std::max(maxAltSeen, absoluteAltitude);

            if (!item->exitCoordinateSameAsEntry()) {
                absoluteAltitude = item->exitCoordinate().altitude();
                if (item->exitCoordinateHasRelativeAltitude()) {
                    absoluteAltitude += homePositionAltitude;
                }
                minAltSeen = std::min(minAltSeen, absoluteAltitude);
                maxAltSeen = std::max(maxAltSeen, absoluteAltitude);
            }

            if (!item->isStandaloneCoordinate()) {
                firstCoordinateItem = false;
                if (lastCoordinateItem != homeItem || (showHomePosition && linkBackToHome)) {
                    double azimuth, distance, altDifference, telemetryDistance;

                    // Subsequent coordinate items link to last coordinate item. If the last coordinate item
                    // is an invalid home position we skip the line
                    _calcPrevWaypointValues(homePositionAltitude, item, lastCoordinateItem, &azimuth, &distance, &altDifference);
                    item->setAltDifference(altDifference);
                    item->setAzimuth(azimuth);
                    item->setDistance(distance);

                    missionDistance += distance;

                    if (item->isSimpleItem()) {
                        _calcHomeDist(item, homeItem, &telemetryDistance);

                        if (vtolCalc) {
                            SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(item);
                            if (simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF || hoverDistanceCalc){
                                hoverDistance += distance;
                            }
                            cruiseDistance = missionDistance - hoverDistance;
                            if(simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_NAV_LAND && !linkBackToHome && !cruiseTransition && !hoverTransition){
                                hoverDistance = cruiseDistance;
                                cruiseDistance = missionDistance - hoverDistance;
                            }
                        }
                    } else {
                        missionDistance += qobject_cast<ComplexMissionItem*>(item)->complexDistance();
                        telemetryDistance = qobject_cast<ComplexMissionItem*>(item)->greatestDistanceTo(homeItem->exitCoordinate());

                        if (vtolCalc){
                            cruiseDistance += qobject_cast<ComplexMissionItem*>(item)->complexDistance(); //assume all survey missions undertaken in cruise
                        }
                    }

                    if (telemetryDistance > missionMaxTelemetry) {
                        missionMaxTelemetry = telemetryDistance;
                    }
                }
                else if (lastCoordinateItem == homeItem && !item->isSimpleItem()){
                    missionDistance += qobject_cast<ComplexMissionItem*>(item)->complexDistance();
                    missionMaxTelemetry = qobject_cast<ComplexMissionItem*>(item)->greatestDistanceTo(homeItem->exitCoordinate());

                    if (vtolCalc){
                        cruiseDistance += qobject_cast<ComplexMissionItem*>(item)->complexDistance(); //assume all survey missions undertaken in cruise
                    }
                }
                lastCoordinateItem = item;
            }
        }
    }

    setMissionDistance(missionDistance);
    setMissionMaxTelemetry(missionMaxTelemetry);
    setCruiseDistance(cruiseDistance);
    setHoverDistance(hoverDistance);

    // Walk the list again calculating altitude percentages
    double altRange = maxAltSeen - minAltSeen;
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));

        if (item->specifiesCoordinate()) {
            double absoluteAltitude = item->coordinate().altitude();
            if (item->coordinateHasRelativeAltitude()) {
                absoluteAltitude += homePositionAltitude;
            }
            if (altRange == 0.0) {
                item->setAltPercent(0.0);
            } else {
                item->setAltPercent((absoluteAltitude - minAltSeen) / altRange);
            }
        }
    }
}

// This will update the sequence numbers to be sequential starting from 0
void MissionController::_recalcSequence(void)
{
    // Setup ascending sequence numbers for all visual items

    int sequenceNumber = 0;
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));

        item->setSequenceNumber(sequenceNumber++);
        if (!item->isSimpleItem()) {
            ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(item);

            if (complexItem) {
                 sequenceNumber = complexItem->lastSequenceNumber() + 1;
            } else {
                qWarning() << "isSimpleItem == false, yet not ComplexMissionItem";
            }
        }
    }
}

// This will update the child item hierarchy
void MissionController::_recalcChildItems(void)
{
    VisualMissionItem* currentParentItem = qobject_cast<VisualMissionItem*>(_visualItems->get(0));

    currentParentItem->childItems()->clear();

    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));

        // Set up non-coordinate item child hierarchy
        if (item->specifiesCoordinate()) {
            item->childItems()->clear();
            currentParentItem = item;
        } else if (item->isSimpleItem()) {
            currentParentItem->childItems()->append(item);
        }
    }
}

void MissionController::_recalcAll(void)
{
    _recalcSequence();
    _recalcChildItems();
    _recalcWaypointLines();
}

/// Initializes a new set of mission items
void MissionController::_initAllVisualItems(void)
{
    SimpleMissionItem* homeItem = NULL;

    // Setup home position at index 0

    homeItem = qobject_cast<SimpleMissionItem*>(_visualItems->get(0));
    if (!homeItem) {
        qWarning() << "homeItem not SimpleMissionItem";
        return;
    }

    homeItem->setHomePositionSpecialCase(true);
    homeItem->setShowHomePosition(_editMode);
    homeItem->missionItem().setCommand(MAV_CMD_NAV_WAYPOINT);
    homeItem->missionItem().setFrame(MAV_FRAME_GLOBAL);
    homeItem->setIsCurrentItem(true);

    if (!_editMode && _activeVehicle && _activeVehicle->homePositionAvailable()) {
        homeItem->setCoordinate(_activeVehicle->homePosition());
        homeItem->setShowHomePosition(true);
    }

    emit plannedHomePositionChanged(plannedHomePosition());

    connect(homeItem, &VisualMissionItem::coordinateChanged, this, &MissionController::_homeCoordinateChanged);

    QmlObjectListModel* newComplexItems = new QmlObjectListModel(this);
    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
        _initVisualItem(item);

        // Set up complex item list
        if (!item->isSimpleItem()) {
            ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(item);

            if (complexItem) {
                newComplexItems->append(item);
            } else {
                qWarning() << "isSimpleItem == false, but not ComplexMissionItem";
            }
        }
    }

    if (_complexItems) {
        _complexItems->deleteLater();
    }
    _complexItems = newComplexItems;

    _recalcAll();

    emit visualItemsChanged();
    emit complexVisualItemsChanged();

    connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::dirtyChanged);

    _visualItems->setDirty(false);
}

void MissionController::_deinitAllVisualItems(void)
{
    for (int i=0; i<_visualItems->count(); i++) {
        _deinitVisualItem(qobject_cast<VisualMissionItem*>(_visualItems->get(i)));
    }

    disconnect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::dirtyChanged);
}

void MissionController::_initVisualItem(VisualMissionItem* visualItem)
{
    _visualItems->setDirty(false);

    connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged,                 this, &MissionController::_recalcWaypointLines);
    connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged,       this, &MissionController::_recalcWaypointLines);
    connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged,   this, &MissionController::_recalcWaypointLines);

    if (visualItem->isSimpleItem()) {
        // We need to track commandChanged on simple item since recalc has special handling for takeoff command
        SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
        if (simpleItem) {
            connect(&simpleItem->missionItem()._commandFact, &Fact::valueChanged, this, &MissionController::_itemCommandChanged);
        } else {
            qWarning() << "isSimpleItem == true, yet not SimpleMissionItem";
        }
    } else {
        // We need to track changes of lastSequenceNumber so we can recalc sequence numbers for subsequence items
        ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
        connect(complexItem, &ComplexMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence);
        connect(complexItem, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcAltitudeRangeBearing);
    }
}

void MissionController::_deinitVisualItem(VisualMissionItem* visualItem)
{
    // Disconnect all signals
    disconnect(visualItem, 0, 0, 0);
}

void MissionController::_itemCommandChanged(void)
{
    _recalcChildItems();
    _recalcWaypointLines();
}

void MissionController::_activeVehicleBeingRemoved(void)
{
    qCDebug(MissionControllerLog) << "MissionController::_activeVehicleBeingRemoved";

    MissionManager* missionManager = _activeVehicle->missionManager();

    disconnect(missionManager, &MissionManager::newMissionItemsAvailable,   this, &MissionController::_newMissionItemsAvailableFromVehicle);
    disconnect(missionManager, &MissionManager::inProgressChanged,          this, &MissionController::_inProgressChanged);
    disconnect(missionManager, &MissionManager::currentItemChanged,         this, &MissionController::_currentMissionItemChanged);
    disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged,      this, &MissionController::_activeVehicleHomePositionAvailableChanged);
    disconnect(_activeVehicle, &Vehicle::homePositionChanged,               this, &MissionController::_activeVehicleHomePositionChanged);

    // We always remove all items on vehicle change. This leaves a user model hole:
    //      If the user has unsaved changes in the Plan view they will lose them
    removeAll();
}

void MissionController::_activeVehicleSet(void)
{
    // We always remove all items on vehicle change. This leaves a user model hole:
    //      If the user has unsaved changes in the Plan view they will lose them
    removeAll();

    MissionManager* missionManager = _activeVehicle->missionManager();

    connect(missionManager, &MissionManager::newMissionItemsAvailable,  this, &MissionController::_newMissionItemsAvailableFromVehicle);
    connect(missionManager, &MissionManager::inProgressChanged,         this, &MissionController::_inProgressChanged);
    connect(missionManager, &MissionManager::currentItemChanged,        this, &MissionController::_currentMissionItemChanged);
    connect(_activeVehicle, &Vehicle::homePositionAvailableChanged,     this, &MissionController::_activeVehicleHomePositionAvailableChanged);
    connect(_activeVehicle, &Vehicle::homePositionChanged,              this, &MissionController::_activeVehicleHomePositionChanged);

    if (_activeVehicle->parameterManager()->parametersReady() && !syncInProgress()) {
        // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle.
        // We don't request mission items for new vehicles since that will happen autamatically.
        loadFromVehicle();
    }

    _activeVehicleHomePositionChanged(_activeVehicle->homePosition());
    _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable());
}

void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePositionAvailable)
{
    if (!_editMode && _visualItems) {
        SimpleMissionItem* homeItem = qobject_cast<SimpleMissionItem*>(_visualItems->get(0));

        if (homeItem) {
            homeItem->setShowHomePosition(homePositionAvailable);
            emit plannedHomePositionChanged(plannedHomePosition());
            _recalcWaypointLines();
        } else {
            qWarning() << "Unabled to cast home item to SimpleMissionItem";
        }
    }
}

void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition)
{
    if (!_editMode && _visualItems) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(0));
        if (item) {
            if (item->coordinate() != homePosition) {
                item->setCoordinate(homePosition);
                qCDebug(MissionControllerLog) << "Home position update" << homePosition;
                emit plannedHomePositionChanged(plannedHomePosition());
                _recalcWaypointLines();
            }
        } else {
            qWarning() << "Unabled to cast home item to VisualMissionItem";
        }
    }
}

void MissionController::setMissionDistance(double missionDistance)
{
    if (!qFuzzyCompare(_missionDistance, missionDistance)) {
        _missionDistance = missionDistance;
        emit missionDistanceChanged(_missionDistance);
    }
}

void MissionController::setMissionMaxTelemetry(double missionMaxTelemetry)
{
    if (!qFuzzyCompare(_missionMaxTelemetry, missionMaxTelemetry)) {
        _missionMaxTelemetry = missionMaxTelemetry;
        emit missionMaxTelemetryChanged(_missionMaxTelemetry);
    }
}

void MissionController::setCruiseDistance(double cruiseDistance)
{
    if (!qFuzzyCompare(_cruiseDistance, cruiseDistance)) {
        _cruiseDistance = cruiseDistance;
        emit cruiseDistanceChanged(_cruiseDistance);
    }
}

void MissionController::setHoverDistance(double hoverDistance)
{
    if (!qFuzzyCompare(_hoverDistance, hoverDistance)) {
        _hoverDistance = hoverDistance;
        emit hoverDistanceChanged(_hoverDistance);
    }
}

void MissionController::_inProgressChanged(bool inProgress)
{
    emit syncInProgressChanged(inProgress);
}

bool MissionController::_findLastAltitude(double* lastAltitude, MAV_FRAME* frame)
{
    bool found = false;
    double foundAltitude;
    MAV_FRAME foundFrame;

    // Don't use home position
    for (int i=1; i<_visualItems->count(); i++) {
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));

        if (visualItem->specifiesCoordinate() && !visualItem->isStandaloneCoordinate()) {

            if (visualItem->isSimpleItem()) {
                SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
                if ((MAV_CMD)simpleItem->command() != MAV_CMD_NAV_TAKEOFF) {
                    foundAltitude = simpleItem->exitCoordinate().altitude();
                    foundFrame = simpleItem->missionItem().frame();
                    found = true;
                }
            }
        }
    }

    if (found) {
        *lastAltitude = foundAltitude;
        *frame = foundFrame;
    }

    return found;
}

bool MissionController::_findLastAcceptanceRadius(double* lastAcceptanceRadius)
{
    bool found = false;
    double foundAcceptanceRadius;

    for (int i=0; i<_visualItems->count(); i++) {
        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));

        if (visualItem->isSimpleItem()) {
            SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);

            if (simpleItem) {
                if ((MAV_CMD)simpleItem->command() == MAV_CMD_NAV_WAYPOINT) {
                    foundAcceptanceRadius = simpleItem->missionItem().param2();
                    found = true;
                }
            } else {
                qWarning() << "isSimpleItem == true, yet not SimpleMissionItem";
            }
        }
    }

    if (found) {
        *lastAcceptanceRadius = foundAcceptanceRadius;
    }

    return found;
}

double MissionController::_normalizeLat(double lat)
{
    // Normalize latitude to range: 0 to 180, S to N
    return lat + 90.0;
}

double MissionController::_normalizeLon(double lon)
{
    // Normalize longitude to range: 0 to 360, W to E
    return lon  + 180.0;
}

/// Add the home position item to the front of the list
void MissionController::_addPlannedHomePosition(QmlObjectListModel* visualItems, bool addToCenter)
{
    SimpleMissionItem* homeItem = new SimpleMissionItem(_activeVehicle, this);
    visualItems->insert(0, homeItem);

    if (visualItems->count() > 1  && addToCenter) {
        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(visualItems->get(1));

        double north = _normalizeLat(item->coordinate().latitude());
        double south = north;
        double east = _normalizeLon(item->coordinate().longitude());
        double west = east;

        for (int i=2; i<visualItems->count(); i++) {
            item = qobject_cast<VisualMissionItem*>(visualItems->get(i));

            double lat = _normalizeLat(item->coordinate().latitude());
            double lon = _normalizeLon(item->coordinate().longitude());

            north = fmax(north, lat);
            south = fmin(south, lat);
            east = fmax(east, lon);
            west = fmin(west, lon);
        }

        homeItem->setCoordinate(QGeoCoordinate((south + ((north - south) / 2)) - 90.0, (west + ((east - west) / 2)) - 180.0, 0.0));
    } else {
        homeItem->setCoordinate(qgcApp()->lastKnownHomePosition());
    }
}

void MissionController::_currentMissionItemChanged(int sequenceNumber)
{
    if (!_editMode) {
        if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
            sequenceNumber++;
        }

        for (int i=0; i<_visualItems->count(); i++) {
            VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
            item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber);
        }
    }
}

bool MissionController::syncInProgress(void) const
{
    return _activeVehicle ? _activeVehicle->missionManager()->inProgress() : false;
}

bool MissionController::dirty(void) const
{
    return _visualItems ? _visualItems->dirty() : false;
}


void MissionController::setDirty(bool dirty)
{
    if (_visualItems) {
        _visualItems->setDirty(dirty);
    }
}

QGeoCoordinate MissionController::plannedHomePosition(void)
{
    if (_visualItems && _visualItems->count() > 0) {
        SimpleMissionItem* item = qobject_cast<SimpleMissionItem*>(_visualItems->get(0));
        if (item && item->showHomePosition()) {
            return item->coordinate();
        }
    }

    return QGeoCoordinate();
}

void MissionController::_homeCoordinateChanged(void)
{
    emit plannedHomePositionChanged(plannedHomePosition());
    _recalcAltitudeRangeBearing();
}

QString MissionController::fileExtension(void) const
{
    return QGCApplication::missionFileExtension;
}