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

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

void WimaController::loadFromFile()
{

}

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


    return QJsonDocument(jsonArray);
}