WimaController.cc

#include "WimaController.h"
#include "utilities.h"
#include "ros_bridge/include/JsonMethodes.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/include/CasePacker.h"

#include "Snake/QtROSJsonFactory.h"
#include "Snake/QtROSTypeFactory.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"

#include "QVector3D"
#include <QScopedPointer>



// const char* WimaController::wimaFileExtension           = "wima";
const char* WimaController::areaItemsName               = "AreaItems";
const char* WimaController::missionItemsName            = "MissionItems";
const char* WimaController::settingsGroup               = "WimaController";
const char* WimaController::enableWimaControllerName    = "EnableWimaController";
const char* WimaController::overlapWaypointsName        = "OverlapWaypoints";
const char* WimaController::maxWaypointsPerPhaseName    = "MaxWaypointsPerPhase";
const char* WimaController::startWaypointIndexName      = "StartWaypointIndex";
const char* WimaController::showAllMissionItemsName     = "ShowAllMissionItems";
const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionItems";
const char* WimaController::flightSpeedName             = "FlightSpeed";
const char* WimaController::arrivalReturnSpeedName      = "ArrivalReturnSpeed";
const char* WimaController::altitudeName                = "Altitude";
const char* WimaController::snakeTileWidthName          = "SnakeTileWidth";
const char* WimaController::snakeTileHeightName         = "SnakeTileHeight";
const char* WimaController::snakeMinTileAreaName        = "SnakeMinTileArea";
const char* WimaController::snakeLineDistanceName       = "SnakeLineDistance";
const char* WimaController::snakeMinTransectLengthName  = "SnakeMinTransectLength";

WimaController::StatusMap WimaController::_nemoStatusMap{
    std::make_pair<int, QString>(0, "No Heartbeat"),
    std::make_pair<int, QString>(1, "Connected"),
    std::make_pair<int, QString>(-1, "Timeout")};

using namespace snake;
using namespace snake_geometry;

WimaController::WimaController(QObject *parent)
    : QObject                   (parent)
    , _joinedArea               ()
    , _measurementArea          ()
    , _serviceArea              ()
    , _corridor                 ()
    , _localPlanDataValid       (false)
    , _areaInterface            (&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
    , _managerSettings          ()
    , _defaultManager           (_managerSettings, _areaInterface)
    , _snakeManager             (_managerSettings, _areaInterface)
    , _rtlManager               (_managerSettings, _areaInterface)
    , _currentManager           (&_defaultManager)
    , _managerList              {&_defaultManager, &_snakeManager, &_rtlManager}
    , _metaDataMap              (FactMetaData::createMapFromJsonFile(
                                     QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
    , _enableWimaController     (settingsGroup, _metaDataMap[enableWimaControllerName])
    , _overlapWaypoints         (settingsGroup, _metaDataMap[overlapWaypointsName])
    , _maxWaypointsPerPhase     (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName])
    , _nextPhaseStartWaypointIndex       (settingsGroup, _metaDataMap[startWaypointIndexName])
    , _showAllMissionItems      (settingsGroup, _metaDataMap[showAllMissionItemsName])
    , _showCurrentMissionItems  (settingsGroup, _metaDataMap[showCurrentMissionItemsName])    
    , _flightSpeed              (settingsGroup, _metaDataMap[flightSpeedName])
    , _arrivalReturnSpeed       (settingsGroup, _metaDataMap[arrivalReturnSpeedName])
    , _altitude                 (settingsGroup, _metaDataMap[altitudeName])
    , _measurementPathLength    (-1)
    , _lowBatteryHandlingTriggered  (false)
    , _snakeCalcInProgress      (false)
    , _snakeTileWidth           (settingsGroup, _metaDataMap[snakeTileWidthName])
    , _snakeTileHeight          (settingsGroup, _metaDataMap[snakeTileHeightName])
    , _snakeMinTileArea         (settingsGroup, _metaDataMap[snakeMinTileAreaName])
    , _snakeLineDistance        (settingsGroup, _metaDataMap[snakeLineDistanceName])
    , _snakeMinTransectLength   (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
    , _nemoHeartbeat            (0 /*status: not connected*/)
    , _fallbackStatus           (0 /*status: not connected*/)
    , _bridgeSetupDone          (false)
    , _pRosBridge               (new ROSBridge::ROSBridge())
{
    // Set up facts.
    _showAllMissionItems.setRawValue(true);
    _showCurrentMissionItems.setRawValue(true);
    connect(&_overlapWaypoints,             &Fact::rawValueChanged, this, &WimaController::_updateOverlap);
    connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints);
    connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged, this, &WimaController::_setStartIndex);
    connect(&_flightSpeed,                  &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed);
    connect(&_arrivalReturnSpeed,           &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
    connect(&_altitude,                     &Fact::rawValueChanged, this, &WimaController::_updateAltitude);

    // Init waypoint managers.
    bool value;
    size_t overlap  = _overlapWaypoints.rawValue().toUInt(&value);
    Q_ASSERT(value);
    size_t N        = _maxWaypointsPerPhase.rawValue().toUInt(&value);
    Q_ASSERT(value);
    size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value);
    Q_ASSERT(value);
    (void)value;
    for (auto manager : _managerList){
        manager->setOverlap(overlap);
        manager->setN(N);
        manager->setStartIndex(startIdx);
    }

    // Periodic.
    connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
    //_eventTimer.setInterval(EVENT_TIMER_INTERVAL);
    _eventTimer.start(EVENT_TIMER_INTERVAL);

    // Snake Worker Thread.
    connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults);
    connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
    connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);

    // Snake.
    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge);
    _startStopRosBridge();
    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
    _initStartSnakeWorker();
    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
    _switchSnakeManager(_enableSnake.rawValue());
}

