HSIDisplay.cc

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

QGroundControl Open Source Ground Control Station

(c) 2009, 2010 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/>.

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

/**
 * @file
 *   @brief Implementation of Horizontal Situation Indicator class
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QFile>
#include <QStringList>
#include <QPainter>
#include <QGraphicsScene>
#include <QHBoxLayout>
#include <QDoubleSpinBox>
#include <QDebug>

#include "UASManager.h"
#include "HSIDisplay.h"
#include "QGC.h"
#include "Waypoint.h"
#include "UASWaypointManager.h"
#include <qmath.h>
#include "MAV2DIcon.h"
#include "QGCApplication.h"

HSIDisplay::HSIDisplay(QWidget *parent) :
    HDDisplay(QStringList(), "HSI", parent),
    dragStarted(false),
    leftDragStarted(false),
    mouseHasMoved(false),
    startX(0.0f),
    startY(0.0f),
    actionPending(false),
    directSending(false),
    gpsSatellites(),
    satellitesUsed(0),
    attXSet(0.0f),
    attYSet(0.0f),
    attYawSet(0.0f),
    altitudeSet(1.0f),
    posXSet(0.0f),
    posYSet(0.0f),
    posZSet(0.0f),
    attXSaturation(0.2f),
    attYSaturation(0.2f),
    attYawSaturation(0.5f),
    posXSaturation(0.05f),
    posYSaturation(0.05f),
    altitudeSaturation(1.0f),
    lat(0.0f),
    lon(0.0f),
    alt(0.0f),
    globalAvailable(0),
    x(0.0f),
    y(0.0f),
    z(0.0f),
    vx(0.0f),
    vy(0.0f),
    vz(0.0f),
    speed(0.0f),
    localAvailable(0),
    roll(0.0f),
    pitch(0.0f),
    yaw(0.0f),
    bodyXSetCoordinate(0.0f),
    bodyYSetCoordinate(0.0f),
    bodyZSetCoordinate(0.0f),
    bodyYawSet(0.0f),
    uiXSetCoordinate(0.0f),
    uiYSetCoordinate(0.0f),
    uiZSetCoordinate(0.0f),
    uiYawSet(0.0f),
    metricWidth(4.0),
    crosstrackError(std::numeric_limits<double>::quiet_NaN()),
    xCenterPos(0),
    yCenterPos(0),
    positionLock(false),
    attControlEnabled(false),
    xyControlEnabled(false),
    zControlEnabled(false),
    yawControlEnabled(false),
    positionFix(0),
    gpsFix(0),
    visionFix(0),
    laserFix(0),
    iruFix(0),
    mavInitialized(false),
    topMargin(20.0f),
    bottomMargin(14.0f),
    attControlKnown(false),
    xyControlKnown(false),
    zControlKnown(false),
    yawControlKnown(false),
    positionFixKnown(false),
    visionFixKnown(false),
    gpsFixKnown(false),
    iruFixKnown(false),
    gyroKnown(false),
    gyroON(false),
    gyroOK(false),
    accelKnown(false),
    accelON(false),
    accelOK(false),
    magKnown(false),
    magON(false),
    magOK(false),
    pressureKnown(false),
    pressureON(false),
    pressureOK(false),
    diffPressureKnown(false),
    diffPressureON(false),
    diffPressureOK(false),
    flowKnown(false),
    flowON(false),
    flowOK(false),
    laserKnown(false),
    laserON(false),
    laserOK(false),
    viconKnown(false),
    viconON(false),
    viconOK(false),
    actuatorsKnown(false),
    actuatorsON(false),
    actuatorsOK(false),
    setPointKnown(false),
    positionSetPointKnown(false),
    userSetPointSet(false),
    userXYSetPointSet(false),
    userZSetPointSet(false),
    userYawSetPointSet(false)
{
    refreshTimer->setInterval(updateInterval);

    columns = 1;
    this->setAutoFillBackground(true);
    QPalette pal = palette();
    pal.setColor(backgroundRole(), QGC::colorBlack);
    setPalette(pal);

    vwidth = 80.0f;
    vheight = 100.0f;

    xCenterPos = vwidth/2.0f;
    yCenterPos = vheight/2.0f + topMargin - bottomMargin;

    uas = NULL;
    resetMAVState();

    // Do first update
    setMetricWidth(metricWidth);
    // Set tooltip
    setToolTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget."));
    setStatusTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget."));

    // XXX this looks a potential recursive issue
    //connect(&statusClearTimer, SIGNAL(timeout()), this, SLOT(clearStatusMessage()));

    if (UASManager::instance()->getActiveUAS())
    {
        setActiveUAS(UASManager::instance()->getActiveUAS());
    }

    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
            this, SLOT(setActiveUAS(UASInterface*)));

    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
            this, SLOT(setActiveUAS(UASInterface*)));

    setFocusPolicy(Qt::StrongFocus);
}

HSIDisplay::~HSIDisplay()
{

}

void HSIDisplay::resetMAVState()
{
    mavInitialized = false;
    attControlKnown = false;
    attControlEnabled = false;
    xyControlKnown = false;
    xyControlEnabled = false;
    zControlKnown = false;
    zControlEnabled = false;
    yawControlKnown = false;
    yawControlEnabled = false;

    // Draw position lock indicators
    positionFixKnown = false;
    positionFix = 0;
    visionFixKnown = false;
    visionFix = 0;
    gpsFixKnown = false;
    gpsFix = 0;
    iruFixKnown = false;
    iruFix = 0;

    // Data
    setPointKnown = false;
    localAvailable = 0;
    globalAvailable = 0;

    // Setpoints
    positionSetPointKnown = false;
    setPointKnown = false;
}

void HSIDisplay::paintEvent(QPaintEvent * event)
{
    Q_UNUSED(event);
    //paintGL();
    //    static quint64 interval = 0;
    //    //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
    //    interval = MG::TIME::getGroundTimeNow();
    renderOverlay();
}

void HSIDisplay::renderOverlay()
{
    if (!isVisible() || !uas) return;
#if (QGC_EVENTLOOP_DEBUG)
    qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif
    // Center location of the HSI gauge items

    //float bottomMargin = 3.0f;

    // Size of the ring instrument
    //const float margin = 0.1f;  // 10% margin of total width on each side
    float baseRadius = (vheight - topMargin - bottomMargin) / 2.0f - (topMargin + bottomMargin) / 2.8f;

    // Draw instruments
    // TESTING THIS SHOULD BE MOVED INTO A QGRAPHICSVIEW
    // Update scaling factor
    // adjust scaling to fit both horizontally and vertically
    scalingFactor = this->width()/vwidth;
    double scalingFactorH = this->height()/vheight;
    if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH;

    QPainter painter(viewport());
    painter.setRenderHint(QPainter::Antialiasing, true);
    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);

    // Draw base instrument
    // ----------------------
    painter.setBrush(Qt::NoBrush);

    // Set the color scheme depending on the light/dark theme employed.
    QColor ringColor;
    QColor positionColor;
    QColor setpointColor;
    QColor labelColor;
    QColor valueColor;
    QColor statusColor;
    QColor waypointLineColor;
    QColor attitudeColor;
    if (qgcApp()->styleIsDark())
    {
        ringColor = QColor(255, 255, 255);
        positionColor = QColor(20, 20, 200);
        setpointColor = QColor(150, 250, 150);
        labelColor = QGC::colorCyan;
        valueColor = QColor(255, 255, 255);
        statusColor = QGC::colorOrange;
        waypointLineColor = QGC::colorYellow;
        attitudeColor = QColor(200, 20, 20);
    }
    else
    {
        ringColor = QGC::colorBlack;
        positionColor = QColor(20, 20, 200);
        setpointColor = QColor(150, 250, 150);
        labelColor = QColor(26, 75, 95);
        valueColor = QColor(40, 40, 40);
        statusColor = QGC::colorOrange;
        waypointLineColor = QGC::colorDarkYellow;
        attitudeColor = QColor(200, 20, 20);
    }

    QPen pen;
    pen.setColor(ringColor);
    pen.setWidth(refLineWidthToPen(1.0f));
    painter.setPen(pen);
    const int ringCount = 2;
    for (int i = 0; i < ringCount; i++)
    {
        float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.35f * i+1) / 2.0f - bottomMargin / 2.0f;
        drawCircle(xCenterPos, yCenterPos, radius, 1.0f, ringColor, &painter);
        paintText(tr("%1 m").arg(refToMetric(radius), 5, 'f', 1, ' '), valueColor, 1.6f, vwidth/2-4, vheight/2+radius+7, &painter);
    }

    // Draw orientation labels
    // Translate and rotate coordinate frame
    painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
    const float yawDeg = ((yaw/M_PI)*180.0f);
    int yawRotate = static_cast<int>(yawDeg) % 360;
    painter.rotate(-yawRotate);
    paintText(tr("N"), ringColor, 3.5f, - 1.0f, - baseRadius - 5.5f, &painter);
    paintText(tr("S"), ringColor, 3.5f, - 1.0f, + baseRadius + 1.5f, &painter);
    paintText(tr("E"), ringColor, 3.5f, + baseRadius + 3.0f, - 1.25f, &painter);
    paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter);
    painter.rotate(+yawRotate);
    painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);

    // Draw trail
    //    QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
    //    // Transform from body to world coordinates
    //    m = metricWorldToBody(m);
    //    // Scale from metric body to screen reference units
    //    QPointF s = metricBodyToRef(m);
    //    drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, uas->getColor(), &painter);


    // Draw center indicator
    //    QPolygonF p(3);
    //    p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f));
    //    p.replace(1, QPointF(xCenterPos-4.0f, yCenterPos+3.5f));
    //    p.replace(2, QPointF(xCenterPos+4.0f, yCenterPos+3.5f));
    //    drawPolygon(p, &painter);

    if (uas)
    {
        // Translate to center
        painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
        QColor uasColor = uas->getColor();
        MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
        //MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
        // Translate back
        painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
    }

    // ----------------------

    // Draw satellites
    drawGPS(painter);

    // Draw state indicator

    // Draw position
    drawPositionDirection(xCenterPos, yCenterPos, baseRadius, positionColor, &painter);

    // Draw attitude
    drawAttitudeDirection(xCenterPos, yCenterPos, baseRadius, attitudeColor, &painter);


    // Draw position setpoints in body coordinates

    float xSpDiff = uiXSetCoordinate - bodyXSetCoordinate;
    float ySpDiff = uiYSetCoordinate - bodyYSetCoordinate;
    float zSpDiff = uiZSetCoordinate - bodyZSetCoordinate;

    float setPointDist = sqrt(xSpDiff*xSpDiff + ySpDiff*ySpDiff + zSpDiff*zSpDiff);

    float angleDiff = uiYawSet - bodyYawSet;

    float normAngleDiff = fabs(atan2(sin(angleDiff), cos(angleDiff)));

    // Labels on outer part and bottom

    // Draw waypoints
    drawWaypoints(painter);

    // Draw status flags
    drawStatusFlag(1,  1, tr("RAT"), rateControlEnabled, rateControlKnown, painter);
    drawStatusFlag(17, 1, tr("ATT"), attControlEnabled, attControlKnown, painter);
    drawStatusFlag(33, 1, tr("PXY"), xyControlEnabled,  xyControlKnown,  painter);
    drawStatusFlag(49, 1, tr("PZ"),  zControlEnabled,   zControlKnown,   painter);
    drawStatusFlag(65, 1, tr("YAW"), yawControlEnabled, yawControlKnown, painter);

    // Draw position lock indicators
    drawPositionLock(1,  6, tr("POS"), positionFix, positionFixKnown, painter);
    drawPositionLock(17, 6, tr("GPS"), gpsFix,      gpsFixKnown,      painter);
    drawStatusFlag(33,   6, tr("FLO"), flowON, flowKnown, flowOK, painter);
    drawPositionLock(49, 6, tr("VIS"), visionFix,   visionFixKnown,   painter);
    drawPositionLock(65, 6, tr("IRU"), iruFix,      iruFixKnown,      painter);

    drawStatusFlag(1,   11, tr("GYR"), gyroON, gyroKnown, gyroOK, painter);
    drawStatusFlag(17,  11, tr("ACC"), accelON, accelKnown, accelOK, painter);
    drawStatusFlag(33,  11, tr("MAG"), magON, magKnown, magOK, painter);
    drawStatusFlag(49,  11, tr("BAR"), pressureON, pressureKnown, pressureOK, painter);
    drawStatusFlag(65,  11, tr("PIT"), diffPressureON, diffPressureKnown, diffPressureOK, painter);

    drawStatusFlag(1, 16, tr("ACT"), actuatorsON, actuatorsKnown, actuatorsOK, painter);
    drawStatusFlag(17, 16, tr("LAS"), laserON, laserKnown, laserOK, painter);
    drawStatusFlag(33, 16, tr("VCN"), viconON, viconKnown, viconOK, painter);

    // Draw speed to top left
    paintText(tr("SPEED"), labelColor, 2.2f, 2, topMargin+2, &painter);
    paintText(tr("%1 m/s").arg(speed, 5, 'f', 2, '0'), valueColor, 2.2f, 12, topMargin+2, &painter);

    // Draw crosstrack error to top right
    paintText(tr("XTRACK"), labelColor, 2.2f, 54, topMargin+2, &painter);
    if (!isnan(crosstrackError)) {
        paintText(tr("%1 m").arg(crosstrackError, 5, 'f', 2, '0'), valueColor, 2.2f, 67, topMargin+2, &painter);
    } else {
        paintText(tr("-- m"), valueColor, 2.2f, 67, topMargin+2, &painter);
    }


    // Draw position to bottom left
    if (localAvailable > 0)
    {
        // Position
        QString str;
        float offset = (globalAvailable > 0) ? -3.0f : 0.0f;
        str.sprintf("%05.2f %05.2f %05.2f m", x, y, z);
        paintText(tr("POS"), labelColor, 2.6f, 2, vheight - offset - 4.0f, &painter);
        paintText(str, valueColor, 2.6f, 10, vheight - offset - 4.0f, &painter);
    }

    if (globalAvailable > 0)
    {
        // Position
        QString str;
        str.sprintf("lat: %05.2f lon: %06.2f alt: %06.2f", lat, lon, alt);
        paintText(tr("GPS"), labelColor, 2.6f, 2, vheight- 4.0f, &painter);
        paintText(str, valueColor, 2.6f, 10, vheight - 4.0f, &painter);
    }

    // Draw Safety
    double x1, y1, z1, x2, y2, z2;
    UASManager::instance()->getLocalNEDSafetyLimits(&x1, &y1, &z1, &x2, &y2, &z2);
    //    drawSafetyArea(QPointF(x1, y1), QPointF(x2, y2), QGC::colorYellow, painter);

    // Draw status message
    paintText(statusMessage, statusColor, 2.8f, 8, 15, &painter);

    // Draw setpoint over waypoints
    if (positionSetPointKnown || setPointKnown)
    {
        // Draw setpoint
        drawSetpointXYZYaw(bodyXSetCoordinate, bodyYSetCoordinate, bodyZSetCoordinate, bodyYawSet, setpointColor, painter);
        // Draw travel direction line
        QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
        // Transform from body to world coordinates
        m = metricWorldToBody(m);
        // Scale from metric body to screen reference units
        QPointF s = metricBodyToRef(m);
        drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, setpointColor, &painter);
    }

    if ((userSetPointSet || dragStarted) && ((normAngleDiff > 0.05f) || !(setPointDist < 0.08f && mavInitialized)))
    {
        drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, setpointColor, painter);
    }
}

void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter)
{
    drawStatusFlag(x, y, label, status, known, true, painter);
}

void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, bool ok, QPainter& painter)
{
    QColor statusColor;
    QColor labelColor;
    if (qgcApp()->styleIsDark())
    {
        statusColor = QColor(250, 250, 250);
        labelColor = QGC::colorCyan;
    }
    else
    {
        statusColor = QColor(40, 40, 40);
        labelColor = QColor(26, 75, 95);
    }

    // Draw the label.
    paintText(label, labelColor, 2.6f, x, y+0.8f, &painter);

    // Determine color of status rectangle.
    if (!ok) {
        painter.setBrush(QGC::colorDarkYellow);
    } else {
        if(status) {
            painter.setBrush(QGC::colorGreen);
        } else {
            painter.setBrush(Qt::darkGray);
        }
    }
    painter.setPen(Qt::NoPen);

    // Draw the status rectangle.
    float indicatorWidth = refToScreenX(7.0f);
    float indicatorHeight = refToScreenY(4.0f);
    painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), indicatorWidth, indicatorHeight));
    paintText((status) ? tr("ON") : tr("OFF"), statusColor, 2.6f, x+7.9f, y+0.8f, &painter);

    // Cross out instrument if state unknown
    if (!known)
    {
        QPen pen(Qt::yellow);
        pen.setWidth(3);
        painter.setPen(pen);
        // Top left to bottom right
        QPointF p1, p2, p3, p4;
        p1.setX(refToScreenX(x));
        p1.setY(refToScreenX(y));
        p2.setX(p1.x()+indicatorWidth+refToScreenX(7.3f));
        p2.setY(p1.y()+indicatorHeight);
        painter.drawLine(p1, p2);
        // Bottom left to top right
        p3.setX(refToScreenX(x));
        p3.setY(refToScreenX(y)+indicatorHeight);
        p4.setX(p1.x()+indicatorWidth+refToScreenX(7.3f));
        p4.setY(p1.y());
        painter.drawLine(p3, p4);
    }
}

void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter)
{
    // Select color scheme based on light or dark window theme.
    QColor labelColor;
    QColor negStatusColor(200, 20, 20);
    QColor intermediateStatusColor (Qt::yellow);
    QColor posStatusColor(20, 200, 20);
    QColor statusColor;
    if (qgcApp()->styleIsDark())
    {
        statusColor = QColor(250, 250, 250);
        labelColor = QGC::colorCyan;
    }
    else
    {
        statusColor = QColor(40, 40, 40);
        labelColor = QColor(26, 75, 95);
    }

    // Draw the label.
    paintText(label, labelColor, 2.6f, x, y+0.8f, &painter);

    // based on the status, choose both the coloring and lock text.
    QString lockText;
    switch (status) {
    case 1:
        painter.setBrush(intermediateStatusColor.dark(150));
        lockText = tr("LOC");
        break;
    case 2:
        painter.setBrush(intermediateStatusColor.dark(150));
        lockText = tr("2D");
        break;
    case 3:
        painter.setBrush(posStatusColor);
        lockText = tr("3D");
        break;
    default:
        painter.setBrush(negStatusColor);
        lockText = tr("NO");
        break;
    }

    float indicatorWidth = refToScreenX(7.0f);
    float indicatorHeight = refToScreenY(4.0f);

    painter.setPen(Qt::NoPen);
    painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), refToScreenX(7.0f), refToScreenY(4.0f)));
    paintText(lockText, statusColor, 2.6f, x+7.9f, y+0.8f, &painter);

    // Cross out instrument if state unknown
    if (!known) {
        QPen pen(Qt::yellow);
        pen.setWidth(3);
        painter.setPen(pen);
        // Top left to bottom right
        QPointF p1, p2, p3, p4;
        p1.setX(refToScreenX(x));
        p1.setY(refToScreenX(y));
        p2.setX(p1.x()+indicatorWidth+refToScreenX(7.3f));
        p2.setY(p1.y()+indicatorHeight);
        painter.drawLine(p1, p2);
        // Bottom left to top right
        p3.setX(refToScreenX(x));
        p3.setY(refToScreenX(y)+indicatorHeight);
        p4.setX(p1.x()+indicatorWidth+refToScreenX(7.3f));
        p4.setY(p1.y());
        painter.drawLine(p3, p4);
    }
}

void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock)
{
    Q_UNUSED(uas);
    bool changed = positionLock != lock;
    positionLock = lock;
    if (changed)
    {
        update();
    }
}

void HSIDisplay::updateAttitudeControllerEnabled(bool enabled)
{
    bool changed = attControlEnabled != enabled;
    attControlEnabled = enabled;
    attControlKnown = true;
    if (changed)
    {
        update();
    }
}

void HSIDisplay::updatePositionXYControllerEnabled(bool enabled)
{
    bool changed = xyControlEnabled != enabled;
    xyControlEnabled = enabled;
    xyControlKnown = true;
    if (changed)
    {
        update();
    }
}

void HSIDisplay::updatePositionZControllerEnabled(bool enabled)
{
    bool changed = zControlEnabled != enabled;
    zControlEnabled = enabled;
    zControlKnown = true;
    if (changed)
    {
        update();
    }
}

void HSIDisplay::updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance)
{
    Q_UNUSED(quality);
    Q_UNUSED(name);
    Q_UNUSED(type);
    Q_UNUSED(id);
    Q_UNUSED(time);
    // FIXME add multi-object support
    QPainter painter(this);
    QColor color(Qt::yellow);
    float radius = vwidth / 20.0f;
    QPen pen(color);
    pen.setWidthF(refLineWidthToPen(0.4f));
    pen.setColor(color);
    painter.setPen(pen);
    painter.setBrush(Qt::NoBrush);
    QPointF in(cos(bearing)-sin(bearing)*distance, sin(bearing)+cos(bearing)*distance);
    // Scale from metric to screen reference coordinates
    QPointF p = metricBodyToRef(in);
    drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
}

QPointF HSIDisplay::metricWorldToBody(QPointF world)
{
    // First translate to body-centered coordinates
    // Rotate around -yaw
    float angle = -yaw - M_PI;
    QPointF result(cos(angle) * (x - world.x()) - sin(angle) * (y - world.y()), sin(angle) * (x - world.x()) + cos(angle) * (y - world.y()));
    return result;
}

QPointF HSIDisplay::metricBodyToWorld(QPointF body)
{
    // First rotate into world coordinates
    // then translate to world position
    QPointF result((cos(-yaw) * body.x()) + (sin(-yaw) * body.y()) + x, (-sin(-yaw) * body.x()) + (cos(-yaw) * body.y()) + y);
    return result;
}

QPointF HSIDisplay::screenToMetricBody(QPointF ref)
{
    return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth);
}

QPointF HSIDisplay::refToMetricBody(QPointF &ref)
{
    return QPointF(-((ref.y() - yCenterPos)/ vwidth) * metricWidth - x, ((ref.x() - xCenterPos) / vwidth) * metricWidth - y);
}

/**
 * @see refToScreenX()
 */
