MapWidget.cc

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

/**
 * @file
 *   @brief Implementation of MapWidget
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *   @author Mariano Lizarraga
 *
 */

#include <QComboBox>
#include <QGridLayout>
#include <QDir>

#include "QGC.h"
#include "MapWidget.h"
#include "ui_MapWidget.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "MAV2DIcon.h"
#include "Waypoint2DIcon.h"
#include "UASWaypointManager.h"

#include "MG.h"


MapWidget::MapWidget(QWidget *parent) :
        QWidget(parent),
        mc(NULL),
        zoomLevel(0),
        uasIcons(),
        uasTrails(),
        mav(NULL),
        lastUpdate(0),
        initialized(false),
        m_ui(new Ui::MapWidget)
{
    m_ui->setupUi(this);
    init();
}

void MapWidget::init()
{
    if (!initialized)
    {
        mc = new qmapcontrol::MapControl(this->size());
        // display the MapControl in the application
        QGridLayout* layout = new QGridLayout(this);
        layout->setMargin(0);
        layout->setSpacing(0);
        layout->addWidget(mc, 0, 0);
        setLayout(layout);

        //   VISUAL MAP STYLE
        QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)} QAbstractButton:checked { border: 2px solid #379AC3; }");
        mc->setPen(QGC::colorCyan.darker(400));

        waypointIsDrag = false;

        // Accept focus by clicking or keyboard
        this->setFocusPolicy(Qt::StrongFocus);

        // create MapControl

        mc->showScale(true);
        mc->showCoord(true);
        mc->enablePersistentCache();
        mc->setMouseTracking(true); // required to update the mouse position for diplay and capture

        // create MapAdapter to get maps from
        //TileMapAdapter* osmAdapter = new TileMapAdapter("tile.openstreetmap.org", "/%1/%2/%3.png", 256, 0, 17);

        qmapcontrol::MapAdapter* mapadapter_overlay = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=2.2&t=h&s=256&x=%2&y=%3&z=%1");

        // MAP BACKGROUND
        mapadapter = new qmapcontrol::GoogleSatMapAdapter();
        l = new qmapcontrol::MapLayer("Google Satellite", mapadapter);
        mc->addLayer(l);

        // STREET OVERLAY
        overlay = new qmapcontrol::MapLayer("Overlay", mapadapter_overlay);
        overlay->setVisible(false);
        mc->addLayer(overlay);

        // MAV FLIGHT TRACKS
        tracks = new qmapcontrol::MapLayer("Tracking", mapadapter);
        mc->addLayer(tracks);

        // WAYPOINT LAYER
        // create a layer with the mapadapter and type GeometryLayer (for waypoints)
        geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter);
        mc->addLayer(geomLayer);

        homePosition = new qmapcontrol::GeometryLayer("Station", mapadapter);
        mc->addLayer(homePosition);


        //
        //    Layer* gsatLayer = new Layer("Google Satellite", gsat, Layer::MapLayer);
        //    mc->addLayer(gsatLayer);


        // Zurich, ETH

        int lastZoom = 16;
        double lastLat = 47.376889;
        double lastLon = 8.548056;

        QSettings settings;
        settings.beginGroup("QGC_MAPWIDGET");
        lastLat = settings.value("LAST_LATITUDE", lastLat).toDouble();
        lastLon = settings.value("LAST_LONGITUDE", lastLon).toDouble();
        lastZoom = settings.value("LAST_ZOOM", lastZoom).toInt();
        settings.endGroup();

        settings.beginGroup("QGC_HOMEPOSITION");
        homeCoordinate.setY(settings.value("HOME_LATITUDE", homeCoordinate.y()).toDouble());
        homeCoordinate.setX(settings.value("HOME_LONGITUDE", homeCoordinate.x()).toDouble());
        settings.endGroup();

        // SET INITIAL POSITION AND ZOOM
        // Set default zoom level
        mc->setZoom(lastZoom);
        mc->setView(QPointF(lastLon, lastLat));

        // Veracruz Mexico
        //mc->setView(QPointF(-96.105208,19.138955));

        // Add controls to select map provider
        /////////////////////////////////////////////////
        QActionGroup* mapproviderGroup = new QActionGroup(this);
        osmAction = new QAction(QIcon(":/images/mapproviders/openstreetmap.png"), tr("OpenStreetMap"), mapproviderGroup);
        yahooActionMap = new QAction(QIcon(":/images/mapproviders/yahoo.png"), tr("Yahoo: Map"), mapproviderGroup);
        yahooActionSatellite = new QAction(QIcon(":/images/mapproviders/yahoo.png"), tr("Yahoo: Satellite"), mapproviderGroup);
        googleActionMap = new QAction(QIcon(":/images/mapproviders/google.png"), tr("Google: Map"), mapproviderGroup);
        googleSatAction = new QAction(QIcon(":/images/mapproviders/google.png"), tr("Google: Sat"), mapproviderGroup);
        osmAction->setCheckable(true);
        yahooActionMap->setCheckable(true);
        yahooActionSatellite->setCheckable(true);
        googleActionMap->setCheckable(true);
        googleSatAction->setCheckable(true);
        googleSatAction->setChecked(true);
        connect(mapproviderGroup, SIGNAL(triggered(QAction*)),
                this, SLOT(mapproviderSelected(QAction*)));


        //mapSettings.beginGroup("Map_Widget");
        //QAction *act = new QAction(mapSettings.value("QAction").toString(), this);
        //mapproviderSelected(act);
        //mapSettings.endGroup();



        // Overlay seems currently broken
        //    yahooActionOverlay = new QAction(tr("Yahoo: street overlay"), this);
        //    yahooActionOverlay->setCheckable(true);
        //    yahooActionOverlay->setChecked(overlay->isVisible());
        //    connect(yahooActionOverlay, SIGNAL(toggled(bool)),
        //            overlay, SLOT(setVisible(bool)));

        //    mapproviderGroup->addAction(googleSatAction);
        //    mapproviderGroup->addAction(osmAction);
        //    mapproviderGroup->addAction(yahooActionOverlay);
        //    mapproviderGroup->addAction(googleActionMap);
        //    mapproviderGroup->addAction(yahooActionMap);
        //    mapproviderGroup->addAction(yahooActionSatellite);

        // Create map provider selection menu
        mapMenu = new QMenu(this);
        mapMenu->addActions(mapproviderGroup->actions());
        mapMenu->addSeparator();
        //    mapMenu->addAction(yahooActionOverlay);

        mapButton = new QPushButton(this);
        mapButton->setText("Map Source");
        mapButton->setMenu(mapMenu);
        mapButton->setStyleSheet(buttonStyle);

        // create buttons to control the map (zoom, GPS tracking and WP capture)
        QPushButton* zoomin = new QPushButton(QIcon(":/images/actions/list-add.svg"), "", this);
        zoomin->setStyleSheet(buttonStyle);
        QPushButton* zoomout = new QPushButton(QIcon(":/images/actions/list-remove.svg"), "", this);
        zoomout->setStyleSheet(buttonStyle);
        createPath = new QPushButton(QIcon(":/images/actions/go-bottom.svg"), "", this);
        createPath->setStyleSheet(buttonStyle);
        createPath->setToolTip(tr("Start / end waypoint add mode"));
        createPath->setStatusTip(tr("Start / end waypoint add mode"));
        //    clearTracking = new QPushButton(QIcon(""), "", this);
        //    clearTracking->setStyleSheet(buttonStyle);
        followgps = new QPushButton(QIcon(":/images/actions/system-lock-screen.svg"), "", this);
        followgps->setStyleSheet(buttonStyle);
        followgps->setToolTip(tr("Follow the position of the current MAV with the map center"));
        followgps->setStatusTip(tr("Follow the position of the current MAV with the map center"));
        QPushButton* goToButton = new QPushButton(QIcon(""), "T", this);
        goToButton->setStyleSheet(buttonStyle);
        goToButton->setToolTip(tr("Enter a latitude/longitude position to move the map to"));
        goToButton->setStatusTip(tr("Enter a latitude/longitude position to move the map to"));

        setHome = new QPushButton(QIcon(":/images/actions/go-home.svg"), "", this);
        setHome->setStyleSheet(buttonStyle);
        setHome->setToolTip(tr("Set home"));
        setHome->setStatusTip(tr("Set home"));

        zoomin->setMaximumWidth(30);
        zoomout->setMaximumWidth(30);
        createPath->setMaximumWidth(30);
        //    clearTracking->setMaximumWidth(30);
        followgps->setMaximumWidth(30);
        setHome->setMaximumWidth(30);
        goToButton->setMaximumWidth(30);

        // Set checkable buttons
        // TODO: Currently checked buttons are are very difficult to distinguish when checked.
        //       create a style and the slots to change the background so it is easier to distinguish
        followgps->setCheckable(true);
        createPath->setCheckable(true);
        setHome->setCheckable(true);

        // add buttons to control the map (zoom, GPS tracking and WP capture)
        QGridLayout* innerlayout = new QGridLayout(mc);
        innerlayout->setMargin(3);
        innerlayout->setSpacing(3);
        innerlayout->addWidget(zoomin, 0, 0);
        innerlayout->addWidget(zoomout, 1, 0);
        innerlayout->addWidget(followgps, 2, 0);
        innerlayout->addWidget(createPath, 3, 0);
        innerlayout->addWidget(setHome, 4, 0);
        //innerlayout->addWidget(clearTracking, 4, 0);
        // Add spacers to compress buttons on the top left
        innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0);
        innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 7);
        innerlayout->addWidget(mapButton, 0, 6);
        innerlayout->addWidget(goToButton, 0, 7);
        innerlayout->setRowStretch(0, 1);
        innerlayout->setRowStretch(1, 100);
        mc->setLayout(innerlayout);

        // Configure the WP Path's pen
        pointPen = new QPen(QColor(0, 255,0));
        pointPen->setWidth(3);
        waypointPath = new qmapcontrol::LineString (wps, "Waypoint path", pointPen);
        mc->layer("Waypoints")->addGeometry(waypointPath);

        // Done set state
        initialized = true;


        // Connect the required signals-slots
        connect(zoomin, SIGNAL(clicked(bool)),
                mc, SLOT(zoomIn()));

        connect(zoomout, SIGNAL(clicked(bool)),
                mc, SLOT(zoomOut()));

        connect(goToButton, SIGNAL(clicked()), this, SLOT(goTo()));

        QList<UASInterface*> systems = UASManager::instance()->getUASList();
        foreach(UASInterface* system, systems)
        {
            addUAS(system);
        }

        connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)),
                this, SLOT(addUAS(UASInterface*)));

        activeUASSet(UASManager::instance()->getActiveUAS());
        connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*)));

        connect(mc, SIGNAL(mouseEventCoordinate(const QMouseEvent*, const QPointF)),
                this, SLOT(captureMapClick(const QMouseEvent*, const QPointF)));

        connect(createPath, SIGNAL(clicked(bool)),
                this, SLOT(createPathButtonClicked(bool)));

        connect(setHome, SIGNAL(clicked(bool)), this, SLOT(createHomePositionClick(bool)));

        connect(mc, SIGNAL(mouseEventCoordinate(const QMouseEvent*,QPointF)), this,
                SLOT(createHomePosition(const QMouseEvent*,QPointF)));
        //connect(setHome, SIGNAL(clicked(bool)), this, SLOT(createHomePosition(bool)));


        connect(geomLayer, SIGNAL(geometryClicked(Geometry*,QPoint)),
                this, SLOT(captureGeometryClick(Geometry*, QPoint)));

        connect(geomLayer, SIGNAL(geometryDragged(Geometry*, QPointF)),
                this, SLOT(captureGeometryDrag(Geometry*, QPointF)));

        connect(geomLayer, SIGNAL(geometryEndDrag(Geometry*, QPointF)),
                this, SLOT(captureGeometryEndDrag(Geometry*, QPointF)));

        connect(homePosition, SIGNAL(geometryClicked(Geometry*,QPoint)),
                this, SLOT(captureGeometryClick(Geometry*, QPoint)));

        connect(homePosition, SIGNAL(geometryDragged(Geometry*, QPointF)),
                this, SLOT(captureGeometryDragHome(Geometry*, QPointF)));

        connect(homePosition, SIGNAL(geometryEndDrag(Geometry*, QPointF)),
                this, SLOT(captureGeometryEndDrag(Geometry*, QPointF)));

        this->loadSettingsMap(settings);
        this->createHomePosition(homeCoordinate);

        qDebug() << "CHECK END";
    }
}

