#include "configuration.h"

#if !MESHTASTIC_EXCLUDE_AIR_QUALITY_SENSOR && __has_include(<SensirionI2cScd4x.h>)

#include "../mesh/generated/meshtastic/telemetry.pb.h"
#include "SCD4XSensor.h"

#define SCD4X_NO_ERROR 0

SCD4XSensor::SCD4XSensor() : TelemetrySensor(meshtastic_TelemetrySensorType_SCD4X, "SCD4X") {}

bool SCD4XSensor::initDevice(TwoWire *bus, ScanI2C::FoundDevice *dev)
{
    LOG_INFO("Init sensor: %s", sensorName);

    _bus = bus;
    _address = dev->address.address;

#ifdef SCD4X_I2C_CLOCK_SPEED
    _port = dev->address.port;
    reClockI2C.setup(_bus, _port);
    LOG_INFO("%s: reclock speed %uHz", sensorName, SCD4X_I2C_CLOCK_SPEED);
    ReClockI2CGuard clockGuard(reClockI2C, SCD4X_I2C_CLOCK_SPEED);
#endif /* SCD4X_I2C_CLOCK_SPEED */

    scd4x.begin(*_bus, _address);

    // From SCD4X library
    delay(30);

    // Stop periodic measurement
    if (!stopMeasurement()) {
        return false;
    }

    // Get sensor variant
    scd4x.getSensorVariant(sensorVariant);

    if (sensorVariant == SCD4X_SENSOR_VARIANT_SCD41) {
        LOG_INFO("%s: Found SCD41", sensorName);
        if (!powerUp()) {
            LOG_ERROR("%s: Error trying to execute powerUp()", sensorName);
            return false;
        }
    }

    if (!getASC(ascActive)) {
        LOG_ERROR("%s: Unable to check if ASC is enabled", sensorName);
        return false;
    }

    // Start measurement in selected power mode (low power by default)
    if (!startMeasurement()) {
        LOG_ERROR("%s: Couldn't start measurement", sensorName);
        return false;
    }

    if (state == SCD4X_MEASUREMENT) {
        status = 1;
    } else {
        status = 0;
    }

    initI2CSensor();

    return true;
}

bool SCD4XSensor::getMetrics(meshtastic_Telemetry *measurement)
{

    if (state != SCD4X_MEASUREMENT) {
        LOG_ERROR("%s: Not in measurement mode", sensorName);
        return false;
    }

    uint16_t co2, error;
    float temperature, humidity;

#ifdef SCD4X_I2C_CLOCK_SPEED
    LOG_DEBUG("%s: reclock speed %uHz", sensorName, SCD4X_I2C_CLOCK_SPEED);
    ReClockI2CGuard clockGuard(reClockI2C, SCD4X_I2C_CLOCK_SPEED);
#endif /* SCD4X_I2C_CLOCK_SPEED */

    bool dataReady = false;
    uint8_t dataReadyTries = 0;

    while (!dataReady && (dataReadyTries < SCD4X_MAX_RETRIES)) {
        error = scd4x.getDataReadyStatus(dataReady);
        if (error != SCD4X_NO_ERROR || !dataReady) {
            LOG_WARN("%s: Error collecting data. Retrying", sensorName);
            delay(100);
            dataReadyTries++;
        }
    }

    if (error != SCD4X_NO_ERROR || !dataReady) {
        LOG_ERROR("SCD4X: Data is not ready");
        return false;
    }

    error = scd4x.readMeasurement(co2, temperature, humidity);

    LOG_DEBUG("Got %s readings: co2=%u, co2_temp=%.2f, co2_hum%.2f", sensorName, co2, temperature, humidity);
    if (error != SCD4X_NO_ERROR) {
        LOG_DEBUG("%s: Error getting measurements: %u", sensorName, error);
        if (co2 == 0) {
            LOG_ERROR("%s: Skipping invalid measurement", sensorName);
        }
        return false;
    } else {
        measurement->variant.air_quality_metrics.has_co2_temperature = true;
        measurement->variant.air_quality_metrics.has_co2_humidity = true;
        measurement->variant.air_quality_metrics.has_co2 = true;
        measurement->variant.air_quality_metrics.co2_temperature = temperature;
        measurement->variant.air_quality_metrics.co2_humidity = humidity;
        measurement->variant.air_quality_metrics.co2 = co2;
        return true;
    }
}

