MissionItem.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/>.

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

#include <QStringList>
#include <QJsonDocument>
#include <QJsonParseError>
#include <QDebug>

#include "MissionItem.h"
#include "FirmwarePluginManager.h"
#include "QGCApplication.h"

QGC_LOGGING_CATEGORY(MissionItemLog, "MissionItemLog")

const double MissionItem::defaultTakeoffPitch =         15.0;
const double MissionItem::defaultHeading =              0.0;
const double MissionItem::defaultAltitude =             25.0;
const double MissionItem::defaultAcceptanceRadius =     3.0;
const double MissionItem::defaultLoiterOrbitRadius =    10.0;
const double MissionItem::defaultLoiterTurns =          1.0;

FactMetaData* MissionItem::_altitudeMetaData =          NULL;
FactMetaData* MissionItem::_commandMetaData =           NULL;
FactMetaData* MissionItem::_defaultParamMetaData =      NULL;
FactMetaData* MissionItem::_frameMetaData =             NULL;
FactMetaData* MissionItem::_latitudeMetaData =          NULL;
FactMetaData* MissionItem::_longitudeMetaData =         NULL;
FactMetaData* MissionItem::_supportedCommandMetaData =  NULL;

const QString MissionItem::_decimalPlacesJsonKey        (QStringLiteral("decimalPlaces"));
const QString MissionItem::_defaultJsonKey              (QStringLiteral("default"));
const QString MissionItem::_descriptionJsonKey          (QStringLiteral("description"));
const QString MissionItem::_friendlyEditJsonKey         (QStringLiteral("friendlyEdit"));
const QString MissionItem::_friendlyNameJsonKey         (QStringLiteral("friendlyName"));
const QString MissionItem::_idJsonKey                   (QStringLiteral("id"));
const QString MissionItem::_labelJsonKey                (QStringLiteral("label"));
const QString MissionItem::_mavCmdInfoJsonKey           (QStringLiteral("mavCmdInfo"));
const QString MissionItem::_param1JsonKey               (QStringLiteral("param1"));
const QString MissionItem::_param2JsonKey               (QStringLiteral("param2"));
const QString MissionItem::_param3JsonKey               (QStringLiteral("param3"));
const QString MissionItem::_param4JsonKey               (QStringLiteral("param4"));
const QString MissionItem::_paramJsonKeyFormat          (QStringLiteral("param%1"));
const QString MissionItem::_rawNameJsonKey              (QStringLiteral("rawName"));
const QString MissionItem::_specifiesCoordinateJsonKey  (QStringLiteral("specifiesCoordinate"));
const QString MissionItem::_unitsJsonKey                (QStringLiteral("units"));
const QString MissionItem::_versionJsonKey              (QStringLiteral("version"));

const QString MissionItem::_degreesUnits                (QStringLiteral("degrees"));

QMap<MAV_CMD, MissionItem::MavCmdInfo_t> MissionItem::_mavCmdInfoMap;

struct EnumInfo_s {
    const char *    label;
    MAV_FRAME       frame;
};

static const struct EnumInfo_s _rgMavFrameInfo[] = {
    { "MAV_FRAME_GLOBAL",                   MAV_FRAME_GLOBAL },
    { "MAV_FRAME_LOCAL_NED",                MAV_FRAME_LOCAL_NED },
    { "MAV_FRAME_MISSION",                  MAV_FRAME_MISSION },
    { "MAV_FRAME_GLOBAL_RELATIVE_ALT",      MAV_FRAME_GLOBAL_RELATIVE_ALT },
    { "MAV_FRAME_LOCAL_ENU",                MAV_FRAME_LOCAL_ENU },
    { "MAV_FRAME_GLOBAL_INT",               MAV_FRAME_GLOBAL_INT },
    { "MAV_FRAME_GLOBAL_RELATIVE_ALT_INT",  MAV_FRAME_GLOBAL_RELATIVE_ALT_INT },
    { "MAV_FRAME_LOCAL_OFFSET_NED",         MAV_FRAME_LOCAL_OFFSET_NED },
    { "MAV_FRAME_BODY_NED",                 MAV_FRAME_BODY_NED },
    { "MAV_FRAME_BODY_OFFSET_NED",          MAV_FRAME_BODY_OFFSET_NED },
    { "MAV_FRAME_GLOBAL_TERRAIN_ALT",       MAV_FRAME_GLOBAL_TERRAIN_ALT },
    { "MAV_FRAME_GLOBAL_TERRAIN_ALT_INT",   MAV_FRAME_GLOBAL_TERRAIN_ALT_INT },
};

QDebug operator<<(QDebug dbg, const MissionItem& missionItem)
{
    QDebugStateSaver saver(dbg);
    dbg.nospace() << "MissionItem(" << missionItem.coordinate() << ")";
    
    return dbg;
}

QDebug operator<<(QDebug dbg, const MissionItem* missionItem)
{
    QDebugStateSaver saver(dbg);
    dbg.nospace() << "MissionItem(" << missionItem->coordinate() << ")";
    
    return dbg;
}