void MapWidget::loadSettingsMap(QSettings &settings)
{
    index = 0;
    settings.beginGroup("QGC_MAPINDEX");
    index = settings.value("MAP_INDEX", index).toInt();
    settings.endGroup();

    switch(index)
    {
    case 0:
        mapproviderSelected(osmAction);
        break;
    case 1:
        mapproviderSelected(yahooActionMap);
        break;
    case 2:
        mapproviderSelected(yahooActionSatellite);
        break;
    case 3:
        mapproviderSelected(googleActionMap);
        break;
    case 4:
        mapproviderSelected(googleSatAction);
        break;
    }
}

void MapWidget::goTo()
{
    bool ok;
    QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
                                         tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
                                         QString("%1,%2").arg(mc->currentCoordinate().y()).arg(mc->currentCoordinate().x()), &ok);
    if (ok && !text.isEmpty())
    {
        QStringList split = text.split(",");
        if (split.length() == 2)
        {
            bool convert;
            double latitude = split.first().toDouble(&convert);
            ok &= convert;
            double longitude = split.last().toDouble(&convert);
            ok &= convert;

            if (ok)
            {
                mc->setView(QPointF(longitude, latitude));
            }
        }
    }
}


void MapWidget::mapproviderSelected(QAction* action)
{
    if (mc)
    {
        //delete mapadapter;
        mapButton->setText(action->text());
        if (action == osmAction)
        {
            int zoom = mapadapter->adaptedZoom();
            mc->setZoom(0);

            mapadapter = new qmapcontrol::OSMMapAdapter();
            l->setMapAdapter(mapadapter);
            geomLayer->setMapAdapter(mapadapter);
            homePosition->setMapAdapter(mapadapter);
            index = 0;

            if (isVisible()) mc->updateRequestNew();
            mc->setZoom(zoom);
            //        yahooActionOverlay->setEnabled(false);
            overlay->setVisible(false);
            //        yahooActionOverlay->setChecked(false);

        }
        else if (action == yahooActionMap)
        {
            int zoom = mapadapter->adaptedZoom();
            mc->setZoom(0);

            mapadapter = new qmapcontrol::YahooMapAdapter();
            l->setMapAdapter(mapadapter);
            geomLayer->setMapAdapter(mapadapter);
            homePosition->setMapAdapter(mapadapter);
            index = 1;

            if (isVisible()) mc->updateRequestNew();
            mc->setZoom(zoom);
            //        yahooActionOverlay->setEnabled(false);
            overlay->setVisible(false);
            //        yahooActionOverlay->setChecked(false);
        }
        else if (action == yahooActionSatellite)
        {
            int zoom = mapadapter->adaptedZoom();
            QPointF a = mc->currentCoordinate();
            mc->setZoom(0);

            mapadapter = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1");
            l->setMapAdapter(mapadapter);
            geomLayer->setMapAdapter(mapadapter);
            homePosition->setMapAdapter(mapadapter);
            index = 2;

            if (isVisible()) mc->updateRequestNew();
            mc->setZoom(zoom);
            overlay->setVisible(false);
            //        yahooActionOverlay->setEnabled(true);
        }
        else if (action == googleActionMap)
        {
            int zoom = mapadapter->adaptedZoom();
            mc->setZoom(0);
            mapadapter = new qmapcontrol::GoogleMapAdapter();
            l->setMapAdapter(mapadapter);
            geomLayer->setMapAdapter(mapadapter);
            homePosition->setMapAdapter(mapadapter);
            index = 3;

            if (isVisible()) mc->updateRequestNew();
            mc->setZoom(zoom);
            //        yahooActionOverlay->setEnabled(false);
            overlay->setVisible(false);
            //        yahooActionOverlay->setChecked(false);
        }
        else if (action == googleSatAction)
        {
            int zoom = mapadapter->adaptedZoom();
            mc->setZoom(0);
            mapadapter = new qmapcontrol::GoogleSatMapAdapter();
            l->setMapAdapter(mapadapter);
            geomLayer->setMapAdapter(mapadapter);
            homePosition->setMapAdapter(mapadapter);
            index = 4;

            if (isVisible()) mc->updateRequestNew();
            mc->setZoom(zoom);
            //        yahooActionOverlay->setEnabled(false);
            overlay->setVisible(false);
            //        yahooActionOverlay->setChecked(false);
        }
        else
        {
            mapButton->setText("Select..");
        }
    }
}


