WimaController.cc

#include "WimaController.h"

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

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

bool WimaController::addVehicleCorridor(WimaGOperationArea *opArea, WimaServiceArea *serviceArea)
{
     WimaVCorridor* corridor = nullptr;
    if (opArea != nullptr && serviceArea != nullptr){
        for (int i = 0; i < _visualItems->count(); i++) {
            corridor = qobject_cast<WimaVCorridor*>(_visualItems->get(i));
            if (corridor != nullptr){
                if (corridor->serviceArea() == serviceArea && corridor->opArea() == opArea){
                    break;
                }else {
                    corridor = nullptr;
                }
            }
        }

        bool newCorridorCreated = false;
        if(corridor == nullptr){
            corridor = new WimaVCorridor(opArea);
            newCorridorCreated = true;
        }else {
            corridor->clear();
        }

        QList<QGeoCoordinate> opAreaPolyline = opArea->polyline()->coordinateList();
        QList<QGeoCoordinate> serAreaPolyline = serviceArea->polyline()->coordinateList();
        if (opAreaPolyline.size() > 1 && serAreaPolyline.size() > 1){
            corridor->appendVertices(opAreaPolyline);
            corridor->appendVertices(serAreaPolyline);

            if (newCorridorCreated){
                corridor->setServiceArea(serviceArea);
                corridor->setOpArea(opArea);
                serviceArea->setVehicleCorridor(corridor);
                opArea->setVehicleCorridor(corridor);
                _visualItems->append(corridor);
                emit visualItemsChanged();
            }

            return true;
        }else {
            qWarning("WimaController::addVehicleCorridor(): OpArea or serviceArea polyline size <= 1!");
            if (newCorridorCreated){
                corridor->deleteLater();
            }
            return false;
        }
    }else {
        return false;
    }
}

void WimaController::startMission()
{

}

void WimaController::abortMission()
{

}

void WimaController::pauseMission()
{

}

void WimaController::resumeMission()
{

}

bool WimaController::updateMission()
{
    // 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;
        }
    }

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

    // calc. vehicle corridor
    if (opArea != nullptr && serArea != nullptr){
        if(!addVehicleCorridor(opArea, serArea)){
            return false;
        }
    }else{
        return false;
    }
    
    // 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());
    // set take off position item
    _missionController->insertSimpleMissionItem(serArea->center(), 1);
    // create survey item, will be extened with more mission types in the future
    _missionController->insertComplexMissionItem(_missionController->surveyComplexItemName(), opArea->center(), 2);
    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());
    }
    // create land position item
    _missionController->insertSimpleMissionItem(serArea->center(), 3);
    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::saveMission()
{

}

void WimaController::loadMission()
{

}

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