PlanMasterController *WimaController::masterController() {
    return _masterController;
}

MissionController *WimaController::missionController() {
    return _missionController;
}

QmlObjectListModel *WimaController::visualItems() {
    return &_areas;
}

QmlObjectListModel *WimaController::missionItems() {
    return const_cast<QmlObjectListModel*>(&_currentManager->missionItems());
}

QmlObjectListModel *WimaController::currentMissionItems() {
    return const_cast<QmlObjectListModel*>(&_currentManager->currentMissionItems());
}

QVariantList WimaController::waypointPath()
{
    return const_cast<QVariantList&>(_currentManager->waypointsVariant());
}

QVariantList WimaController::currentWaypointPath()
{
    return const_cast<QVariantList&>(_currentManager->currentWaypointsVariant());
}

Fact *WimaController::enableWimaController() {
    return &_enableWimaController;
}

Fact *WimaController::overlapWaypoints() {
    return &_overlapWaypoints;
}

Fact *WimaController::maxWaypointsPerPhase() {
    return &_maxWaypointsPerPhase;
}

Fact *WimaController::startWaypointIndex() {
    return &_nextPhaseStartWaypointIndex;
}

Fact *WimaController::showAllMissionItems() {
    return &_showAllMissionItems;
}

Fact *WimaController::showCurrentMissionItems() {
    return &_showCurrentMissionItems;
}

Fact *WimaController::flightSpeed() {
    return &_flightSpeed;
}

Fact *WimaController::arrivalReturnSpeed() {
    return &_arrivalReturnSpeed;
}

Fact *WimaController::altitude() {
    return &_altitude;
}

QVector<int> WimaController::nemoProgress() {
    if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() )
        return _nemoProgress.progress();
    else
        return QVector<int>(_snakeTileCenterPoints.size(), 0);
}

double WimaController::phaseDistance() const
{
    return 0.0;
}

double WimaController::phaseDuration() const
{
    return 0.0;
}

int WimaController::nemoStatus() const
{
    return _nemoHeartbeat.status();
}

QString WimaController::nemoStatusString() const
{
    return _nemoStatusMap.at(_nemoHeartbeat.status());
}

bool WimaController::snakeCalcInProgress() const
{
    return _snakeCalcInProgress;
}

void WimaController::setMasterController(PlanMasterController *masterC)
{
    _masterController = masterC;
    _managerSettings.setMasterController(masterC);
    emit masterControllerChanged();
}

void WimaController::setMissionController(MissionController *missionC)
{
    _missionController = missionC;
    _managerSettings.setMissionController(missionC);
    emit missionControllerChanged();
}

void WimaController::nextPhase()
{
    _calcNextPhase();
}