void MapWidget::createPathButtonClicked(bool checked)
{
    if (mc)
    {
        Q_UNUSED(checked);

        if (createPath->isChecked())
        {
            // change the cursor shape
            this->setCursor(Qt::PointingHandCursor);
            mc->setMouseMode(qmapcontrol::MapControl::None);


            // emit signal start to create a Waypoint global
            //emit createGlobalWP(true, mc->currentCoordinate());

            //        // Clear the previous WP track
            //        // TODO: Move this to an actual clear track button and add a warning dialog
            //        mc->layer("Waypoints")->clearGeometries();
            //        wps.clear();
            //        path->setPoints(wps);
            //        mc->layer("Waypoints")->addGeometry(path);
            //        wpIndex.clear();
        }
        else
        {

            this->setCursor(Qt::ArrowCursor);
            mc->setMouseMode(qmapcontrol::MapControl::Panning);
        }
    }
}

/**
 * Captures a click on the map and if in create WP path mode, it adds the WP on MouseButtonRelease
 *
 * @param event The mouse event
 * @param coordinate The coordinate in which it occured the mouse event
 * @note  This slot is connected to the mouseEventCoordinate of the QMapControl object
 */
void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordinate)
{
    if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked())
    {
        // Create waypoint name
        QString str;

        // create the WP and set everything in the LineString to display the path
        Waypoint2DIcon* tempCirclePoint;

        if (mav)
        {
            double altitude = 0.0;
            double yaw = 0.0;
            int wpListCount = mav->getWaypointManager()->getWaypointList().count();
            if (wpListCount > 0)
            {
                altitude = mav->getWaypointManager()->getWaypointList().at(wpListCount-1)->getAltitude();
                yaw = mav->getWaypointManager()->getWaypointList().at(wpListCount-1)->getYaw();
            }
            mav->getWaypointManager()->addWaypoint(new Waypoint(wpListCount, coordinate.y(), coordinate.x(), altitude, yaw, true));
        }
        else
        {
            str = QString("%1").arg(waypointPath->numberOfPoints());
            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
            wpIcons.append(tempCirclePoint);

            mc->layer("Waypoints")->addGeometry(tempCirclePoint);

            qmapcontrol::Point* tempPoint = new qmapcontrol::Point(coordinate.x(), coordinate.y(),str);
            wps.append(tempPoint);
            waypointPath->addPoint(tempPoint);

            // Refresh the screen
            if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
        }
        // emit signal mouse was clicked
        //emit captureMapCoordinateClick(coordinate);
    }
}