MissionItem::MissionItem(QObject* parent)
    : QObject(parent)
    , _rawEdit(false)
    , _dirty(false)
    , _sequenceNumber(0)
    , _isCurrentItem(false)
    , _azimuth(0.0)
    , _distance(0.0)
    , _homePositionSpecialCase(false)
    , _homePositionValid(false)
    , _altitudeRelativeToHomeFact   (0, "Altitude is relative to home", FactMetaData::valueTypeUint32)
    , _autoContinueFact             (0, "AutoContinue",                 FactMetaData::valueTypeUint32)
    , _commandFact                  (0, "Command:",                     FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "Frame:",                       FactMetaData::valueTypeUint32)
    , _param1Fact                   (0, "Param1:",                      FactMetaData::valueTypeDouble)
    , _param2Fact                   (0, "Param2:",                      FactMetaData::valueTypeDouble)
    , _param3Fact                   (0, "Param3:",                      FactMetaData::valueTypeDouble)
    , _param4Fact                   (0, "Param4:",                      FactMetaData::valueTypeDouble)
    , _param5Fact                   (0, "Latitude:",                    FactMetaData::valueTypeDouble)
    , _param6Fact                   (0, "Longitude:",                   FactMetaData::valueTypeDouble)
    , _param7Fact                   (0, "Altitude:",                    FactMetaData::valueTypeDouble)
    , _supportedCommandFact         (0, "Command:",                     FactMetaData::valueTypeUint32)
    , _param1MetaData(FactMetaData::valueTypeDouble)
    , _param2MetaData(FactMetaData::valueTypeDouble)
    , _param3MetaData(FactMetaData::valueTypeDouble)
    , _param4MetaData(FactMetaData::valueTypeDouble)
    , _syncingAltitudeRelativeToHomeAndFrame    (false)
    , _syncingHeadingDegreesAndParam4           (false)
    , _syncingSupportedCommandAndCommand        (false)
{
    _setupMetaData();
    _connectSignals();

    setAutoContinue(true);
}

MissionItem::MissionItem(int             sequenceNumber,
                         MAV_CMD         command,
                         MAV_FRAME       frame,
                         double          param1,
                         double          param2,
                         double          param3,
                         double          param4,
                         double          param5,
                         double          param6,
                         double          param7,
                         bool            autoContinue,
                         bool            isCurrentItem,
                         QObject*        parent)
    : QObject(parent)
    , _rawEdit(false)
    , _dirty(false)
    , _sequenceNumber(sequenceNumber)
    , _isCurrentItem(isCurrentItem)
    , _azimuth(0.0)
    , _distance(0.0)
    , _homePositionSpecialCase(false)
    , _homePositionValid(false)
    , _altitudeRelativeToHomeFact   (0, "Altitude is relative to home", FactMetaData::valueTypeUint32)
    , _commandFact                  (0, "Command:",                     FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "Frame:",                       FactMetaData::valueTypeUint32)
    , _param1Fact                   (0, "Param1:",                      FactMetaData::valueTypeDouble)
    , _param2Fact                   (0, "Param2:",                      FactMetaData::valueTypeDouble)
    , _param3Fact                   (0, "Param3:",                      FactMetaData::valueTypeDouble)
    , _param4Fact                   (0, "Param4:",                      FactMetaData::valueTypeDouble)
    , _param5Fact                   (0, "Lat/X:",                       FactMetaData::valueTypeDouble)
    , _param6Fact                   (0, "Lon/Y:",                       FactMetaData::valueTypeDouble)
    , _param7Fact                   (0, "Alt/Z:",                       FactMetaData::valueTypeDouble)
    , _supportedCommandFact         (0, "Command:",                     FactMetaData::valueTypeUint32)
    , _param1MetaData(FactMetaData::valueTypeDouble)
    , _param2MetaData(FactMetaData::valueTypeDouble)
    , _param3MetaData(FactMetaData::valueTypeDouble)
    , _param4MetaData(FactMetaData::valueTypeDouble)
    , _syncingAltitudeRelativeToHomeAndFrame    (false)
    , _syncingHeadingDegreesAndParam4           (false)
    , _syncingSupportedCommandAndCommand        (false)
{
    _setupMetaData();
    _connectSignals();

    setCommand(command);
    setFrame(frame);
    setAutoContinue(autoContinue);

    _syncFrameToAltitudeRelativeToHome();
    _syncCommandToSupportedCommand(QVariant(this->command()));

    _param1Fact.setRawValue(param1);
    _param2Fact.setRawValue(param2);
    _param3Fact.setRawValue(param3);
    _param4Fact.setRawValue(param4);
    _param5Fact.setRawValue(param5);
    _param6Fact.setRawValue(param6);
    _param7Fact.setRawValue(param7);
}