QPointF HSIDisplay::metricBodyToRef(QPointF &metric)
{
    return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos);
}

double HSIDisplay::metricToRef(double metric)
{
    return (metric / metricWidth) * vwidth;
}

double HSIDisplay::refToMetric(double ref)
{
    return (ref/vwidth) * metricWidth;
}

QPointF HSIDisplay::metricBodyToScreen(QPointF metric)
{
    QPointF ref = metricBodyToRef(metric);
    ref.setX(refToScreenX(ref.x()));
    ref.setY(refToScreenY(ref.y()));
    return ref;
}

void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
{
    if (event->type() == QMouseEvent::MouseButtonDblClick)
    {
        QPointF p = screenToMetricBody(event->localPos());
        if (!directSending)
        {
            setBodySetpointCoordinateXY(p.x(), p.y());
            if (!userZSetPointSet) setBodySetpointCoordinateZ(0.0);
        }
        //        qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos;
    }
}

void HSIDisplay::mouseReleaseEvent(QMouseEvent * event)
{
    // FIXME hardcode yaw to current value
    //setBodySetpointCoordinateYaw(0);
    if (mouseHasMoved)
    {
        if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton)
        {
            if (dragStarted)
            {
                if (!directSending) setBodySetpointCoordinateYaw(uiYawSet);
                dragStarted = false;
            }
        }
        if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::LeftButton)
        {
            if (leftDragStarted)
            {
                if (!directSending) setBodySetpointCoordinateZ(uiZSetCoordinate);
                leftDragStarted = false;
            }
        }
    }
    mouseHasMoved = false;
}

