void VehicleTemplateExportDialog::importTemplate()
{
    QJsonObject *tmpl = ui->selectionWidget->selectedTemplate();

    if (tmpl != NULL) {
        QList<UAVObject *> updatedObjects;
        m_uavoManager->fromJson(*tmpl, &updatedObjects);
        UAVObjectUpdaterHelper helper;
        foreach(UAVObject * object, updatedObjects) {
            UAVDataObject *dataObj = dynamic_cast<UAVDataObject *>(object);

            if (dataObj != NULL && dataObj->isKnown()) {
                helper.doObjectAndWait(dataObj);

                ObjectPersistence *objper = ObjectPersistence::GetInstance(m_uavoManager);
                ObjectPersistence::DataFields data;
                data.Operation  = ObjectPersistence::OPERATION_SAVE;
                data.Selection  = ObjectPersistence::SELECTION_SINGLEOBJECT;
                data.ObjectID   = dataObj->getObjID();
                data.InstanceID = dataObj->getInstID();
                objper->setData(data);

                helper.doObjectAndWait(objper);
            }
        }

void ConfigStabilizationWidget::resetThrottleCurveToDefault()
{
    UAVDataObject *defaultStabBank = (UAVDataObject *)getObjectManager()->getObject(QString(m_stabTabBars.at(0)->tabData(m_currentStabSettingsBank).toString()));

    Q_ASSERT(defaultStabBank);
    defaultStabBank = defaultStabBank->dirtyClone();

    UAVObjectField *field = defaultStabBank->getField("ThrustPIDScaleCurve");
    Q_ASSERT(field);

    QList<double> curve;
    for (quint32 i = 0; i < field->getNumElements(); i++) {
        curve.append(field->getValue(i).toDouble());
    }

    ui->thrustPIDScalingCurve->setCurve(&curve);

    field = defaultStabBank->getField("EnableThrustPIDScaling");
    Q_ASSERT(field);

    bool enabled = field->getValue() == "TRUE";
    ui->enableThrustPIDScalingCheckBox->setChecked(enabled);
    ui->thrustPIDScalingCurve->setEnabled(enabled);

    delete defaultStabBank;
}

/**
   Sends the config to the board (airframe type)

   We do all the tasks common to all airframes, or family of airframes, and
   we call additional methods for specific frames, so that we do not have a code
   that is too heavy.

   Note: The default behavior of ConfigTaskWidget is bypassed.
   Therefore no automatic synchronization of UI to UAV Objects is done.
 */
void ConfigVehicleTypeWidget::updateObjectsFromWidgets()
{
    // Airframe type defaults to Custom
    QString airframeType = "Custom";

    VehicleConfig *vehicleConfig = (VehicleConfig *)m_aircraft->airframesWidget->currentWidget();

    if (vehicleConfig) {
        airframeType = vehicleConfig->updateConfigObjectsFromWidgets();
    }

    // set the airframe type
    UAVDataObject *system = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(QString("SystemSettings")));
    Q_ASSERT(system);

    QPointer<UAVObjectField> field = system->getField(QString("AirframeType"));
    if (field) {
        field->setValue(airframeType);
    }

    // Update feed forward settings
    UAVDataObject *mixer = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(QString("MixerSettings")));
    Q_ASSERT(mixer);

    QPointer<VehicleConfig> vconfig = new VehicleConfig();

    vconfig->setMixerValue(mixer, "FeedForward", m_aircraft->feedForwardSlider->value() / 100.0);
    vconfig->setMixerValue(mixer, "AccelTime", m_aircraft->accelTime->value());
    vconfig->setMixerValue(mixer, "DecelTime", m_aircraft->decelTime->value());
    vconfig->setMixerValue(mixer, "MaxAccel", m_aircraft->maxAccelSlider->value());

    // TODO call refreshWidgetsValues() to reflect actual saved values ?
    updateFeedForwardUI();
}

/**
   Refreshes the current value of the SystemSettings which holds the aircraft type
   Note: The default behavior of ConfigTaskWidget is bypassed.
   Therefore no automatic synchronization of UAV Objects to UI is done.
 */