MissionItem::MissionItem(const MissionItem& other, QObject* parent)
    : QObject(parent)
    , _rawEdit(false)
    , _dirty(false)
    , _sequenceNumber(0)
    , _isCurrentItem(false)
    , _azimuth(0.0)
    , _distance(0.0)
    , _homePositionSpecialCase(false)
    , _homePositionValid(false)
    , _altitudeRelativeToHomeFact   (0, "Altitude is relative to home", FactMetaData::valueTypeUint32)
    , _commandFact                  (0, "Command:",                     FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "Frame:",                       FactMetaData::valueTypeUint32)
    , _param1Fact                   (0, "Param1:",                      FactMetaData::valueTypeDouble)
    , _param2Fact                   (0, "Param2:",                      FactMetaData::valueTypeDouble)
    , _param3Fact                   (0, "Param3:",                      FactMetaData::valueTypeDouble)
    , _param4Fact                   (0, "Param4:",                      FactMetaData::valueTypeDouble)
    , _param5Fact                   (0, "Lat/X:",                       FactMetaData::valueTypeDouble)
    , _param6Fact                   (0, "Lon/Y:",                       FactMetaData::valueTypeDouble)
    , _param7Fact                   (0, "Alt/Z:",                       FactMetaData::valueTypeDouble)
    , _supportedCommandFact         (0, "Command:",                     FactMetaData::valueTypeUint32)
    , _param1MetaData(FactMetaData::valueTypeDouble)
    , _param2MetaData(FactMetaData::valueTypeDouble)
    , _param3MetaData(FactMetaData::valueTypeDouble)
    , _param4MetaData(FactMetaData::valueTypeDouble)
    , _syncingAltitudeRelativeToHomeAndFrame    (false)
    , _syncingHeadingDegreesAndParam4           (false)
    , _syncingSupportedCommandAndCommand        (false)
{
    _setupMetaData();
    _connectSignals();

    *this = other;
}

const MissionItem& MissionItem::operator=(const MissionItem& other)
{
    setCommand(other.command());
    setFrame(other.frame());
    setRawEdit(other._rawEdit);
    setDirty(other._dirty);
    setSequenceNumber(other._sequenceNumber);
    setAutoContinue(other.autoContinue());
    setIsCurrentItem(other._isCurrentItem);
    setAzimuth(other._distance);
    setDistance(other._distance);
    setHomePositionSpecialCase(other._homePositionSpecialCase);
    setHomePositionValid(other._homePositionValid);

    _syncFrameToAltitudeRelativeToHome();
    _syncCommandToSupportedCommand(QVariant(this->command()));

    _param1Fact.setRawValue(other._param1Fact.rawValue());
    _param2Fact.setRawValue(other._param2Fact.rawValue());
    _param3Fact.setRawValue(other._param3Fact.rawValue());
    _param4Fact.setRawValue(other._param4Fact.rawValue());
    _param5Fact.setRawValue(other._param5Fact.rawValue());
    _param6Fact.setRawValue(other._param6Fact.rawValue());
    _param7Fact.setRawValue(other._param7Fact.rawValue());

    return *this;
}

void MissionItem::_connectSignals(void)
{
    // Connect to change signals to track dirty state
    connect(&_param1Fact,   &Fact::valueChanged,                    this, &MissionItem::_setDirtyFromSignal);
    connect(&_param2Fact,   &Fact::valueChanged,                    this, &MissionItem::_setDirtyFromSignal);
    connect(&_param3Fact,   &Fact::valueChanged,                    this, &MissionItem::_setDirtyFromSignal);
    connect(&_param4Fact,   &Fact::valueChanged,                    this, &MissionItem::_setDirtyFromSignal);
    connect(&_param5Fact,   &Fact::valueChanged,                    this, &MissionItem::_setDirtyFromSignal);
    connect(&_param6Fact,   &Fact::valueChanged,                    this, &MissionItem::_setDirtyFromSignal);
    connect(&_param7Fact,   &Fact::valueChanged,                    this, &MissionItem::_setDirtyFromSignal);
    connect(&_frameFact,    &Fact::valueChanged,                    this, &MissionItem::_setDirtyFromSignal);
    connect(&_commandFact,  &Fact::valueChanged,                    this, &MissionItem::_setDirtyFromSignal);
    connect(this,           &MissionItem::sequenceNumberChanged,    this, &MissionItem::_setDirtyFromSignal);

    // Values from these facts must propogate back and forth between the real object storage
    connect(&_supportedCommandFact,         &Fact::valueChanged,        this, &MissionItem::_syncSupportedCommandToCommand);
    connect(&_commandFact,                  &Fact::valueChanged,        this, &MissionItem::_syncCommandToSupportedCommand);
    connect(&_altitudeRelativeToHomeFact,   &Fact::valueChanged,        this, &MissionItem::_syncAltitudeRelativeToHomeToFrame);
    connect(this,                           &MissionItem::frameChanged, this, &MissionItem::_syncFrameToAltitudeRelativeToHome);

    // These are parameter coordinates, they must emit coordinateChanged signal
    connect(&_param5Fact, &Fact::valueChanged, this, &MissionItem::_sendCoordinateChanged);
    connect(&_param6Fact, &Fact::valueChanged, this, &MissionItem::_sendCoordinateChanged);
    connect(&_param7Fact, &Fact::valueChanged, this, &MissionItem::_sendCoordinateChanged);

    // The following changes may also change friendlyEditAllowed
    connect(&_autoContinueFact, &Fact::valueChanged,        this, &MissionItem::_sendFriendlyEditAllowedChanged);
    connect(&_commandFact,      &Fact::valueChanged,        this, &MissionItem::_sendFriendlyEditAllowedChanged);
    connect(&_frameFact,        &Fact::valueChanged,        this, &MissionItem::_sendFriendlyEditAllowedChanged);

    // Whenever these properties change the ui model changes as well
    connect(this, &MissionItem::commandChanged, this, &MissionItem::_sendUiModelChanged);
    connect(this, &MissionItem::rawEditChanged, this, &MissionItem::_sendUiModelChanged);

    // These fact signals must alway signal out through MissionItem signals
    connect(&_commandFact,  &Fact::valueChanged, this, &MissionItem::_sendCommandChanged);
    connect(&_frameFact,    &Fact::valueChanged, this, &MissionItem::_sendFrameChanged);

    // When the command changes we need to set defaults
    connect(&_commandFact,  &Fact::valueChanged, this, &MissionItem::setDefaultsForCommand);
}