void WimaController::previousPhase()
{        
    if ( !_currentManager->previous() ) {
        Q_ASSERT(false);
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
}

void WimaController::resetPhase()
{
    if ( !_currentManager->reset() ) {
        Q_ASSERT(false);
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
}

void WimaController::requestSmartRTL()
{
    QString errorString("Smart RTL requested. ");
    if ( !_checkSmartRTLPreCondition(errorString) ){
        qgcApp()->showMessage(errorString);
        return;
    }
    emit smartRTLRequestConfirm();
}

bool WimaController::upload()
{
    auto &currentMissionItems = _defaultManager.currentMissionItems();
    if (   !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate())
        && currentMissionItems.count() > 0) {
        emit forceUploadConfirm();
        return false;
    }

    return forceUpload();
}

bool WimaController::forceUpload()
{
    auto &currentMissionItems = _defaultManager.currentMissionItems();
    if (currentMissionItems.count() < 1)
        return false;

    _missionController->removeAll();
    // Set homeposition of settingsItem.
    QmlObjectListModel* visuals = _missionController->visualItems();
    MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0);
    if (settingsItem == nullptr) {
        Q_ASSERT(false);
        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
        return false;
    }
    settingsItem->setCoordinate(_managerSettings.homePosition());

    // Copy mission items to _missionController.
    for (int i = 1; i < currentMissionItems.count(); i++){
        auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
        _missionController->insertSimpleMissionItem(*item, visuals->count());
    }

    _masterController->sendToVehicle();

    return true;
}

void WimaController::removeFromVehicle()
{
    _masterController->removeAllFromVehicle();
    _missionController->removeAll();
}

void WimaController::executeSmartRTL()
{
    forceUpload();
    masterController()->managerVehicle()->startMission();
}

void WimaController::initSmartRTL()
{
    _initSmartRTL();
}

void WimaController::removeVehicleTrajectoryHistory()
{
    Vehicle *managerVehicle = masterController()->managerVehicle();
    managerVehicle->trajectoryPoints()->clear();
}

bool WimaController::_calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path)
{
    using namespace GeoUtilities;
    using namespace PolygonCalculus;
    QPolygonF polygon2D;
    toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
    QPointF start2D(0,0);
    QPointF end2D;
    toCartesian(destination, start, end2D);
    QVector<QPointF> path2D;

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

    return  retVal;
}

