/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    QGROUNDCONTROL is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

#include "MissionController.h"
#include "ScreenToolsController.h"
#include "MultiVehicleManager.h"
#include "MissionManager.h"
#include "CoordinateVector.h"

MissionController::MissionController(QObject *parent)
    : QObject(parent)
    , _missionItems(NULL)
    , _activeVehicle(NULL)
{
    MultiVehicleManager* multiVehicleMgr = MultiVehicleManager::instance();
    
    connect(multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &MissionController::_activeVehicleChanged);
    
    Vehicle* activeVehicle = multiVehicleMgr->activeVehicle();
    if (activeVehicle) {
        _activeVehicleChanged(activeVehicle);
    } else {
        _newMissionItemsAvailable();
    }
}

MissionController::~MissionController()
{
}

void MissionController::_newMissionItemsAvailable(void)
{
    if (_missionItems) {
        _missionItems->deleteLater();
    }
    
    MissionManager* missionManager = NULL;
    Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle();
    if (activeVehicle) {
        missionManager = activeVehicle->missionManager();
    }

    if (!missionManager || missionManager->inProgress()) {
        _missionItems = new QmlObjectListModel(this);
    } else {
        _missionItems = missionManager->copyMissionItems();
    }

    _initAllMissionItems();
}

void MissionController::_recalcWaypointLines(void)
{
    bool firstCoordinateItem = true;
    MissionItem* lastCoordinateItem = qobject_cast<MissionItem*>(_missionItems->get(0));
    
    _waypointLines.clear();
    
    for (int i=1; i<_missionItems->count(); i++) {
        MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
        
        if (item->specifiesCoordinate()) {
            if (firstCoordinateItem) {
                if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
                    // The first coordinate we hit is a takeoff command so link back to home position
                    _waypointLines.append(new CoordinateVector(qobject_cast<MissionItem*>(_missionItems->get(0))->coordinate(), item->coordinate()));
                } else {
                    // First coordiante is not a takeoff command, it does not link backwards to anything
                }
                firstCoordinateItem = false;
            } else {
                // Subsequent coordinate items link to last coordinate item
                _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate()));
            }
            lastCoordinateItem = item;
        }
    }
    
    emit waypointLinesChanged();
}

// This will update the child item hierarchy
void MissionController::_recalcChildItems(void)
{
    MissionItem* currentParentItem = qobject_cast<MissionItem*>(_missionItems->get(0));
    
    currentParentItem->childItems()->clear();
    
    for (int i=1; i<_missionItems->count(); i++) {
        MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
        
        // Set up non-coordinate item child hierarchy
        if (item->specifiesCoordinate()) {
            item->childItems()->clear();
            currentParentItem = item;
        } else {
            currentParentItem->childItems()->append(item);
        }
    }
}

void MissionController::_recalcAll(void)
{
    _recalcChildItems();
    _recalcWaypointLines();
}

/// Initializes a new set of mission items which may have come from the vehicle or have been loaded from a file
void MissionController::_initAllMissionItems(void)
{
    // Add the home position item to the front
    MissionItem* homeItem = new MissionItem(this);
    homeItem->setHomePositionSpecialCase(true);
    homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT);
    _missionItems->insert(0, homeItem);
    
    _recalcChildItems();
    _recalcWaypointLines();
    
    emit missionItemsChanged();
}

void MissionController::_activeVehicleChanged(Vehicle* activeVehicle)
{
    if (_activeVehicle) {
        MissionManager* missionManager = _activeVehicle->missionManager();
        disconnect(missionManager, &MissionManager::newMissionItemsAvailable,   this, &MissionController::_newMissionItemsAvailable);
        _activeVehicle = NULL;
    }
    
    _activeVehicle = activeVehicle;
    
    if (_activeVehicle) {
        MissionManager* missionManager = activeVehicle->missionManager();
        connect(missionManager, &MissionManager::newMissionItemsAvailable,  this, &MissionController::_newMissionItemsAvailable);
        _newMissionItemsAvailable();
    }
}