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 "time.h"
#include "assert.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";

using namespace snake;
using namespace snake_geometry;

WimaController::WimaController(QObject *parent)
    : QObject                   (parent)
    , _container                (nullptr)
    , _joinedArea               ()
    , _measurementArea          ()
    , _serviceArea              ()
    , _corridor                 ()
    , _localPlanDataValid       (false)
    , _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])
    , _areaInterface(&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
    , _managerSettings()
    , _defaultManager(_managerSettings, _areaInterface)
    , _snakeManager(_managerSettings, _areaInterface)
    , _currentManager(_defaultManager)
    , _uploadOverrideRequired   (false)
    , _measurementPathLength    (-1)
    , _arrivalPathLength        (-1)
    , _returnPathLength         (-1)
    , _phaseDistance            (-1)
    , _phaseDuration            (-1)
    , _phaseDistanceBuffer      (-1)
    , _phaseDurationBuffer      (-1)
    , _vehicleHasLowBattery         (false)
    , _lowBatteryHandlingTriggered  (false)
    , _executingSmartRTL            (false)
    , _snakeConnectionStatus    (SnakeConnectionStatus::Connected) // TODO: implement automatic connection
    , _snakeCalcInProgress      (false)
    , _scenarioDefinedBool      (false)
    , _snakeTileWidth           (settingsGroup, _metaDataMap[snakeTileWidthName])
    , _snakeTileHeight          (settingsGroup, _metaDataMap[snakeTileHeightName])
    , _snakeMinTileArea         (settingsGroup, _metaDataMap[snakeMinTileAreaName])
    , _snakeLineDistance        (settingsGroup, _metaDataMap[snakeLineDistanceName])
    , _snakeMinTransectLength   (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
    , _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::_calcNextPhase);
    connect(&_flightSpeed,                  &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed);
    connect(&_arrivalReturnSpeed,           &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
    connect(&_altitude,                     &Fact::rawValueChanged, this, &WimaController::_updateAltitude);

    _defaultManager.setOverlap(_overlapWaypoints.rawValue().toUInt());
    _defaultManager.setN(_maxWaypointsPerPhase.rawValue().toUInt());
    _defaultManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt());

    // setup low battery handling
    connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
    _eventTimer.setInterval(EVENT_TIMER_INTERVAL);

    Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
    connect(enableLowBatteryHandling, &Fact::rawValueChanged, this, &WimaController::_enableDisableLowBatteryHandling);
    _enableDisableLowBatteryHandling(enableLowBatteryHandling->rawValue());

    // 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);

    // Start, stop RosBridge.
    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge);
    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
    _startStopRosBridge();
}

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());
}

//QStringList WimaController::loadNameFilters() const
//{
//    QStringList filters;

//    filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
//               tr("All Files (*.*)");
//    return filters;
//}

//QStringList WimaController::saveNameFilters() const
//{
//    QStringList filters;

//    filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
//    return filters;
//}

bool WimaController::uploadOverrideRequired() const
{
    return _uploadOverrideRequired;
}

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

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

bool WimaController::vehicleHasLowBattery() const
{
    return _vehicleHasLowBattery;
}

long WimaController::snakeConnectionStatus() const
{
    return _snakeConnectionStatus;
}

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();
}

/*!
 * \fn void WimaController::setDataContainer(WimaDataContainer *container)
 * Sets the pointer to the \c WimaDataContainer, which is meant to exchange data between the \c WimaController and the \c WimaPlaner.
 *
 * \sa WimaPlaner, WimaDataContainer, WimaPlanData
 */
void WimaController::setDataContainer(WimaDataContainer *container)
{
    if (container != nullptr) {
        if (_container != nullptr) {
           disconnect(_container, &WimaDataContainer::newDataAvailable, this, &WimaController::_fetchContainerData);
        }

        _container = container;
        connect(_container, &WimaDataContainer::newDataAvailable, this, &WimaController::_fetchContainerData);

        emit dataContainerChanged();
    }
}

void WimaController::setUploadOverrideRequired(bool overrideRequired)
{
    if (_uploadOverrideRequired != overrideRequired) {
        _uploadOverrideRequired = overrideRequired;

        emit uploadOverrideRequiredChanged();
    }
}

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