bool WimaController::setWimaPlanData(const WimaPlanData &planData)
{
    // reset visual items
    _areas.clear();
    _defaultManager.clear();
    _snakeTiles.polygons().clear();
    _snakeTilesLocal.polygons().clear();
    _snakeTileCenterPoints.clear();

    emit visualItemsChanged();
    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit waypointPathChanged();
    emit currentWaypointPathChanged();
    emit snakeTilesChanged();
    emit snakeTileCenterPointsChanged();

    _localPlanDataValid = false;

    // extract list with WimaAreas
    QList<const WimaAreaData*> areaList = planData.areaList();

    int areaCounter = 0;
    const int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored
    for (int i = 0; i < areaList.size(); i++) {
        const WimaAreaData *areaData = areaList[i];

        if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area?
            _serviceArea = *qobject_cast<const WimaServiceAreaData*>(areaData);
            areaCounter++;
            _areas.append(&_serviceArea);

            continue;
        }

        if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area?
            _measurementArea =  *qobject_cast<const WimaMeasurementAreaData*>(areaData);
            areaCounter++;
            _areas.append(&_measurementArea);

            continue;
        }

        if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor?
            _corridor =  *qobject_cast<const WimaCorridorData*>(areaData);
            areaCounter++;
            //_visualItems.append(&_corridor); // not needed

            continue;
        }

        if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor?
            _joinedArea =  *qobject_cast<const WimaJoinedAreaData*>(areaData);
            areaCounter++;
            _areas.append(&_joinedArea);

            continue;
        }

        if (areaCounter >= numAreas)
            break;
    }

    if (areaCounter != numAreas) {
        Q_ASSERT(false);
        return false;
    }

    emit visualItemsChanged();

    // extract mission items
    QList<MissionItem> tempMissionItems = planData.missionItems();
    if (tempMissionItems.size() < 1) {
        qWarning("WimaController: Mission items from WimaPlaner empty!");
        return false;
    }

    for (auto item : tempMissionItems) {
        _defaultManager.push_back(item.coordinate());
    }

    _managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(),
                                                     _serviceArea.center().longitude(),
                                                     0) );

    if( !_defaultManager.reset() ){
        Q_ASSERT(false);
        return false;
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit waypointPathChanged();
    emit currentWaypointPathChanged();



    // Initialize _scenario.
    Area mArea;
    for (auto variant : _measurementArea.path()){
        QGeoCoordinate c{variant.value<QGeoCoordinate>()};
        mArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
    }
    mArea.type = AreaType::MeasurementArea;

    Area sArea;
    for (auto variant : _serviceArea.path()){
        QGeoCoordinate c{variant.value<QGeoCoordinate>()};
        sArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
    }
    sArea.type = AreaType::ServiceArea;

    Area corridor;
    for (auto variant : _corridor.path()){
        QGeoCoordinate c{variant.value<QGeoCoordinate>()};
        corridor.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
    }
    corridor.type = AreaType::Corridor;

    _scenario.addArea(mArea);
    _scenario.addArea(sArea);
    _scenario.addArea(corridor);

    // Check if scenario is defined.
    if ( !_isScenarioDefinedErrorMessage() ) {
        Q_ASSERT(false);
        return false;
    }

    {
        // Get tiles and origin.
        auto origin = _scenario.getOrigin();
        _snakeOrigin.setLatitude(origin[0]);
        _snakeOrigin.setLongitude(origin[1]);
        _snakeOrigin.setAltitude(origin[2]);
        const auto &tiles = _scenario.getTiles();
        const auto &cps = _scenario.getTileCenterPoints();
        for ( unsigned int i=0; i < tiles.size(); ++i ) {
            const auto &tile = tiles[i];
            SnakeTile Tile;
            for ( const auto &vertex : tile) {
                QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
                Tile.append(QVertex);
            }
            const auto &centerPoint = cps[i];
            QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]);
            Tile.setCenter(QCenterPoint);
            _snakeTiles.polygons().append(Tile);
            _snakeTileCenterPoints.append(QVariant::fromValue(QCenterPoint));
        }
    }

    {
        // Get local tiles.
        const auto &tiles = _scenario.getTilesENU();
        for ( unsigned int i=0; i < tiles.size(); ++i ) {
            const auto &tile = tiles[i];
            Polygon2D Tile;
            for ( const auto &vertex : tile.outer()) {
                QPointF QVertex(vertex.get<0>(), vertex.get<1>());
                Tile.path().append(QVertex);
            }
            _snakeTilesLocal.polygons().append(Tile);
        }
    }
    emit snakeTilesChanged();
    emit snakeTileCenterPointsChanged();

    _localPlanDataValid = true;
    return true;
}

WimaController *WimaController::thisPointer()
{
    return this;
}

bool WimaController::_calcNextPhase()
{
    if ( !_currentManager->next() ) {
        Q_ASSERT(false);
        return false;
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();

    return true;
}

bool WimaController::_setStartIndex()
{
    bool value;
    _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1);
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
        return false;
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();

    return true;
}

void WimaController::_recalcCurrentPhase()
{
    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
}

void WimaController::_updateOverlap()
{
    bool value;
    _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager->update() ) {
        assert(false);
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
}

void WimaController::_updateMaxWaypoints()
{
    bool value;
    _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
}

void WimaController::_updateflightSpeed()
{
    bool value;
    _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value));
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
}

void WimaController::_updateArrivalReturnSpeed()
{
    bool value;
    _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble(&value));
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
}

void WimaController::_updateAltitude()
{
    bool value;
    _managerSettings.setAltitude(_altitude.rawValue().toDouble(&value));
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
}

void WimaController::_checkBatteryLevel()
{
    Vehicle *managerVehicle     = masterController()->managerVehicle();
    WimaSettings* wimaSettings  = qgcApp()->toolbox()->settingsManager()->wimaSettings();
    int batteryThreshold        = wimaSettings->lowBatteryThreshold()->rawValue().toInt();
    bool enabled                = _enableWimaController.rawValue().toBool();
    unsigned int minTime        = wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt();

    if (managerVehicle != nullptr && enabled == true) {
        Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
        Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);


        if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold
                && battery2percentRemaining->rawValue().toDouble() < batteryThreshold) {
            if (!_lowBatteryHandlingTriggered) {                
                _lowBatteryHandlingTriggered = true;
                if ( !(_missionController->remainingTime() <= minTime) ) {
                    requestSmartRTL();
                }
            }
        }
        else {
            _lowBatteryHandlingTriggered = false;
        }

    }
}