/**
 * @brief Perform a forced recalibration (FRC) of the CO₂ concentration.
 *
 * From Sensirion SCD4X I2C Library
 *
 * 1. Operate the SCD4x in the operation mode later used for normal sensor
 * operation (e.g. periodic measurement) for at least 3 minutes in an
 * environment with a homogenous and constant CO2 concentration. The sensor
 * must be operated at the voltage desired for the application when
 * performing the FRC sequence. 2. Issue the stop_periodic_measurement
 * command. 3. Issue the perform_forced_recalibration command.
 * @note This function should not change the clock
 */
bool SCD4XSensor::performFRC(uint32_t targetCO2)
{
    uint16_t error, frcCorr;

    LOG_INFO("%s: Issuing FRC. Needs 3+ min in stable target environment", sensorName);

    if (!stopMeasurement()) {
        return false;
    }

    LOG_INFO("%s: Target CO2: %u ppm", sensorName, targetCO2);
    error = scd4x.performForcedRecalibration((uint16_t)targetCO2, frcCorr);

    // SCD4X Sensirion datasheet
    delay(400);

    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't perform FRC", sensorName);
        return false;
    }

    if (frcCorr == 0xFFFF) {
        LOG_ERROR("%s: FRC failed", sensorName);
        return false;
    }

    LOG_INFO("%s: FRC done. Correction output: %u", sensorName, (uint16_t)(frcCorr - 0x8000));

    return true;
}

/**
 * @brief Start measurement mode
 * @note This function should not change the clock
 */
bool SCD4XSensor::startMeasurement()
{
    uint16_t error;

    if (state == SCD4X_MEASUREMENT) {
        LOG_DEBUG("%s: Already in measurement mode", sensorName);
        return true;
    }

    if (lowPower) {
        error = scd4x.startLowPowerPeriodicMeasurement();
    } else {
        error = scd4x.startPeriodicMeasurement();
    }

    if (error == SCD4X_NO_ERROR) {
        LOG_INFO("%s: Started measurement mode", sensorName);
        if (lowPower) {
            LOG_INFO("%s: Low power mode", sensorName);
        } else {
            LOG_INFO("%s: Normal power mode", sensorName);
        }

        state = SCD4X_MEASUREMENT;
        return true;
    } else {
        LOG_ERROR("%s: Can't start measurement mode", sensorName);
        return false;
    }
}

/**
 * @brief Stop measurement mode
 * @note This function should not change the clock
 */
bool SCD4XSensor::stopMeasurement()
{
    uint16_t error;

    error = scd4x.stopPeriodicMeasurement();
    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't stop measurement", sensorName);
        return false;
    }

    state = SCD4X_IDLE;
    co2MeasureStarted = 0;
    return true;
}

/**
 * @brief Set power mode
 * Pass true to set low power mode
 * @note This function should not change the clock
 */
bool SCD4XSensor::setPowerMode(bool _lowPower)
{
    lowPower = _lowPower;

    if (!stopMeasurement()) {
        return false;
    }

    if (lowPower) {
        LOG_DEBUG("%s: Set low power mode", sensorName);
    } else {
        LOG_DEBUG("%s: Set normal power mode", sensorName);
    }

    return true;
}

/**
 * @brief Check the current mode (ASC or FRC)
 * From Sensirion SCD4X I2C Library
 * @note This function should not change the clock
 */
bool SCD4XSensor::getASC(uint16_t &_ascActive)
{
    uint16_t error;
    LOG_INFO("%s: Getting ASC", sensorName);

    if (!stopMeasurement()) {
        return false;
    }
    error = scd4x.getAutomaticSelfCalibrationEnabled(_ascActive);

    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't send command", sensorName);
        return false;
    }

    LOG_INFO("%s: ASC is %s", sensorName, _ascActive ? "enabled" : "disabled");

    return true;
}

