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

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])
    , _nestPhaseStartWaypointIndex       (settingsGroup, _metaDataMap[startWaypointIndexName])
    , _showAllMissionItems      (settingsGroup, _metaDataMap[showAllMissionItemsName])
    , _showCurrentMissionItems  (settingsGroup, _metaDataMap[showCurrentMissionItemsName])
{
    _nestPhaseStartWaypointIndex.setRawValue(int(1));
    _showAllMissionItems.setRawValue(true);
    _showCurrentMissionItems.setRawValue(true);
    connect(&_overlapWaypoints,     &Fact::rawValueChanged, this, &WimaController::updateNextWaypoint);
    connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::recalcCurrentPhase);
    connect(&_nestPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::calcNextPhase);
}

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

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::dataValidChanged, this, &WimaController::fetchContainerData);
        }

        _container = container;
        connect(_container, &WimaDataContainer::dataValidChanged, this, &WimaController::fetchContainerData);

        emit dataContainerChanged();
    }
}

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

void WimaController::previousPhase()
{
    if (_nestPhaseStartWaypointIndex.rawValue().toInt() > 0) {
        _nestPhaseStartWaypointIndex.setRawValue(   1 + std::max(_startWaypointIndex
                                                  - _maxWaypointsPerPhase.rawValue().toInt()
                                                  + _overlapWaypoints.rawValue().toInt(), 0));
    }
}

void WimaController::resetPhase()
{
    _nestPhaseStartWaypointIndex.setRawValue(int(1));
}

void WimaController::uploadToVehicle()
{
    if (_currentMissionItems.count() < 1)
        return;

    _missionController->removeAll();
    // set homeposition of settingsItem
    QmlObjectListModel* visuals = _missionController->visualItems();
    MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0);
    if (settingsItem == nullptr) {
        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
        return;
    }
    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());
    }

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

//    // check vehicle type, before setting land command
//    Vehicle* controllerVehicle = _masterController->controllerVehicle();
//    MAV_CMD landCmd = controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
//    if (controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
//        landItem->setCommand(landCmd);
//    } else {
//        _missionController->removeAll();
//        return;
//    }

    for (int i = 1; i < visuals->count(); i++) {
        SimpleMissionItem* item = visuals->value<SimpleMissionItem *>(i);
        qWarning() << item->coordinate();
        qWarning() << item->command();
        qWarning() << item->missionItem().param1();
        qWarning() << item->missionItem().param2();
        qWarning() << item->missionItem().param3();
        qWarning() << item->missionItem().param4();
        qWarning() << item->missionItem().param5();
        qWarning() << item->missionItem().param6();
        qWarning() << item->missionItem().param7();
        qWarning(" ");
    }
    _masterController->sendToVehicle();
}

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

void WimaController::startMission()
{

}

void WimaController::abortMission()
{

}

void WimaController::pauseMission()
{

}

void WimaController::resumeMission()
{

}

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 (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
 */
void WimaController::fetchContainerData(bool valid)
{
    if ( valid ) {
        if (_container == nullptr) {
            qWarning("WimaController::containerDataValidChanged(): No container assigned!");
        }
        _localPlanDataValid = false;
        _visualItems.clear();
        _missionItems.clear();
        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;
        }
        QList<const MissionItem*> tempMissionItems = planData.missionItems();

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


        // find takeoff command
        int begin = -1;
        for (int i = 0; i < tempMissionItems.size(); i++) {
            const MissionItem *missionItem = tempMissionItems[i];
            if (    missionItem->command() == MAV_CMD_NAV_VTOL_TAKEOFF
                 || missionItem->command() == MAV_CMD_NAV_TAKEOFF) {

                _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count());

                QGeoCoordinate coordinate = missionItem->coordinate();
                _takeoffLandPostion.setAltitude(coordinate.altitude());
                _takeoffLandPostion.setLatitude(coordinate.latitude());
                _takeoffLandPostion.setLongitude(coordinate.longitude());

                begin = i + 1;
                break;
            }
        }

        // check if takeoff command found
        if (begin < 0) {
            qWarning("WimaController::containerDataValidChanged(): No takeoff found.");
            return;
        }

        // copy mission items and create SimpleMissionItem by using _missionController
        for ( int i = begin; i < tempMissionItems.size(); i++) {
            const MissionItem *missionItem = tempMissionItems[i];
            _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count());

            if (    missionItem->command() == MAV_CMD_NAV_VTOL_LAND
                 || missionItem->command() == MAV_CMD_NAV_LAND)
                 break;
        }

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

        if (areaCounter == numAreas)
            _localPlanDataValid = true;

        updateWaypointPath();
        _nestPhaseStartWaypointIndex.setRawValue(int(1));
        calcNextPhase();

    } else {
        _localPlanDataValid = false;
        _visualItems.clear();
        _missionItems.clear();
    }

    emit visualItemsChanged();
    emit missionItemsChanged();