void WimaController::_eventTimerHandler()
{
    static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL);
    static EventTicker snakeTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
    static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000);

    // Battery level check necessary?
    Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
    if ( enableLowBatteryHandling->rawValue().toBool() && batteryLevelTicker.ready() )
        _checkBatteryLevel();

    // Snake flight plan update necessary?
//    if ( snakeEventLoopTicker.ready() ) {
//        if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
//        }
//    }

    if ( nemoStatusTicker.ready() ) {
        this->_nemoHeartbeat.setStatus(_fallbackStatus);
        emit WimaController::nemoStatusChanged();
        emit WimaController::nemoStatusStringChanged();
    }

    if ( snakeTicker.ready() ) {
        if ( _enableSnake.rawValue().toBool()
             && _pRosBridge->connected() ) {
            if ( !_bridgeSetupDone ) {
                qWarning() << "ROS Bridge setup. ";
                auto start = std::chrono::high_resolution_clock::now();
                _pRosBridge->start();
                auto end = std::chrono::high_resolution_clock::now();
                qWarning() << "_pRosBridge->start() time: "
                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                           << " ms";
                start = std::chrono::high_resolution_clock::now();
                _pRosBridge->publish(_snakeOrigin, "/snake/origin");
                end = std::chrono::high_resolution_clock::now();
                qWarning() << "_pRosBridge->publish(_snakeOrigin, \"/snake/origin\") time: "
                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                           << " ms";
                start = std::chrono::high_resolution_clock::now();
                _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
                end = std::chrono::high_resolution_clock::now();
                qWarning() << "_pRosBridge->publish(_snakeTilesLocal, \"/snake/tiles\") time: "
                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                           << " ms";


                start = std::chrono::high_resolution_clock::now();
                _pRosBridge->subscribe("/nemo/progress",
                                       /* callback */ [this](JsonDocUPtr pDoc){
                    int requiredSize = this->_snakeTilesLocal.polygons().size();
                    auto& progress = this->_nemoProgress;
                    if ( !this->_pRosBridge->casePacker()->unpack(pDoc, progress)
                         || progress.progress().size() != requiredSize ) {
                        progress.progress().fill(0, requiredSize);
                    }

                    emit WimaController::nemoProgressChanged();
                });
                end = std::chrono::high_resolution_clock::now();
                qWarning() << "_pRosBridge->subscribe(\"/nemo/progress\" time: "
                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                           << " ms";

                start = std::chrono::high_resolution_clock::now();
                _pRosBridge->subscribe("/nemo/heartbeat",
                                       /* callback */ [this, &nemoStatusTicker](JsonDocUPtr pDoc){
                    if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
                        if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
                            return;
                        this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
                    }

                    nemoStatusTicker.reset();
                    this->_fallbackStatus = -1; /*Timeout*/
                    emit WimaController::nemoStatusChanged();
                    emit WimaController::nemoStatusStringChanged();
                });
                end = std::chrono::high_resolution_clock::now();
                qWarning() << "_pRosBridge->subscribe(\"/nemo/heartbeat\" time: "
                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                           << " ms";
                _bridgeSetupDone = true;
            }
        } else if ( _bridgeSetupDone) {
            _pRosBridge->reset();
            _bridgeSetupDone = false;
        }
    }
}

void WimaController::_smartRTLCleanUp(bool flying)
{
    if ( !flying) { // vehicle has landed
        _switchWaypointManager(_defaultManager);
        _missionController->removeAllFromVehicle();
        _missionController->removeAll();
        disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
    }
}

void WimaController::_setPhaseDistance(double distance)
{
    (void)distance;
//    if (!qFuzzyCompare(distance, _phaseDistance)) {
//        _phaseDistance = distance;

//        emit phaseDistanceChanged();
//    }
}