/**
 * @brief Enable or disable automatic self calibration (ASC).
 *
 * From Sensirion SCD4X I2C Library
 *
 * Sets the current state (enabled / disabled) of the ASC. By default, ASC
 * is enabled.
 * @note This function should not change the clock
 */
bool SCD4XSensor::setASC(bool ascEnabled)
{
    uint16_t error;

    LOG_INFO("%s: %s ASC", sensorName, ascEnabled ? "Enabling" : "Disabling");

    if (!stopMeasurement()) {
        return false;
    }

    error = scd4x.setAutomaticSelfCalibrationEnabled((uint16_t)ascEnabled);

    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't send command", sensorName);
        return false;
    }

    error = scd4x.persistSettings();
    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't persist settings", sensorName);
        return false;
    }

    if (!getASC(ascActive)) {
        LOG_ERROR("%s: Can't check if ASC enabled", sensorName);
        return false;
    }

    return true;
}

/**
 * @brief Set the value of ASC baseline target in ppm.
 *
 * From Sensirion SCD4X I2C Library.
 *
 * Sets the value of the ASC baseline target, i.e. the CO₂ concentration in
 * ppm which the ASC algorithm will assume as lower-bound background to
 * which the SCD4x is exposed to regularly within one ASC period of
 * operation. To save the setting to the EEPROM, the persist_settings
 * command must be issued subsequently. The factory default value is 400
 * ppm.
 * @note This function should not change the clock
 */
bool SCD4XSensor::setASCBaseline(uint32_t targetCO2)
{
    // Available in library, but not described in datasheet.
    uint16_t error;
    LOG_INFO("%s: Setting ASC baseline to: %u", sensorName, targetCO2);

    getASC(ascActive);
    if (!ascActive) {
        LOG_ERROR("%s: Can't set ASC baseline, ASC not active", sensorName);
        return false;
    }

    if (!stopMeasurement()) {
        return false;
    }

    error = scd4x.setAutomaticSelfCalibrationTarget((uint16_t)targetCO2);

    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't send command", sensorName);
        return false;
    }

    error = scd4x.persistSettings();
    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't persist settings", sensorName);
        return false;
    }

    LOG_INFO("%s: ASC baseline set", sensorName);

    return true;
}

/**
 * @brief Set the temperature compensation reference.
 *
 * From Sensirion SCD4X I2C Library.
 *
 * Setting the temperature offset of the SCD4x inside the customer device
 * allows the user to optimize the RH and T output signal.
 * By default, the temperature offset is set to 4 °C. To save
 * the setting to the EEPROM, the persist_settings command may be issued.
 * Equation (1) details how the characteristic temperature offset can be
 * calculated using the current temperature output of the sensor (TSCD4x), a
 * reference temperature value (TReference), and the previous temperature
 * offset (Toffset_pervious) obtained using the get_temperature_offset_raw
 * command:
 *
 * Toffset_actual = TSCD4x - TReference + Toffset_pervious.
 *
 * Recommended temperature offset values are between 0 °C and 20 °C. The
 * temperature offset does not impact the accuracy of the CO2 output.
 * @note This function should not change the clock
 */
bool SCD4XSensor::setTemperature(float tempReference)
{
    uint16_t error;
    float prevTempOffset;
    float updatedTempOffset;
    float tempOffset;
    bool dataReady;
    uint16_t co2;
    float temperature;
    float humidity;

    LOG_INFO("%s: Setting reference temp at: %.2f", sensorName, tempReference);

    error = scd4x.getDataReadyStatus(dataReady);
    if (error != SCD4X_NO_ERROR || !dataReady) {
        LOG_ERROR("%s: Data is not ready", sensorName);
        return false;
    }

    error = scd4x.readMeasurement(co2, temperature, humidity);
    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't read current temp, rc=%u", sensorName, error);
        return false;
    }

    LOG_INFO("%s: Current sensor temp: %.2f", sensorName, temperature);

    if (!stopMeasurement()) {
        return false;
    }

    error = scd4x.getTemperatureOffset(prevTempOffset);

    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't get temp offset, rc=%u", sensorName, error);
        return false;
    }
    LOG_INFO("%s: Current sensor temp offset: %.2f", sensorName, prevTempOffset);

    tempOffset = temperature - tempReference + prevTempOffset;

    LOG_INFO("%s: Setting temp offset: %.2f", sensorName, tempOffset);
    error = scd4x.setTemperatureOffset(tempOffset);
    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't set temp offset, rc=%u", sensorName, error);
        return false;
    }

    error = scd4x.persistSettings();
    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't persist settings, rc=%u", sensorName, error);
        return false;
    }

    scd4x.getTemperatureOffset(updatedTempOffset);
    LOG_INFO("%s: Updated sensor temp offset: %.2f", sensorName, updatedTempOffset);

    return true;
}