void WimaController::previousPhase()
{        
    if ( !_currentManager.previous() ) {
        assert(false);
    }

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

void WimaController::resetPhase()
{
    if ( !_currentManager.reset() ) {
        assert(false);
    }

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

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

    return forceUploadToVehicle();
}

bool WimaController::forceUploadToVehicle()
{
    auto &currentMissionItems = _defaultManager.currentMissionItems();
    setUploadOverrideRequired(false);
    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) {
        assert(false);
        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
        return false;
    }
    settingsItem->setCoordinate(_managerSettings.homePosition());

    // Copy mission items to _missionController.
    for (int i = 0; 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();
}

bool WimaController::checkSmartRTLPreCondition()
{
    QString errorString;
    bool retValue = _checkSmartRTLPreCondition(errorString);
    if (retValue == false) {
        qgcApp()->showMessage(errorString);
        return false;
    }
    return true;
}

bool WimaController::calcReturnPath()
{
    QString errorString;
//    bool retValue = _calcReturnPath(errorString);
//    if (retValue == false) {
//        qgcApp()->showMessage(errorString);
//        return false;
//    }
//    return true;
}

void WimaController::executeSmartRTL()
{
    _executeSmartRTL();
}

void WimaController::initSmartRTL()
{
    _srtlReason = UserRequest;
    _initSmartRTL();
}

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

//void WimaController::saveToFile(const QString& filename)
//{
//    QString file = filename;
//}

//bool WimaController::loadFromCurrent()
//{
//    return true;
//}

//bool WimaController::loadFromFile(const QString &filename)
//{
//    QString file = filename;
//    return true;
//}



//QJsonDocument WimaController::saveToJson(FileType fileType)
//{
//    if(fileType)
//    {

//    }
//    return QJsonDocument();
//}

bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path)
{
    using namespace GeoUtilities;
    using namespace PolygonCalculus;
    QVector<QPointF> path2D;
    bool retVal = PolygonCalculus::shortestPath(
                                   toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/ start)),
                                   /*start point*/ QPointF(0,0),
                                   /*destination*/ toCartesian2D(destination, start),
                                   /*shortest path*/ path2D);
    path.append(toGeo(path2D, /*origin*/ start));

    return  retVal;
}

/*!
 * \fn void WimaController::containerDataValidChanged(bool valid)
 * Pulls plan data generated by \c WimaPlaner from the \c _container if the data is valid (\a valid equals true).
 * Is connected to the dataValidChanged() signal of the \c WimaDataContainer.
 *
 * \sa WimaDataContainer, WimaPlaner, WimaPlanData
 */
bool WimaController::_fetchContainerData()
{
    // fetch only if valid, return true on success

    // 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;

    if (_container == nullptr) {
        qWarning("WimaController::fetchContainerData(): No container assigned!");
        assert(false);
        return false;
    }

    WimaPlanData planData = _container->pull();

    // 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) {
        assert(false);
        return false;
    }

    // extract mission items
    QList<MissionItem> tempMissionItems = planData.missionItems();
    if (tempMissionItems.size() < 1) {
        assert(false);
        return false;
    }

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

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

    if( !_defaultManager.update() ){
        assert(false);
        return false;
    }

    // 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 ( !_verifyScenarioDefinedWithErrorMessage() )
        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 visualItemsChanged();
    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit snakeTilesChanged();
    emit snakeTileCenterPointsChanged();

    _localPlanDataValid = true;
    return true;
}

bool WimaController::_calcNextPhase()
{
    bool value;
    _currentManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value));
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager.next() ) {
        assert(false);
        return false;
    }

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

    return true;
}

void WimaController::_recalcCurrentPhase()
{
    if ( !_currentManager.update() ) {
        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() ) {
        assert(false);
    }

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

}

void WimaController::_updateflightSpeed()
{
    _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble());
    _currentManager.update();

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

void WimaController::_updateArrivalReturnSpeed()
{
    _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble());
    _currentManager.update();

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

void WimaController::_updateAltitude()
{
    _managerSettings.setAltitude(_altitude.rawValue().toDouble());
    _currentManager.update();

    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) {
            _setVehicleHasLowBattery(true);
            if (!_lowBatteryHandlingTriggered) {

                if (_missionController->remainingTime() <= minTime) {
                    _lowBatteryHandlingTriggered = true;
                }
                else {
                    _lowBatteryHandlingTriggered = true;
                    _srtlReason = BatteryLow;
                    _initSmartRTL();
                }
            }
        }
        else {
            _setVehicleHasLowBattery(false);
            _lowBatteryHandlingTriggered = false;
        }

    }
}

