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)
  ,_flyView            (true)
  ,_currentPolygonIndex (-1)
  ,_container(nullptr)
{
    connect(this, &WimaController::currentPolygonIndexChanged, this, &WimaController::recalcPolygonInteractivity);
}

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

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

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

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

        emit currentPolygonIndexChanged(index);
    }
}

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

        emit dataContainerChanged();
    }
}

void WimaController::startWimaController(bool flyView)
{
    _flyView = flyView;
}

void WimaController::removeArea(int index)
{
    if(_container != nullptr && index >= 0 && index < _container->visualItems()->count()){
        WimaArea* area = qobject_cast<WimaArea*>(_container->visualItems()->removeAt(index));

        if ( area == nullptr) {
            qWarning("WimaController::removeArea(): nullptr catched, internal error.");
            return;
        }

        emit visualItemsChanged();

        if (_container->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 >= _container->visualItems()->count()){
            setCurrentPolygonIndex(_container->visualItems()->count() - 1);
        }else{
            recalcPolygonInteractivity(_currentPolygonIndex);
        }
    }else{
        qWarning("Index out of bounds!");
    }

}

bool WimaController::addGOperationArea()
{
    if (_container == nullptr) {
        qWarning("WimaController::addGOperationArea(): container not initialized!");
        return false;
    }

    // check if opArea exists already
    WimaGOperationArea* opArea = nullptr;
    for (int i = 0; i < _container->visualItems()->count(); i++) {
        WimaGOperationArea* currentArea = qobject_cast<WimaGOperationArea*>(_container->visualItems()->get(i));
        if ( currentArea != nullptr ) {
            opArea = currentArea;
            return false;
        }
    }

    // create one if no opArea available
    opArea = new WimaGOperationArea(this);
    _container->visualItems()->append(opArea);
    int newIndex = _container->visualItems()->count()-1;
    setCurrentPolygonIndex(newIndex);

    emit visualItemsChanged();
    return true;
}

bool WimaController::addServiceArea()
{
    if (_container == nullptr) {
        qWarning("WimaController::addGOperationArea(): container not initialized!");
        return false;
    }

    // check if serArea exists already
    WimaServiceArea* serArea = nullptr;
    for (int i = 0; i < _container->visualItems()->count(); i++) {
        WimaServiceArea* currentArea = qobject_cast<WimaServiceArea*>(_container->visualItems()->get(i));
        if ( currentArea != nullptr ) {
            serArea = currentArea;
            return false;
        }
    }

    // create one if no serArea available
    serArea = new WimaServiceArea(this);

    _container->visualItems()->append(serArea);
    int newIndex = _container->visualItems()->count()-1;
    setCurrentPolygonIndex(newIndex);

    emit visualItemsChanged();
    return true;
}

bool WimaController::addVehicleCorridor()
{
    if (_container == nullptr) {
        qWarning("WimaController::addGOperationArea(): container not initialized!");
        return false;
    }

    // check if corridor exists already
    WimaVCorridor* corridor = nullptr;
    for (int i = 0; i < _container->visualItems()->count(); i++) {
        WimaVCorridor* currentArea = qobject_cast<WimaVCorridor*>(_container->visualItems()->get(i));
        if ( currentArea != nullptr ) {
            corridor = currentArea;
            return false;
        }
    }

    // create one if no corridor available
    corridor = new WimaVCorridor(this);

    _container->visualItems()->append(corridor);
    int newIndex = _container->visualItems()->count()-1;
    setCurrentPolygonIndex(newIndex);

    emit visualItemsChanged();
    return true;
}

