WimaController.cc

#include "WimaController.h"

const char* WimaController::wimaFileExtension = "wima";

WimaController::WimaController(QObject *parent) :
    QObject             (parent)
  ,_planView            (true)
  ,_visualItems         (new QmlObjectListModel(parent))
  ,_currentPolygonIndex (-1)
{
    connect(this, &WimaController::currentPolygonIndexChanged, this, &WimaController::recalcPolygonInteractivity);
}

QStringList WimaController::loadNameFilters() const
{
    QStringList filters;

    filters << tr("Supported types (*.%1)").arg(wimaFileExtension) <<
               tr("All Files (*.*)");
    return filters;

}

void WimaController::setMasterController(PlanMasterController *masterC)
{
    _masterController = masterC;
    emit masterControllerChanged();
}

void WimaController::setMissionController(MissionController *missionC)
{
    _missionController = missionC;
    emit missionControllerChanged();
}

void WimaController::setCurrentPolygonIndex(int index)
{
    if(index >= 0 && index < _visualItems->count() && index != _currentPolygonIndex){
        _currentPolygonIndex = index;

        emit currentPolygonIndexChanged(index);
    }
}

void WimaController::removeArea(int index)
{
    if(index >= 0 && index < _visualItems->count()){
        _visualItems->removeAt(index);

        emit visualItemsChanged();

        if (_visualItems->count() == 0) {
            // this branch is reached if all items are removed
            // to guarentee proper behavior, _currentPolygonIndex must be set to a invalid value, as on constructor init.
            _currentPolygonIndex = -1;
            return;
        }

        if(_currentPolygonIndex >= _visualItems->count()){
            setCurrentPolygonIndex(_visualItems->count() - 1);
        }else{
            recalcPolygonInteractivity(_currentPolygonIndex);
        }
    }else{
        qWarning("Index out of bounds!");
    }

}

void WimaController::addGOperationArea()
{
    WimaGOperationArea* newPoly = new WimaGOperationArea(this);
    _visualItems->append(newPoly);
    int newIndex = _visualItems->count()-1;
    setCurrentPolygonIndex(newIndex);
    emit visualItemsChanged();
}

void WimaController::addServiceArea()
{
    WimaServiceArea* newPoly = new WimaServiceArea(this);
    _visualItems->append(newPoly);
    int newIndex = _visualItems->count()-1;
    setCurrentPolygonIndex(newIndex);
    emit visualItemsChanged();
}

void WimaController::addVehicleCorridor()
{
    WimaVCorridor* corridor = new WimaVCorridor(this);
    _visualItems->append(corridor);
    int newIndex = _visualItems->count()-1;
    setCurrentPolygonIndex(newIndex);
    emit visualItemsChanged();
}

void WimaController::startMission()
{

}

void WimaController::abortMission()
{

}

void WimaController::pauseMission()
{

}

void WimaController::resumeMission()
{

}