bool MissionItem::_validateKeyTypes(QJsonObject& jsonObject, const QStringList& keys, const QList<QJsonValue::Type>& types)
{
    for (int i=0; i<keys.count(); i++) {
        if (jsonObject.contains(keys[i])) {
            if (jsonObject.value(keys[i]).type() != types[i]) {
                qWarning() << "Incorrect type key:type:expected" << keys[i] << jsonObject.value(keys[i]).type() << types[i];
                return false;
            }
        }
    }

    return true;
}

bool MissionItem::_loadMavCmdInfoJson(void)
{
    QFile jsonFile(":/json/MavCmdInfo.json");
    if (!jsonFile.open(QIODevice::ReadOnly | QIODevice::Text)) {
        qWarning() << "Unable to open MavCmdInfo.json" << jsonFile.errorString();
        return false;
    }

    QByteArray bytes = jsonFile.readAll();
    jsonFile.close();
    QJsonParseError jsonParseError;
    QJsonDocument doc = QJsonDocument::fromJson(bytes, &jsonParseError);
    if (jsonParseError.error != QJsonParseError::NoError) {
        qWarning() <<  "Unable to open json document" << jsonParseError.errorString();
        return false;
    }

    QJsonObject json = doc.object();

    int version = json.value(_versionJsonKey).toInt();
    if (version != 1) {
        qWarning() << "Invalid version" << version;
        return false;
    }

    QJsonValue jsonValue = json.value(_mavCmdInfoJsonKey);
    if (!jsonValue.isArray()) {
        qWarning() << "mavCmdInfo not array";
        return false;
    }

    QJsonArray jsonArray = jsonValue.toArray();
    foreach(QJsonValue info, jsonArray) {
        if (!info.isObject()) {
            qWarning() << "mavCmdArray should contain objects";
            return false;
        }
        QJsonObject jsonObject = info.toObject();

        // Make sure we have the required keys
        QStringList requiredKeys;
        requiredKeys << _idJsonKey << _rawNameJsonKey;
        foreach (QString key, requiredKeys) {
            if (!jsonObject.contains(key)) {
                qWarning() << "Mission required key" << key;
                return false;
            }
        }

        // Validate key types

        QStringList             keys;
        QList<QJsonValue::Type> types;
        keys << _idJsonKey << _rawNameJsonKey << _friendlyNameJsonKey << _descriptionJsonKey << _specifiesCoordinateJsonKey << _friendlyEditJsonKey
             << _param1JsonKey << _param2JsonKey << _param3JsonKey << _param4JsonKey;
        types << QJsonValue::Double << QJsonValue::String << QJsonValue::String<< QJsonValue::String << QJsonValue::Bool << QJsonValue::Bool
              << QJsonValue::Object << QJsonValue::Object << QJsonValue::Object << QJsonValue::Object;
        if (!_validateKeyTypes(jsonObject, keys, types)) {
            return false;
        }

        MavCmdInfo_t mavCmdInfo;

        mavCmdInfo.command = (MAV_CMD)      jsonObject.value(_idJsonKey).toInt();
        mavCmdInfo.rawName =                jsonObject.value(_rawNameJsonKey).toString();
        mavCmdInfo.friendlyName =           jsonObject.value(_friendlyNameJsonKey).toString(QString());
        mavCmdInfo.description =            jsonObject.value(_descriptionJsonKey).toString(QString());
        mavCmdInfo.specifiesCoordinate =    jsonObject.value(_specifiesCoordinateJsonKey).toBool(false);
        mavCmdInfo.friendlyEdit =           jsonObject.value(_friendlyEditJsonKey).toBool(false);

        if (_mavCmdInfoMap.contains(mavCmdInfo.command)) {
            qWarning() << "Duplicate command" << mavCmdInfo.command;
            return false;
        }

        _mavCmdInfoMap[mavCmdInfo.command] = mavCmdInfo;

        // Read params

        for (int i=1; i<5; i++) {
            QString paramKey = QString(_paramJsonKeyFormat).arg(i);

            if (jsonObject.contains(paramKey)) {
                QJsonObject paramObject = jsonObject.value(paramKey).toObject();

                // Validate key types
                QStringList             keys;
                QList<QJsonValue::Type> types;
                keys << _labelJsonKey << _unitsJsonKey << _defaultJsonKey << _decimalPlacesJsonKey;
                types << QJsonValue::String << QJsonValue::String << QJsonValue::Double << QJsonValue::Double;
                if (!_validateKeyTypes(paramObject, keys, types)) {
                    return false;
                }

                if (paramObject.contains(_labelJsonKey)) {
                    _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].label = paramObject.value(_labelJsonKey).toString();
                } else {
                    qWarning() << "param object missing label key" << mavCmdInfo.rawName << paramKey;
                    return false;
                }

                _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].param =          i;
                _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].units =          paramObject.value(_unitsJsonKey).toString();
                _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].defaultValue =   paramObject.value(_defaultJsonKey).toDouble(0.0);
                _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].decimalPlaces =  paramObject.value(_decimalPlacesJsonKey).toInt(FactMetaData::defaultDecimalPlaces);
            }
        }
    }

    return true;
}

