WimaPlaner.cc

#include "WimaPlaner.h"

#include "CircularSurveyComplexItem.h"



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

WimaPlaner::WimaPlaner(QObject *parent)
    : QObject               (parent)
    , _missionReady         (false)
    , _currentAreaIndex     (-1)
    , _container            (nullptr)
    , _joinedArea           (this)
    , _measurementArea      (this)
    , _serviceArea          (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) {
        if (_container != nullptr) {
           disconnect(this, &WimaPlaner::missionReadyChanged, _container, &WimaDataContainer::setDataValid);
        }

        _container = container;
        connect(this, &WimaPlaner::missionReadyChanged, _container, &WimaDataContainer::setDataValid);

        emit dataContainerChanged();
    }
}

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();
        area->borderPolygon()->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::addMeasurementArea()
{
    if (!_visualItems.contains(&_measurementArea)) {
        _visualItems.append(&_measurementArea);

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

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

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

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

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

bool WimaPlaner::addCorridor()
{
    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()
{
#if TEST_FUN
   TestPlanimetryCalculus::test();
   TestPolygonCalculus::test();
#endif
    QString errorString;
    setMissionReady(false);
    #define debug 0

    if ( !recalcJoinedArea(errorString)) {
        qgcApp()->showMessage(tr(errorString.toLocal8Bit().data()));
        return false;
    }

    #if debug
        _visualItems.append(&_joinedArea);
    #endif

    // extract old survey data
    QmlObjectListModel* missionItems        = _missionController->visualItems();
    CircularSurveyComplexItem* OldSurveyPt  = nullptr;

    QGeoCoordinate oldSurveyRef;
    bool oldSurveyExists = false;

    for (int i = 0; i < _missionController->visualItems()->count(); i++) {
        OldSurveyPt = qobject_cast<CircularSurveyComplexItem*>(missionItems->get(i));
        if ( OldSurveyPt != nullptr) {
            oldSurveyRef        = OldSurveyPt->refPoint();
            oldSurveyExists     = true;
            break;
        }

    }

    // reset visual items
    _missionController->removeAll();
    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 = _serviceArea.center();
    center.setAltitude(0);
    _serviceArea.setCenter(center);
    center = _measurementArea.center();
    center.setAltitude(0);
    _measurementArea.setCenter(center);
    center = _corridor.center();
    center.setAltitude(0);
    _corridor.setCenter(center);
    // set HomePos. to serArea center
    settingsItem->setCoordinate(_serviceArea.center());

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

    _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
    SimpleMissionItem* takeOffItem = qobject_cast<SimpleMissionItem*>(missionItems->get(missionItems->count()-1));
    if (takeOffItem == nullptr){
        qWarning("WimaPlaner::updateMission(): takeOffItem == nullptr");
        return false;
    } else {
        takeOffItem->setCoordinate(_serviceArea.center()); //necessary, insertSimpleMissionItem does not copy coordinate (why?)
    }

    // create survey item, will be extened with more()-> mission types in the future
    _missionController->insertComplexMissionItem(_missionController->circularSurveyComplexItemName(), _measurementArea.center(), missionItems->count());
    CircularSurveyComplexItem* survey = qobject_cast<CircularSurveyComplexItem*>(missionItems->get(missionItems->count()-1));

    if (survey == nullptr){
        qWarning("WimaPlaner::updateMission(): survey == nullptr");
        return false;
    } else {
        if ( oldSurveyExists ) {
           survey->setRefPoint(oldSurveyRef);
        } else  {
            survey->setRefPoint(_measurementArea.center());
        }

        survey->setAutoGenerated(true); // prevents reinitialisation from gui
        survey->surveyAreaPolygon()->clear();
        survey->surveyAreaPolygon()->appendVertices(_measurementArea.coordinateList());
    }


    // calculate path from take off to opArea
    if (survey->visualTransectPoints().size() == 0) {
        qWarning("WimaPlaner::updateMission(): survey no points.");
        return false;
    }
    QGeoCoordinate start = _serviceArea.center();
    QGeoCoordinate end = survey->coordinate();

#ifdef QT_DEBUG
    if (!_visualItems.contains(&_joinedArea))
        _visualItems.append(&_joinedArea);
#endif

    QList<QGeoCoordinate> path;
    if ( !calcShortestPath(start, end, path)) {
        qgcApp()->showMessage( QString(tr("Not able to calculate the path from takeoff position to measurement area.")).toLocal8Bit().data());
        return false;
    }
//    path.clear();
//    path.append(end);
//    path.append(start);
//    path.append(end);
//    path.append(end);
    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->exitCoordinate();
    end     = _serviceArea.center();
    path.clear();
    if ( !calcShortestPath(start, end, path)) {
        qgcApp()->showMessage(QString(tr("Not able to calculate the path from measurement area to landing position.")).toLocal8Bit().data());
        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(_serviceArea.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);
        } else {
            return false;
        }
    }

    pushToContainer(); // exchange plan data with the WimaController via the _container
    setMissionReady(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(_measurementArea);
                    bool success = _measurementArea.loadFromJson(jsonArea, errorString);
                    print(_measurementArea);

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

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

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

                    validAreaCounter++;
                    _visualItems.append(&_serviceArea);
                    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

//        bool ret = json.contains(missionItemsName);
//        qWarning() << ret;

        QJsonObject missionObject = json[missionItemsName].toObject();

        //qWarning() << json[missionItemsName].type();

        QJsonDocument missionJsonDoc = QJsonDocument(missionObject);
        // 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 + QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension);
            // qWarning() << temporaryFileName;

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

        // qWarning() << missionJsonDoc.toVariant().toString();
        temporaryFile.write(missionJsonDoc.toJson());
        temporaryFile.close();

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

bool WimaPlaner::recalcJoinedArea(QString &errorString)
{
    // check if area paths form simple polygons
    if ( !_serviceArea.isSimplePolygon() ) {
        errorString.append(tr("Service area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
        return false;
    }

    if ( !_corridor.isSimplePolygon() && _corridor.count() > 0) {
        errorString.append(tr("Corridor is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
        return false;
    }

    if ( !_measurementArea.isSimplePolygon() ) {
        errorString.append(tr("Measurement area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
        return false;
    }

    _joinedArea.setPath(_serviceArea.path());
    _joinedArea.join(_corridor);
    if ( !_joinedArea.join(_measurementArea) ) {
        errorString.append(tr("Not able to join areas. Service area and measurement"
                           " must have a overlapping section, or be connected through a corridor."));
        return false; // this happens if all areas are pairwise disjoint
    }
    // join service area, op area and corridor
    return true;
}

/*!
 * \fn void WimaPlaner::pushToContainer()
 * Pushes the \c WimaPlanData object generated by \c toPlanData() to the \c WimaDataContainer.
 * Should be called only after \c updateMission() was successful.
 *
 * \sa WimaDataContainer, WimaPlanData
 */
void WimaPlaner::pushToContainer()
{
    if (_container != nullptr) {
        WimaPlanData planData = toPlanData();
        _container->push(planData);
    } else {
        qWarning("WimaPlaner::uploadToContainer(): no container assigned.");
    }
}

bool WimaPlaner::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;
}

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

void WimaPlaner::setInteractive()
{
    recalcPolygonInteractivity(_currentAreaIndex);
}

/*!
 * \fn WimaPlanData WimaPlaner::toPlanData()
 *
 * Returns a \c WimaPlanData object containing information about the current mission.
 * The \c WimaPlanData object holds only the data which is relevant for the \c WimaController class.
 * Should only be called if updateMission() was successful.
 *
 * \sa WimaController, WimaPlanData
 */
WimaPlanData WimaPlaner::toPlanData()
{
    WimaPlanData planData;

    // store areas
    planData.append(WimaMeasurementAreaData(_measurementArea));
    planData.append(WimaServiceAreaData(_serviceArea));
    planData.append(WimaCorridorData(_corridor));
    planData.append(WimaJoinedAreaData(_joinedArea));

    // convert mission items to mavlink commands
    QList<MissionItem*> rgMissionItems;
    MissionController::convertToMissionItems(_missionController->visualItems(), rgMissionItems, this);

    // remove warnings if you read this
//    qWarning("WimaPlaner::toPlanData(): rgMissionItems");
//    for (auto item : rgMissionItems) {
//        qWarning() << item->command();
//        qWarning() << item->param1();
//        qWarning() << item->param2();
//        qWarning() << item->param3();
//        qWarning() << item->param4();
//        qWarning() << item->param5();
//        qWarning() << item->param6();
//        qWarning() << item->param7();
//        qWarning(" ");
//    }

    QList<const MissionItem*> rgMissionItemsConst;
    for (auto missionItem : rgMissionItems)
        rgMissionItemsConst.append(missionItem);
    // store mavlink commands
    planData.append(rgMissionItemsConst);

    return planData;
}

void WimaPlaner::setMissionReady(bool ready)
{
    if(_missionReady != ready)
    {
        _missionReady = ready;

        emit missionReadyChanged(_missionReady);
    }
}

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