#ifdef QT_DEBUG
    //qWarning("Mission Items count: ");
    //qWarning() << _missionItems.count();
#endif
}

void WimaController::calcNextPhase()
{
    _startWaypointIndex = _nestPhaseStartWaypointIndex.rawValue().toInt()-1;
    // check if data was fetched and mission end is not reached yet
    if (_missionItems.count() < 1 || !_localPlanDataValid || _startWaypointIndex >= _missionItems.count()-2)
        return;

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

    // determine end waypoint index
    _endWaypointIndex = std::min(_startWaypointIndex + maxWaypointsPerPhaseInt - 1, _missionItems.count()-2); // -2 -> last item is land item

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

    // set start waypoint index for next phase
    if (_endWaypointIndex < _missionItems.count()-2) {
        _startWaypointIndexList.append(_nestPhaseStartWaypointIndex.rawValue().toInt());

        disconnect(&_nestPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::calcNextPhase);
        int temp = 1 + std::max(_endWaypointIndex + 1 - _overlapWaypoints.rawValue().toInt(), 0);
        _nestPhaseStartWaypointIndex.setRawValue(std::min(temp , _missionItems.count()-1));
        connect(&_nestPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::calcNextPhase);
    }
    else {
        disconnect(&_nestPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::calcNextPhase);
        _nestPhaseStartWaypointIndex.setRawValue(_missionItems.count()-1); // marks that end of mission is reached
        connect(&_nestPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::calcNextPhase);
    }

    // calculate path from home to first waypoint
    QList<QGeoCoordinate> path;
    if ( !calcShortestPath(_takeoffLandPostion, geoCoordinateList[0], path) ) {
        qWarning("WimaController::updateCurrentMissionItems(): Not able to calc path from home to first waypoint.");
        _currentMissionItems.clear();
        return;
    }
    // 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::updateCurrentMissionItems(): Not able to calc path from home to first waypoint.");
        _currentMissionItems.clear();
        return;
    }
    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::updateCurrentMissionItems(): nullptr");
        _currentMissionItems.clear();
        return;
    }
    settingsItem->setCoordinate(_takeoffLandPostion);

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

    // set homeposition for take off item (somehow not working with insertSimpleMissionItem)
    SimpleMissionItem* takeoff = missionControllerVisuals->value<SimpleMissionItem *>(1);
    if (takeoff == nullptr) {
        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
        _currentMissionItems.clear();
        return;
    }
    takeoff->setCoordinate(_takeoffLandPostion);


    // set land command for last mission item
    SimpleMissionItem *landItem = missionControllerVisuals->value<SimpleMissionItem*>(missionControllerVisuals->count()-1);
    if (landItem == nullptr) {
        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
        _currentMissionItems.clear();
        return;
    }
    // check vehicle type, before setting land command
    Vehicle* controllerVehicle = _masterController->controllerVehicle();
    MAV_CMD landCmd = controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
    if (controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
        landItem->setCommand(landCmd);
    } else {
        _currentMissionItems.clear();
        return;
    }

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

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

    _missionController->removeAll(); // remove items from _missionController, will be added on upload

    updateCurrentPath();
    emit currentMissionItemsChanged();
}

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(&_nestPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::calcNextPhase);
        _nestPhaseStartWaypointIndex.setRawValue(1 + std::max(_endWaypointIndex + 1 - _overlapWaypoints.rawValue().toInt(), 0));
        connect(&_nestPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::calcNextPhase);
    }
}

void WimaController::recalcCurrentPhase()
{
    _nestPhaseStartWaypointIndex.setRawValue(_startWaypointIndex + 1);
}