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

#include <iostream>

#include <QGeoCoordinate>


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

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

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

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

            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 ) {
            updateSequenceNumbers(list);
            updateHirarchy(list);
            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::updateSequenceNumbers(QmlObjectListModel &list, size_t firstSeqNumber)
{
    using namespace WaypointManager::Utils;

    if ( list.count() < 1)
        return false;

    // Setup ascending sequence numbers for all visual items.
    VisualMissionItem* startItem = list.value<VisualMissionItem*>(0);
    assert(startItem != nullptr); // list empty?
    startItem->setSequenceNumber(firstSeqNumber);
    int sequenceNumber = startItem->lastSequenceNumber() + 1;
    for (int i=1; i < list.count(); ++i) {
        VisualMissionItem* item = list.value<VisualMissionItem*>(i);
        item->setSequenceNumber(sequenceNumber);
        sequenceNumber = item->lastSequenceNumber() + 1;
    }
    return true;
}

bool WaypointManager::Utils::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::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(3, 0);
            c.setAltitude(0);
            settingsItem->setCoordinate(c);
            return true;
        }
    }

    return false;
}

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

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

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

    return ret;
}

void WaypointManager::Utils::printList(QmlObjectListModel &list)
{
    for (int i = 0; i < list.count(); ++i) {
        auto item = list.value<VisualMissionItem*>(i);
        qWarning() << "list index: " << i << "\n"
                   << "coordinate: " << item->coordinate() << "\n"
                   << "seq: " << item->sequenceNumber() << "\n"
                   << "last seq: " << item->lastSequenceNumber() << "\n"
                   << "is simple item: " << item->isSimpleItem() << "\n\n";
    }
}

void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed)
{
    assert(speed > 0);
    auto c = item->coordinate();
    item->setCommand(MAV_CMD_DO_CHANGE_SPEED);
    // Set coordinate must be after setCommand (setCommand sets coordinate to zero).
    item->setCoordinate(c);
    item->missionItem().setParam2(speed);
}