void MissionItem::_setupMetaData(void)
{
    QStringList enumStrings;
    QVariantList enumValues;

    if (!_altitudeMetaData) {
        _loadMavCmdInfoJson();

        _altitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble);
        _altitudeMetaData->setUnits("meters");
        _altitudeMetaData->setDecimalPlaces(3);

        enumStrings.clear();
        enumValues.clear();
        foreach (MavCmdInfo_t mavCmdInfo, _mavCmdInfoMap) {
            enumStrings.append(mavCmdInfo.rawName);
            enumValues.append(QVariant(mavCmdInfo.command));
        }
        _commandMetaData = new FactMetaData(FactMetaData::valueTypeUint32);
        _commandMetaData->setEnumInfo(enumStrings, enumValues);

        _defaultParamMetaData = new FactMetaData(FactMetaData::valueTypeDouble);
        _defaultParamMetaData->setDecimalPlaces(7);

        enumStrings.clear();
        enumValues.clear();
        for (size_t i=0; i<sizeof(_rgMavFrameInfo)/sizeof(_rgMavFrameInfo[0]); i++) {
            const struct EnumInfo_s* mavFrameInfo = &_rgMavFrameInfo[i];

            enumStrings.append(mavFrameInfo->label);
            enumValues.append(QVariant(mavFrameInfo->frame));
        }
        _frameMetaData = new FactMetaData(FactMetaData::valueTypeUint32);
        _frameMetaData->setEnumInfo(enumStrings, enumValues);

        _latitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble);
        _latitudeMetaData->setUnits("deg");
        _latitudeMetaData->setDecimalPlaces(7);

        _longitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble);
        _longitudeMetaData->setUnits("deg");
        _longitudeMetaData->setDecimalPlaces(7);

        enumStrings.clear();
        enumValues.clear();
        // FIXME: Hack hardcode tp PX4
        QList<MAV_CMD> supportedCommands = qgcApp()->toolbox()->firmwarePluginManager()->firmwarePluginForAutopilot(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR)->supportedMissionCommands();
        if (supportedCommands.count()) {
            foreach (MAV_CMD command, supportedCommands) {
                enumStrings.append(_mavCmdInfoMap[command].friendlyName);
                enumValues.append(QVariant(command));
            }
        } else {
            foreach (MavCmdInfo_t mavCmdInfo, _mavCmdInfoMap) {
                enumStrings.append(mavCmdInfo.friendlyName);
                enumValues.append(QVariant(mavCmdInfo.command));
            }
        }
        _supportedCommandMetaData = new FactMetaData(FactMetaData::valueTypeUint32);
        _supportedCommandMetaData->setEnumInfo(enumStrings, enumValues);
    }

    _commandFact.setMetaData(_commandMetaData);
    _frameFact.setMetaData(_frameMetaData);
    _supportedCommandFact.setMetaData(_supportedCommandMetaData);
}

MissionItem::~MissionItem()
{    
}

void MissionItem::save(QTextStream &saveStream)
{
    // FORMAT: <INDEX> <CURRENT WP> <COORD FRAME> <COMMAND> <PARAM1> <PARAM2> <PARAM3> <PARAM4> <PARAM5/X/LONGITUDE> <PARAM6/Y/LATITUDE> <PARAM7/Z/ALTITUDE> <autoContinue> <DESCRIPTION>
    // as documented here: http://qgroundcontrol.org/waypoint_protocol
    saveStream << sequenceNumber() << "\t"
               << isCurrentItem() << "\t"
               << frame() << "\t"
               << command() << "\t"
               << QString("%1").arg(param1(), 0, 'g', 18) << "\t"
               << QString("%1").arg(param2(), 0, 'g', 18) << "\t"
               << QString("%1").arg(param3(), 0, 'g', 18) << "\t"
               << QString("%1").arg(param4(), 0, 'g', 18) << "\t"
               << QString("%1").arg(param5(), 0, 'g', 18) << "\t"
               << QString("%1").arg(param6(), 0, 'g', 18) << "\t"
               << QString("%1").arg(param7(), 0, 'g', 18) << "\t"
               << this->autoContinue() << "\r\n";
}