void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject *o)
{
    Q_UNUSED(o);

    if (!allObjectsUpdated()) {
        return;
    }

    bool dirty = isDirty();

    // Get the Airframe type from the system settings:
    UAVDataObject *system = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(QString("SystemSettings")));
    Q_ASSERT(system);

    UAVObjectField *field = system->getField(QString("AirframeType"));
    Q_ASSERT(field);

    // At this stage, we will need to have some hardcoded settings in this code
    QString frameType = field->getValue().toString();

    // Always update custom tab from others airframe settings : debug/learn hardcoded mixers
    int category = frameCategory("Custom");
    m_aircraft->aircraftType->setCurrentIndex(category);

    VehicleConfig *vehicleConfig = getVehicleConfigWidget(category);

    if (vehicleConfig) {
        vehicleConfig->refreshWidgetsValues("Custom");
    }

    // Switch to Airframe currently used
    category = frameCategory(frameType);

    if (frameType != "Custom") {
        m_aircraft->aircraftType->setCurrentIndex(category);

        VehicleConfig *vehicleConfig = getVehicleConfigWidget(category);

        if (vehicleConfig) {
            vehicleConfig->refreshWidgetsValues(frameType);
        }
    }

    field = system->getField(QString("VehicleName"));
    Q_ASSERT(field);
    QString name;
    for (uint i = 0; i < field->getNumElements(); ++i) {
        QChar chr = field->getValue(i).toChar();
        if (chr != 0) {
            name.append(chr);
        } else {
            break;
        }
    }
    m_aircraft->nameEdit->setText(name);

    updateFeedForwardUI();

    setDirty(dirty);
}

/**
 * Initiate object retrieval, initialize queue with objects to be retrieved.
 */
void TelemetryMonitor::startRetrievingObjects()
{
    // Clear object queue
    queue.clear();
    // Get all objects, add metaobjects, settings and data objects with OnChange update mode to the queue
    QList< QList<UAVObject *> > objs = objMngr->getObjects();
    for (int n = 0; n < objs.length(); ++n) {
        UAVObject *obj = objs[n][0];
        UAVMetaObject *mobj = dynamic_cast<UAVMetaObject *>(obj);
        UAVDataObject *dobj = dynamic_cast<UAVDataObject *>(obj);
        UAVObject::Metadata mdata = obj->getMetadata();
        if (mobj != NULL) {
            queue.enqueue(obj);
        } else if (dobj != NULL) {
            if (dobj->isSettings()) {
                queue.enqueue(obj);
            } else {
                if (UAVObject::GetFlightTelemetryUpdateMode(mdata) == UAVObject::UPDATEMODE_ONCHANGE) {
                    queue.enqueue(obj);
                }
            }
        }
    }
    // Start retrieving
    qDebug() << tr("Starting to retrieve meta and settings objects from the autopilot (%1 objects)")
        .arg(queue.length());
    retrieveNextObject();
}

/*!
  \brief Called when the gcs control is toggled and enabled or disables flight write access to manual control command
  */
void GCSControlGadgetWidget::toggleControl(int state)
{
    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
    UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );

    UAVObject::Metadata mdata = obj->getMetadata();
    if (state)
    {
        mccInitialData = mdata;
        UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY);
        UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
        UAVObject::SetGcsTelemetryAcked(mdata, false);
        UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
        mdata.gcsTelemetryUpdatePeriod = 100;
        m_gcscontrol->checkBoxUDPControl->setEnabled(true);

    }
    else
    {
        mdata = mccInitialData;
        toggleUDPControl(false);
        m_gcscontrol->checkBoxUDPControl->setEnabled(false);
    }
    obj->setMetadata(mdata);
}

void LevellingUtil::stopMeasurement()
{
    m_isMeasuring = false;

    //Stop timeout timer
    m_timeoutTimer.stop();
    disconnect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout()));

    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
    UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
    Q_ASSERT(uavObjectManager);

    // Stop listening for updates from accels
    UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager);
    disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*)));
    uavObject->setMetadata(m_previousAccelMetaData);

    // Stop listening for updates from gyros
    uavObject = Gyros::GetInstance(uavObjectManager);
    disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*)));
    uavObject->setMetadata(m_previousGyroMetaData);

    // Enable gyro bias correction again
    AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
    attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
    AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
}

void GCSControlGadgetWidget::mccChanged(UAVObject * obj)
{
    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
    UAVDataObject* flightStatus = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("FlightStatus")) );
    m_gcscontrol->comboBoxFlightMode->setCurrentIndex(m_gcscontrol->comboBoxFlightMode->findText(flightStatus->getField("FlightMode")->getValue().toString()));
    m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed");
}

/*!
  \brief Called when the flight mode drop down is changed and sets the ManualControlCommand->FlightMode accordingly
  */
void GCSControlGadgetWidget::selectFlightMode(int state)
{
    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
    UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("FlightStatus")) );
    UAVObjectField * field = obj->getField("FlightMode");
    field->setValue(field->getOptions()[state]);
    obj->updated();
}