/**
 * @brief Get the sensor altitude.
 *
 * From Sensirion SCD4X I2C Library.
 *
 * Altitude in meters above sea level can be set after device installation.
 * Valid value between 0 and 3000m. This overrides pressure offset.
 * @note This function should not change the clock
 */
bool SCD4XSensor::getAltitude(uint16_t &altitude)
{
    uint16_t error;
    LOG_INFO("%s: Requesting sensor altitude", sensorName);

    if (!stopMeasurement()) {
        return false;
    }

    error = scd4x.getSensorAltitude(altitude);

    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't get altitude, rc=%u", sensorName, error);
        return false;
    }
    LOG_INFO("%s: Sensor altitude: %u", sensorName, altitude);

    return true;
}

/**
 * @brief Get the ambient pressure around the sensor.
 *
 * From Sensirion SCD4X I2C Library.
 *
 * Gets the ambient pressure in Pa.
 * @note This function should not change the clock
 */
bool SCD4XSensor::getAmbientPressure(uint32_t &ambientPressure)
{
    uint16_t error;
    LOG_INFO("%s: Requesting sensor ambient pressure", sensorName);

    error = scd4x.getAmbientPressure(ambientPressure);

    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't get ambient pressure, rc=%u", sensorName, error);
        return false;
    }
    LOG_INFO("%s: Sensor ambient pressure: %u", sensorName, ambientPressure);

    return true;
}

/**
 * @brief Set the sensor altitude.
 *
 * From Sensirion SCD4X I2C Library.
 *
 * Altitude in meters above sea level can be set after device installation.
 * Valid value between 0 and 3000m. This overrides pressure offset.
 * @note This function should not change the clock
 */
bool SCD4XSensor::setAltitude(uint32_t altitude)
{
    uint16_t error;

    if (!stopMeasurement()) {
        return false;
    }
    LOG_INFO("%s: setting altitude at %um", sensorName, altitude);

    error = scd4x.setSensorAltitude(altitude);

    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't set altitude, rc=%u", sensorName, error);
        return false;
    }

    // NOTE: this gives an error if issued. Sensirion's library
    // doesn't indicate it's needed.
    // error = scd4x.persistSettings();
    // if (error != SCD4X_NO_ERROR) {
    //     LOG_ERROR("%s: Can't make settings persistent. Error code: %u", sensorName, error);
    //     return false;
    // }

    LOG_INFO("%s: altitude set", sensorName);

    return true;
}

/**
 * @brief Set the ambient pressure around the sensor.
 *
 * From Sensirion SCD4X I2C Library.
 *
 * The set_ambient_pressure command can be sent during periodic measurements
 * to enable continuous pressure compensation. Note that setting an ambient
 * pressure overrides any pressure compensation based on a previously set
 * sensor altitude. Use of this command is highly recommended for
 * applications experiencing significant ambient pressure changes to ensure
 * sensor accuracy. Valid input values are between 70000 - 120000 Pa. The
 * default value is 101300 Pa.
 * @note This function should not change the clock
 */
bool SCD4XSensor::setAmbientPressure(uint32_t ambientPressure)
{
    uint16_t error;

    LOG_INFO("%s: setting ambient pressure at %u Pa", sensorName, ambientPressure);

    error = scd4x.setAmbientPressure(ambientPressure);

    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't set ambient pressure, rc=%u", sensorName, error);
        return false;
    }

    // Sensirion doesn't indicate if this is necessary. We send it anyway
    error = scd4x.persistSettings();
    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't persist settings, rc=%u", sensorName, error);
        return false;
    }

    LOG_INFO("%s: ambient pressure set", sensorName);

    return true;
}

