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::arrivalReturnSpeedName      = "ArrivalReturnSpeed";
const char* WimaController::altitudeName                = "Altitude";
const char* WimaController::reverseName                 = "Reverse";

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)
    , _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::updateflightSpeed);
    connect(&_altitude,                     &Fact::rawValueChanged, this, &WimaController::updateAltitude);
    connect(&_reverse,                      &Fact::rawValueChanged, this, &WimaController::reverseChangedHandler);

    // setup low battery handling
    connect(&_checkBatteryTimer, &QTimer::timeout, this, &WimaController::checkBatteryLevel);
    _checkBatteryTimer.setInterval(500);
    Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
    connect(enableLowBatteryHandling, &Fact::rawValueChanged, this, &WimaController::enableDisableLowBatteryHandling);
    enableDisableLowBatteryHandling(enableLowBatteryHandling->rawValue());
}

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::arrivalReturnSpeed()
{
    return &_arrivalReturnSpeed;
}

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

Fact *WimaController::reverse()
{
    return &_reverse;
}

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

    _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, 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 sucess

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

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

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



    emit visualItemsChanged();
    emit missionItemsChanged();


    _localPlanDataValid = true;
    return true;
}

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

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

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


    // 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;
    }
    arrivalPath.removeFirst();
    arrivalPath.removeLast();

    // 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 first waypoint.");
        return false;
    }    
    int returnPathLength = returnPath.size()-2;
    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);

//    qDebug("homeposition");
//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
//        if (item != nullptr) {
//            qDebug() << item->coordinate();
//        } else {
//            qDebug("Error");
//        }
//    }

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

//    qDebug("takeoff");
//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
//        if (item != nullptr) {
//            qDebug() << item->coordinate();
//        } else {
//            qDebug("Error");
//        }
//    }

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

//    qDebug("speed");
//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
//        if (item != nullptr) {
//            qDebug() << item->coordinate();
//        } else {
//            qDebug("Error");
//        }
//    }

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

//    qDebug("arrival");
//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
//        if (item != nullptr) {
//            qDebug() << item->coordinate();
//        } else {
//            qDebug("Error");
//        }
//    }

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

//    qDebug("speed");
//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
//        if (item != nullptr) {
//            qDebug() << item->coordinate();
//        } else {
//            qDebug("Error");
//        }
//    }

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

//    qDebug("survey");
//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
//        if (item != nullptr) {
//            qDebug() << item->coordinate();
//        } else {
//            qDebug("Error");
//        }
//    }

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

//    qDebug("speed");
//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
//        if (item != nullptr) {
//            qDebug() << item->coordinate();
//        } else {
//            qDebug("Error");
//        }
//    }

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

//    qDebug("return path");
//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
//        if (item != nullptr) {
//            qDebug() << item->coordinate();
//        } else {
//            qDebug("Error");
//        }
//    }

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

//    qDebug("land");
//    for (int i = 0; i < missionControllerVisuals->count(); ++i) {
//        VisualMissionItem *item =  missionControllerVisuals->value<VisualMissionItem*>(i);
//        if (item != nullptr) {
//            qDebug() << item->coordinate();
//        } else {
//            qDebug("Error");
//        }
//    }

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

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

            if (speedItemCounter == 3)
                break;
        }
    }

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

    static long attemptCounter = 0;

    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) {
                QString errorString;
                attemptCounter++;
                if (attemptCounter > 3) {
                    _lowBatteryHandlingTriggered = true;
                    attemptCounter = 0;
                }
                if (_checkSmartRTLPreCondition(errorString) == true) {
                    managerVehicle->pauseVehicle();
                    if (_calcReturnPath(errorString)) {
                        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::enableDisableLowBatteryHandling(QVariant enable)
{
    if (enable.toBool()) {
        _checkBatteryTimer.start();
    } else {
        _checkBatteryTimer.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;
   }

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

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

    updateCurrentPath();
}


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