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

#include "MissionSettingsItem.h"

WaypointManager::DefaultManager::DefaultManager(Settings &settings,
                                                AreaInterface &interface)
    : ManagerBase(settings)
    , _areaInterface(&interface)
{

}

void WaypointManager::DefaultManager::clear()
{
    _dirty = true;
    _waypoints.clear();
    _currentWaypoints.clear();
    _missionItems.clearAndDeleteContents();
    _currentMissionItems.clearAndDeleteContents();
    _waypointsVariant.clear();
    _currentWaypointsVariant.clear();
}

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

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

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

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

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

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

bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c,
                                                         size_t index,
                                                         bool doUpdate)
{
    return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
}

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 || !_settings->valid()) {
        return false;
    }

    if (_dirty) {
        _missionItems.clearAndDeleteContents();
        _waypointsVariant.clear();
        initialize(_missionItems, _settings->vehicle(), _settings->isFlyView());
        for (size_t i = 0; i < size_t(_waypoints.size()); ++i) {
            const QGeoCoordinate &c = _waypoints.at(i);
            _insertMissionItem(c, _missionItems.count(), _missionItems, false /*update list*/);
            _waypointsVariant.push_back(QVariant::fromValue(c));
        }
        Q_ASSERT(_missionItems.value<MissionSettingsItem*>(0) != nullptr);
        doUpdate(_missionItems);

        _dirty = false;
    }

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

    // calculate path from home to first waypoint
    QVector<QGeoCoordinate> arrivalPath;
    if (!_settings->homePosition().isValid()){
        Q_ASSERT(false);
        qWarning("WaypointManager::DefaultManager::next(): home position invalid.");
        return false;
    }
    if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), arrivalPath) ) {
        Q_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
    qDebug() << "_currentWaypoints.size(): " << _currentWaypoints.size();
    qDebug() << "_currentWaypoints.first(): " << _currentWaypoints.first();
    qDebug() << "_currentWaypoints.last(): " << _currentWaypoints.last();
    qDebug() << "_settings->homePosition(): " << _settings->homePosition();
    QVector<QGeoCoordinate> returnPath;
    //if ( !_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(), returnPath) ) {
    if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), returnPath) ) {
        Q_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 = _currentMissionItems.value<MissionSettingsItem *>(0);
    if (settingsItem == nullptr) {
        Q_ASSERT(false);
        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
        return false;
    }
    settingsItem->setCoordinate(_settings->homePosition());

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

    // Create change speed item.
    _insertMissionItem(_settings->homePosition(), 2 /*insertion index*/, false /*do update*/);
    SimpleMissionItem *speedItem = _currentMissionItems.value<SimpleMissionItem*>(2);
    if (speedItem == nullptr) {
        Q_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(coordinate,
                           _currentMissionItems.count() /*insertion index*/,
                           false /*do update*/);
    }

    // Create change speed item (after arrival path).
    int index = _currentMissionItems.count();
    _insertMissionItem(_currentWaypoints.first(), index /*insertion index*/, false /*do update*/);
    speedItem = _currentMissionItems.value<SimpleMissionItem*>(index);
    if (speedItem == nullptr) {
        Q_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(_currentWaypoints.first());
    speedItem->missionItem().setParam2(_settings->flightSpeed());

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

    // Create change speed item, after slice.
    index = _currentMissionItems.count();
    _insertMissionItem(_currentWaypoints.last(), index /*insertion index*/, false /*do update*/);
    speedItem = _currentMissionItems.value<SimpleMissionItem*>(index);
    if (speedItem == nullptr) {
        Q_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(_currentWaypoints.last());
    speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed());

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

    // Set land command for last mission item.
    index = _currentMissionItems.count();
    _insertMissionItem(_settings->homePosition(), index /*insertion index*/, false /*do update*/);
    SimpleMissionItem *landItem = _currentMissionItems.value<SimpleMissionItem*>(index);
    if (landItem == nullptr) {
        Q_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 {
        Q_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 )
        _currentWaypoints.push_front(arrivalPath[i]);
    // Append return path to slice.
    for ( auto c : returnPath )
        _currentWaypoints.push_back(c);

    _currentWaypointsVariant.clear();
    for ( auto c : _currentWaypoints)
        _currentWaypointsVariant.push_back(QVariant::fromValue(c));


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

    return true;
}