/**
 * @brief Perform factory reset to erase the settings stored in the EEPROM.
 *
 * From Sensirion SCD4X I2C Library.
 *
 * The perform_factory_reset command resets all configuration settings
 * stored in the EEPROM and erases the FRC and ASC algorithm history.
 * @note This function should not change the clock
 */
bool SCD4XSensor::factoryReset()
{
    uint16_t error;

    LOG_INFO("%s: Requesting factory reset", sensorName);

    if (!stopMeasurement()) {
        return false;
    }

    error = scd4x.performFactoryReset();

    if (error != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Can't factory reset, rc=%u", sensorName, error);
        return false;
    }

    LOG_INFO("%s: Factory reset done", sensorName);

    return true;
}

/**
 * @brief Put the sensor into sleep mode from idle mode.
 *
 * From Sensirion SCD4X I2C Library.
 *
 * Put the sensor from idle to sleep to reduce power consumption. Can be
 * used to power down when operating the sensor in power-cycled single shot
 * mode.
 * @note This command is only available in idle mode. Only for SCD41.
 */
bool SCD4XSensor::powerDown()
{
    LOG_INFO("%s: Sending sensor to sleep", sensorName);

    if (sensorVariant != SCD4X_SENSOR_VARIANT_SCD41) {
        LOG_WARN("SCD4X: Can't sleep: wrong variant, ignoring");
        return true;
    }

#ifdef SCD4X_I2C_CLOCK_SPEED
    LOG_INFO("%s: reclock speed %uHz", sensorName, SCD4X_I2C_CLOCK_SPEED);
    ReClockI2CGuard clockGuard(reClockI2C, SCD4X_I2C_CLOCK_SPEED);
#endif /* SCD4X_I2C_CLOCK_SPEED */

    if (!stopMeasurement()) {
        return false;
    }

    if (scd4x.powerDown() != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: Error trying to execute sleep()", sensorName);
        return false;
    }

    state = SCD4X_OFF;
    return true;
}

/**
 * @brief Wake up sensor from sleep mode to idle mode (powerUp)
 *
 * From Sensirion SCD4X I2C Library.
 *
 * Wake up the sensor from sleep mode into idle mode. Note that the SCD4x
 * does not acknowledge the wake_up command. The sensor's idle state after
 * wake up can be verified by reading out the serial number.
 * @note This command is only available for SCD41.
 * @note This function can't change clock (used in init)
 */
bool SCD4XSensor::powerUp()
{
    LOG_INFO("%s Waking", sensorName);

    if (scd4x.wakeUp() != SCD4X_NO_ERROR) {
        LOG_ERROR("%s: wakeUp() failed", sensorName);
        return false;
    }

    state = SCD4X_IDLE;

    return true;
}

/**
 * @brief Check if sensor is in measurement mode
 */
bool SCD4XSensor::isActive()
{
    return state == SCD4X_MEASUREMENT;
}

/**
 * @brief Start measurement mode
 * @note Not used in admin comands, getMetrics or init, can change clock.
 */
uint32_t SCD4XSensor::wakeUp()
{

#ifdef SCD4X_I2C_CLOCK_SPEED
    LOG_INFO("%s: reclock speed %uHz", sensorName, SCD4X_I2C_CLOCK_SPEED);
    ReClockI2CGuard clockGuard(reClockI2C, SCD4X_I2C_CLOCK_SPEED);
#endif /* SCD4X_I2C_CLOCK_SPEED */

    if (startMeasurement()) {
        co2MeasureStarted = millis();
        return SCD4X_WARMUP_MS;
    }

    return 0;
}

/**
 * @brief Stop measurement mode
 * @note Not used in admin comands, getMetrics or init, can change clock.
 */
void SCD4XSensor::sleep()
{
#ifdef SCD4X_I2C_CLOCK_SPEED
    LOG_INFO("%s: reclock speed %uHz", sensorName, SCD4X_I2C_CLOCK_SPEED);
    ReClockI2CGuard clockGuard(reClockI2C, SCD4X_I2C_CLOCK_SPEED);
#endif /* SCD4X_I2C_CLOCK_SPEED */

    stopMeasurement();
}