void MapWidget::updateWaypoint(int uas, Waypoint* wp)
{
    // Update waypoint list and redraw map (last parameter)
    updateWaypoint(uas, wp, true);
}

/**
 * This function is called if a a single waypoint is updated and
 * also if the whole list changes.
 */
void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
{
    if (mc)
    {
        // Make sure this is the right UAS
        if (uas == this->mav->getUASID())
        {
            // Only accept waypoints in global coordinate frame
            if (wp->getFrame() == MAV_FRAME_GLOBAL)
            {
                // We're good, this is a global waypoint

                // Get the index of this waypoint
                // note the call to getGlobalFrameIndexOf()
                // as we're only handling global waypoints
                int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameIndexOf(wp);
                // If not found, return (this should never happen, but helps safety)
                if (wpindex == -1) return;

                // Check if wp exists yet in map
                if (!(wpIcons.count() > wpindex))
                {
                    // Waypoint is new, a new icon is created
                    QPointF coordinate;
                    coordinate.setX(wp->getLongitude());
                    coordinate.setY(wp->getLatitude());
                    createWaypointGraphAtMap(wpindex, coordinate);
                }
                else
                {
                    // Waypoint exists, update it if we're not
                    // currently dragging it with the mouse
                    if(!waypointIsDrag)
                    {
                        QPointF coordinate;
                        coordinate.setX(wp->getLongitude());
                        coordinate.setY(wp->getLatitude());

                        Point* waypoint;
                        waypoint = wps.at(wpindex);
                        if (waypoint)
                        {
                            // First set waypoint coordinate
                            waypoint->setCoordinate(coordinate);
                            // Now update icon position
                            wpIcons.at(wpindex)->setCoordinate(coordinate);
                            // Update pen
                            wpIcons.at(wpindex)->setPen(mavPens.value(uas));
                            // Then waypoint line coordinate
                            Point* linesegment = NULL;
                            // If the line segment already exists, just update it
                            // else create a new one
                            if (waypointPath->points().size() > wpindex)
                            {
                                linesegment = waypointPath->points().at(wpindex);
                                if (linesegment) linesegment->setCoordinate(coordinate);
                            }
                            else
                            {
                                waypointPath->addPoint(waypoint);
                            }

                            // Update view
                            if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect());
                        }
                    }
                }
            }
            else
            {
                // Check if the index of this waypoint is larger than the global
                // waypoint list. This implies that the coordinate frame of this
                // waypoint was changed and the list containing only global
                // waypoints was shortened. Thus update the whole list
                if (waypointPath->points().count() > UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameCount())
                {
                    updateWaypointList(uas);
                }
            }
        }
    }
}

