WimaPlaner.cc

#include "WimaPlaner.h"

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

WimaPlaner::WimaPlaner(QObject *parent)
    : QObject               (parent)
    , _readyForSaveSend     (false)
    , _currentAreaIndex     (-1)
    , _container            (nullptr)
    , _joinedArea           (this)
    , _opArea               (this)
    , _serArea              (this)
    , _corridor             (this)
{
    connect(this, &WimaPlaner::currentPolygonIndexChanged, this, &WimaPlaner::recalcPolygonInteractivity);
}

QmlObjectListModel* WimaPlaner::visualItems()
{
    return &_visualItems;
}

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

    filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
               tr("All Files (*.*)");
    return filters;
}

QStringList WimaPlaner::saveNameFilters() const
{
    QStringList filters;

    filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
    return filters;
}

QGeoCoordinate WimaPlaner::joinedAreaCenter() const
{
    return _joinedArea.center();
}

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

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

void WimaPlaner::setCurrentPolygonIndex(int index)
{
    if(index >= 0 && index < _visualItems.count() && index != _currentAreaIndex){
        _currentAreaIndex = index;

        emit currentPolygonIndexChanged(index);
    }
}

void WimaPlaner::setDataContainer(WimaDataContainer *container)
{
    if (_container == nullptr && container != nullptr) {
        _container = container;

        emit dataContainerChanged();
    }
}

void WimaPlaner::startWimaPlaner(bool flyView)
{

}

void WimaPlaner::removeArea(int index)
{
    if(index >= 0 && index < _visualItems.count()){
        WimaArea* area = qobject_cast<WimaArea*>(_visualItems.removeAt(index));

        if ( area == nullptr) {
            qWarning("WimaPlaner::removeArea(): nullptr catched, internal error.");
            return;
        }
        area->clear();

        emit visualItemsChanged();

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

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

}

bool WimaPlaner::addGOperationArea()
{
    if (!_visualItems.contains(&_opArea)) {
        _visualItems.append(&_opArea);

        int newIndex = _visualItems.count()-1;
        setCurrentPolygonIndex(newIndex);

        emit visualItemsChanged();
        return true;
    } else {
        return false;
    }
}

bool WimaPlaner::addServiceArea()
{
    if (!_visualItems.contains(&_serArea)) {
        _visualItems.append(&_serArea);

        int newIndex = _visualItems.count()-1;
        setCurrentPolygonIndex(newIndex);

        emit visualItemsChanged();
        return true;
    } else {
        return false;
    }
}

bool WimaPlaner::addVehicleCorridor()
{
    if (!_visualItems.contains(&_corridor)) {
        _visualItems.append(&_corridor);

        int newIndex = _visualItems.count()-1;
        setCurrentPolygonIndex(newIndex);

        emit visualItemsChanged();
        return true;
    } else {
        return false;
    }
}

void WimaPlaner::removeAll()
{
    bool changesApplied = false;
    while (_visualItems.count() > 0) {
        removeArea(0);
        changesApplied = true;
    }

    _missionController->removeAll();

    _currentFile = "";

    emit currentFileChanged();
    if ( changesApplied )
         emit visualItemsChanged();
}

void WimaPlaner::startMission()
{

}

void WimaPlaner::abortMission()
{

}

void WimaPlaner::pauseMission()
{

}

void WimaPlaner::resumeMission()
{

}

bool WimaPlaner::updateMission()
{

    setReadyForSaveSend(false);
    #define debug 0

    if ( !recalcJoinedArea()) {
        qgcApp()->showMessage(tr("Not able to join areas. Areas must be overlapping"));
        return false;
    }

    #if debug
        _visualItems.append(&_joinedArea);
    #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("WimaPlaner::updateMission(): settingsItem == nullptr");
        return false;
    }

    // set altitudes, temporary measure to solve bugs
    QGeoCoordinate center = _serArea.center();
    center.setAltitude(0);
    _serArea.setCenter(center);
    center = _opArea.center();
    center.setAltitude(0);
    _opArea.setCenter(center);
    center = _corridor.center();
    center.setAltitude(0);
    _corridor.setCenter(center);


    // set HomePos. to serArea center
    settingsItem->setCoordinate(_serArea.center());

    // create take off position item
    int sequenceNumber = _missionController->insertSimpleMissionItem(_serArea.center(), missionItems->count());
    _missionController->setCurrentPlanViewIndex(sequenceNumber, true);


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

    // calculate path from take off to opArea
    QGeoCoordinate start = _serArea.center();
    QGeoCoordinate end = survey->visualTransectPoints().first().value<QGeoCoordinate>();
    QList<QGeoCoordinate> path;
    if ( !WimaArea::dijkstraPath(start, end, _joinedArea, path)) {
        qgcApp()->showMessage(tr("Not able to calculate the path from takeoff position to measurement area."));
        return false;
    }
    for (int i = 1; i < path.count()-1; i++) {
        sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()-1);
        _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
    }

    // calculate return path
    start   = survey->visualTransectPoints().last().value<QGeoCoordinate>();
    end     = _serArea.center();
    path.clear();
    if ( ! WimaArea::dijkstraPath(start, end, _joinedArea, path)) {
        qgcApp()->showMessage(tr("Not able to calculate the path from measurement area to landing position."));
        return false;
    }
    for (int i = 1; i < path.count()-1; i++) {
        sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count());
        _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
    }

    // create land position item
    sequenceNumber = _missionController->insertSimpleMissionItem(_serArea.center(), missionItems->count());
    _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
    SimpleMissionItem* landItem = qobject_cast<SimpleMissionItem*>(missionItems->get(missionItems->count()-1));
    if (landItem == nullptr){
        qWarning("WimaPlaner::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);
        }
    }

    pushToContainer();
    setReadyForSaveSend(true);
    return true;
}