bool WimaController::updateMission()
{
    #define debug 0
    // pick first WimaGOperationArea
    WimaGOperationArea* opArea = nullptr;
    for (int i = 0; i < _visualItems->count(); i++) {
        WimaGOperationArea* currentArea = qobject_cast<WimaGOperationArea*>(_visualItems->get(i));
        if (currentArea != nullptr){
            opArea = currentArea;
            break;
        }
    }
    if (opArea == nullptr)
        return false;

    // pick first WimaServiceArea
    WimaServiceArea* serArea = nullptr;
    for (int i = 0; i < _visualItems->count(); i++) {
        WimaServiceArea* currentArea = qobject_cast<WimaServiceArea*>(_visualItems->get(i));
        if (currentArea != nullptr){
            serArea = currentArea;
            break;
        }
    }
    if ( serArea == nullptr )
        return false;

    // pick first WimaVCorridor
    WimaVCorridor* corridor = nullptr;
    for (int i = 0; i < _visualItems->count(); i++) {
        WimaVCorridor* currentArea = qobject_cast<WimaVCorridor*>(_visualItems->get(i));
        if (currentArea != nullptr){
            corridor = currentArea;
            break;
        }
    }
    // join service area and op area
    WimaArea joinedArea;
    if (corridor != nullptr) {
        WimaArea::join(*corridor, *serArea, joinedArea);
        joinedArea.join(*opArea);
    } else {
        WimaArea::join(*serArea, *opArea, joinedArea);
    }

    #if debug
        WimaArea* joinedAreaPt = new WimaArea(joinedArea, this);
        _visualItems->append(joinedAreaPt);
    #endif



    // reset visual items
    _missionController->removeAll();
    QmlObjectListModel* missionItems = _missionController->visualItems();
    // set home position to serArea center
    MissionSettingsItem* settingsItem= qobject_cast<MissionSettingsItem*>(missionItems->get(0));
    if (settingsItem == nullptr){
        qWarning("WimaController::updateMission(): settingsItem == nullptr");
        return false;
    }
    settingsItem->setCoordinate(serArea->center());

    // create take off position item
    int index = 1;
    _missionController->insertSimpleMissionItem(serArea->center(), index++);

    // create survey item, will be extened with more mission types in the future
    _missionController->insertComplexMissionItem(_missionController->surveyComplexItemName(), opArea->center(), index++);
    SurveyComplexItem* survey = qobject_cast<SurveyComplexItem*>(missionItems->get(missionItems->count()-1));
    if (survey == nullptr){
        qWarning("WimaController::updateMission(): survey == nullptr");
        return false;
    } else {
        survey->surveyAreaPolygon()->clear();
        survey->surveyAreaPolygon()->appendVertices(opArea->coordinateList());
    }

    // calculate path from take off to opArea
    QGeoCoordinate start = serArea->center();
    QGeoCoordinate end = survey->visualTransectPoints().first().value<QGeoCoordinate>();
    QList<QGeoCoordinate> path;
    WimaArea::dijkstraPath(start, end, joinedArea, path);
    for (int i = 1; i < path.count()-1; i++) {
        _missionController->insertSimpleMissionItem(path.value(i), i+1);
        index++;
    }

    // calculate return path
    start   = survey->visualTransectPoints().last().value<QGeoCoordinate>();
    end     = serArea->center();
    path.clear();
    WimaArea::dijkstraPath(start, end, joinedArea, path);
    for (int i = 1; i < path.count()-1; i++) {
        _missionController->insertSimpleMissionItem(path.value(i), index++);
    }

    // create land position item
    _missionController->insertSimpleMissionItem(serArea->center(), index++);
    SimpleMissionItem* landItem = qobject_cast<SimpleMissionItem*>(missionItems->get(missionItems->count()-1));
    if (landItem == nullptr){
        qWarning("WimaController::updateMission(): landItem == nullptr");
        return false;
    } else {
        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);
        }
    }

    //saveToFile("TestFile.wima");
    //loadFromFile("TestFile.wima");

    return true;
}

void WimaController::saveToCurrent()
{

}

void WimaController::saveToFile(const QString& filename)
{
    if (filename.isEmpty()) {
        return;
    }

    QString planFilename = filename;
    if (!QFileInfo(filename).fileName().contains(".")) {
        planFilename += QString(".%1").arg(wimaFileExtension);
    }

    QFile file(planFilename);

    if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
        qgcApp()->showMessage(tr("Plan save error %1 : %2").arg(filename).arg(file.errorString()));
        _currentFile.clear();
        emit currentFileChanged();
    } else {
        QJsonDocument saveDoc = saveToJson();
        file.write(saveDoc.toJson());
        if(_currentFile != planFilename) {
            _currentFile = planFilename;
            emit currentFileChanged();
        }
    }
}

bool WimaController::loadFromCurrent()
{
    return loadFromFile(_currentFile);
}