void HSIDisplay::mousePressEvent(QMouseEvent * event)
{
    if (event->type() == QMouseEvent::MouseButtonPress)
    {
        if (event->button() == Qt::RightButton)
        {
            startX = event->x();
            // Start tracking mouse move
            dragStarted = true;
        }
        else if (event->button() == Qt::LeftButton)
        {
            startY = event->y();
            leftDragStarted = true;
        }
    }
    mouseHasMoved = false;
}

void HSIDisplay::mouseMoveEvent(QMouseEvent * event)
{
    if (event->type() == QMouseEvent::MouseMove)
    {
        if (dragStarted) uiYawSet -= 0.06f*(startX - event->x()) / this->frameSize().width();

        if (leftDragStarted)
        {
            //            uiZSetCoordinate -= 0.06f*(startY - event->y()) / this->frameSize().height();
            //            setBodySetpointCoordinateZ(uiZSetCoordinate);
        }

        if (leftDragStarted || dragStarted) mouseHasMoved = true;
    }
}

void HSIDisplay::keyPressEvent(QKeyEvent* event)
{
    QPointF bodySP = metricWorldToBody(QPointF(uiXSetCoordinate, uiYSetCoordinate));

    if ((event->key() == Qt::Key_Enter || event->key() == Qt::Key_Return) && actionPending)
    {
        actionPending = false;
        statusMessage = "SETPOINT SENT";
        statusClearTimer.start();
        sendBodySetPointCoordinates();
    }
    else
    {
        // FIXME hardcode yaw to current value
        setBodySetpointCoordinateYaw(0);


        // Reset setpoints to current position / orientation
        // if not initialized
        if (!userYawSetPointSet)
        {
            setBodySetpointCoordinateYaw(0);
        }

        if (!userZSetPointSet)
        {
            setBodySetpointCoordinateZ(0);
        }

        if (!userXYSetPointSet)
        {
            setBodySetpointCoordinateXY(0, 0);
        }

        if ((event->key() ==  Qt::Key_W))
        {
            setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()+0.2, +1.5), bodySP.y());
        }

        if ((event->key() ==  Qt::Key_S))
        {
            setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y());
        }

        if ((event->key() ==  Qt::Key_A))
        {
            setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()-0.2, +1.5));
        }

        if ((event->key() ==  Qt::Key_D))
        {
            setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()+0.2, +1.5));
        }

        if ((event->key() ==  Qt::Key_Up))
        {
            setBodySetpointCoordinateZ(-0.2);
        }

        if ((event->key() ==  Qt::Key_Down))
        {
            setBodySetpointCoordinateZ(+0.2);
        }

        if ((event->key() ==  Qt::Key_Left))
        {
            setBodySetpointCoordinateYaw(-0.2);
        }

        if ((event->key() ==  Qt::Key_Right))
        {
            setBodySetpointCoordinateYaw(0.2);
        }
    }


    // Overwrite any existing non-zero body setpoints
    // for escape
    if ((event->key() == Qt::Key_Escape) || (event->key() == Qt::Key_R))
    {
        setBodySetpointCoordinateXY(0, 0);
        setBodySetpointCoordinateZ(0);
        setBodySetpointCoordinateYaw(0);
        statusMessage = "CANCELLED, PRESS <ENTER> TO SEND";
    }

    if ((event->key() == Qt::Key_T))
    {
        directSending = !directSending;
        statusMessage = (directSending) ? "DIRECT SEND ON" : "DIRECT SEND OFF";
        statusClearTimer.start();
    }

    if (actionPending && (directSending || (event->key() == Qt::Key_Escape)))
    {
        actionPending = false;
        statusMessage = "SETPOINT SENT";
        statusClearTimer.start();
        sendBodySetPointCoordinates();
    }

    HDDisplay::keyPressEvent(event);
}