bool MissionItem::load(QTextStream &loadStream)
{
    const QStringList &wpParams = loadStream.readLine().split("\t");
    if (wpParams.size() == 12) {
        setSequenceNumber(wpParams[0].toInt());
        setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false);
        setFrame((MAV_FRAME)wpParams[2].toInt());
        setCommand((MAV_CMD)wpParams[3].toInt());
        setParam1(wpParams[4].toDouble());
        setParam2(wpParams[5].toDouble());
        setParam3(wpParams[6].toDouble());
        setParam4(wpParams[7].toDouble());
        setParam5(wpParams[8].toDouble());
        setParam6(wpParams[9].toDouble());
        setParam7(wpParams[10].toDouble());
        setAutoContinue(wpParams[11].toInt() == 1 ? true : false);
        return true;
    }
    return false;
}


void MissionItem::setSequenceNumber(int sequenceNumber)
{
    _sequenceNumber = sequenceNumber;
    emit sequenceNumberChanged(_sequenceNumber);
}

void MissionItem::setCommand(MAV_CMD command)
{
    if ((MAV_CMD)this->command() != command) {
        _commandFact.setRawValue(command);
        setDefaultsForCommand();
        emit commandChanged(this->command());
    }
}

void MissionItem::setCommand(MavlinkQmlSingleton::Qml_MAV_CMD command)
{
    setCommand((MAV_CMD)command);
}


void MissionItem::setFrame(MAV_FRAME frame)
{
    if (this->frame() != frame) {
        _frameFact.setRawValue(frame);
        frameChanged(frame);
    }
}

void MissionItem::setAutoContinue(bool autoContinue)
{
    if (this->autoContinue() != autoContinue) {
        _autoContinueFact.setRawValue(autoContinue);
    }
}

void MissionItem::setIsCurrentItem(bool isCurrentItem)
{
    if (_isCurrentItem != isCurrentItem) {
        _isCurrentItem = isCurrentItem;
        emit isCurrentItemChanged(isCurrentItem);
    }
}

void MissionItem::setParam1(double param)
{
    if (param1() != param) {
        _param1Fact.setRawValue(param);
    }
}

void MissionItem::setParam2(double param)
{
    if (param2() != param) {
        _param2Fact.setRawValue(param);
    }
}

void MissionItem::setParam3(double param)
{
    if (param3() != param) {
        _param3Fact.setRawValue(param);
    }
}

void MissionItem::setParam4(double param)
{
    if (param4() != param) {
        _param4Fact.setRawValue(param);
    }
}

void MissionItem::setParam5(double param)
{
    if (param5() != param) {
        _param5Fact.setRawValue(param);
    }
}

void MissionItem::setParam6(double param)
{
    if (param6() != param) {
        _param6Fact.setRawValue(param);
    }
}

void MissionItem::setParam7(double param)
{
    if (param7() != param) {
        _param7Fact.setRawValue(param);
    }
}

bool MissionItem::specifiesCoordinate(void) const
{
    return _mavCmdInfoMap[(MAV_CMD)command()].specifiesCoordinate;
}

QString MissionItem::commandDescription(void) const
{
    return _mavCmdInfoMap[(MAV_CMD)command()].description;
}

void MissionItem::_clearParamMetaData(void)
{
    _param1MetaData.setUnits("");
    _param1MetaData.setDecimalPlaces(FactMetaData::defaultDecimalPlaces);
    _param1MetaData.setTranslators(FactMetaData::defaultTranslator, FactMetaData::defaultTranslator);
    _param2MetaData.setUnits("");
    _param2MetaData.setDecimalPlaces(FactMetaData::defaultDecimalPlaces);
    _param2MetaData.setTranslators(FactMetaData::defaultTranslator, FactMetaData::defaultTranslator);
    _param3MetaData.setUnits("");
    _param3MetaData.setDecimalPlaces(FactMetaData::defaultDecimalPlaces);
    _param3MetaData.setTranslators(FactMetaData::defaultTranslator, FactMetaData::defaultTranslator);
    _param4MetaData.setUnits("");
    _param4MetaData.setDecimalPlaces(FactMetaData::defaultDecimalPlaces);
    _param4MetaData.setTranslators(FactMetaData::defaultTranslator, FactMetaData::defaultTranslator);
}

