#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 "QtROSJsonFactory.h"
#include "QtROSTypeFactory.h"
#include "NemoProgress.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::reverseName                 = "Reverse";
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               (this)
    , _measurementArea          (this)
    , _serviceArea              (this)
    , _corridor                 (this)
    , _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])
    , _reverse                  (settingsGroup, _metaDataMap[reverseName])
    , _endWaypointIndex         (0)
    , _startWaypointIndex       (0)
    , _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())
{
    _showAllMissionItems.setRawValue(true);
    _showCurrentMissionItems.setRawValue(true);
    connect(&_overlapWaypoints,             &Fact::rawValueChanged, this, &WimaController::_updateNextWaypoint);
    connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged, this, &WimaController::_recalcCurrentPhase);
    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);
    connect(&_reverse,                      &Fact::rawValueChanged, this, &WimaController::_reverseChangedHandler);

    // 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::resultReady, this, &WimaController::_snakeStoreWorkerResults);

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

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

void WimaController::setMissionController(MissionController *missionC)
{
    _missionController = 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()
{    
    bool reverseBool = _reverse.rawValue().toBool();
    if (!reverseBool){
        int startIndex = _nextPhaseStartWaypointIndex.rawValue().toInt();
        if (startIndex > 0) {
            _nextPhaseStartWaypointIndex.setRawValue(1+std::max(_startWaypointIndex
                                                              - _maxWaypointsPerPhase.rawValue().toInt()
                                                              + _overlapWaypoints.rawValue().toInt(), 0));
        }
    }
    else {
        int startIndex = _nextPhaseStartWaypointIndex.rawValue().toInt();
        if (startIndex <= _missionItems.count()) {
            _nextPhaseStartWaypointIndex.setRawValue(1+std::min(_startWaypointIndex
                                                              + _maxWaypointsPerPhase.rawValue().toInt()
                                                              - _overlapWaypoints.rawValue().toInt(), _missionItems.count()-1));
        }
    }
}

void WimaController::resetPhase()
{
    bool reverseBool = _reverse.rawValue().toBool();
    if (!reverseBool) {
        _nextPhaseStartWaypointIndex.setRawValue(int(1));
    }
    else {
        _nextPhaseStartWaypointIndex.setRawValue(_missionItems.count());
    }
}

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

    return forceUploadToVehicle();
}

bool WimaController::forceUploadToVehicle()
{
    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) {
        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
        return false;
    }
    settingsItem->setCoordinate(_takeoffLandPostion);

    // copy mission items from _currentMissionItems to _missionController
    for (int i = 0; i < _currentMissionItems.count(); i++){
        SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
        _missionController->insertSimpleMissionItem(*item, visuals->count());
        if (item->command() == MAV_CMD_DO_CHANGE_SPEED) {
        }
    }

    for (int i = 0; i < _missionController->visualItems()->count(); i++){
        SimpleMissionItem *item =  _missionController->visualItems()->value<SimpleMissionItem*>(i);
        if (item == nullptr)
            continue;
        if (item->command() == MAV_CMD_DO_CHANGE_SPEED) {
        }
    }
    for (int i = 0; i < _missionController->visualItems()->count(); i++){
        SimpleMissionItem *item =  _missionController->visualItems()->value<SimpleMissionItem*>(i);
        if (item == nullptr)
            continue;
    }

    _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::saveToCurrent()
{

}

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

bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVector<QGeoCoordinate> &coordinateList)
{
    return extractCoordinateList(missionItems, coordinateList, 0, missionItems.count()-1);
}

bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVector<QGeoCoordinate> &coordinateList, int startIndex, int endIndex)
{
    if (   startIndex >= 0
        && startIndex < missionItems.count()
        && endIndex >= 0
        && endIndex < missionItems.count()) {
        if (startIndex > endIndex) {
            if (!extractCoordinateList(missionItems, coordinateList, startIndex, missionItems.count()-1))
                return false;
            if (!extractCoordinateList(missionItems, coordinateList, 0, endIndex))
                return false;
        } else {
            for (int i = startIndex; i <= endIndex; i++) {
                SimpleMissionItem *mItem = missionItems.value<SimpleMissionItem *>(i);

                if (mItem == nullptr) {
                    coordinateList.clear();
                    return false;
                }
                coordinateList.append(mItem->coordinate());
            }
        }
    } else
        return false;

    return true;
}

bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList)
{
    return extractCoordinateList(missionItems, coordinateList, 0 , missionItems.count()-1);
}

bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex)
{
    QVector<QGeoCoordinate> geoCoordintateList;

    bool retValue = extractCoordinateList(missionItems, geoCoordintateList, startIndex, endIndex);

    if (!retValue)
        return false;

    for (int i = 0; i < geoCoordintateList.size(); i++) {
        QGeoCoordinate vertex = geoCoordintateList[i];
        if (   (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude()))
            || !vertex.isValid())
            geoCoordintateList.removeAt(i);
    }

    for (auto coordinate : geoCoordintateList)
        coordinateList.append(QVariant::fromValue(coordinate));

    return true;
}

/*!
 * \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
    _visualItems.clear();
    _missionItems.clearAndDeleteContents();
    _currentMissionItems.clearAndDeleteContents();
    _waypointPath.clear();
    _currentWaypointPath.clear();
    _snakeTiles.polygons().clear();
    _snakeTilesLocal.polygons().clear();
    _snakeTileCenterPoints.clear();

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

    _localPlanDataValid = false;

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

    WimaPlanData planData = _container->pull();

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

    int areaCounter = 0;
    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++;
            _visualItems.append(&_serviceArea);

            continue;
        }

        if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area?
            _measurementArea =  *qobject_cast<const WimaMeasurementAreaData*>(areaData);
            areaCounter++;
            _visualItems.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++;
            _visualItems.append(&_joinedArea);

            continue;
        }

        if (areaCounter >= numAreas)
            break;
    }

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

    // create mission items
    _missionController->removeAll();
    QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();

    // create SimpleMissionItem by using _missionController
    for ( int i = 0; i < tempMissionItems.size(); i++) {
        _missionController->insertSimpleMissionItem(tempMissionItems[i], missionControllerVisualItems->count());
    }
    // copy mission items from _missionController to _missionItems
    for ( int i = 1; i < missionControllerVisualItems->count(); i++) {
        SimpleMissionItem *visualItem     = qobject_cast<SimpleMissionItem *>((*missionControllerVisualItems)[i]);
        if (visualItem == nullptr) {
            qWarning("WimaController::fetchContainerData(): Nullptr at SimpleMissionItem!");
            return false;
        }
        SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
        _missionItems.append(visualItemCopy);
    }
    if (areaCounter != numAreas)
        return false;

    if (!_setTakeoffLandPosition())
        return false;

    _updateWaypointPath();

    // set _nextPhaseStartWaypointIndex to 1
    disconnect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
    bool reverse = _reverse.rawValue().toBool();
    _nextPhaseStartWaypointIndex.setRawValue(reverse? _missionItems.count() : int(1));
    connect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);

    if(!_calcNextPhase())
        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() )
        return false;

    {
        // Get tiles.
        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);
        }

        QtROSJsonFactory JsonFactory;
        QScopedPointer<rapidjson::Document> doc(JsonFactory.create(_snakeTilesLocal));

        auto &temp = _scenario.getOrigin();
        ::GeoPoint3D origin(temp[0], temp[1], temp[2]);
        QScopedPointer<rapidjson::Document> doc2(JsonFactory.create(origin));

        cout.precision(10);
        cout << "Origin" << endl;
        rapidjson::OStreamWrapper out(std::cout);
        rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
        doc2->Accept(writer);
        std::cout << std::endl << std::endl;

        cout << "Snake Tiles" << endl;
        rapidjson::Writer<rapidjson::OStreamWrapper> writer2(out);
        //doc->Accept(writer2);
        std::cout << std::endl << std::endl;

        QtROSTypeFactory TypeFactory;
      ::GeoPoint3D origin_same;
        TypeFactory.create(*doc2.data(), origin_same);

        cout << "Origin2" << endl;
        std::cout << origin_same << std::endl;

        std::cout << "TypeFactory test true: ";
        bool isSame = origin_same == origin;
        std::cout << isSame << endl;


        ::SnakeTilesLocal tiles_same;
          TypeFactory.create(*doc.data(), tiles_same);


          Polygon2D tile1;
          tile1.path().push_back(QPointF(1,0));
          tile1.path().push_back(QPointF(1,1));
          tile1.path().push_back(QPointF(1,2));
          Polygon2D tile2;
          tile2.path().push_back(QPointF(2,0));
          tile2.path().push_back(QPointF(2,1));
          tile2.path().push_back(QPointF(2,2));
          Polygon2D tile3;
          tile3.path().push_back(QPointF(3,0));
          tile3.path().push_back(QPointF(3,1));
          tile3.path().push_back(QPointF(3,2));
          SnakeTilesLocal tilesSmall;
          tilesSmall.polygons().push_back(tile1);
          tilesSmall.polygons().push_back(tile2);
          tilesSmall.polygons().push_back(tile3);


          QScopedPointer<rapidjson::Document> jsonTileSmall(JsonFactory.create(tilesSmall));

          SnakeTilesLocal tilesSmallSame;
          TypeFactory.create(*jsonTileSmall.data(), tilesSmallSame);

          QScopedPointer<rapidjson::Document> jsonTileSmallSame(JsonFactory.create(tilesSmallSame));

          cout << "Snake Tiles small" << endl;
          rapidjson::Writer<rapidjson::OStreamWrapper> writer4(out);
          jsonTileSmall->Accept(writer4);
          std::cout << std::endl << std::endl;

          cout << "Snake Tiles small same" << endl;
          rapidjson::Writer<rapidjson::OStreamWrapper> writer5(out);
          jsonTileSmallSame->Accept(writer5);
          std::cout << std::endl << std::endl;

          // progress
          QNemoProgress progress;
          progress.progress().push_back(1);
          progress.progress().push_back(2);
          progress.progress().push_back(3);
          progress.progress().push_back(4);
          progress.progress().push_back(13);
          progress.progress().push_back(600);


          QScopedPointer<rapidjson::Document> jsonProgress(JsonFactory.create(progress));
          cout << "Progress" << endl;
          rapidjson::Writer<rapidjson::OStreamWrapper> writer6(out);
          jsonProgress->Accept(writer6);
          std::cout << std::endl << std::endl;

          QNemoProgress progressSame;
          TypeFactory.create(*jsonProgress.data(), progressSame);
          cout << "Progress Type created." << endl;

          QScopedPointer<rapidjson::Document> jsonProgressSame(JsonFactory.create(progressSame));

          cout << "Progress same" << endl;
          rapidjson::Writer<rapidjson::OStreamWrapper> writer7(out);
          jsonProgressSame->Accept(writer7);
          std::cout << std::endl << std::endl;
    }


    emit visualItemsChanged();
    emit missionItemsChanged();
    emit snakeTilesChanged();
    emit snakeTileCenterPointsChanged();

    _localPlanDataValid = true;
    return true;
}

bool WimaController::_calcNextPhase()
{
    if (_missionItems.count() < 1) {
        _startWaypointIndex = 0;
        _endWaypointIndex = 0;
        return false;
    }

    _currentMissionItems.clearAndDeleteContents();

    bool reverse = _reverse.rawValue().toBool(); // Reverses the phase direction. Phases go from high to low waypoint numbers, if true.
    int startIndex = _nextPhaseStartWaypointIndex.rawValue().toInt()-1;
    if (!reverse) {
        if (startIndex > _missionItems.count()-1)
            return false;
    }
    else {
        if (startIndex < 0)
            return false;
    }    
    _startWaypointIndex = startIndex;

    int maxWaypointsPerPhase = _maxWaypointsPerPhase.rawValue().toInt();
    // determine end waypoint index
    bool lastMissionPhaseReached = false;
    if (!reverse) {
        _endWaypointIndex = std::min(_startWaypointIndex + maxWaypointsPerPhase - 1, _missionItems.count()-1);
        if (_endWaypointIndex == _missionItems.count() - 1)
            lastMissionPhaseReached = true;
    }
    else {
        _endWaypointIndex = std::max(_startWaypointIndex - maxWaypointsPerPhase + 1, 0);
        if (_endWaypointIndex == 0)
            lastMissionPhaseReached = true;
    }


    // extract waypoints
    QVector<QGeoCoordinate> CSWpList; // list with potential waypoints (from _missionItems), for _currentMissionItems

    if (!reverse) {
        if (!extractCoordinateList(_missionItems, CSWpList, _startWaypointIndex, _endWaypointIndex)) {
            qWarning("WimaController::calcNextPhase(): error on waypoint extraction.");
            return false;
        }
    } else {
        if (!extractCoordinateList(_missionItems, CSWpList, _endWaypointIndex, _startWaypointIndex)) {
            qWarning("WimaController::calcNextPhase(): error on waypoint extraction.");
            return false;
        }

        // reverse path
        QVector<QGeoCoordinate> reversePath;
        for (QGeoCoordinate c : CSWpList)
            reversePath.prepend(c);
        CSWpList.clear();
        CSWpList.append(reversePath);
    }


    // calculate phase length
    _measurementPathLength = 0;
    for (int i = 0; i < CSWpList.size()-1; ++i)
        _measurementPathLength += CSWpList[i].distanceTo(CSWpList[i+1]);


    // set start waypoint index for next phase
    if (!lastMissionPhaseReached) {
        disconnect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
        if (!reverse) {
            int untruncated = std::max(_endWaypointIndex + 1 - _overlapWaypoints.rawValue().toInt(), 0);
            int truncated   = std::min(untruncated , _missionItems.count()-1);
            _nextPhaseStartWaypointIndex.setRawValue(truncated  + 1);
        }
        else {
            int untruncated = std::min(_endWaypointIndex - 1 + _overlapWaypoints.rawValue().toInt(), _missionItems.count()-1);
            int truncated   = std::max(untruncated , 0);
            _nextPhaseStartWaypointIndex.setRawValue(truncated  + 1);
        }
        connect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
    }

    // calculate path from home to first waypoint
    QVector<QGeoCoordinate> arrivalPath;
    if (!_takeoffLandPostion.isValid()){
        qWarning("WimaController::calcNextPhase(): _takeoffLandPostion not valid.");
        return false;
    }
    if ( !calcShortestPath(_takeoffLandPostion, CSWpList.first(), arrivalPath) ) {
        qWarning("WimaController::calcNextPhase(): Not able to calc path from home to first waypoint.");
        return false;
    }

    // calculate arrival path length
    _arrivalPathLength = 0;
    for (int i = 0; i < arrivalPath.size()-1; ++i)
        _arrivalPathLength += arrivalPath[i].distanceTo(arrivalPath[i+1]);

    arrivalPath.removeFirst();

    // calculate path from last waypoint to home
    QVector<QGeoCoordinate> returnPath;
    if ( !calcShortestPath(CSWpList.last(), _takeoffLandPostion, returnPath) ) {
        qWarning("WimaController::calcNextPhase(): Not able to calc path from home to last waypoint.");
        return false;
    }

    // calculate arrival path length
    _returnPathLength = 0;
    for (int i = 0; i < returnPath.size()-1; ++i)
        _returnPathLength += returnPath[i].distanceTo(returnPath[i+1]);

    returnPath.removeFirst();
    returnPath.removeLast();



    // create Mission Items
    _missionController->removeAll();
    QmlObjectListModel* missionControllerVisuals = _missionController->visualItems();

    // set homeposition of settingsItem
    MissionSettingsItem* settingsItem = missionControllerVisuals->value<MissionSettingsItem *>(0);
    if (settingsItem == nullptr) {
        qWarning("WimaController::calcNextPhase(): nullptr");
        return false;
    }
    settingsItem->setCoordinate(_takeoffLandPostion);

    // set takeoff position for first mission item (bug)
    missionController()->insertSimpleMissionItem(_takeoffLandPostion, 1);
    SimpleMissionItem *takeoffItem = missionControllerVisuals->value<SimpleMissionItem*>(1);
    if (takeoffItem == nullptr) {
        qWarning("WimaController::calcNextPhase(): nullptr");
        return false;
    }
    takeoffItem->setCoordinate(_takeoffLandPostion);

    // create change speed item, after take off
    _missionController->insertSimpleMissionItem(_takeoffLandPostion, 2);
    SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
    if (speedItem == nullptr) {
        qWarning("WimaController::calcNextPhase(): nullptr");
        return false;
    }
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);// set coordinate must be after setCommand (setCommand sets coordinate to zero)
    speedItem->setCoordinate(_takeoffLandPostion);
    speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());

    // insert arrival path
    for (auto coordinate : arrivalPath)
        _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());

    // create change speed item, after arrival path
    int index = missionControllerVisuals->count();
    _missionController->insertSimpleMissionItem(CSWpList.first(), index);
    speedItem = missionControllerVisuals->value<SimpleMissionItem*>(index);
    if (speedItem == nullptr) {
        qWarning("WimaController::calcNextPhase(): nullptr");
        return false;
    }
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
    speedItem->setCoordinate(CSWpList.first());
    speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble());

    // insert Circular Survey coordinates
    for (auto coordinate : CSWpList)
        _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());

    // create change speed item, after circular survey
    index = missionControllerVisuals->count();
    _missionController->insertSimpleMissionItem(CSWpList.last(), index);
    speedItem = missionControllerVisuals->value<SimpleMissionItem*>(index);
    if (speedItem == nullptr) {
        qWarning("WimaController::calcNextPhase(): nullptr");
        return false;
    }
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
    speedItem->setCoordinate(CSWpList.last());
    speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());

    // insert return path coordinates
    for (auto coordinate : returnPath)
        _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());

    // set land command for last mission item
    index = missionControllerVisuals->count();
    _missionController->insertSimpleMissionItem(_takeoffLandPostion, index);
    SimpleMissionItem *landItem = missionControllerVisuals->value<SimpleMissionItem*>(index);
    if (landItem == nullptr) {
        qWarning("WimaController::calcNextPhase(): nullptr");
        return false;
    }
    _missionController->setLandCommand(*landItem);

    // copy to _currentMissionItems
    for ( int i = 1; i < missionControllerVisuals->count(); i++) {
        SimpleMissionItem *visualItem     = missionControllerVisuals->value<SimpleMissionItem*>(i);
        if (visualItem == nullptr) {
            qWarning("WimaController::calcNextPhase(): Nullptr at SimpleMissionItem!");
            _currentMissionItems.clear();
            return false;
        }

        SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
        _currentMissionItems.append(visualItemCopy);
    }

    double dist = 0;
    double time = 0;
    if (!_missionController->distanceTimeToMissionEnd(dist, time, 1, false))
        qWarning("WimaController::calcNextPhase: distanceTimeToMissionEnd returned false!");
    _setPhaseDistance(dist);
    _setPhaseDuration(time);
    _missionController->removeAll(); // remove items from _missionController, will be added on upload
    _updateAltitude();

    _updateCurrentPath();
    emit currentMissionItemsChanged();

    return true;
}

void WimaController::_updateWaypointPath()
{
    _waypointPath.clear();
    extractCoordinateList(_missionItems, _waypointPath, 0, _missionItems.count()-1);

    emit waypointPathChanged();
}
void WimaController::_updateCurrentPath()
{
    _currentWaypointPath.clear();
    extractCoordinateList(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1);

    emit currentWaypointPathChanged();
}

void WimaController::_updateNextWaypoint()
{
    if (_endWaypointIndex < _missionItems.count()-2) {
        disconnect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
        _nextPhaseStartWaypointIndex.setRawValue(1 + std::max(_endWaypointIndex + 1 - _overlapWaypoints.rawValue().toInt(), 0));
        connect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
    }
}

void WimaController::_recalcCurrentPhase()
{
    disconnect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
    _nextPhaseStartWaypointIndex.setRawValue(_startWaypointIndex + 1);    
    connect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
    _calcNextPhase();
}

bool WimaController::_setTakeoffLandPosition()
{
    _takeoffLandPostion.setAltitude(0);
    _takeoffLandPostion.setLongitude(_serviceArea.center().longitude());
    _takeoffLandPostion.setLatitude(_serviceArea.center().latitude());

    return true;
}

void WimaController::_updateflightSpeed()
{
    int speedItemCounter = 0;
    for (int i = 0; i < _currentMissionItems.count(); i++) {
        SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
        if (item != nullptr && item->command() == MAV_CMD_DO_CHANGE_SPEED) {
            speedItemCounter++;
            if (speedItemCounter == 2) {
                item->missionItem().setParam2(_flightSpeed.rawValue().toDouble());
            }
        }
    }

    _setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble()
                      + (_arrivalPathLength + _returnPathLength)
                      / _arrivalReturnSpeed.rawValue().toDouble());

    if (speedItemCounter != 3)
        qWarning("WimaController::updateflightSpeed(): internal error.");
}

void WimaController::_updateArrivalReturnSpeed()
{
    int speedItemCounter = 0;
    for (int i = 0; i < _currentMissionItems.count(); i++) {
        SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
        if (item != nullptr && item->command() == MAV_CMD_DO_CHANGE_SPEED) {
            speedItemCounter++;
            if (speedItemCounter != 2) {
                item->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
            }
        }
    }

    _setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble()
                      + (_arrivalPathLength + _returnPathLength)
                      / _arrivalReturnSpeed.rawValue().toDouble());

    if (speedItemCounter != 3)
        qWarning("WimaController::updateArrivalReturnSpeed(): internal error.");

}

void WimaController::_updateAltitude()
{
    for (int i = 0; i < _currentMissionItems.count(); i++) {
        SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
        if (item == nullptr) {
            qWarning("WimaController::updateAltitude(): nullptr");
            return;
        }
        item->altitude()->setRawValue(_altitude.rawValue().toDouble());
    }
}

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) {

            _snakeProgress.clear();
            long n = _scenario.getTilesENU().size();
            _snakeProgress.reserve(n);
            std::srand(time(NULL));
            for (long i=0; i<n; ++i){
                int r{rand()%200};
                if ( r > 100 )
                    r = 100;
                _snakeProgress.append(r);
            }

            _snakeWorker.setScenario(_scenario);
            _snakeWorker.setProgress(_snakeProgress);
            _snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble());
            _snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble());
            _setSnakeCalcInProgress(true);
            _snakeWorker.start();

            emit snakeProgressChanged();
        }
    }

    if (rosBridgeTicker.ready()) {
//        using namespace ros_bridge;

        _pRosBridge->publish(_snakeTilesLocal, "/snake/snake_tiles");
//        // Time
//        class Time time(1, 2);
//        rapidjson::Document doc(rapidjson::kObjectType);
//        // Write to stdout
//        cout << "Time" << endl;
//        cout << "Return value : " << time.toJson(doc, doc.GetAllocator()) << std::endl;
//        rapidjson::OStreamWrapper out(std::cout);
//        rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
//        doc.Accept(writer);
//        std::cout << std::endl << std::endl;


//        // Header
//        Header header(1, &time, "/map");
//        // Write to stdout
//        cout << "Header" << endl;
//        rapidjson::Document doc2(rapidjson::kObjectType);
//        cout << "Return value : " << header.toJson(doc2, doc2.GetAllocator()) << std::endl;
//        rapidjson::Writer<rapidjson::OStreamWrapper> writer2(out);
//        doc2.Accept(writer2);
//        std::cout << std::endl << std::endl;


//        // Point 3D
//        QVector3D p1(1, 2, 3);
//        typedef Point32<QVector3D> PointWrapper;
//        PointWrapper pw1(&p1);
//        // Write to stdout
//        cout << "Point 3D" << endl;
//        rapidjson::Document doc3(rapidjson::kObjectType);
//        cout << "Return value : " << pw1.toJson(doc3, doc3.GetAllocator()) << std::endl;
//        rapidjson::Writer<rapidjson::OStreamWrapper> writer3(out);
//        doc3.Accept(writer3);
//        std::cout << std::endl << std::endl;


//        // Point 2D
//        QPoint p2D(1, 2);
//        p2D.x();
//        Point32<QPoint> pw2D(&p2D);
//        // Write to stdout
//        cout << "Point 2D" << endl;
//        rapidjson::Document docI(rapidjson::kObjectType);
//        cout << "Return value : " << pw2D.toJson(docI, docI.GetAllocator()) << std::endl;
//        rapidjson::Writer<rapidjson::OStreamWrapper> writerI(out);
//        docI.Accept(writerI);
//        std::cout << std::endl << std::endl;


//        // Polygon
//        QVector3D p2(4, 5, 6);
//        QVector3D p3(7, 8, 9);
//        PointWrapper pw2(&p2);
//        PointWrapper pw3(&p3);
//        QVector<PointWrapper> pointList;
//        pointList.append(pw1);
//        pointList.append(pw2);
//        pointList.append(pw3);
//        typedef Polygon<PointWrapper, QVector> Poly;
//        Poly polyW(&pointList);
//        // Write to stdout
//        cout << "Polygon" << endl;
//        rapidjson::Document doc4(rapidjson::kObjectType);
//        cout << "Return value : " << polyW.toJson(doc4, doc4.GetAllocator()) << std::endl;
//        rapidjson::Writer<rapidjson::OStreamWrapper> writer4(out);
//        doc4.Accept(writer4);
//        std::cout << std::endl << std::endl;


//        // PolygonStamped
//        typedef PolygonStamped<PointWrapper, Poly> PolyWrapper;
//        PolyWrapper polyStamped(&header, &polyW);
//        // Write to stdout
//        cout << "PolygonStamped" << endl;
//        rapidjson::Document doc5(rapidjson::kObjectType);
//        cout << "Return value : " << polyStamped.toJson(doc5, doc5.GetAllocator()) << std::endl;
//        rapidjson::Writer<rapidjson::OStreamWrapper> writer5(out);
//        doc5.Accept(writer5);
//        std::cout << std::endl << std::endl;


//        // PolygonArray
//        // Define second Polygon
//        QVector3D p4(10, 11, 12);
//        QVector3D p5(13, 14, 15);
//        QVector3D p6(16, 17, 18);
//        PointWrapper pw4(&p4);
//        PointWrapper pw5(&p5);
//        PointWrapper pw6(&p6);
//        QVector<PointWrapper> pointList2{pw4, pw5, pw6};
//        Poly polyW2(&pointList2);
//        typedef PolygonArray<PointWrapper, Poly, QVector> PolyArray;
//        QVector<Poly*> polygons{&polyW, &polyW2};
//        PolyArray polyArray{};
//        polyArray.setHeader(&header);
//        polyArray.setPolygons(&polygons);
//        // Write to stdout
//        cout << "PolygonStamped" << endl;
//        rapidjson::Document doc6(rapidjson::kObjectType);
//        cout << "Return value : " << polyArray.toJson(doc6, doc6.GetAllocator()) << std::endl;
//        rapidjson::Writer<rapidjson::OStreamWrapper> writer6(out);
//        doc6.Accept(writer6);
//        std::cout << std::endl << std::endl;
    }
}

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::_reverseChangedHandler()
{
    disconnect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
    _nextPhaseStartWaypointIndex.setRawValue(_endWaypointIndex+1);
    connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);

    _calcNextPhase();
}

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

bool WimaController::_calcReturnPath(QString &errorSring)
{
    // it is assumed that checkSmartRTLPreCondition() was called first and true was returned


    Vehicle *managerVehicle = masterController()->managerVehicle();

    QGeoCoordinate currentVehiclePosition = managerVehicle->coordinate();
    // check if vehicle inside _joinedArea, this statement is not inside checkSmartRTLPreCondition() because during checkSmartRTLPreCondition() vehicle is not paused yet
    if (!_joinedArea.containsCoordinate(currentVehiclePosition)) {
        errorSring.append(tr("Vehicle not inside joined area. Action not supported."));
        return false;
    }

    // calculate return path
    QVector<QGeoCoordinate> returnPath;
    calcShortestPath(currentVehiclePosition, _takeoffLandPostion, returnPath);
    // successful?
    if (returnPath.isEmpty()) {
        errorSring.append(tr("Not able to calculate return path."));
        return false;
    }
    // qWarning() << "returnPath.size()" << returnPath.size();

    _saveCurrentMissionItemsToBuffer();
    _phaseDistanceBuffer = _phaseDistance;
    _phaseDurationBuffer = _phaseDuration;


    // create Mission Items
    removeFromVehicle();
    QmlObjectListModel* missionControllerVisuals = _missionController->visualItems();

    // set homeposition of settingsItem
    MissionSettingsItem* settingsItem = missionControllerVisuals->value<MissionSettingsItem *>(0);
    if (settingsItem == nullptr) {
        qWarning("WimaController: nullptr");
        return false;
    }
    settingsItem->setCoordinate(_takeoffLandPostion);

    // copy from returnPath to _missionController
    QGeoCoordinate speedItemCoordinate = returnPath.first();
    for (auto coordinate : returnPath) {
        _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
    }
    //qWarning() << "missionControllerVisuals->count()" << missionControllerVisuals->count();

    // create speed item
    int speedItemIndex = 1;
    _missionController->insertSimpleMissionItem(speedItemCoordinate, speedItemIndex);
    SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(speedItemIndex);
    if (speedItem == nullptr) {
        qWarning("WimaController: nullptr");
        return false;
    }
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
    speedItem->setCoordinate(speedItemCoordinate);
    speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());

    // set second item command to ordinary waypoint (is takeoff)
    SimpleMissionItem *secondItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
    if (secondItem == nullptr) {
        qWarning("WimaController: nullptr");
        return false;
    }
    secondItem->setCoordinate(speedItemCoordinate);
    secondItem->setCommand(MAV_CMD_NAV_WAYPOINT);


    // set land command for last mission item
    SimpleMissionItem *landItem = missionControllerVisuals->value<SimpleMissionItem*>(missionControllerVisuals->count()-1);
    if (landItem == nullptr) {
        qWarning("WimaController: nullptr");
        return false;
    }
    _missionController->setLandCommand(*landItem);

    // copy to _currentMissionItems
    //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
    for ( int i = 1; i < missionControllerVisuals->count(); i++) {
        SimpleMissionItem *visualItem     = missionControllerVisuals->value<SimpleMissionItem*>(i);
        if (visualItem == nullptr) {
            qWarning("WimaController: Nullptr at SimpleMissionItem!");
            _currentMissionItems.clear();
            return false;
        }

        SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
        _currentMissionItems.append(visualItemCopy);
    }
    //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();

    double dist = 0;
    double time = 0;
    if (!_missionController->distanceTimeToMissionEnd(dist, time, 1, false))
        qWarning("WimaController::calcNextPhase: distanceTimeToMissionEnd returned false!");
    _setPhaseDistance(dist);
    _setPhaseDuration(time);
    _missionController->removeAll(); // remove items from _missionController, will be added on upload
    _updateAltitude();

    _updateCurrentPath();
    emit currentMissionItemsChanged();


    //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
    _showAllMissionItems.setRawValue(false);
    managerVehicle->trajectoryPoints()->clear();

    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();
    _missionItems.clearAndDeleteContents();
    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);
    QVector<MissionItem> missionItems;
    missionItems.reserve(int(n));

    // Remove all items from mission controller.
    _missionController->removeAll();
    QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();

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

    // create SimpleMissionItem by using _missionController
    long insertIdx = missionControllerVisualItems->count();
    for (auto wp : waypoints)
        _missionController->insertSimpleMissionItem(wp, insertIdx++);
    SimpleMissionItem *takeOffItem = missionControllerVisualItems->value<SimpleMissionItem*>(1);
    if (takeOffItem == nullptr) {
        qWarning("WimaController::_snakeStoreWorkerResults(): Nullptr at SimpleMissionItem!");
        return;
    }
    takeOffItem->setCommand(MAV_CMD_NAV_WAYPOINT);
    takeOffItem->setCoordinate(waypoints[0]);

    // copy mission items from _missionController to _missionItems
    for ( int i = 1; i < missionControllerVisualItems->count(); i++) {
        SimpleMissionItem *visualItem     = qobject_cast<SimpleMissionItem *>((*missionControllerVisualItems)[i]);
        if (visualItem == nullptr) {
            qWarning("WimaController::_snakeStoreWorkerResults(): Nullptr at SimpleMissionItem!");
            return;
        }
        SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
        _missionItems.append(visualItemCopy);
    }

    _updateWaypointPath();

    // set _nextPhaseStartWaypointIndex to 1
    disconnect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
    bool reverse = _reverse.rawValue().toBool();
    _nextPhaseStartWaypointIndex.setRawValue(reverse? _missionItems.count() : int(1));
    connect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);

    _calcNextPhase();
    emit missionItemsChanged();


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

void WimaController::_loadCurrentMissionItemsFromBuffer()
{
    _currentMissionItems.clear();
    int numItems = _missionItemsBuffer.count();
    for (int i = 0; i < numItems; i++)
        _currentMissionItems.append(_missionItemsBuffer.removeAt(0));

    _updateCurrentPath();
}


void WimaController::_saveCurrentMissionItemsToBuffer()
{
    _missionItemsBuffer.clear();
    int numCurrentMissionItems = _currentMissionItems.count();
    for (int i = 0; i < numCurrentMissionItems; i++)
        _missionItemsBuffer.append(_currentMissionItems.removeAt(0));
}