void WimaController::removeAll()
{
    if (_container == nullptr) {
        qWarning("WimaController::addGOperationArea(): container not initialized!");
        return;
    }

    bool changesApplied = false;
    while (_container->visualItems()->count() > 0) {
        removeArea(0);
        changesApplied = true;
    }

    _missionController->removeAll();

    _currentFile = "";

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

void WimaController::startMission()
{

}

void WimaController::abortMission()
{

}

void WimaController::pauseMission()
{

}

void WimaController::resumeMission()
{

}

bool WimaController::updateMission()
{
    if (_container == nullptr) {
        qWarning("WimaController::addGOperationArea(): container not initialized!");
        return false;
    }

    #define debug 0
    // pick first WimaGOperationArea
    WimaGOperationArea* opArea = nullptr;
    for (int i = 0; i < _container->visualItems()->count(); i++) {
        WimaGOperationArea* currentArea = qobject_cast<WimaGOperationArea*>(_container->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 < _container->visualItems()->count(); i++) {
        WimaServiceArea* currentArea = qobject_cast<WimaServiceArea*>(_container->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 < _container->visualItems()->count(); i++) {
        WimaVCorridor* currentArea = qobject_cast<WimaVCorridor*>(_container->visualItems()->get(i));
        if (currentArea != nullptr){
            corridor = currentArea;
            break;
        }
    }


    updateJoinedArea();

    #if debug
        _container->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("WimaController::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("WimaController::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;
    WimaArea::dijkstraPath(start, end, *_container->joinedArea(), path);
    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();
    WimaArea::dijkstraPath(start, end, *_container->joinedArea(), path);
    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("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);
        }
    }

    return true;
}

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

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 {
        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 WimaController::loadFromCurrent()
{
    return loadFromFile(_currentFile);
}

bool WimaController::loadFromFile(const QString &filename)
{
    if (_container == nullptr) {
        qWarning("WimaController::addGOperationArea(): container not initialized!");
        return false;
    }

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

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

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

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

                    _container->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();
        updateJoinedArea();

        // 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("WimaController::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("WimaController::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 WimaController::recalcVehicleCorridor()
{

}

void WimaController::recalcVehicleMeasurementAreas()
{

}

void WimaController::recalcAll()
{

}

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

void WimaController::updateJoinedArea()
{
    if (_container == nullptr) {
        qWarning("WimaController::addGOperationArea(): container not initialized!");
        return;
    }

    // pick first WimaGOperationArea
    WimaGOperationArea* opArea = nullptr;
    for (int i = 0; i < _container->visualItems()->count(); i++) {
        WimaGOperationArea* currentArea = qobject_cast<WimaGOperationArea*>(_container->visualItems()->get(i));
        if (currentArea != nullptr){
            opArea = currentArea;
            break;
        }
    }
    if (opArea == nullptr)
        return;

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

    // pick first WimaVCorridor
    WimaVCorridor* corridor = nullptr;
    for (int i = 0; i < _container->visualItems()->count(); i++) {
        WimaVCorridor* currentArea = qobject_cast<WimaVCorridor*>(_container->visualItems()->get(i));
        if (currentArea != nullptr){
            corridor = currentArea;
            break;
        }
    }

    // join service area, op area and corridor
    if (corridor != nullptr) {
        WimaArea::join(*serArea, *corridor, *_container->joinedArea());
        _container->joinedArea()->join(*opArea);
    } else {
        WimaArea::join(*serArea, *opArea, *_container->joinedArea());
    }

    emit joinedAreaChanged() ;
}

void WimaController::resetAllInteractive()
{
    if (_container == nullptr) {
        qWarning("WimaController::addGOperationArea(): container not initialized!");
        return ;
    }

    int itemCount = _container->visualItems()->count();
    if (itemCount > 0){
        for (int i = 0; i < itemCount; i++) {
            WimaArea* iteratorPoly = qobject_cast<WimaArea*>(_container->visualItems()->get(i));
            iteratorPoly->setInteractive(false);
        }
    }
}

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

QJsonDocument WimaController::saveToJson(FileType fileType)
{
    if (_container == nullptr) {
        qWarning("WimaController::addGOperationArea(): container not initialized!");
        return QJsonDocument();
    }

    QJsonObject json;

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

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

            WimaArea* area = qobject_cast<WimaArea*>(_container->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);
        }

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

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

    return QJsonDocument(json);
}