DefaultManager.cpp

#include "DefaultManager.h"
#include "Wima/Geometry/GeoUtilities.h"
#include "Wima/Geometry/PolygonCalculus.h"

#include "MissionSettingsItem.h"
#include "SimpleMissionItem.h"


WaypointManager::DefaultManager::DefaultManager(Settings &settings,
                                                AreaInterface &interface)
    : ManagerBase(settings)
    , _areaInterface(&interface)
{

}

void WaypointManager::DefaultManager::clear()
{
    _dirty = true;
    _waypoints.clear();
    _currentWaypoints.clear();
    _missionItems.clearAndDeleteContents();
    _currentMissionItems.clearAndDeleteContents();
    _waypointsVariant.clear();
    _currentWaypointsVariant.clear();
}

bool WaypointManager::DefaultManager::update()
{
    // extract waypoints
    _currentWaypoints.clear();
    Slicer::update(_waypoints, _currentWaypoints);
    return _worker();
}

bool WaypointManager::DefaultManager::next()
{
    // extract waypoints
    _currentWaypoints.clear();
    Slicer::next(_waypoints, _currentWaypoints);
    return _worker();
}

bool WaypointManager::DefaultManager::previous()
{
    // extract waypoints
    _currentWaypoints.clear();
    Slicer::previous(_waypoints, _currentWaypoints);
    return _worker();
}

bool WaypointManager::DefaultManager::reset()
{
    // extract waypoints
    _currentWaypoints.clear();
    Slicer::reset(_waypoints, _currentWaypoints);
    return _worker();
}

bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c,
                                                         size_t index,
                                                         QmlObjectListModel &list,
                                                         bool doUpdate)
{
    using namespace WaypointManager::Utils;

    if ( !insertMissionItem(c,
                            index /*insertion index*/,
                            list,
                            _settings->vehicle(),
                            _settings->isFlyView(),
                            &list /*parent*/,
                            doUpdate /*do update*/) )
    {
        qWarning("WaypointManager::DefaultManager::next(): insertMissionItem failed.");
        Q_ASSERT(false);
        return false;
    }
    return true;
}

bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c,
                                                         size_t index,
                                                         bool doUpdate)
{
    return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
}

bool WaypointManager::DefaultManager::_calcShortestPath(
        const QGeoCoordinate &start,
        const QGeoCoordinate &destination,
        QVector<QGeoCoordinate> &path)
{
        using namespace GeoUtilities;
        using namespace PolygonCalculus;
        QPolygonF joinedArea2D;
        toCartesianList(_areaInterface->joinedArea()->coordinateList(), /*origin*/ start, joinedArea2D);
        QPointF start2D(0,0);
        QPointF end2D;
        toCartesian(destination, start, end2D);
        QVector<QPointF> path2DOut;

        bool retVal = PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut);
        toGeoList(path2DOut, /*origin*/ start, path);

        return  retVal;
}

