#include "DefaultManager.h"
#include "Wima/Geometry/GeoUtilities.h"
#include "Wima/Geometry/PolygonCalculus.h"

#include "MissionSettingsItem.h"

bool WaypointManager::DefaultManager::update()
{
    // extract waypoints
    _slice.clear();
    Slicer::update(_waypoints, _slice);
    return _worker();
}

bool WaypointManager::DefaultManager::next()
{
    // extract waypoints
    _slice.clear();
    Slicer::next(_waypoints, _slice);
    return _worker();
}

bool WaypointManager::DefaultManager::previous()
{
    // extract waypoints
    _slice.clear();
    Slicer::previous(_waypoints, _slice);
    return _worker();
}

bool WaypointManager::DefaultManager::reset()
{
    // extract waypoints
    _slice.clear();
    Slicer::reset(_waypoints, _slice);
    return _worker();
}

bool WaypointManager::DefaultManager::_insertMissionItem(size_t index, const QGeoCoordinate &c, bool doUpdate)
{
    using namespace WaypointManager::Utils;

    if ( !insertMissionItem(c,
                            index /*insertion index*/,
                            _missionItems,
                            _settings->vehicle(),
                            _settings->isFlyView(),
                            &_missionItems /*parent*/,
                            doUpdate /*do update*/) )
    {
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): insertMissionItem failed.");
        return false;
    }
    return true;
}

bool WaypointManager::DefaultManager::_calcShortestPath(
        const QGeoCoordinate &start,
        const QGeoCoordinate &destination,
        QVector<QGeoCoordinate> &path)
{
        using namespace GeoUtilities;
        using namespace PolygonCalculus;
        QVector<QPointF> path2D;
        bool retVal = PolygonCalculus::shortestPath(
                                       toQPolygonF(toCartesian2D(_areaInterface->joinedArea()->coordinateList(), /*origin*/ start)),
                                       /*start point*/ QPointF(0,0),
                                       /*destination*/ toCartesian2D(destination, start),
                                       /*shortest path*/ path2D);
        path.append(toGeo(path2D, /*origin*/ start));

        return  retVal;
}

bool WaypointManager::DefaultManager::_worker()
{
    using namespace WaypointManager::Utils;

    if (_waypoints.count() < 1) {
        return false;
    }

    _missionItems.clearAndDeleteContents();
    initialize(_missionItems, _settings->vehicle(), _settings->isFlyView());

    // calculate path from home to first waypoint
    QVector<QGeoCoordinate> arrivalPath;
    if (!_settings->homePosition().isValid()){
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): home position invalid.");
        return false;
    }
    if ( !_calcShortestPath(_settings->homePosition(), _slice.first(), arrivalPath) ) {
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint.");
        return false;
    }
    arrivalPath.removeFirst();

    // calculate path from last waypoint to home
    QVector<QGeoCoordinate> returnPath;
    if ( !_calcShortestPath(_slice.last(), _settings->homePosition(), returnPath) ) {
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to last waypoint.");
        return false;
    }
    returnPath.removeFirst();
    returnPath.removeLast();



    // Create mission items.
    // Set home position of MissionSettingsItem.
    MissionSettingsItem* settingsItem = _missionItems.value<MissionSettingsItem *>(0);
    if (settingsItem == nullptr) {
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }
    settingsItem->setCoordinate(_settings->homePosition());

    // Set takeoff position for first mission item (bug).
    _insertMissionItem(1 /*insertion index*/, _settings->homePosition(), false /*do update*/);
    SimpleMissionItem *takeoffItem = _missionItems.value<SimpleMissionItem*>(1);
    if (takeoffItem == nullptr) {
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }
    takeoffItem->setCoordinate(_settings->homePosition());

    // Create change speed item.
    _insertMissionItem(2 /*insertion index*/, _settings->homePosition(), false /*do update*/);
    SimpleMissionItem *speedItem = _missionItems.value<SimpleMissionItem*>(2);
    if (speedItem == nullptr) {
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
    // Set coordinate must be after setCommand (setCommand sets coordinate to zero).
    speedItem->setCoordinate(_settings->homePosition());
    speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed());

    // insert arrival path
    for (auto coordinate : arrivalPath) {
        _insertMissionItem(_missionItems.count() /*insertion index*/,
                           coordinate,
                           false /*do update*/);
    }

    // Create change speed item (after arrival path).
    int index = _missionItems.count();
    _insertMissionItem(index /*insertion index*/, _slice.first(), false /*do update*/);
    speedItem = _missionItems.value<SimpleMissionItem*>(index);
    if (speedItem == nullptr) {
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
    speedItem->setCoordinate(_slice.first());
    speedItem->missionItem().setParam2(_settings->flightSpeed());

    // Insert slice.
    for (auto coordinate : _slice) {
        _insertMissionItem(_missionItems.count() /*insertion index*/,
                           coordinate,
                           false /*do update*/);
    }

    // Create change speed item, after slice.
    index = _missionItems.count();
    _insertMissionItem(index /*insertion index*/, _slice.last(), false /*do update*/);
    speedItem = _missionItems.value<SimpleMissionItem*>(index);
    if (speedItem == nullptr) {
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }
    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
    speedItem->setCoordinate(_slice.last());
    speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed());

    // Insert return path coordinates.
    for (auto coordinate : returnPath) {
        _insertMissionItem(_missionItems.count() /*insertion index*/,
                           coordinate,
                           false /*do update*/);
    }

    // Set land command for last mission item.
    index = _missionItems.count();
    _insertMissionItem(index /*insertion index*/, _settings->homePosition(), false /*do update*/);
    SimpleMissionItem *landItem = _missionItems.value<SimpleMissionItem*>(index);
    if (landItem == nullptr) {
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }

    MAV_CMD landCmd = _settings->vehicle()->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
    if (_settings->vehicle()->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
        landItem->setCommand(landCmd);
    } else {
        assert(false);
        qWarning("WaypointManager::DefaultManager::next(): Land command not supported!");
        return false;
    }

    // Prepend arrival path to slice.
    for ( long i = arrivalPath.size()-1; i >=0; --i )
        _slice.push_front(arrivalPath[i]);
    // Append return path to slice.
    for ( auto c : returnPath )
        _slice.push_back(c);


    // Set altitude.
    for (int i = 0; i < _missionItems.count(); ++i) {
        SimpleMissionItem *item = _missionItems.value<SimpleMissionItem *>(i);
        if (item == nullptr) {
            assert(false);
            qWarning("WimaController::updateAltitude(): nullptr");
            return false;
        }
        item->altitude()->setRawValue(_settings->altitude());
    }

    return true;
}