void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate)
{
    //if (!wpExists(coordinate))
    {
        // Create waypoint name
        QString str;

        // create the WP and set everything in the LineString to display the path
        //CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str);
        Waypoint2DIcon* tempCirclePoint;

        if (mav)
        {
            int uas = mav->getUASID();
            str = QString("%1").arg(id);
            qDebug() << "Waypoint list count:" << str;
            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, mavPens.value(uas));
        }
        else
        {
            str = QString("%1").arg(id);
            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
        }


        mc->layer("Waypoints")->addGeometry(tempCirclePoint);
        wpIcons.append(tempCirclePoint);

        Point* tempPoint = new Point(coordinate.x(), coordinate.y(),str);
        wps.append(tempPoint);
        waypointPath->addPoint(tempPoint);

        //wpIndex.insert(str,tempPoint);
        qDebug()<<"Funcion createWaypointGraphAtMap WP= "<<str<<" -> x= "<<tempPoint->latitude()<<" y= "<<tempPoint->longitude();

        // Refresh the screen
        if (isVisible()) if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
    }

    ////    // emit signal mouse was clicked
    //    emit captureMapCoordinateClick(coordinate);
}

int MapWidget::wpExists(const QPointF coordinate)
{
    if (mc)
    {
        for (int i = 0; i < wps.size(); i++){
            if (wps.at(i)->latitude() == coordinate.y() &&
                wps.at(i)->longitude()== coordinate.x())
            {
                return 1;
            }
        }
    }
    return 0;
}


