WimaController.cc

#include "WimaController.h"

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::altitudeName                = "Altitude";

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])
    , _altitude                 (settingsGroup, _metaDataMap[altitudeName])
    , _startWaypointIndex       (0)
    , _lastMissionPhaseReached  (false)
    , _uploadOverrideRequired   (false)
    , _phaseDistance(-1)
    , _phaseDuration(-1)
    , _vehicleHasLowBattery(false)
    , _lowBatteryHandlingTriggered(false)
    , _executingSmartRTL(false)

{
    _nextPhaseStartWaypointIndex.setRawValue(int(1));
    _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::updateSpeed);
    connect(&_altitude,     &Fact::rawValueChanged, this, &WimaController::updateAltitude);

    // battery timer
    connect(&_checkBatteryTimer, &QTimer::timeout, this, &WimaController::checkBatteryLevel);
    _checkBatteryTimer.setInterval(500);
    _checkBatteryTimer.start();
}

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

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

WimaDataContainer *WimaController::dataContainer()
{
    return _container;
}

QmlObjectListModel *WimaController::missionItems()
{
    return &_missionItems;
}

QmlObjectListModel *WimaController::currentMissionItems()
{
    return &_currentMissionItems;
}

QVariantList WimaController::waypointPath()
{
    return  _waypointPath;
}

QVariantList WimaController::currentWaypointPath()
{
    return _currentWaypointPath;
}

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

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

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

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

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

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

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

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

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

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

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

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

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()
{
    if (_nextPhaseStartWaypointIndex.rawValue().toInt() > 0) {
        _lastMissionPhaseReached = false;
        _nextPhaseStartWaypointIndex.setRawValue(std::max(_startWaypointIndex
                                                          - _maxWaypointsPerPhase.rawValue().toInt()
                                                          + _overlapWaypoints.rawValue().toInt(), 1));
    }
}

void WimaController::resetPhase()
{
    _lastMissionPhaseReached = false;
    _nextPhaseStartWaypointIndex.setRawValue(int(1));
}

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

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

bool WimaController::executeSmartRTL()
{
    QString errorString;
    bool retValue = _executeSmartRTL(errorString);
    if (!retValue) {
        qgcApp()->showMessage(errorString);
    }

    return retValue;
}

bool WimaController::initSmartRTL()
{
    masterController()->managerVehicle()->pauseVehicle();
    QString errorString;
    bool retValue = calcReturnPath();
    if (!retValue)
        return false;
    return true;
}

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, QList<QGeoCoordinate> &path)
{
    using namespace GeoUtilities;
    using namespace PolygonCalculus;
    QList<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, QList<QGeoCoordinate> &coordinateList)
{
    return extractCoordinateList(missionItems, coordinateList, 0, missionItems.count()-1);
}

bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QList<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)
{
    QList<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()))
            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 sucess

    // reset visual items
    _visualItems.clear();
    _missionItems.clearAndDeleteContents();
    _currentMissionItems.clearAndDeleteContents();
    _waypointPath.clear();
    _currentWaypointPath.clear();

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

    _localPlanDataValid = false;
    _lastMissionPhaseReached = 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);
    _nextPhaseStartWaypointIndex.setRawValue(int(1));
    connect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::calcNextPhase);

    if(!calcNextPhase())
        return false;



    emit visualItemsChanged();
    emit missionItemsChanged();


    _localPlanDataValid = true;
    return true;
}