void objUpdatedManual(UAVObject *obj)
{
	boost::recursive_timed_mutex::scoped_lock lock(mutex);
	upmanual++;
	std::cout << "[Manual] " << obj->toString();

	UAVDataObject *dobj = dynamic_cast<UAVDataObject *>(obj);
	if (dobj)
		std::cout << dobj->toStringData();
}

void updRequested(UAVObject *obj)
{
	boost::recursive_timed_mutex::scoped_lock lock(mutex);
	upreq++;
	std::cout << "[Object Requested] " << obj->toString();

	UAVDataObject *dobj = dynamic_cast<UAVDataObject *>(obj);
	if (dobj)
		std::cout << dobj->toStringData();
}

void newInstance(UAVObject *obj)
{
	boost::recursive_timed_mutex::scoped_lock lock(mutex);
	newinst++;
	std::cout << "[NEW Instance] " << obj->toString();

	UAVDataObject *dobj = dynamic_cast<UAVDataObject *>(obj);
	if (dobj)
		std::cout << dobj->toStringData();
}

void ExtendedDebugLogEntry::setData(const DebugLogEntry::DataFields &data, UAVObjectManager *objectManager)
{
    DebugLogEntry::setData(data);

    if (getType() == DebugLogEntry::TYPE_UAVOBJECT) {
        UAVDataObject *object = (UAVDataObject *)objectManager->getObject(getObjectID(), getInstanceID());
        Q_ASSERT(object);
        m_object = object->clone(getInstanceID());
        m_object->unpack(getData().Data);
    }
}

void bind_objUpdated(UAVObject *obj)
{
	boost::recursive_timed_mutex::scoped_lock lock(mutex);
	bind_updated++;
	std::cout << "[BIND Updated] " << obj->toString();

	UAVDataObject *dobj = dynamic_cast<UAVDataObject *>(obj);
	if (dobj)
		std::cout << dobj->toStringData();

}

void GCSControlGadgetWidget::toggleArmed(int state)
{
    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
    UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("FlightStatus")) );
    if(state)
        obj->getField("Armed")->setValue("Armed");
    else
        obj->getField("Armed")->setValue("Disarmed");
    obj->updated();
}

void LevellingUtil::startMeasurement()
{
    QMutexLocker locker(&m_measurementMutex);

    m_isMeasuring = true;

    // Reset variables
    m_receivedAccelUpdates = 0;
    m_accelerometerX = 0;
    m_accelerometerY = 0;
    m_accelerometerZ = 0;

    m_receivedGyroUpdates = 0;
    m_gyroX = 0;
    m_gyroY = 0;
    m_gyroZ = 0;

    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
    UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
    Q_ASSERT(uavObjectManager);

    // Disable gyro bias correction to see raw data
    AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
    attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
    AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);

    // Set up to receive updates for accels
    UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager);
    connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*)));

    // Set update period for accels
    m_previousAccelMetaData = uavObject->getMetadata();
    UAVObject::Metadata newMetaData = m_previousAccelMetaData;
    UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
    newMetaData.flightTelemetryUpdatePeriod = m_accelMeasurementRate;
    uavObject->setMetadata(newMetaData);

    // Set up to receive updates from gyros
    uavObject = Gyros::GetInstance(uavObjectManager);
    connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*)));

    // Set update period for gyros
    m_previousGyroMetaData = uavObject->getMetadata();
    newMetaData = m_previousGyroMetaData;
    UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
    newMetaData.flightTelemetryUpdatePeriod = m_gyroMeasurementRate;
    uavObject->setMetadata(newMetaData);
}

/**
   Sends the config to the board (airframe type)

   We do all the tasks common to all airframes, or family of airframes, and
   we call additional methods for specific frames, so that we do not have a code
   that is too heavy.

   Note: The default behavior of ConfigTaskWidget is bypassed.
   Therefore no automatic synchronization of UI to UAV Objects is done.
 */