void MapWidget::captureGeometryClick(Geometry* geom, QPoint point)
{
    Q_UNUSED(geom);
    Q_UNUSED(point);

    if (mc) mc->setMouseMode(qmapcontrol::MapControl::None);
}

void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
{
    waypointIsDrag = true;

    // Refresh the screen
    if (isVisible()) mc->updateRequest(geom->boundingBox().toRect());

    int temp = 0;

    // Get waypoint index in list
    bool wpIndexOk;
    int index = geom->name().toInt(&wpIndexOk);

    Waypoint2DIcon* point2Find = dynamic_cast <Waypoint2DIcon*> (geom);

    if (wpIndexOk && point2Find && wps.count() > index)
    {
        // Update visual
        point2Find->setCoordinate(coordinate);
        waypointPath->points().at(index)->setCoordinate(coordinate);
        if (isVisible()) mc->updateRequest(waypointPath->boundingBox().toRect());

        // Update waypoint data storage
        if (mav)
        {
            QVector<Waypoint*> wps = mav->getWaypointManager()->getGlobalFrameWaypointList();

            if (wps.size() > index)
            {
                Waypoint* wp = wps.at(index);
                wp->setLatitude(coordinate.y());
                wp->setLongitude(coordinate.x());
                mav->getWaypointManager()->notifyOfChange(wp);
            }
        }

        // qDebug() << geom->name();
        temp = geom->get_myIndex();
        //qDebug() << temp;
        emit sendGeometryEndDrag(coordinate,temp);
    }

    waypointIsDrag = false;
}