bool WimaController::calcNextPhase()
{
    if (_missionItems.count() < 1 || _lastMissionPhaseReached)
        return false;

    _currentMissionItems.clearAndDeleteContents();
    _currentWaypointPath.clear();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();

    _startWaypointIndex = _nextPhaseStartWaypointIndex.rawValue().toInt()-1;
    if (_startWaypointIndex > _missionItems.count()-1)
        return false;


    int maxWaypointsPerPhaseInt = _maxWaypointsPerPhase.rawValue().toInt();

    // determine end waypoint index
    _endWaypointIndex = std::min(_startWaypointIndex + maxWaypointsPerPhaseInt - 1, _missionItems.count()-1);
    if (_endWaypointIndex == _missionItems.count() - 1)
        _lastMissionPhaseReached = true;

    // extract waypoints
    QList<QGeoCoordinate> geoCoordinateList; // list with potential waypoints (from _missionItems), for _currentMissionItems
    if (!extractCoordinateList(_missionItems, geoCoordinateList, _startWaypointIndex, _endWaypointIndex)) {
        qWarning("WimaController::calcNextPhase(): error on waypoint extraction.");
        return false;
    }

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

    // calculate path from home to first waypoint
    QList<QGeoCoordinate> path;
    if (!_takeoffLandPostion.isValid()){
        qWarning("WimaController::calcNextPhase(): _takeoffLandPostion not valid.");
        return false;
    }
    if ( !calcShortestPath(_takeoffLandPostion, geoCoordinateList[0], path) ) {
        qWarning("WimaController::calcNextPhase(): Not able to calc path from home to first waypoint.");
        return false;
    }
    // prepend to geoCoordinateList
    for (int i = path.size()-2; i >= 0; i--) //  i = path.size()-2 : last coordinate already in geoCoordinateList
        geoCoordinateList.prepend(path[i]);

    // calculate path from last waypoint to home
    path.clear();
    if ( !calcShortestPath(geoCoordinateList.last(), _takeoffLandPostion, path) ) {
        qWarning("WimaController::calcNextPhase(): Not able to calc path from home to first waypoint.");
        return false;
    }
    path.removeFirst(); // first coordinate already in geoCoordinateList
    geoCoordinateList.append(path);


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

    for (auto coordinate : geoCoordinateList) {
        _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
    }

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

    // create change speed item
    _missionController->insertSimpleMissionItem(_takeoffLandPostion, 2);
    SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
    if (speedItem == nullptr) {
        qWarning("WimaController::calcNextPhase(): nullptr");
        return false;
    }
    speedItem->setCoordinate(_takeoffLandPostion);
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
    speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble());


    // set land command for last mission item
    SimpleMissionItem *landItem = missionControllerVisuals->value<SimpleMissionItem*>(missionControllerVisuals->count()-1);
    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);
    }

    _setPhaseDistance(_missionController->missionDistance());
    _setPhaseDuration(_missionController->missionDistance()/_flightSpeed.rawValue().toDouble());
    _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()
{
    _lastMissionPhaseReached = false;
    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::updateSpeed()
{
    SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(1); // speed item
    if (item != nullptr && item->command() == MAV_CMD_DO_CHANGE_SPEED) {
        item->missionItem().setParam2(_flightSpeed.rawValue().toDouble());
        _setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble());
    }
    else {
        qWarning("WimaController::updateSpeed(): 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();
    int batteryThreshold = 94; // percent
    if (managerVehicle != nullptr) {
        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) {
                QString errorString;
                if (_checkSmartRTLPreCondition(errorString) == true) {
                    managerVehicle->pauseVehicle();
                    if (_calcReturnPath(errorString)) {                        
                        _lowBatteryHandlingTriggered = true;
                        emit returnBatteryLowConfirmRequired();
                    } else {
                        qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible."));
                        qgcApp()->showMessage(errorString);
                    }
                }
            }
        }
        else {
            _setVehicleHasLowBattery(false);
            _lowBatteryHandlingTriggered = false;
        }

    }
}

void WimaController::smartRTLCleanUp(bool flying)
{

    if ( !flying) { // vehicle has landed
        if (_executingSmartRTL) {
            _executingSmartRTL = false;
            _loadCurrentMissionItemsFromBuffer();
            _showAllMissionItems.setRawValue(true);
            disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp);
        }
    }
}

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

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

    // 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->setCoordinate(speedItemCoordinate);
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
    speedItem->missionItem().setParam2(_flightSpeed.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();

    _setPhaseDistance(_missionController->missionDistance());
    _setPhaseDuration(_missionController->missionDistance()/_flightSpeed.rawValue().toDouble());
    _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();

    emit uploadAndExecuteConfirmRequired();
    return true;
}

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

        emit vehicleHasLowBatteryChanged();
    }
}

bool WimaController::_executeSmartRTL(QString &errorSring)
{
    Q_UNUSED(errorSring);
    _executingSmartRTL = true;
    connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp);
    forceUploadToVehicle();
    masterController()->managerVehicle()->startMission();

    return true;
}

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


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