QmlObjectListModel* MissionItem::textFieldFacts(void)
{
    QmlObjectListModel* model = new QmlObjectListModel(this);
    
    if (rawEdit()) {
        _param1Fact._setName("Param1:");
        _param1Fact.setMetaData(_defaultParamMetaData);
        model->append(&_param1Fact);
        _param2Fact._setName("Param2:");
        _param2Fact.setMetaData(_defaultParamMetaData);
        model->append(&_param2Fact);
        _param3Fact._setName("Param3:");
        _param3Fact.setMetaData(_defaultParamMetaData);
        model->append(&_param3Fact);
        _param4Fact._setName("Param4:");
        _param4Fact.setMetaData(_defaultParamMetaData);
        model->append(&_param4Fact);
        _param5Fact._setName("Lat/X:");
        _param5Fact.setMetaData(_defaultParamMetaData);
        model->append(&_param5Fact);
        _param6Fact._setName("Lon/Y:");
        _param6Fact.setMetaData(_defaultParamMetaData);
        model->append(&_param6Fact);
        _param7Fact._setName("Alt/Z:");
        _param7Fact.setMetaData(_defaultParamMetaData);
        model->append(&_param7Fact);
    } else {
        _clearParamMetaData();

        MAV_CMD command = (MAV_CMD)this->command();

        if (_mavCmdInfoMap[command].paramInfoMap.contains(1)) {
            _param1Fact._setName(_mavCmdInfoMap[command].paramInfoMap[1].label);
            _param1MetaData.setUnits(_mavCmdInfoMap[command].paramInfoMap[1].units);
            _param1MetaData.setDecimalPlaces(_mavCmdInfoMap[command].paramInfoMap[1].decimalPlaces);
            if (_mavCmdInfoMap[command].paramInfoMap[1].units == _degreesUnits) {
                _param1MetaData.setTranslators(_radiansToDegrees, _degreesToRadians);
            }
            _param1Fact.setMetaData(&_param1MetaData);
            model->append(&_param1Fact);
        }

        if (_mavCmdInfoMap[command].paramInfoMap.contains(2)) {
            _param2Fact._setName(_mavCmdInfoMap[command].paramInfoMap[2].label);
            _param2MetaData.setUnits(_mavCmdInfoMap[command].paramInfoMap[2].units);
            _param2MetaData.setDecimalPlaces(_mavCmdInfoMap[command].paramInfoMap[2].decimalPlaces);
            if (_mavCmdInfoMap[command].paramInfoMap[2].units == _degreesUnits) {
                _param2MetaData.setTranslators(_radiansToDegrees, _degreesToRadians);
            }
            _param2Fact.setMetaData(&_param2MetaData);
            model->append(&_param2Fact);
        }

        if (_mavCmdInfoMap[command].paramInfoMap.contains(3)) {
            _param3Fact._setName(_mavCmdInfoMap[command].paramInfoMap[3].label);
            _param3MetaData.setUnits(_mavCmdInfoMap[command].paramInfoMap[3].units);
            _param3MetaData.setDecimalPlaces(_mavCmdInfoMap[command].paramInfoMap[3].decimalPlaces);
            if (_mavCmdInfoMap[command].paramInfoMap[3].units == _degreesUnits) {
                _param3MetaData.setTranslators(_radiansToDegrees, _degreesToRadians);
            }
            _param3Fact.setMetaData(&_param3MetaData);
            model->append(&_param3Fact);
        }

        if (_mavCmdInfoMap[command].paramInfoMap.contains(4)) {
            _param4Fact._setName(_mavCmdInfoMap[command].paramInfoMap[4].label);
            _param4MetaData.setUnits(_mavCmdInfoMap[command].paramInfoMap[4].units);
            _param4MetaData.setDecimalPlaces(_mavCmdInfoMap[command].paramInfoMap[4].decimalPlaces);
            if (_mavCmdInfoMap[command].paramInfoMap[4].units == _degreesUnits) {
                _param4MetaData.setTranslators(_radiansToDegrees, _degreesToRadians);
            }
            _param4Fact.setMetaData(&_param4MetaData);
            model->append(&_param4Fact);
        }

        if (specifiesCoordinate()) {
            _param7Fact._setName("Altitude:");
            _param7Fact.setMetaData(_altitudeMetaData);
            model->append(&_param7Fact);
        }
    }
    
    return model;
}

QmlObjectListModel* MissionItem::checkboxFacts(void)
{
    QmlObjectListModel* model = new QmlObjectListModel(this);
    

    if (rawEdit()) {
        model->append(&_autoContinueFact);
    } else if (specifiesCoordinate()) {
        model->append(&_altitudeRelativeToHomeFact);
    }

    return model;
}

QmlObjectListModel* MissionItem::comboboxFacts(void)
{
    QmlObjectListModel* model = new QmlObjectListModel(this);

    if (rawEdit()) {
        model->append(&_commandFact);
        model->append(&_frameFact);
    }

    return model;
}

QGeoCoordinate MissionItem::coordinate(void) const
{
    return QGeoCoordinate(_param5Fact.rawValue().toDouble(), _param6Fact.rawValue().toDouble(), _param7Fact.rawValue().toDouble());
}