void ConfigVehicleTypeWidget::updateObjectsFromWidgets()
{
    // Airframe type defaults to Custom
    QString airframeType = "Custom";

    VehicleConfig *vehicleConfig = (VehicleConfig *)m_aircraft->airframesWidget->currentWidget();

    if (vehicleConfig) {
        airframeType = vehicleConfig->updateConfigObjectsFromWidgets();
    }

    // set the airframe type
    UAVDataObject *system = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(QString("SystemSettings")));
    Q_ASSERT(system);

    QPointer<UAVObjectField> field = system->getField(QString("AirframeType"));
    if (field) {
        field->setValue(airframeType);
    }

    // Update feed forward settings
    UAVDataObject *mixer = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(QString("MixerSettings")));
    Q_ASSERT(mixer);

    QPointer<VehicleConfig> vconfig = new VehicleConfig();

    vconfig->setMixerValue(mixer, "FeedForward", m_aircraft->feedForwardSlider->value() / 100.0);
    vconfig->setMixerValue(mixer, "AccelTime", m_aircraft->accelTime->value());
    vconfig->setMixerValue(mixer, "DecelTime", m_aircraft->decelTime->value());
    vconfig->setMixerValue(mixer, "MaxAccel", m_aircraft->maxAccelSlider->value());

    field = system->getField(QString("VehicleName"));
    Q_ASSERT(field);
    QString name = m_aircraft->nameEdit->text();
    for (uint i = 0; i < field->getNumElements(); ++i) {
        if (i < (uint)name.length()) {
            field->setValue(name.at(i).toLatin1(), i);
        } else {
            field->setValue(0, i);
        }
    }

    // call refreshWidgetsValues() to reflect actual saved values
    refreshWidgetsValues();
    updateFeedForwardUI();
}

/**
  Sends the channel value to the UAV to move the servo.
  Returns immediately if we are not in testing mode
  */
void ConfigOutputWidget::sendChannelTest(int value)
{
	int in_value = value;

	QSlider *ob = (QSlider *)QObject::sender();
	if (!ob) return;
    int index = outSliders.indexOf(ob);
	if (index < 0) return;

	if (reversals[index]->isChecked())
		value = outMin[index]->value() - value + outMax[index]->value();	// the chsnnel is reversed

	// update the label
	outLabels[index]->setText(QString::number(value));

	if (links[index]->checkState())
	{	// the channel is linked to other channels
		// set the linked channels to the same value
		for (int i = 0; i < outSliders.count(); i++)
		{
			if (i == index) continue;
			if (!links[i]->checkState()) continue;

			int val = in_value;
			if (val < outSliders[i]->minimum()) val = outSliders[i]->minimum();
			if (val > outSliders[i]->maximum()) val = outSliders[i]->maximum();

			if (outSliders[i]->value() == val) continue;

			outSliders[i]->setValue(val);
			outLabels[i]->setText(QString::number(val));
		}
	}

	if (!m_config->channelOutTest->isChecked())
		return;

	UAVDataObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("ActuatorCommand")));
	if (!obj) return;
	UAVObjectField *channel = obj->getField("Channel");
	if (!channel) return;
	channel->setValue(value, index);
    obj->updated();
}

GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
{
    m_gcscontrol = new Ui_GCSControl();
    m_gcscontrol->setupUi(this);


    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
    UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );
    UAVObject::Metadata mdata = obj->getMetadata();
    m_gcscontrol->checkBoxGcsControl->setChecked(UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READONLY);

    // Set up the drop down box for the flightmode
    UAVDataObject* flightStatus = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("FlightStatus")) );
    m_gcscontrol->comboBoxFlightMode->addItems(flightStatus->getField("FlightMode")->getOptions());

    // Set up slots and signals for joysticks
    connect(m_gcscontrol->widgetLeftStick,SIGNAL(positionClicked(double,double)),this,SLOT(leftStickClicked(double,double)));
    connect(m_gcscontrol->widgetRightStick,SIGNAL(positionClicked(double,double)),this,SLOT(rightStickClicked(double,double)));

    // Connect misc controls
    connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(toggleControl(int)));
    connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
    connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int)));

    connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)),this,SLOT(toggleUDPControl(int))); //UDP control checkbox

    // Connect object updated event from UAVObject to also update check boxes and dropdown
    connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*)));




    leftX = 0;
    leftY = 0;
    rightX = 0;
    rightY = 0;

    // No point enabling OpenGL for the joysticks, and causes
    // issues on some computers:
//    m_gcscontrol->widgetLeftStick->enableOpenGL(true);
//    m_gcscontrol->widgetRightStick->enableOpenGL(true);
}

/**
 * Initialize queue with settings objects to be retrieved.
 */
void LoggingThread::retrieveSettings()
{
    // Clear object queue
    queue.clear();
    // Get all objects, add metaobjects, settings and data objects with OnChange update mode to the queue
    // Get UAVObjectManager instance
    ExtensionSystem::PluginManager *pm   = ExtensionSystem::PluginManager::instance();
    UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
    QList< QList<UAVDataObject *> > objs = objMngr->getDataObjects();
    for (int n = 0; n < objs.length(); ++n) {
        UAVDataObject *obj = objs[n][0];
        if (obj->isSettingsObject()) {
            queue.enqueue(obj);
        }
    }
    // Start retrieving
    qDebug() << tr("Logging: retrieve settings objects from the autopilot (%1 objects)")
        .arg(queue.length());
    retrieveNextObject();
}