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

#include "MissionSettingsItem.h"
#include "SimpleMissionItem.h"


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

}

void WaypointManager::RTLManager::setWaypoints(const QVector<QGeoCoordinate> &waypoints)
{
    (void)waypoints;
    return;
}

void WaypointManager::RTLManager::push_back(const QGeoCoordinate &wp)
{
    (void)wp;
    return;
}

void WaypointManager::RTLManager::push_front(const QGeoCoordinate &wp)
{
    (void)wp;
    return;
}

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

void WaypointManager::RTLManager::insert(std::size_t i, const QGeoCoordinate &wp)
{
    (void)i;
    (void)wp;
    return;
}

std::size_t WaypointManager::RTLManager::size() const
{
    return 0;
}

void WaypointManager::RTLManager::remove(std::size_t i)
{
    (void)i;
}

bool WaypointManager::RTLManager::update()
{
    this->clear();
    return _worker();
}

bool WaypointManager::RTLManager::next()
{
    return true;
}

bool WaypointManager::RTLManager::previous()
{
    return true;
}

bool WaypointManager::RTLManager::reset()
{
    return true;
}

QString WaypointManager::RTLManager::checkPrecondition()
{
    Vehicle *managerVehicle = _settings->masterController()->managerVehicle();
    if (!managerVehicle->flying()) {
        return QString("Vehicle is not flying. Smart RTL not available.");
    }

    if (!managerVehicle->isOfflineEditingVehicle()) {
        return QString("Not connected to any vehicle. Smart RTL not available.");
    }

    if (!_areaInterface->joinedArea()->containsCoordinate(managerVehicle->coordinate())) {
        return QString("Vehicle not inside save area. Smart RTL not available.");
    }

    return QString();
}

bool WaypointManager::RTLManager::_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(),
                            &list /*parent*/,
                            doUpdate /*do update*/) )
    {
        qWarning("WaypointManager::RTLManager::next(): insertMissionItem failed.");
        Q_ASSERT(false);
        return false;
    }
    return true;
}

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

bool WaypointManager::RTLManager::_calcShortestPath(
        const QGeoCoordinate &start,
        const QGeoCoordinate &destination,
        QVector<QGeoCoordinate> &path)
{
        using namespace GeoUtilities;
        using namespace PolygonCalculus;
        QPolygonF joinedArea2D;
        toCartesianList(_areaInterface->joinedArea()->coordinateList(), /*origin*/ start, joinedArea2D);
        QPointF start2D(0,0);
        QPointF end2D;
        toCartesian(destination, start, end2D);
        QVector<QPointF> path2DOut;

        bool retVal = PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut);
        toGeoList(path2DOut, /*origin*/ start, path);

        return  retVal;
}

bool WaypointManager::RTLManager::_worker()
{
    // Precondition: settings must be valid.

    using namespace WaypointManager::Utils;

    if (!_settings->valid()) {
        return false;
    }

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

    // Calculate path from vehicle to home position.
    QVector<QGeoCoordinate> returnPath;
    auto vehicleCoordinate = _settings->masterController()->managerVehicle()->coordinate();
    if ( !_calcShortestPath(vehicleCoordinate, _settings->homePosition(), returnPath) ) {
        qWarning("WaypointManager::RTLManager::next(): Not able to calc path from vehicle to home position.");
        return false;
    }

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

    // Create change speed item.
    _insertMissionItem(_settings->homePosition(),
                       _currentMissionItems.count() /*insertion index*/,
                       false /*do update*/);
    SimpleMissionItem *speedItem = _currentMissionItems.value<SimpleMissionItem*>(2);
    if (speedItem == nullptr) {
        qWarning("WaypointManager::RTLManager::next(): nullptr.");
        Q_ASSERT(speedItem != nullptr);
        return false;
    }
    makeSpeedCmd(speedItem, _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.
    int 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::RTLManager::next(): nullptr.");
        return false;
    }
    makeLandCmd(landItem, _settings->masterController()->managerVehicle());


    // Set altitude.
    for (int i = 1; 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());
    }

    // Update list _currentMissionItems.
    updateHirarchy(_currentMissionItems);
    updateSequenceNumbers(_currentMissionItems);


    // Append return path to _currentWaypoints.
    for ( auto c : returnPath )
        _currentWaypoints.push_back(c);

    // Create variant list.
    _currentWaypointsVariant.clear();
    for ( auto c : _currentWaypoints)
        _currentWaypointsVariant.push_back(QVariant::fromValue(c));

    return true;
}