void WimaController::_eventTimerHandler()
{
    static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL);
    static EventTicker snakeEventLoopTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
    static EventTicker rosBridgeTicker(EVENT_TIMER_INTERVAL, 1000);

    // Battery level check necessary?
    if ( batteryLevelTicker.ready() )
        _checkBatteryLevel();

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

    if (rosBridgeTicker.ready()) {

        _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
        _pRosBridge->publish(_snakeOrigin, "/snake/origin");

        using namespace std::placeholders;
        auto callBack = std::bind(&WimaController::_progressFromJson,
                                  this,
                                  _1,
                                  std::ref(_nemoProgress));
        _pRosBridge->subscribe("/nemo/progress", callBack);
    }
}

void WimaController::_smartRTLCleanUp(bool flying)
{

//    if ( !flying) { // vehicle has landed
//        if (_executingSmartRTL) {
//            _executingSmartRTL = false;
//            _loadCurrentMissionItemsFromBuffer();
//            _setPhaseDistance(_phaseDistanceBuffer);
//            _setPhaseDuration(_phaseDurationBuffer);
//            _showAllMissionItems.setRawValue(true);
//            _missionController->removeAllFromVehicle();
//            _missionController->removeAll();
//            disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
//        }
//    }
}

void WimaController::_enableDisableLowBatteryHandling(QVariant enable)
{
    if (enable.toBool()) {
        _eventTimer.start();
    } else {
        _eventTimer.stop();
    }
}

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

        emit phaseDistanceChanged();
    }
}

void WimaController::_setPhaseDuration(double 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;
    }
    Vehicle *managerVehicle = masterController()->managerVehicle();
    if (!managerVehicle->flying()) {
         errorString.append(tr("Vehicle is not flying. Smart RTL not available."));
         return false;
    }

    if (!_joinedArea.containsCoordinate(managerVehicle->coordinate())) {
         errorString.append(tr("Vehicle not inside save area. Smart RTL not available."));
         return false;
    }

   return true;
}

void WimaController::_setVehicleHasLowBattery(bool batteryLow)
{
    if (_vehicleHasLowBattery != batteryLow) {
        _vehicleHasLowBattery = batteryLow;

        emit vehicleHasLowBatteryChanged();
    }
}

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

//    if (_checkSmartRTLPreCondition(errorString) == true) {
//        _masterController->managerVehicle()->pauseVehicle();
//        connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
//        if (_calcReturnPath(errorString)) {
//            _executingSmartRTL = true;
//            attemptCounter = 0;

//            switch(_srtlReason) {
//                case BatteryLow:
//                    emit returnBatteryLowConfirmRequired();
//                break;
//                case UserRequest:
//                    emit returnUserRequestConfirmRequired();
//                break;
//            }

//            return;
//        }
//    }
//    if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { // try 3 times, somtimes vehicle is outside joined area
//        errorString.append(tr("Smart RTL: No success after maximum number of attempts."));
//        qgcApp()->showMessage(errorString);
//        attemptCounter = 0;
//    } else {
//        _smartRTLAttemptTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
//    }
}

void WimaController::_executeSmartRTL()
{
    forceUploadToVehicle();
    masterController()->managerVehicle()->startMission();
}

void WimaController::_setSnakeConnectionStatus(WimaController::SnakeConnectionStatus status)
{
    if (_snakeConnectionStatus != status) {
        _snakeConnectionStatus = status;
        emit snakeConnectionStatusChanged();
    }
}

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

bool WimaController::_verifyScenarioDefined()
{
    _scenarioDefinedBool = _scenario.defined(_snakeTileWidth.rawValue().toDouble(), _snakeTileHeight.rawValue().toDouble(), _snakeMinTileArea.rawValue().toDouble());
    return _scenarioDefinedBool;
}

bool WimaController::_verifyScenarioDefinedWithErrorMessage()
{
    bool value = _verifyScenarioDefined();
    if (!value){
        QString errorString;
        for (auto c : _scenario.errorString)
            errorString.push_back(c);
        qgcApp()->showMessage(errorString);
    }
    return value;
}

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

    const WorkerResult_t &r = _snakeWorker.getResult();
    _setSnakeCalcInProgress(false);
    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;
    assert(n >= 1);

    // 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::_progressFromJson(JsonDocUPtr pDoc,
                                       QNemoProgress &progress)
{
    int requiredSize = _snakeTilesLocal.polygons().size();
    if ( !_pRosBridge->casePacker()->unpack(pDoc, progress)
         || progress.progress().size() != requiredSize ) {
        progress.progress().fill(0, requiredSize);
    }

    emit WimaController::nemoProgressChanged();
}

template<>
QVariant getCoordinate<QVariant>(const SimpleMissionItem* item)
{
    return QVariant::fromValue(item->coordinate());
}