bool WimaController::loadFromFile(const QString &filename)
{
    QString errorString;
    QString errorMessage = tr("Error loading Plan file (%1). %2").arg(filename).arg("%1");

    if (filename.isEmpty()) {
        return false;
    }

    QFileInfo fileInfo(filename);
    QFile file(filename);

    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        errorString = file.errorString() + QStringLiteral(" ") + filename;
        qgcApp()->showMessage(errorMessage.arg(errorString));
        return false;
    }

    if(fileInfo.suffix() == wimaFileExtension) {
        QJsonDocument   jsonDoc;
        QByteArray      bytes = file.readAll();

        if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) {
            qgcApp()->showMessage(errorMessage.arg(errorString));
            return false;
        }

        QJsonObject json = jsonDoc.object();
        QJsonArray areaArray = json["AreaItems"].toArray();
        _visualItems->clear();

        for( int i = 0; i < areaArray.size(); i++) {
            QJsonObject jsonArea = areaArray[i].toObject();

            if (jsonArea.contains(WimaArea::areaTypeName) && jsonArea[WimaArea::areaTypeName].isString()) {
                if ( jsonArea[WimaArea::areaTypeName] == WimaArea::wimaAreaName ) {
                    WimaArea* area = new WimaArea(this);
                    bool success = area->loadFromJson(jsonArea, errorString);

                    if ( !success ) {
                        qgcApp()->showMessage(errorMessage.arg(errorString));
                        return false;
                    }

                    _visualItems->append(area);
                    emit visualItemsChanged();
                } else if ( jsonArea[WimaArea::areaTypeName] == WimaGOperationArea::wimaGOperationAreaName) {
                    WimaGOperationArea* opArea = new WimaGOperationArea(this);
                    bool success = opArea->loadFromJson(jsonArea, errorString);

                    if ( !success ) {
                        qgcApp()->showMessage(errorMessage.arg(errorString));
                        return false;
                    }

                    _visualItems->append(opArea);
                    emit visualItemsChanged();
                } else if ( jsonArea[WimaArea::areaTypeName] == WimaServiceArea::wimaServiceAreaName) {
                    WimaServiceArea* serArea = new WimaServiceArea(this);
                    bool success = serArea->loadFromJson(jsonArea, errorString);

                    if ( !success ) {
                        qgcApp()->showMessage(errorMessage.arg(errorString));
                        return false;
                    }

                    _visualItems->append(serArea);
                    emit visualItemsChanged();
                } else if ( jsonArea[WimaArea::areaTypeName] == WimaVCorridor::wimaVCorridorName) {
                    WimaVCorridor* corridor = new WimaVCorridor(this);
                    bool success = corridor->loadFromJson(jsonArea, errorString);

                    if ( !success ) {
                        qgcApp()->showMessage(errorMessage.arg(errorString));
                        return false;
                    }

                    _visualItems->append(corridor);
                    emit visualItemsChanged();
                } else {
                    errorString += QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName));
                    qgcApp()->showMessage(errorMessage.arg(errorString));
                    return false;
                }
            } else {
                errorString += QString(tr("Invalid or non existing entry for %s.\n").arg(WimaArea::areaTypeName));
                return false;
            }
        }

        _currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(), fileInfo.completeBaseName().toLocal8Bit().data(), wimaFileExtension);

        emit currentFileChanged();

        return true;

    } else {
        errorString += QString(tr("File extension not supported.\n"));
        qgcApp()->showMessage(errorMessage.arg(errorString));
        return false;
    }
}

void WimaController::recalcVehicleCorridor()
{

}

void WimaController::recalcVehicleMeasurementAreas()
{

}

void WimaController::recalcAll()
{

}

void WimaController::recalcPolygonInteractivity(int index)
{
    if (index >= 0 && index < _visualItems->count()) {
        resetAllInteractive();
        WimaArea* interactivePoly = qobject_cast<WimaArea*>(_visualItems->get(index));
        interactivePoly->setInteractive(true);
    }
}

void WimaController::resetAllInteractive()
{
    int itemCount = _visualItems->count();
    if (itemCount > 0){
        for (int i = 0; i < itemCount; i++) {
            WimaArea* iteratorPoly = qobject_cast<WimaArea*>(_visualItems->get(i));
            iteratorPoly->setInteractive(false);
        }
    }
}

void WimaController::setInteractive()
{
    recalcPolygonInteractivity(_currentPolygonIndex);
}

QJsonDocument WimaController::saveToJson()
{
    QJsonArray jsonArray;

    for (int i = 0; i < _visualItems->count(); i++) {
        QJsonObject json;

        WimaArea* area = qobject_cast<WimaArea*>(_visualItems->get(i));

        if (area == nullptr) {
            qWarning("WimaController::saveToJson(): Internal error, area == nullptr!");
            return QJsonDocument();
        }

        WimaGOperationArea* opArea =  qobject_cast<WimaGOperationArea*>(area);
        if (opArea != nullptr) {
            opArea->saveToJson(json);
            jsonArray.append(json);
            continue;
        }

        WimaServiceArea* serArea =  qobject_cast<WimaServiceArea*>(area);
        if (serArea != nullptr) {
            serArea->saveToJson(json);
            jsonArray.append(json);
            continue;
        }

        WimaVCorridor* corridor =  qobject_cast<WimaVCorridor*>(area);
        if (corridor != nullptr) {
            corridor->saveToJson(json);
            jsonArray.append(json);
            continue;
        }

        // if non of the obove branches was trigger, type must be WimaArea
        area->saveToJson(json);
        jsonArray.append(json);
    }

    QJsonObject json;
    json["AreaItems"] = jsonArray;


    return QJsonDocument(json);
}