void WimaController::_setPhaseDuration(double duration)
{
    (void)duration;
//    if (!qFuzzyCompare(duration, _phaseDuration)) {
//        _phaseDuration = duration;

//        emit phaseDurationChanged();
//    }
}

bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
{
    if (!_localPlanDataValid) {
        errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area."));
        return false;
    }

    return _rtlManager.checkPrecondition(errorString);
}

void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manager)
{
    if (_currentManager != &manager) {
        _currentManager = &manager;

        disconnect(&_overlapWaypoints,             &Fact::rawValueChanged,
                   this, &WimaController::_updateOverlap);
        disconnect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged,
                   this, &WimaController::_updateMaxWaypoints);
        disconnect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged,
                   this, &WimaController::_setStartIndex);

        _maxWaypointsPerPhase.setRawValue(_currentManager->N());
        _overlapWaypoints.setRawValue(_currentManager->overlap());
        _nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex());

        connect(&_overlapWaypoints,             &Fact::rawValueChanged,
                this, &WimaController::_updateOverlap);
        connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged,
                this, &WimaController::_updateMaxWaypoints);
        connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged,
                this, &WimaController::_setStartIndex);

        emit missionItemsChanged();
        emit currentMissionItemsChanged();
        emit waypointPathChanged();
        emit currentWaypointPathChanged();

        qWarning() << "WimaController::_switchWaypointManager: statistics update missing.";
    }
}

void WimaController::_initSmartRTL()
{
    QString errorString;
    static int attemptCounter = 0;
    attemptCounter++;

    if ( _checkSmartRTLPreCondition(errorString) ) {
        _masterController->managerVehicle()->pauseVehicle();
        connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
        if ( _rtlManager.update() ) { // Calculate return path.
            _switchWaypointManager(_rtlManager);
            attemptCounter = 0;
            emit smartRTLPathConfirm();
            return;
        }
    } else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) {
        errorString.append(tr("Smart RTL: No success after maximum number of attempts."));
        qgcApp()->showMessage(errorString);
        attemptCounter = 0;
    } else {
        _smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
    }
}

void WimaController::_setSnakeCalcInProgress(bool inProgress)
{
    if (_snakeCalcInProgress != inProgress){
        _snakeCalcInProgress = inProgress;
        emit snakeCalcInProgressChanged();
    }
}

void WimaController::_snakeStoreWorkerResults()
{
    _setSnakeCalcInProgress(false);
    auto start = std::chrono::high_resolution_clock::now();
    _snakeManager.clear();

    const WorkerResult_t &r = _snakeWorker.getResult();
    if (!r.success) {
        //qgcApp()->showMessage(r.errorMessage);
        return;
    }

    // create Mission items from r.waypoints
    long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2;
    if (n < 1) {
        return;
    }

    // Create QVector<QGeoCoordinate> containing all waypoints;
    unsigned long startIdx = r.arrivalPathIdx.last();
    unsigned  long endIdx = r.returnPathIdx.first();
    for (unsigned long i = startIdx; i <= endIdx; ++i) {
        _snakeManager.push_back(r.waypoints[int(i)].value<QGeoCoordinate>());
    }

    _snakeManager.update();

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();

    auto end = std::chrono::high_resolution_clock::now();
    double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
    qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms.";
}

void WimaController::_startStopRosBridge()
{
    if ( _enableSnake.rawValue().toBool() ) {
        _pRosBridge->start();
    } else {
        _pRosBridge->reset();
    }
}

void WimaController::_initStartSnakeWorker()
{
    if ( !_enableSnake.rawValue().toBool() )
        return;

    // Stop worker thread if running.
    if ( _snakeWorker.isRunning() ) {
        _snakeWorker.quit();
    }

    // Initialize _snakeWorker.
    _snakeWorker.setScenario(_scenario);
    _snakeWorker.setProgress(_nemoProgress.progress());
    _snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble());
    _snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble());
    _setSnakeCalcInProgress(true);

    // Start worker thread.
    _snakeWorker.start();
}

void WimaController::_switchSnakeManager(QVariant variant)
{
    if (variant.value<bool>()){
        _switchWaypointManager(_snakeManager);
    } else {
        _switchWaypointManager(_defaultManager);
    }
}