void WimaPlaner::saveToCurrent()
{
    saveToFile(_currentFile);
}

void WimaPlaner::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 {
        FileType fileType = FileType::WimaFile;
        if ( planFilename.contains(QString(".%1").arg(wimaFileExtension)) ) {
            fileType = FileType::WimaFile;
        } else if ( planFilename.contains(QString(".%1").arg(AppSettings::planFileExtension)) ) {
            fileType = FileType::PlanFile;
        } else {
            if ( planFilename.contains(".") ) {
                qgcApp()->showMessage(tr("File format not supported"));
            } else {
                qgcApp()->showMessage(tr("File without file extension not accepted."));
                return;
            }
        }

        QJsonDocument saveDoc = saveToJson(fileType);
        file.write(saveDoc.toJson());
        if(_currentFile != planFilename) {
            _currentFile = planFilename;
            emit currentFileChanged();
        }
    }
}

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

bool WimaPlaner::loadFromFile(const QString &filename)
{
    #define debug 0
    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();
        // AreaItems
        QJsonArray areaArray = json[areaItemsName].toArray();
        _visualItems.clear();

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

            if (jsonArea.contains(WimaArea::areaTypeName) && jsonArea[WimaArea::areaTypeName].isString()) {
                if ( jsonArea[WimaArea::areaTypeName] == WimaMeasurementArea::WimaMeasurementAreaName) {
                    print(_opArea);
                    bool success = _opArea.loadFromJson(jsonArea, errorString);
                    print(_opArea);

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

                    validAreaCounter++;
                    _visualItems.append(&_opArea);
                    emit visualItemsChanged();
                } else if ( jsonArea[WimaArea::areaTypeName] == WimaServiceArea::wimaServiceAreaName) {
                    bool success = _serArea.loadFromJson(jsonArea, errorString);

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

                    validAreaCounter++;
                    _visualItems.append(&_serArea);
                    emit visualItemsChanged();
                } else if ( jsonArea[WimaArea::areaTypeName] == WimaCorridor::WimaCorridorName) {
                    bool success = _corridor.loadFromJson(jsonArea, errorString);

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

                    validAreaCounter++;
                    _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();
        recalcJoinedArea();

        // MissionItems
        // extrac MissionItems part
        QJsonDocument missionJsonDoc = QJsonDocument(json[missionItemsName].toObject());
        // create temporary file with missionItems
        QFile temporaryFile;
        QString cropedFileName = filename.section("/",0,-2);
        #if debug
            qWarning() << cropedFileName;
        #endif
        QString temporaryFileName;
        for (int i = 0; ; i++) {
            temporaryFileName = cropedFileName.append("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension);

            if ( !QFile::exists(temporaryFileName) ) {
                temporaryFile.setFileName(temporaryFileName);
                if ( temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text) ) {
                    break;
                }
            }

            if ( i > 1000) {
                qWarning("WimaPlaner::loadFromFile(): not able to create temporary file.");
                return false;
            }
        }

        temporaryFile.write(missionJsonDoc.toJson());

        // load from temporary file
        _masterController->loadFromFile(temporaryFileName);

        // remove temporary file
        if ( !temporaryFile.remove() ){
            qWarning("WimaPlaner::loadFromFile(): not able to remove temporary file.");
        }

        return true;

    } else if ( fileInfo.suffix() == AppSettings::planFileExtension ){
        _masterController->loadFromFile(filename);

        return true;// might be wrong return value
    } else {
        errorString += QString(tr("File extension not supported.\n"));
        qgcApp()->showMessage(errorMessage.arg(errorString));

        return false;
    }
}

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

bool WimaPlaner::recalcJoinedArea()
{
    // join service area, op area and corridor
    _joinedArea = _serArea;
    _joinedArea.join(_corridor);

    if ( !_joinedArea.join(_opArea) )
        return false; // this happens if all areas are pairwise disjoint
    else {
        return true;
    }


}

void WimaPlaner::pushToContainer()
{
    // Sets the pointers (inside _container) to the areas (of this).
    // Should be called only (once) after a _container has been assigned.
    if (_container != nullptr) {
        _container->setOpArea(&_opArea);
        _container->setSerArea(&_serArea);
        _container->setCorridor(&_corridor);
        _container->setJoinedArea(&_joinedArea);
        _container->setVisualItems(&_visualItems);
    } else {
        qWarning("WimaPlaner::uploadToContainer(): no container assigned.");
    }
}

void WimaPlaner::resetAllInteractive()
{
    // Marks all areas as inactive (area.interactive == false)
    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 WimaPlaner::setInteractive()
{
    recalcPolygonInteractivity(_currentAreaIndex);
}

QJsonDocument WimaPlaner::saveToJson(FileType fileType)
{
    /// This function save all areas (of WimaPlaner) and all mission items (of MissionController) to a QJsonDocument
    /// @param fileType is either WimaFile or PlanFile (enum), if fileType == PlanFile only mission items are stored
    QJsonObject json;

    if ( fileType == FileType::WimaFile ) {
        QJsonArray jsonArray;

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

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

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

            // check the type of area, create and append the JsonObject to the JsonArray once determined
            WimaMeasurementArea* opArea =  qobject_cast<WimaMeasurementArea*>(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;
            }

            WimaCorridor* corridor =  qobject_cast<WimaCorridor*>(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);
        }

        json[areaItemsName] = jsonArray;
        json[missionItemsName] = _masterController->saveToJson().object();

        return QJsonDocument(json);
    } else if (fileType == FileType::PlanFile) {
        return _masterController->saveToJson();
    }

    return QJsonDocument(json);
}

void WimaPlaner::setReadyForSaveSend(bool ready)
{
    if (ready != _readyForSaveSend) {
        _readyForSaveSend = ready;
        emit readyForSaveSendChanged(ready);
    }
}