WimaController.cc

#include "WimaController.h"

const char* WimaController::wimaFileExtension   = "wima";
const char* WimaController::areaItemsName       = "AreaItems";
const char* WimaController::missionItemsName    = "MissionItems";

WimaController::WimaController(QObject *parent)
    : QObject               (parent)
    , _container            (nullptr)
    , _joinedArea           (this)
    , _measurementArea      (this)
    , _serviceArea          (this)
    , _corridor             (this)
    , _localPlanDataValid   (false)
    , _startWaypointIndex   (1)
    , _endWaypointIndex     (1)
{

}

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

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

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

        emit dataContainerChanged();
    }
}

void WimaController::nextPhase()
{
    updateCurrentMissionItems();
    updateCurrentPath();
}

void WimaController::startMission()
{

}

void WimaController::abortMission()
{

}

void WimaController::pauseMission()
{

}

void WimaController::resumeMission()
{

}

bool WimaController::updateMission()
{
    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 (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::containerDataValidChanged(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;
        }

#ifdef QT_DEBUG
        //qWarning("containerDataValidChanged(): count:");
        //qWarning() << planData.missionItems().count();
#endif

        QList<const MissionItem*> tempMissionItems = planData.missionItems();

        // create mission items
        _missionController->removeAll();
        QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();
        bool copyON = false;
        for ( int i = 0; i < tempMissionItems.size(); i++) {
            const MissionItem *missionItem = tempMissionItems[i];
            if (copyON || missionItem->command() == MAV_CMD_NAV_VTOL_TAKEOFF
                       || missionItem->command() == MAV_CMD_NAV_TAKEOFF) {
                _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count());
                copyON = true;
            }

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


        // copy mission items to _missionItems
//        MissionSettingsItem *settingsItem     = qobject_cast<MissionSettingsItem *>((*missionControllerVisualItems)[0]);
//        if (settingsItem == nullptr) {
//            qWarning("WimaController::containerDataValidChanged(): Nullptr at MissionSettingsItem!");
//            return;
//        }
//        _missionItems.append(settingsItem);

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

        updateWaypointPath();

        _startWaypointIndex = 1;
        updateCurrentMissionItems();
        updateCurrentPath();

        if (areaCounter == numAreas)
            _localPlanDataValid = true;

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

    emit visualItemsChanged();
    emit missionItemsChanged();

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

void WimaController::updateCurrentMissionItems()
{
    int numberWaypoints = 30; // the number of waypoints currentMissionItems must not exceed
    int overlapping = 2; // number of overlapping waypoints of consecutive mission phases

    SimpleMissionItem *homeItem = _missionItems.value<SimpleMissionItem *>(0);
    if (homeItem == nullptr) {
        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
        _currentMissionItems.clear();
        return;
    }
    QGeoCoordinate homeCoordinate(homeItem->coordinate());

    QList<QGeoCoordinate> geoCoordinateList; // list with potential waypoints (from _missionItems), for _currentMissionItems
    _endWaypointIndex = std::min(_startWaypointIndex + numberWaypoints - 1, _missionItems.count()-2); // -2 -> last item is land item
    if (!extractCoordinateList(_missionItems, geoCoordinateList, _startWaypointIndex, _endWaypointIndex)) {
        qWarning("WimaController::updateCurrentMissionItems(): error on waypoint extraction.");
        _currentMissionItems.clear();
        return;
    }
    _startWaypointIndex = _endWaypointIndex + 1 - overlapping;

    // calculate path from home to first waypoint
    QList<QGeoCoordinate> path;
    if ( !calcShortestPath(homeCoordinate, 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(), homeCoordinate, 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();
    for (auto coordinate : geoCoordinateList)
        _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());

    // 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 mission items to _currentMissionItems
//    MissionSettingsItem *settingsItem     = qobject_cast<MissionSettingsItem *>((*missionControllerVisuals)[0]);
//    if (settingsItem == nullptr) {
//        qWarning("WimaController::containerDataValidChanged(): Nullptr at MissionSettingsItem!");
//        return;
//    }
//    _missionItems.append(settingsItem);

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

    emit currentMissionItemsChanged();
}

void WimaController::updateWaypointPath()
{
    _waypointPath.clear();
    if (!extractCoordinateList(_missionItems, _waypointPath, 0, _missionItems.count()-1))
        return;

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

    emit currentWaypointPathChanged();
}