void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate)
{
    Q_UNUSED(geom);
    Q_UNUSED(coordinate);
    // TODO: Investigate why when creating the waypoint path this slot is being called

    // Only change the mouse mode back to panning when not creating a WP path
    if (!createPath->isChecked())
    {
        waypointIsDrag = false;
        mc->setMouseMode(qmapcontrol::MapControl::Panning);
    }

    if (!setHome->isChecked())
    {
        mc->setMouseMode(qmapcontrol::MapControl::Panning);

        if(mav)
        {
            // Update homePosition
            UASManager::instance()->setHomePosition(
                                static_cast<double>(homeCoordinate.y()),
                                static_cast<double>(homeCoordinate.x()), 0);
        }
    }

}

void MapWidget::captureGeometryDragHome(Geometry *geom, QPointF coordinate)
{
    if (isVisible()) mc->updateRequest(geom->boundingBox().toRect());

    Waypoint2DIcon* point2Find = dynamic_cast <Waypoint2DIcon*> (geom);

    if (point2Find)// && wps.count() > index)
    {
        // Update visual
        point2Find->setCoordinate(coordinate);
        homeCoordinate.setX(coordinate.x());
        homeCoordinate.setY(coordinate.y());

        qmapcontrol::Point* tempPoint = new qmapcontrol::Point(homeCoordinate.x(), homeCoordinate.y(),"g");

        if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
    }
}

MapWidget::~MapWidget()
{
    delete mc;
    delete m_ui;
}

/**
 *
 * @param uas the UAS/MAV to monitor/display with the HUD
 */
void MapWidget::addUAS(UASInterface* uas)
{
    connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
    connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
    connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int)));
}

/**
 * Update the whole list of waypoints. This is e.g. necessary if the list order changed.
 * The UAS manager will emit the appropriate signal whenever updating the list
 * is necessary.
 */
void MapWidget::updateWaypointList(int uas)
{
    if (mc)
    {
        // Get already existing waypoints
        UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
        if (uasInstance)
        {
            // Get update rect of old content, this is what will be redrawn
            // in the last step
            QRect updateRect = waypointPath->boundingBox().toRect();

            // Get all waypoints, including non-global waypoints
            QVector<Waypoint*> wpList = uasInstance->getWaypointManager()->getWaypointList();

            // Clear if necessary
            if (wpList.count() == 0)
            {
                clearWaypoints(uas);
                return;
            }

            // Trim internal list to number of global waypoints in the waypoint manager list
            int overSize = waypointPath->points().count() - uasInstance->getWaypointManager()->getGlobalFrameCount();
            if (overSize > 0)
            {
                // Remove n waypoints at the end of the list
                // the remaining waypoints will be updated
                // in the next step
                for (int i = 0; i < overSize; ++i)
                {
                    wps.removeLast();
                    mc->layer("Waypoints")->removeGeometry(wpIcons.last());
                    wpIcons.removeLast();
                    waypointPath->points().removeLast();
                }
            }

            // Load all existing waypoints into map view
            foreach (Waypoint* wp, wpList)
            {
                // Block map draw updates, since we update everything in the next step
                // but update internal data structures.
                // Please note that updateWaypoint() ignores non-global waypoints
                updateWaypoint(mav->getUASID(), wp, false);
            }

            // Update view
            if (isVisible()) mc->updateRequest(updateRect);
        }
    }
}

