#include "Utils.h"
#include "MissionSettingsItem.h"


template<>
QVariant WaypointManager::Utils::getCoordinate(const SimpleMissionItem* item)
{
    return QVariant::fromValue(item->coordinate());
}

bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
                                               long index,
                                               QmlObjectListModel &list,
                                               Vehicle* vehicle,
                                               bool flyView,
                                               QObject *parent,
                                               bool doUpdates)
{
    using namespace WaypointManager::Utils;

    if ( !coordinate.isValid() ){
        return false;
    } else {
        if (parent == nullptr)
            parent = &list;
        SimpleMissionItem * newItem = new SimpleMissionItem(vehicle, flyView, parent);

        int sequenceNumber = detail::nextSequenceNumber(list);
        newItem->setSequenceNumber(sequenceNumber);
        newItem->setCoordinate(coordinate);

        // Set command (takeoff if first command).
        newItem->setCommand(MAV_CMD_NAV_WAYPOINT);
        if (list.count() == 1
                && ( vehicle->fixedWing()
                     || vehicle->vtol()
                     || vehicle->multiRotor() ) ) {
            MAV_CMD takeoffCmd = vehicle->vtol()
                    ? MAV_CMD_NAV_VTOL_TAKEOFF
                    : MAV_CMD_NAV_TAKEOFF;
            if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) {
                newItem->setCommand(takeoffCmd);
            }
        }

        // Set altitude and altitude mode from previous item.
        if (newItem->specifiesAltitude()) {
            double  altitude;
            int     altitudeMode;

            if (detail::previousAltitude(list, index-1, altitude, altitudeMode)) {
                newItem->altitude()->setRawValue(altitude);
                newItem->setAltitudeMode(
                            static_cast<QGroundControlQmlGlobal::AltitudeMode>(
                                altitudeMode) );
            }
        }

        list.insert(index, newItem);

        if ( doUpdates ) {
            detail::updateSequenceNumbers(list, index);
            detail::updateHirarchy(list);
            detail::updateHomePosition(list);
        }
    }
    return true;
}

size_t WaypointManager::Utils::detail::nextSequenceNumber(QmlObjectListModel &list)
{
    if (list.count() >= 1) {
        VisualMissionItem* lastItem = list.value<VisualMissionItem*>(list.count() - 1);
        return lastItem->lastSequenceNumber() + 1;
    }
    return 0;
}

bool WaypointManager::Utils::detail::previousAltitude(QmlObjectListModel &list,
                                                      size_t index,
                                                      double &altitude,
                                                      int &altitudeMode)
{
    if (index >= size_t(list.count())) {
        return false;
    }

    for (long i = index; i >= 0; --i) {
        VisualMissionItem* visualItem = list.value<VisualMissionItem*>(i);

        if ( visualItem->specifiesCoordinate()
                 && !visualItem->isStandaloneCoordinate() ) {
            if ( visualItem->isSimpleItem() ) {
                SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
                if (simpleItem->specifiesAltitude()) {
                    altitude = simpleItem->altitude()->rawValue().toDouble();
                    altitudeMode = simpleItem->altitudeMode();
                    return true;
                }
            }
        }
    }

    return false;
}

bool WaypointManager::Utils::detail::updateSequenceNumbers(QmlObjectListModel &list, size_t startIdx)
{
    using namespace WaypointManager::Utils;

    if ( list.count() < 1 || startIdx >= size_t(list.count()) )
        return false;

    // Setup ascending sequence numbers for all visual items.
    VisualMissionItem* startItem = list.value<VisualMissionItem*>(
                std::max(long(startIdx)-1,long(0)) );
    if (list.count() == 1){
        startItem->setSequenceNumber(0);
    } else {
        int sequenceNumber = startItem->lastSequenceNumber() + 1;
        for (int i=startIdx; i < list.count(); ++i) {
            VisualMissionItem* item = list.value<VisualMissionItem*>(i);
            item->setSequenceNumber(sequenceNumber);
            sequenceNumber = item->lastSequenceNumber() + 1;
        }
    }
    return true;
}

bool WaypointManager::Utils::detail::updateHirarchy(QmlObjectListModel &list)
{
    if ( list.count() < 1)
        return false;

    VisualMissionItem* currentParentItem = list.value<VisualMissionItem*>(0);

    currentParentItem->childItems()->clear();

    for (size_t i = 1; i < size_t( list.count() ); ++i) {
        VisualMissionItem* item = list.value<VisualMissionItem*>(i);

        if (item->specifiesCoordinate()) {
            item->childItems()->clear();
            currentParentItem = item;
        } else if (item->isSimpleItem()) {
            currentParentItem->childItems()->append(item);
        }
    }

    return true;
}

bool WaypointManager::Utils::detail::updateHomePosition(QmlObjectListModel &list)
{
    MissionSettingsItem *settingsItem = list.value<MissionSettingsItem *>(0);
    assert(settingsItem); // list not initialized?

    // Set the home position to be a delta from first coordinate.
    for (int i = 1; i < list.count(); ++i) {
        VisualMissionItem* item = list.value<VisualMissionItem*>(i);
        assert(item);

        if (item->specifiesCoordinate() && item->coordinate().isValid()) {
            QGeoCoordinate c = item->coordinate().atDistanceAndAzimuth(30, 0);
            c.setAltitude(0);
            settingsItem->setCoordinate(c);
            return true;
        }
    }

    return false;
}

void WaypointManager::Utils::initialize(QmlObjectListModel &list,
                                        Vehicle *vehicle,
                                        bool flyView)
{
    MissionSettingsItem* settingsItem = new MissionSettingsItem(vehicle, flyView, &list);
    list.insert(0, settingsItem);
}

bool WaypointManager::Utils::doUpdate(QmlObjectListModel &list)
{
    using namespace WaypointManager::Utils;

    bool ret = detail::updateSequenceNumbers(list, 0);
    ret = detail::updateHirarchy(list) == false ? false : ret;
    (void)detail::updateHomePosition(list);

    return ret;
}