bool WaypointManager::DefaultManager::_worker()
{
    using namespace WaypointManager::Utils;

    if (_waypoints.count() < 1 || !_settings->valid()) {
        return false;
    }

    if (_dirty) {
        _missionItems.clearAndDeleteContents();
        _waypointsVariant.clear();
        // No initialization of _missionItems, don't need MissionSettingsItem here.
        for (size_t i = 0; i < size_t(_waypoints.size()); ++i) {
            auto &c = _waypoints.at(i);
            _insertMissionItem(c, _missionItems.count(), _missionItems, false /*update list*/);
            _waypointsVariant.push_back(QVariant::fromValue(c));
        }
        updateHirarchy(_missionItems);
        updateSequenceNumbers(_missionItems, 1); // Start with 1, since MissionSettingsItem missing.
        _dirty = false;
    }

    _currentMissionItems.clearAndDeleteContents();
    initialize(_currentMissionItems, _settings->vehicle(), _settings->isFlyView());

    // Precondition.
    if (!_settings->homePosition().isValid()){
        qWarning("WaypointManager::DefaultManager::next(): home position invalid.");
        Q_ASSERT(false);
        return false;
    }

    // Calculate path from home to first waypoint.
    QVector<QGeoCoordinate> arrivalPath;
    if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), arrivalPath) ) {
        qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint.");
        return false;
    }
    if (!arrivalPath.empty())
        arrivalPath.removeFirst();
    if (!arrivalPath.empty())
        arrivalPath.removeLast();

    // calculate path from last waypoint to home
    QVector<QGeoCoordinate> returnPath;
    if ( !_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(), returnPath) ) {
        qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to last waypoint.");
        return false;
    }
    if (!returnPath.empty())
        returnPath.removeFirst();
    if (!returnPath.empty())
        returnPath.removeLast();



    // Create mission items.
    // Set home position of MissionSettingsItem.
    MissionSettingsItem* settingsItem = _currentMissionItems.value<MissionSettingsItem *>(0);
    if (settingsItem == nullptr) {
        Q_ASSERT(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }
    settingsItem->setCoordinate(_settings->homePosition());

    // First mission item is takeoff command.
    _insertMissionItem(_settings->homePosition(), 1 /*insertion index*/,  false /*do update*/);
    SimpleMissionItem *takeoffItem = _currentMissionItems.value<SimpleMissionItem*>(1);
    if (takeoffItem == nullptr) {
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        Q_ASSERT(takeoffItem != nullptr);
        return false;
    }
    makeTakeOffCmd(takeoffItem, _settings->masterController()->managerVehicle());

    // Create change speed item.
    _insertMissionItem(_settings->homePosition(), 2 /*insertion index*/, false /*do update*/);
    SimpleMissionItem *speedItem = _currentMissionItems.value<SimpleMissionItem*>(2);
    if (speedItem == nullptr) {
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        Q_ASSERT(speedItem != nullptr);
        return false;
    }
    makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());

    // insert arrival path
    for (auto coordinate : arrivalPath) {
        _insertMissionItem(coordinate,
                           _currentMissionItems.count() /*insertion index*/,
                           false /*do update*/);
    }

    // Create change speed item (after arrival path).
    int index = _currentMissionItems.count();
    _insertMissionItem(_currentWaypoints.first(), index /*insertion index*/, false /*do update*/);
    speedItem = _currentMissionItems.value<SimpleMissionItem*>(index);
    if (speedItem == nullptr) {
        Q_ASSERT(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }
    makeSpeedCmd(speedItem, _settings->flightSpeed());

    // Insert slice.
    for (auto coordinate : _currentWaypoints) {
        _insertMissionItem(coordinate,
                           _currentMissionItems.count() /*insertion index*/,
                           false /*do update*/);
    }

    // Create change speed item, after slice.
    index = _currentMissionItems.count();
    _insertMissionItem(_currentWaypoints.last(), index /*insertion index*/, false /*do update*/);
    speedItem = _currentMissionItems.value<SimpleMissionItem*>(index);
    if (speedItem == nullptr) {
        Q_ASSERT(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
    speedItem->setCoordinate(_currentWaypoints.last());
    speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed());

    // Insert return path coordinates.
    for (auto coordinate : returnPath) {
        _insertMissionItem(coordinate,
                           _currentMissionItems.count() /*insertion index*/,
                           false /*do update*/);
    }

    // Set land command for last mission item.
    index = _currentMissionItems.count();
    _insertMissionItem(_settings->homePosition(), index /*insertion index*/, false /*do update*/);
    SimpleMissionItem *landItem = _currentMissionItems.value<SimpleMissionItem*>(index);
    if (landItem == nullptr) {
        Q_ASSERT(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }

    MAV_CMD landCmd = _settings->vehicle()->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
    if (_settings->vehicle()->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
        landItem->setCommand(landCmd);
    } else {
        Q_ASSERT(false);
        qWarning("WaypointManager::DefaultManager::next(): Land command not supported!");
        return false;
    }

    // Prepend arrival path to slice.
    for ( long i = arrivalPath.size()-1; i >=0; --i )
        _currentWaypoints.push_front(arrivalPath[i]);
    // Append return path to slice.
    for ( auto c : returnPath )
        _currentWaypoints.push_back(c);

    _currentWaypointsVariant.clear();
    for ( auto c : _currentWaypoints)
        _currentWaypointsVariant.push_back(QVariant::fromValue(c));


    // Set altitude.
    for (int i = 1; i < _currentMissionItems.count(); ++i) {
        SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
        if (item == nullptr) {
            Q_ASSERT(false);
            qWarning("WimaController::updateAltitude(): nullptr");
            return false;
        }
        item->altitude()->setRawValue(_settings->altitude());
    }

    return true;
}