/**
 * @brief Can sleep function
 *
 * Power consumption is very low on lowPower mode, modify this function if
 * you still want to override this behaviour. Otherwise, sleep is disabled
 * routinely in low power mode
 */
bool SCD4XSensor::canSleep()
{
    return lowPower ? false : true;
}

int32_t SCD4XSensor::wakeUpTimeMs()
{
    return SCD4X_WARMUP_MS;
}

int32_t SCD4XSensor::pendingForReadyMs()
{
    uint32_t sinceCO2MeasureStarted = millis() - co2MeasureStarted;
    LOG_DEBUG("%s: Since measure started: %ums", sensorName, sinceCO2MeasureStarted);

    if (sinceCO2MeasureStarted < SCD4X_WARMUP_MS) {
        LOG_INFO("%s: not enough time since measurement start", sensorName);
        return SCD4X_WARMUP_MS - sinceCO2MeasureStarted;
    }
    return 0;
}

AdminMessageHandleResult SCD4XSensor::handleAdminMessage(const meshtastic_MeshPacket &mp, meshtastic_AdminMessage *request,
                                                         meshtastic_AdminMessage *response)
{
    AdminMessageHandleResult result;

#ifdef SCD4X_I2C_CLOCK_SPEED
    LOG_INFO("%s: reclock speed %uHz", sensorName, SCD4X_I2C_CLOCK_SPEED);
    ReClockI2CGuard clockGuard(reClockI2C, SCD4X_I2C_CLOCK_SPEED);
#endif /* SCD4X_I2C_CLOCK_SPEED */

    // TODO: potentially add selftest command?
    switch (request->which_payload_variant) {
    case meshtastic_AdminMessage_sensor_config_tag:
        // Check for ASC-FRC request first
        if (!request->sensor_config.has_scd4x_config) {
            result = AdminMessageHandleResult::NOT_HANDLED;
            break;
        }

        {
            const auto &cfg = request->sensor_config.scd4x_config;
            bool ok = true;

            // FRC/ASC/altitude/pressure/factory-reset calibration branching is shared with
            // SCD30Sensor and the CO2-capable SEN6X variants via CO2CalibrationSensor.
            if (cfg.has_factory_reset || cfg.has_set_asc || cfg.has_set_altitude || cfg.has_set_ambient_pressure) {
                Co2AdminRequest co2req;
                co2req.hasFactoryReset = cfg.has_factory_reset;
                co2req.hasSetAsc = cfg.has_set_asc;
                co2req.setAsc = cfg.set_asc;
                co2req.hasTargetCo2 = cfg.has_set_target_co2_conc;
                co2req.targetCo2 = cfg.set_target_co2_conc;
                co2req.hasSetAltitude = cfg.has_set_altitude;
                co2req.setAltitude = cfg.set_altitude;
                co2req.hasSetAmbientPressure = cfg.has_set_ambient_pressure;
                co2req.setAmbientPressure = cfg.set_ambient_pressure;
                ok &= this->handleCo2AdminRequest(co2req, sensorName);
            }

            // A factory reset erases calibration history outright - matches the original
            // behavior of skipping every other field when it's requested.
            if (ok && !cfg.has_factory_reset) {
                // Check for temperature offset
                // NOTE: this requires to have a sensor working on stable environment
                // And to make it between readings
                if (cfg.has_set_temperature) {
                    ok &= this->setTemperature(cfg.set_temperature);
                }

                // Check for low power mode
                // NOTE: to switch from one mode to another do:
                // setPowerMode -> startMeasurement
                if (cfg.has_set_power_mode) {
                    ok &= this->setPowerMode(cfg.set_power_mode);
                }
            }

            if (!ok) {
                result = AdminMessageHandleResult::NOT_HANDLED;
                break;
            }
        }

        result = AdminMessageHandleResult::HANDLED;
        break;

    default:
        result = AdminMessageHandleResult::NOT_HANDLED;
    }

    // Start measurement mode
    this->startMeasurement();

    return result;
}

#endif