void HSIDisplay::contextMenuEvent (QContextMenuEvent* event)
{
    event->ignore();
}

void HSIDisplay::setMetricWidth(double width)
{
    if (width != metricWidth) {
        metricWidth = width;
        emit metricWidthChanged(metricWidth);
    }
}

/**
 *
 * @param uas the UAS/MAV to monitor/display with the HUD
 */
void HSIDisplay::setActiveUAS(UASInterface* uas)
{
    if (this->uas != NULL) {
        disconnect(this->uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
        disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
        disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64)));
        disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64)));
        disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
        disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
        disconnect(this->uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
        disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));

        disconnect(this->uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool)));
        disconnect(this->uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(bool)));
        disconnect(this->uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(bool)));
        disconnect(this->uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(bool)));

        disconnect(this->uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int)));
        disconnect(this->uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int)));
        disconnect(this->uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int)));
        disconnect(this->uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int)));
        disconnect(this->uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float)));

        disconnect(this->uas, SIGNAL(gyroStatusChanged(bool,bool,bool)), this, SLOT(updateGyroStatus(bool,bool,bool)));
        disconnect(this->uas, SIGNAL(accelStatusChanged(bool,bool,bool)), this, SLOT(updateAccelStatus(bool,bool,bool)));
        disconnect(this->uas, SIGNAL(magSensorStatusChanged(bool,bool,bool)), this, SLOT(updateMagSensorStatus(bool,bool,bool)));
        disconnect(this->uas, SIGNAL(baroStatusChanged(bool,bool,bool)), this, SLOT(updateBaroStatus(bool,bool,bool)));
        disconnect(this->uas, SIGNAL(airspeedStatusChanged(bool,bool,bool)), this, SLOT(updateAirspeedStatus(bool,bool,bool)));
        disconnect(this->uas, SIGNAL(opticalFlowStatusChanged(bool,bool,bool)), this, SLOT(updateOpticalFlowStatus(bool,bool,bool)));
        disconnect(this->uas, SIGNAL(laserStatusChanged(bool,bool,bool)), this, SLOT(updateLaserStatus(bool,bool,bool)));
        disconnect(this->uas, SIGNAL(groundTruthSensorStatusChanged(bool,bool,bool)), this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool)));
        disconnect(this->uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)), this, SLOT(updateActuatorStatus(bool,bool,bool)));
        disconnect(this->uas, &UASInterface::navigationControllerErrorsChanged,
                   this, &HSIDisplay::UpdateNavErrors);
    }

    if (uas)
    {
        connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)),
                this, SLOT(updateSatellite(int,int,float,float,float,bool)));
        connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)),
                this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
        connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,double,quint64)),
                this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64)));
        connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)),
                this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64)));
        connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)),
                this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
        connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)),
                this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
        connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)),
                this, SLOT(updateSpeed(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(attitudeControlEnabled(bool)),
                this, SLOT(updateAttitudeControllerEnabled(bool)));
        connect(uas, SIGNAL(positionXYControlEnabled(bool)),
                this, SLOT(updatePositionXYControllerEnabled(bool)));
        connect(uas, SIGNAL(positionZControlEnabled(bool)),
                this, SLOT(updatePositionZControllerEnabled(bool)));
        connect(uas, SIGNAL(positionYawControlEnabled(bool)),
                this, SLOT(updatePositionYawControllerEnabled(bool)));

        connect(uas, SIGNAL(localizationChanged(UASInterface*,int)),
                this, SLOT(updateLocalization(UASInterface*,int)));
        connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)),
                this, SLOT(updateVisionLocalization(UASInterface*,int)));
        connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)),
                this, SLOT(updateGpsLocalization(UASInterface*,int)));
        connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)),
                this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int)));
        connect(uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)),
                this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float)));

        connect(uas, SIGNAL(gyroStatusChanged(bool,bool,bool)),
                this, SLOT(updateGyroStatus(bool,bool,bool)));
        connect(uas, SIGNAL(accelStatusChanged(bool,bool,bool)),
                this, SLOT(updateAccelStatus(bool,bool,bool)));
        connect(uas, SIGNAL(magSensorStatusChanged(bool,bool,bool)),
                this, SLOT(updateMagSensorStatus(bool,bool,bool)));