void MissionItem::setCoordinate(const QGeoCoordinate& coordinate)
{
    setParam5(coordinate.latitude());
    setParam6(coordinate.longitude());
    setParam7(coordinate.altitude());
}

bool MissionItem::friendlyEditAllowed(void) const
{
    if (_mavCmdInfoMap[(MAV_CMD)command()].friendlyEdit) {
        if (!autoContinue()) {
            return false;
        }

        if (specifiesCoordinate()) {
            return frame() == MAV_FRAME_GLOBAL || frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT;
        } else {
            return frame() == MAV_FRAME_MISSION;
        }
    }

    return false;
}

bool MissionItem::rawEdit(void) const
{
    return _rawEdit || !friendlyEditAllowed();
}

void MissionItem::setRawEdit(bool rawEdit)
{
    if (_rawEdit != rawEdit) {
        _rawEdit = rawEdit;
        emit rawEditChanged(this->rawEdit());
    }
}

void MissionItem::setDirty(bool dirty)
{
    if (!_homePositionSpecialCase || !dirty) {
        // Home position never affects dirty bit

        _dirty = dirty;
        // We want to emit dirtyChanged even if _dirty didn't change. This can be handy signal for
        // any value within the item changing.
        emit dirtyChanged(_dirty);
    }
}

void MissionItem::_setDirtyFromSignal(void)
{
    setDirty(true);
}

void MissionItem::setHomePositionValid(bool homePositionValid)
{
    _homePositionValid = homePositionValid;
    emit homePositionValidChanged(_homePositionValid);
}

void MissionItem::setDistance(double distance)
{
    _distance = distance;
    emit distanceChanged(_distance);
}

void MissionItem::setAzimuth(double azimuth)
{
    _azimuth = azimuth;
    emit azimuthChanged(_azimuth);
}

void MissionItem::_sendCoordinateChanged(void)
{
    emit coordinateChanged(coordinate());
}

void MissionItem::_syncAltitudeRelativeToHomeToFrame(const QVariant& value)
{
    if (!_syncingAltitudeRelativeToHomeAndFrame) {
        _syncingAltitudeRelativeToHomeAndFrame = true;
        setFrame(value.toBool() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL);
        _syncingAltitudeRelativeToHomeAndFrame = false;
    }
}

void MissionItem::_syncFrameToAltitudeRelativeToHome(void)
{
    if (!_syncingAltitudeRelativeToHomeAndFrame) {
        _syncingAltitudeRelativeToHomeAndFrame = true;
        _altitudeRelativeToHomeFact.setRawValue(relativeAltitude());
        _syncingAltitudeRelativeToHomeAndFrame = false;
    }
}

void MissionItem::_syncSupportedCommandToCommand(const QVariant& value)
{
    if (!_syncingSupportedCommandAndCommand) {
        _syncingSupportedCommandAndCommand = true;
        _commandFact.setRawValue(value.toInt());
        _syncingSupportedCommandAndCommand = false;
    }
}

void MissionItem::_syncCommandToSupportedCommand(const QVariant& value)
{
    if (!_syncingSupportedCommandAndCommand) {
        _syncingSupportedCommandAndCommand = true;
        _supportedCommandFact.setRawValue(value.toInt());
        _syncingSupportedCommandAndCommand = false;
    }
}

void MissionItem::setDefaultsForCommand(void)
{
    foreach (ParamInfo_t paramInfo, _mavCmdInfoMap[(MAV_CMD)command()].paramInfoMap) {
        double defaultValue = paramInfo.defaultValue;

        switch (paramInfo.param) {
            case 1:
                _param1Fact.setRawValue(defaultValue);
                break;
            case 2:
                _param2Fact.setRawValue(defaultValue);
                break;
            case 3:
                _param3Fact.setRawValue(defaultValue);
                break;
            case 4:
                _param4Fact.setRawValue(defaultValue);
                break;
        }
    }

    setParam7(defaultAltitude);
    setAutoContinue(true);
    setFrame(specifiesCoordinate() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_MISSION);
    setRawEdit(false);
}

void MissionItem::_sendUiModelChanged(void)
{
    emit uiModelChanged();
}

void MissionItem::_sendFrameChanged(void)
{
    emit frameChanged(frame());
}

void MissionItem::_sendCommandChanged(void)
{
    emit commandChanged(command());
}

QString MissionItem::commandName(void) const
{
    MavCmdInfo_t& mavCmdInfo = _mavCmdInfoMap[(MAV_CMD)command()];

    return mavCmdInfo.friendlyName.isEmpty() ? mavCmdInfo.rawName : mavCmdInfo.friendlyName;
}

QVariant MissionItem::_degreesToRadians(const QVariant& degrees)
{
    return QVariant(degrees.toDouble() * (M_PI / 180.0));
}

QVariant MissionItem::_radiansToDegrees(const QVariant& radians)
{
    return QVariant(radians.toDouble() * (180 / M_PI));
}

void MissionItem::_sendFriendlyEditAllowedChanged(void)
{
    emit friendlyEditAllowedChanged(friendlyEditAllowed());
}