void MapWidget::redoWaypoints(int uas)
{
    //    QObject* sender = QObject::sender();
    //    UASWaypointManager* manager = dynamic_cast<UASWaypointManager*>(sender);
    //    if (sender)
    //    {
    // Get waypoint list for this MAV

    // Clear all waypoints
    //clearWaypoints();
    // Re-add the updated waypoints

    //    }

    updateWaypointList(uas);
}

void MapWidget::activeUASSet(UASInterface* uas)
{
    // Disconnect old MAV
    if (mav)
    {
        // Disconnect the waypoint manager / data storage from the UI
        disconnect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
        disconnect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
        disconnect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
    }

    if (uas && mc)
    {
        mav = uas;
        QColor color = mav->getColor();
        color.setAlphaF(0.9);
        QPen* pen = new QPen(color);
        pen->setWidth(3.0);
        mavPens.insert(mav->getUASID(), pen);
        // FIXME Remove after refactoring
        waypointPath->setPen(pen);

        // Delete all waypoints and add waypoint from new system
        //redoWaypoints();
        updateWaypointList(uas->getUASID());

        // Connect the waypoint manager / data storage to the UI
        connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
        connect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
        connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));

        updateSystemSpecs(mav->getUASID());
        updateSelectedSystem(mav->getUASID());
        mc->updateRequest(waypointPath->boundingBox().toRect());
    }
}

void MapWidget::updateSystemSpecs(int uas)
{
    if (mc)
    {
        foreach (qmapcontrol::Point* p, uasIcons.values())
        {
            MAV2DIcon* icon = dynamic_cast<MAV2DIcon*>(p);
            if (icon && icon->getUASId() == uas)
            {
                // Set new airframe
                icon->setAirframe(UASManager::instance()->getUASForId(uas)->getAirframe());
                icon->drawIcon();
            }
        }
    }
}

void MapWidget::updateSelectedSystem(int uas)
{
    if (mc)
    {
        foreach (qmapcontrol::Point* p, uasIcons.values())
        {
            MAV2DIcon* icon = dynamic_cast<MAV2DIcon*>(p);
            if (icon)
            {
                // Set as selected if ids match
                icon->setSelectedUAS((icon->getUASId() == uas));
            }
        }
    }
}

void MapWidget::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec)
{
    Q_UNUSED(roll);
    Q_UNUSED(pitch);
    Q_UNUSED(usec);
    if (mc)
    {

        if (uas)
        {
            MAV2DIcon* icon = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID(), NULL));
            if (icon)
            {
                icon->setYaw(yaw);
            }
        }
    }
}

/**
 * Updates the global position of one MAV and append the last movement to the trail
 *
 * @param uas The unmanned air system
 * @param lat Latitude in WGS84 ellipsoid
 * @param lon Longitutde in WGS84 ellipsoid
 * @param alt Altitude over mean sea level
 * @param usec Timestamp of the position message in milliseconds FIXME will move to microseconds
 */
void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec)
{
    Q_UNUSED(usec);
    Q_UNUSED(alt); // FIXME Use altitude
    if (mc)
    {
        // create a LineString
        //QList<Point*> points;
        // Points with a circle
        // A QPen can be used to customize the
        //pointpen->setWidth(3);
        //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen));

        qmapcontrol::Point* p;
        QPointF coordinate;
        coordinate.setX(lon);
        coordinate.setY(lat);