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

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

    // 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 = new WimaArea(this);
    WimaArea::join(corridor, serArea, joinedArea);
    joinedArea->join(opArea);

    _visualItems->append(joinedArea);
    
    // 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);
}