// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2024 Intel Corporation. All Rights Reserved.

#include "lrs-device-controller.h"

#include <common/metadata-helper.h>

#include <realdds/topics/image-msg.h>
#include <realdds/topics/imu-msg.h>
#include <realdds/topics/blob-msg.h>
#include <realdds/topics/dds-topic-names.h>
#include <realdds/topics/ros2/ros2vector3.h>
#include <realdds/topics/flexible-msg.h>
#include <realdds/topics/dds-topic-names.h>
#include <realdds/dds-device-server.h>
#include <realdds/dds-stream-server.h>
#include <realdds/dds-topic-reader-thread.h>
#include <realdds/dds-participant.h>
#include <realdds/dds-guid.h>

#include <fastdds/dds/subscriber/SampleInfo.hpp>

#include <rsutils/number/crc32.h>
#include <rsutils/easylogging/easyloggingpp.h>
#include <rsutils/json.h>
#include <rsutils/string/hexarray.h>
#include <rsutils/time/timer.h>
#include <rsutils/string/from.h>

#include <algorithm>
#include <iostream>

using rsutils::string::hexarray;
using rsutils::json;
using namespace realdds;
using tools::lrs_device_controller;


#define CREATE_SERVER_IF_NEEDED( X )                                                                                   \
    if( server )                                                                                                       \
    {                                                                                                                  \
        if( strcmp( server->type_string(), #X ) )                                                                      \
        {                                                                                                              \
            LOG_ERROR( #X " profile type on a stream '" << stream_name << "' that already has type "                   \
                                                        << server->type_string() );                                    \
            return;                                                                                                    \
        }                                                                                                              \
    }                                                                                                                  \
    else                                                                                                               \
    {                                                                                                                  \
        server = std::make_shared< realdds::dds_##X##_stream_server >( stream_name, sensor_name );                     \
    }                                                                                                                  \
    break


realdds::video_intrinsics to_realdds( const rs2_intrinsics & intr )
{
    realdds::video_intrinsics ret;

    ret.width = intr.width;
    ret.height = intr.height;
    ret.principal_point_x = intr.ppx;
    ret.principal_point_y = intr.ppy;
    ret.focal_lenght_x = intr.fx;
    ret.focal_lenght_y = intr.fy;
    ret.distortion_model = intr.model;
    memcpy( ret.distortion_coeffs.data(), intr.coeffs, sizeof( ret.distortion_coeffs ) );

    return ret;
}

realdds::motion_intrinsics to_realdds( const rs2_motion_device_intrinsic & rs2_intr )
{
    realdds::motion_intrinsics intr;

    memcpy( intr.data.data(), rs2_intr.data, sizeof( intr.data ) );
    memcpy( intr.noise_variances.data(), rs2_intr.noise_variances, sizeof( intr.noise_variances ) );
    memcpy( intr.bias_variances.data(), rs2_intr.bias_variances, sizeof( intr.bias_variances ) );

    return intr;
}

realdds::extrinsics to_realdds( const rs2_extrinsics & rs2_extr )
{
    realdds::extrinsics extr;

    memcpy( extr.rotation.data(), rs2_extr.rotation, sizeof( extr.rotation ) );
    memcpy( extr.translation.data(), rs2_extr.translation, sizeof( extr.translation ) );

    return extr;
}


static std::string stream_name_from_rs2( const rs2::stream_profile & profile )
{
    // ROS stream names cannot contain spaces! We use underscores instead:
    std::string stream_name;
    switch( profile.stream_type() )
    {
    case RS2_STREAM_ACCEL:
    case RS2_STREAM_GYRO:
        stream_name = "Motion";
        break;
    default:
        stream_name = rs2_stream_to_string( profile.stream_type() );
        if( profile.stream_index() )
            stream_name += '_' + std::to_string( profile.stream_index() );
        break;
    }
    return stream_name;
}


static std::string stream_name_from_rs2( rs2::sensor const & sensor )
{
    static std::map< std::string, std::string > sensor_stream_name{
        { "Stereo Module", rs2_stream_to_string( RS2_STREAM_DEPTH ) },
        { "RGB Camera", rs2_stream_to_string( RS2_STREAM_COLOR ) },
        { "Motion Module", "Motion" },
    };

    auto it = sensor_stream_name.find( sensor.get_info( RS2_CAMERA_INFO_NAME ) );
    if( it == sensor_stream_name.end() )
        return {};
    return it->second;
}


std::vector< std::shared_ptr< realdds::dds_stream_server > > lrs_device_controller::get_supported_streams()
{
    std::map< std::string, realdds::dds_stream_profiles > stream_name_to_profiles;
    std::map< std::string, size_t > stream_name_to_default_profile;
    std::map< std::string, std::set< realdds::video_intrinsics > > stream_name_to_video_intrinsics;

    // Iterate over all profiles of all sensors and build appropriate dds_stream_servers
    for( auto & sensor : _rs_dev.query_sensors() )
    {
        std::string const sensor_name = sensor.get_info( RS2_CAMERA_INFO_NAME );
        // We keep a copy of the sensors throughout the run time:
        // Otherwise problems could arise like opening streams and they would close at on_open_streams scope end.
        _rs_sensors[sensor_name] = sensor;

        auto stream_profiles = sensor.get_stream_profiles();
        std::for_each( stream_profiles.begin(), stream_profiles.end(), [&]( const rs2::stream_profile & sp )
        {
            std::string stream_name = stream_name_from_rs2( sp );

            //Create a realdds::dds_stream_server object for each unique profile type+index
            auto & server = _stream_name_to_server[stream_name];
            switch( sp.stream_type() )
            {
            case RS2_STREAM_DEPTH: CREATE_SERVER_IF_NEEDED( depth );
            case RS2_STREAM_INFRARED: CREATE_SERVER_IF_NEEDED( ir );
            case RS2_STREAM_COLOR: CREATE_SERVER_IF_NEEDED( color );
            case RS2_STREAM_CONFIDENCE: CREATE_SERVER_IF_NEEDED( confidence );
            case RS2_STREAM_ACCEL:
            case RS2_STREAM_GYRO:
                CREATE_SERVER_IF_NEEDED( motion );
            default:
                LOG_ERROR( "unsupported stream type " << sp.stream_type() );
                return;
            }

            // Create appropriate realdds::profile for each sensor profile and map to a stream
            auto & profiles = stream_name_to_profiles[stream_name];
            std::shared_ptr< realdds::dds_stream_profile > profile;
            bool insert_profile = true;
            if( auto const vsp = rs2::video_stream_profile( sp ) )
            {
                profile = std::make_shared< realdds::dds_video_stream_profile >(
                    static_cast< int16_t >( vsp.fps() ),
                    realdds::dds_video_encoding::from_rs2( vsp.format() ),
                    static_cast< uint16_t >( vsp.width() ),
                    static_cast< int16_t >( vsp.height() ) );
                try
                {
                    auto intr = to_realdds( vsp.get_intrinsics() );
                    stream_name_to_video_intrinsics[stream_name].insert( intr );
                }
                catch( ... ) {} //Some profiles don't have intrinsics
            }
            else if( auto const msp = rs2::motion_stream_profile( sp ) )
            {
                profile = std::make_shared< realdds::dds_motion_stream_profile >(
                    static_cast< int16_t >( msp.fps() ) );

                auto motion_server = std::dynamic_pointer_cast< dds_motion_stream_server >( server );
                if( RS2_STREAM_ACCEL == msp.stream_type() )
                {
                    insert_profile = false;  // We report only Gyro profiles
                    motion_server->set_accel_intrinsics( to_realdds( msp.get_motion_intrinsics() ) );
                }
                else
                    motion_server->set_gyro_intrinsics( to_realdds( msp.get_motion_intrinsics() ) );
            }
            else
            {
                LOG_ERROR( "unknown profile type of uid " << sp.unique_id() );
                return;
            }
            if( insert_profile )
            {
                if( sp.is_default() )
                    stream_name_to_default_profile[stream_name] = profiles.size();
                profiles.push_back( profile );
                LOG_DEBUG( stream_name << ": " << profile->to_string() );
            }
        } );
    }

    override_default_profiles( stream_name_to_profiles, stream_name_to_default_profile );

    // Iterate over the mapped streams and initialize
    std::vector< std::shared_ptr< realdds::dds_stream_server > > servers;
    std::set< std::string > sensors_handled;
    for( auto & it : stream_name_to_profiles )
    {
        auto const & stream_name = it.first;

        size_t default_profile_index = 0;
        auto default_profile_it = stream_name_to_default_profile.find( stream_name );
        if( default_profile_it != stream_name_to_default_profile.end() )
            default_profile_index = default_profile_it->second;
        else
            LOG_ERROR( "no default profile found; using first available in " << stream_name );

        auto const & profiles = it.second;
        if( profiles.empty() )
        {
            LOG_ERROR( "ignoring stream '" << stream_name << "' with no profiles" );
            continue;
        }
        auto server = _stream_name_to_server[stream_name];
        if( ! server )
        {
            LOG_ERROR( "ignoring stream '" << stream_name << "' with no server" );
            continue;
        }

        if( auto video_server = std::dynamic_pointer_cast< dds_video_stream_server >( server ) )
        {
            video_server->set_intrinsics( std::move( stream_name_to_video_intrinsics[stream_name] ) );
            // Set stream metadata support (currently if the device supports metadata all streams does)
            // Must be done before calling init_profiles()
            if( _md_enabled )
                server->enable_metadata();
        }

        server->init_profiles( profiles, default_profile_index );

        // Get supported options and recommended filters for this stream
        for( auto & sensor : _rs_dev.query_sensors() )
        {
            std::string const sensor_name = sensor.get_info( RS2_CAMERA_INFO_NAME );
            if( server->sensor_name().compare( sensor_name ) != 0 )
                continue;

            // Multiple streams map to the same sensor so we may go over the same sensor multiple times - we
            // only need to do this once per sensor!
            if( sensors_handled.emplace( sensor_name ).second )
            {
                realdds::dds_options stream_options;
                auto supported_options = sensor.get_supported_options();
                for( auto option_id : supported_options )
                {
                    // Certain options are automatically added by librealsense and shouldn't actually be shared
                    if( option_id == RS2_OPTION_FRAMES_QUEUE_SIZE )
                        continue;  // Added automatically for every sensor_base

                    std::string option_name = sensor.get_option_name( option_id );
                    try
                    {
                        json j = json::array();
                        json props = json::array();
                        j += option_name;
                        json option_value;  // null - no value
                        try
                        {
                            option_value = sensor.get_option( option_id );
                        }
                        catch( ... )
                        {
                            // Some options can be queried only if certain conditions exist skip them for now
                            props += "optional";
                        }
                        j += option_value;
                        {
                            // Even read-only options have ranges in librealsense
                            auto const range = sensor.get_option_range( option_id );
                            j += range.min;
                            j += range.max;
                            j += range.step;
                            j += range.def;
                        }
                        j += sensor.get_option_description( option_id );
                        if( sensor.is_option_read_only( option_id ) )
                            props += "read-only";
                        if( ! props.empty() )
                            j += props;
                        auto dds_opt = realdds::dds_option::from_json( j );
                        LOG_DEBUG( "... option -> " << j << " -> " << dds_opt->to_json() );
                        stream_options.push_back( dds_opt );  // TODO - filter options relevant for stream type
                    }
                    catch( ... )
                    {
                        LOG_ERROR( "Cannot query details of option " << option_id );
                        // Some options can be queried only if certain conditions exist skip them for now
                    }
                }

                auto recommended_filters = sensor.get_recommended_filters();
                std::vector< std::string > filter_names;
                for( auto const & filter : recommended_filters )
                    filter_names.push_back( filter.get_info( RS2_CAMERA_INFO_NAME ) );

                server->init_options( stream_options );
                server->set_recommended_filters( std::move( filter_names ) );
            }
        }

        servers.push_back( server );
    }

    return servers;
}

#undef CREATE_SERVER_IF_NEEDED

extrinsics_map get_extrinsics_map( const rs2::device & dev )
{
    extrinsics_map ret;
    std::map< std::string, rs2::stream_profile > stream_name_to_rs2_stream_profile;

    // Iterate over profiles of all sensors and split to streams
    for( auto & sensor : dev.query_sensors() )
    {
        auto stream_profiles = sensor.get_stream_profiles();
        std::for_each( stream_profiles.begin(),
                       stream_profiles.end(),
                       [&]( const rs2::stream_profile & sp )
                       {
                           if( RS2_STREAM_ACCEL == sp.stream_type() )
                               return;  // Ignore the accelerometer; we want the gyro extrinsics!
                           std::string stream_name = stream_name_from_rs2( sp );
                           auto & rs2_stream_profile = stream_name_to_rs2_stream_profile[stream_name];
                           if( ! rs2_stream_profile )
                               rs2_stream_profile = sp;  // Any profile of this stream will do, take the first
                       } );
    }

    // For each stream, get extrinsics to all other streams
    for( auto & from : stream_name_to_rs2_stream_profile )
    {
        auto const & from_stream_name = from.first;
        for( auto & to : stream_name_to_rs2_stream_profile )
        {
            auto & to_stream_name = to.first;
            if( from_stream_name != to_stream_name )
            {
                // Get rs2::stream_profile objects for get_extrinsics API call
                const rs2::stream_profile & from_profile = from.second;
                const rs2::stream_profile & to_profile = to.second;
                const auto & extrinsics = from_profile.get_extrinsics_to( to_profile );
                ret[std::make_pair( from_stream_name, to_stream_name )] =
                    std::make_shared< realdds::extrinsics >( to_realdds( extrinsics ) );
                LOG_DEBUG( "have extrinsics from " << from_stream_name << " to " << to_stream_name );
            }
        }
    }

    return ret;
}


std::shared_ptr< dds_stream_profile > create_dds_stream_profile( std::string const & type_string, json const & j )
{
    if( "motion" == type_string )
        return dds_stream_profile::from_json< dds_motion_stream_profile >( j );

    static const std::set< std::string > video_types = { "depth", "color", "ir", "confidence" };
    if( video_types.find( type_string ) != video_types.end() )
        return dds_stream_profile::from_json< dds_video_stream_profile >( j );

    throw std::runtime_error( "unsupported stream type '" + type_string + "'" );
}


rs2_stream stream_name_to_type( std::string const & type_string )
{
    static const std::map< std::string, rs2_stream > type_to_rs2 = {
        { "Depth", RS2_STREAM_DEPTH },
        { "Color", RS2_STREAM_COLOR },
        { "Infrared", RS2_STREAM_INFRARED },
        { "Infrared_1", RS2_STREAM_INFRARED },
        { "Infrared_2", RS2_STREAM_INFRARED },
        { "Motion", RS2_STREAM_GYRO },  // We report only gyro profiles
        { "Gpio", RS2_STREAM_GPIO },
        { "Confidence", RS2_STREAM_CONFIDENCE },
    };
    auto it = type_to_rs2.find( type_string );
    if( it == type_to_rs2.end() )
    {
        LOG_ERROR( "Unknown stream type '" << type_string << "'" );
        return RS2_STREAM_ANY;
    }
    return it->second;
}


int stream_name_to_index( std::string const & type_string )
{
    int index = 0;
    static const std::map< std::string, int > type_to_index = {
        { "Infrared_1", 1 },
        { "Infrared_2", 2 },
    };
    auto it = type_to_index.find( type_string );
    if( it != type_to_index.end() )
    {
        index = it->second;
    }

    return index;
}

rs2_option option_name_to_id( const std::string & name )
{
    auto option_id = rs2_option_from_string( name.c_str() );
    if( RS2_OPTION_COUNT == option_id )
        throw std::runtime_error( "Option '" + name + "' not found" );
    return option_id;
}


bool profiles_are_compatible( std::shared_ptr< dds_stream_profile > const & p1,
                              std::shared_ptr< dds_stream_profile > const & p2,
                              bool any_encoding = false )
{
    auto vp1 = std::dynamic_pointer_cast< realdds::dds_video_stream_profile >( p1 );
    auto vp2 = std::dynamic_pointer_cast< realdds::dds_video_stream_profile >( p2 );
    if( ! ! vp1 != ! ! vp2 )
        return false;  // types aren't the same
    if( vp1 && vp2 )
    {
        if( vp1->width() != vp2->width() || vp1->height() != vp2->height() )
            return false;
        if( ! any_encoding && vp1->encoding() != vp2->encoding() )
            return false;
    }
    return p1->frequency() == p2->frequency();
}


rs2::stream_profile get_required_profile( const rs2::sensor & sensor,
                                          std::vector< rs2::stream_profile > const & sensor_stream_profiles,
                                          std::string const & stream_name,
                                          std::shared_ptr< dds_stream_profile > const & profile )
{
    auto const stream_type = stream_name_to_type( stream_name );
    auto const stream_index = stream_name_to_index( stream_name );

    auto profile_iter = std::find_if( sensor_stream_profiles.begin(),
                                      sensor_stream_profiles.end(),
                                      [&]( rs2::stream_profile const & sp ) {
                                          auto vp = sp.as< rs2::video_stream_profile >();
                                          auto dds_vp = std::dynamic_pointer_cast< dds_video_stream_profile >( profile );
                                          bool video_params_match = ( vp && dds_vp )
                                                                      ? vp.width() == dds_vp->width()
                                                                            && vp.height() == dds_vp->height()
                                                                            && vp.format() == dds_vp->encoding().to_rs2()
                                                                      : true;
                                          return sp.stream_type() == stream_type
                                              && sp.stream_index() == stream_index
                                              && sp.fps() == profile->frequency()
                                              && video_params_match;
                                      } );
    if( profile_iter == sensor_stream_profiles.end() )
    {
        throw std::runtime_error( "Could not find required profile" );
    }

    return *profile_iter;
}


static std::shared_ptr< realdds::dds_stream_profile >
find_profile( std::shared_ptr< realdds::dds_stream_server > const & stream,
              std::shared_ptr< realdds::dds_stream_profile > const & profile,
              bool any_encoding = false )
{
    auto & stream_profiles = stream->profiles();
    auto it = std::find_if( stream_profiles.begin(),
                            stream_profiles.end(),
                            [&]( std::shared_ptr< realdds::dds_stream_profile > const & sp )
                            {
                                return profiles_are_compatible( sp, profile, any_encoding );
                            } );
    std::shared_ptr< realdds::dds_stream_profile > found_profile;
    if( it != stream_profiles.end() )
        found_profile = *it;
    return found_profile;
}


std::shared_ptr< realdds::dds_stream_server >
lrs_device_controller::frame_to_streaming_server( rs2::frame const & f, rs2::stream_profile * p_profile ) const
{
    rs2::stream_profile profile_;
    if( ! p_profile )
        p_profile = &profile_;
    rs2::stream_profile & profile = *p_profile;

    profile = f.get_profile();
    auto const stream_name = stream_name_from_rs2( profile );
    auto it = _stream_name_to_server.find( stream_name );
    if( it == _stream_name_to_server.end() )
        return {};

    auto & server = it->second;
    if( ! _bridge.is_streaming( server ) )
        return {};

    return server;
}


lrs_device_controller::lrs_device_controller( rs2::device dev, std::shared_ptr< realdds::dds_device_server > dds_device_server )
    : _rs_dev( dev )
    , _dds_device_server( dds_device_server )
{
    if( ! _dds_device_server )
        throw std::runtime_error( "Empty dds_device_server" );

    _dds_device_server->on_set_option( [&]( const std::shared_ptr< realdds::dds_option > & option, json & value ) {
        set_option( option, value );
    } );
    _dds_device_server->on_query_option( [&]( const std::shared_ptr< realdds::dds_option > & option ) -> json {
        return query_option( option );
    } );
    _dds_device_server->on_control(
        [this]( std::string const & id, json const & control, json & reply )
        { return on_control( id, control, reply ); } );

    std::vector< std::shared_ptr< realdds::dds_stream_server > > supported_streams;
    extrinsics_map extrinsics;
    realdds::dds_options options;

    if( is_recovery() )
    {
        _device_sn = _rs_dev.get_info( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID );
        LOG_DEBUG( "LRS device manager for recovery device: " << _device_sn << " created" );

        // Initialize with nothing: no streams, no options, empty extrinsics, etc.
        _md_enabled = false;
        _dds_device_server->init( supported_streams, options, extrinsics );
        return;
    }

    _device_sn = _rs_dev.get_info( RS2_CAMERA_INFO_SERIAL_NUMBER );
    LOG_DEBUG( "LRS device manager for device: " << _device_sn << " created" );

    // Some camera models support metadata for frames. can_support_metadata will tell us if this model does.
    // Also, to get the metadata driver support needs to be enabled, requires administrator rights on Windows and Linux.
    // is_enabled will return current state. If one of the conditions is false we cannot get metadata from the device.
    _md_enabled = rs2::metadata_helper::instance().can_support_metadata( _rs_dev.get_info( RS2_CAMERA_INFO_PRODUCT_LINE ) )
               && rs2::metadata_helper::instance().is_enabled( _rs_dev.get_info( RS2_CAMERA_INFO_PHYSICAL_PORT ) );

    // Create a supported streams list for initializing the relevant DDS topics
    supported_streams = get_supported_streams();

    _bridge.on_start_sensor(
        [this]( std::string const & sensor_name, dds_stream_profiles const & active_profiles )
        {
            auto & sensor = _rs_sensors[sensor_name];
            auto rs2_profiles = get_rs2_profiles( active_profiles );
            sensor.open( rs2_profiles );
            if( sensor.is< rs2::motion_sensor >() )
            {
                struct imu_context
                {
                    realdds::topics::imu_msg message;
                    // We will be getting gyro and accel together, from different threads:
                    // Don't want to change something mid-send
                    std::mutex mutex;
                };
                sensor.start(
                    [this, imu = std::make_shared< imu_context >()]( rs2::frame f )
                    {
                        rs2::stream_profile stream_profile;
                        auto motion = std::dynamic_pointer_cast< realdds::dds_motion_stream_server >(
                            frame_to_streaming_server( f, &stream_profile ) );
                        if( ! motion )
                            return;

                        auto xyz = reinterpret_cast< float const * >( f.get_data() );
                        if( RS2_STREAM_ACCEL == stream_profile.stream_type() )
                        {
                            std::unique_lock< std::mutex > lock( imu->mutex );
                            imu->message.accel_data().x( xyz[0] );  // in m/s^2
                            imu->message.accel_data().y( xyz[1] );
                            imu->message.accel_data().z( xyz[2] );
                            return;  // Don't actually publish
                        }
                        imu->message.gyro_data().x( xyz[0] );  // rad/sec, which is what we need
                        imu->message.gyro_data().y( xyz[1] );
                        imu->message.gyro_data().z( xyz[2] );
                        imu->message.timestamp(  // in sec.nsec
                            static_cast< long double >( f.get_timestamp() ) / 1e3 );
                        std::unique_lock< std::mutex > lock( imu->mutex );
                        motion->publish_motion( std::move( imu->message ) );

                        // motion streams have no metadata!
                    } );
            }
            else
            {
                sensor.start(
                    [this]( rs2::frame f )
                    {
                        auto video = std::dynamic_pointer_cast< realdds::dds_video_stream_server >(
                            frame_to_streaming_server( f ) );
                        if( ! video )
                            return;

                        dds_time const timestamp  // in sec.nsec
                            ( static_cast< long double >( f.get_timestamp() ) / 1e3 );

                        realdds::topics::image_msg image;
                        auto data = static_cast< const uint8_t * >( f.get_data() );
                        image.raw_data.assign( data, data + f.get_data_size() );
                        image.height = video->get_image_header().height;
                        image.width = video->get_image_header().width;
                        image.timestamp = timestamp;
                        video->publish_image( std::move( image ) );

                        publish_frame_metadata( f, timestamp );
                    } );
            }
            std::cout << sensor_name << " sensor started" << std::endl;
        } );
    _bridge.on_stop_sensor(
        [this]( std::string const & sensor_name )
        {
            auto & sensor = _rs_sensors[sensor_name];
            sensor.stop();
            sensor.close();
            std::cout << sensor_name << " sensor stopped" << std::endl;
        } );
    _bridge.on_error(
        [this]( std::string const & error_string )
        {
            json j = json::object( {
                { "id", "error" },
                { "error", error_string },
            } );
            _dds_device_server->publish_notification( std::move( j ) );
        } );
    _bridge.init( supported_streams );

    extrinsics = get_extrinsics_map( dev );

    // Initialize the DDS device server with the supported streams
    _dds_device_server->init( supported_streams, options, extrinsics );

    for( auto & name_sensor : _rs_sensors )
    {
        auto & sensor = name_sensor.second;
        sensor.on_options_changed(
            [this, weak_sensor = std::weak_ptr< rs2_sensor >( sensor.get() )]( rs2::options_list const & options )
            {
                if( auto strong_sensor = weak_sensor.lock() )
                {
                    rs2::sensor sensor( strong_sensor );
                    json option_values = json::object();
                    for( auto changed_option : options )
                    {
                        std::string const option_name = sensor.get_option_name( changed_option->id );
                        std::string const stream_name = stream_name_from_rs2( sensor );
                        if( stream_name.empty() )
                        {
                            LOG_ERROR( "Unknown option '" << option_name
                                                          << "' stream: " << sensor.get_info( RS2_CAMERA_INFO_NAME ) );
                            continue;
                        }
                        auto dds_option = _dds_device_server->find_option( option_name, stream_name );
                        if( ! dds_option )
                        {
                            LOG_ERROR( "Missing option '" << option_name
                                                          << "' stream: " << sensor.get_info( RS2_CAMERA_INFO_NAME ) );
                            continue;
                        }
                        json value;
                        if( changed_option->is_valid )
                        {
                            switch( changed_option->type )
                            {
                            case RS2_OPTION_TYPE_FLOAT:
                                value = changed_option->as_float;
                                break;
                            case RS2_OPTION_TYPE_STRING:
                                value = changed_option->as_string;
                                break;
                            case RS2_OPTION_TYPE_INTEGER:
                                value = changed_option->as_integer;
                                break;
                            case RS2_OPTION_TYPE_BOOLEAN:
                                value = (bool)changed_option->as_integer;
                                break;
                            default:
                                LOG_ERROR( "Unknown option '" << option_name << "' type: "
                                                              << rs2_option_type_to_string( changed_option->type ) );
                                continue;
                            }
                        }
                        else
                        {
                            // No value available
                            value = rsutils::null_json;
                        }
                        dds_option->set_value( std::move( value ) );
                        option_values[stream_name][option_name] = dds_option->get_value();
                    }
                    if( option_values.size() )
                    {
                        json j = json::object( {
                            { realdds::topics::notification::key::id, realdds::topics::reply::query_options::id },
                            { realdds::topics::reply::query_options::key::option_values, std::move( option_values ) },
                        } );
                        LOG_DEBUG( "[" << _dds_device_server->debug_name() << "] options changed: " << j.dump( 4 ) );
                        _dds_device_server->publish_notification( std::move( j ) );
                    }
                }
            } );
    }
}


lrs_device_controller::~lrs_device_controller()
{
    LOG_DEBUG( "LRS device manager for device: " << _device_sn << " deleted" );
}


bool lrs_device_controller::is_recovery() const
{
    auto update_device = rs2::update_device( _rs_dev );
    return update_device;
}


bool lrs_device_controller::on_open_streams( json const & control, json & reply )
{
    // Note that this function is called "start-streaming" but it's really a response to "open-streams" so does not
    // actually start streaming. It simply sets and locks in which streams should be open when streaming starts.
    // This effectively lets one control _specifically_ which streams should be streamable, and nothing else: if left
    // out, a sensor is reset back to its default state using implicit stream selection.
    // (For example, the 'Stereo Module' sensor controls Depth, IR1, IR2: but turning on all 3 has performance
    // implications and may not be desirable. So you can open only Depth and IR1/2 will stay inactive...)
    if( control.nested( topics::control::open_streams::key::reset ).default_value( true ) )
        _bridge.reset();

    auto const & msg_profiles = control[topics::control::open_streams::key::stream_profiles];
    for( auto const & name2profile : msg_profiles.items() )
    {
        std::string const & stream_name = name2profile.key();
        auto name2server = _stream_name_to_server.find( stream_name );
        if( name2server == _stream_name_to_server.end() )
            throw std::runtime_error( "invalid stream name '" + stream_name + "'" );
        auto server = name2server->second;

        auto requested_profile
            = create_dds_stream_profile( server->type_string(), name2profile.value() );
        auto profile = find_profile( server, requested_profile );
        if( ! profile )
            throw std::runtime_error( "invalid profile " + requested_profile->to_string() + " for stream '"
                                      + stream_name + "'" );

        _bridge.open( profile );
    }

    // We're here so all the profiles were acceptable; lock them in -- with no implicit profiles!
    if( control.nested( topics::control::open_streams::key::commit ).default_value( true ) )
        _bridge.commit();

    // We don't touch the reply - it's already filled in for us
    return true;
}


void lrs_device_controller::publish_frame_metadata( const rs2::frame & f, realdds::dds_time const & timestamp )
{
    if( ! _dds_device_server->has_metadata_readers() )
        return;

    json md_header = json::object( {
        { topics::metadata::header::key::frame_number, f.get_frame_number() },               // communicated; up to client to pick up
        { topics::metadata::header::key::timestamp, timestamp.to_ns() },                     // syncer key: needs to match the image timestamp, bit-for-bit!
        { topics::metadata::header::key::timestamp_domain, f.get_frame_timestamp_domain() }  // needed if we're dealing with different domains!
    } );
    if( f.is< rs2::depth_frame >() )
        md_header[topics::metadata::header::key::depth_units] = f.as< rs2::depth_frame >().get_units();

    json metadata = json::object();
    for( size_t i = 0; i < static_cast< size_t >( RS2_FRAME_METADATA_COUNT ); ++i )
    {
        rs2_frame_metadata_value val = static_cast< rs2_frame_metadata_value >( i );
        if( f.supports_frame_metadata( val ) )
            metadata[rs2_frame_metadata_to_string( val )] = f.get_frame_metadata( val );
    }

    json md_msg = json::object( {
        { topics::metadata::key::stream_name, stream_name_from_rs2( f.get_profile() ) },
        { topics::metadata::key::header, std::move( md_header ) },
        { topics::metadata::key::metadata, std::move( metadata ) },
    } );
    _dds_device_server->publish_metadata( std::move( md_msg ) );
}


std::vector< rs2::stream_profile >
lrs_device_controller::get_rs2_profiles( realdds::dds_stream_profiles const & dds_profiles ) const
{
    std::vector< rs2::stream_profile > rs_profiles;
    for( auto & dds_profile : dds_profiles )
    {
        std::string stream_name = dds_profile->stream()->name();
        std::string sensor_name = dds_profile->stream()->sensor_name();

        auto it = _rs_sensors.find( sensor_name );
        if( it == _rs_sensors.end() )
        {
            LOG_ERROR( "invalid sensor name '" << sensor_name << "'" );
            continue;
        }
        auto & sensor = it->second;
        auto const sensor_stream_profiles = sensor.get_stream_profiles();
        auto rs2_profile = get_required_profile( sensor, sensor_stream_profiles, stream_name, dds_profile );
        rs_profiles.push_back( rs2_profile );

        if( rs2_profile.stream_type() == RS2_STREAM_GYRO )
        {
            // When we start the Gyro, we want to start the Accelerometer, too!
            rs2::stream_profile accel_profile;
            assert( accel_profile.fps() == 0 );
            for( auto & sp : sensor_stream_profiles )
            {
                if( sp.stream_type() != RS2_STREAM_ACCEL )
                    continue;
                if( sp.format() != rs2_profile.format() )
                    continue;
                if( sp.fps() > accel_profile.fps() )
                    accel_profile = sp;
            }
            if( accel_profile )
            {
                LOG_DEBUG( "adding accel profile @ " << accel_profile.fps() << " FPS" );
                rs_profiles.push_back( accel_profile );
            }
        }
    }
    return rs_profiles;
}


void lrs_device_controller::set_option( const std::shared_ptr< realdds::dds_option > & option, float new_value )
{
    auto stream = option->stream();
    if( ! stream )
        // TODO device option? not implemented yet
        throw std::runtime_error( "device option not implemented" );
    auto it = _stream_name_to_server.find( stream->name() );
    if( it == _stream_name_to_server.end() )
        throw std::runtime_error( "no stream '" + stream->name() + "' in device" );
    auto server = it->second;
    auto & sensor = _rs_sensors[server->sensor_name()];
    sensor.set_option( option_name_to_id( option->get_name() ), new_value );
}


json lrs_device_controller::query_option( const std::shared_ptr< realdds::dds_option > & option )
{
    auto stream = option->stream();
    if( ! stream )
        // TODO device option? not implemented yet
        throw std::runtime_error( "device option not implemented" );
    auto it = _stream_name_to_server.find( stream->name() );
    if( it == _stream_name_to_server.end() )
        throw std::runtime_error( "no stream '" + stream->name() + "' in device" );
    auto server = it->second;
    auto & sensor = _rs_sensors[server->sensor_name()];
    try
    {
        return sensor.get_option( option_name_to_id( option->get_name() ) );
    }
    catch( rs2::invalid_value_error const & )
    {
        // It's possible for an option to no longer be supported (temperates do this) because we stopped streaming...
        if( option->is_optional() )
            return rsutils::null_json;
        throw;
    }
}


void lrs_device_controller::override_default_profiles( const std::map< std::string, realdds::dds_stream_profiles > & stream_name_to_profiles,
                                                       std::map< std::string, size_t > & stream_name_to_default_profile ) const
{
    // Default resolution for RealSense modules, set according to system SW architect definitions
    std::string const product_line = _rs_dev.get_info( RS2_CAMERA_INFO_PRODUCT_LINE );
    if( product_line == "D400" )
    {
        static const std::string RS405_PID( "0B5B", 4 );
        static const std::string RS410_PID( "0AD2", 4 );
        static const std::string RS415_PID( "0AD3", 4 );

        std::string product_id = _rs_dev.get_info( RS2_CAMERA_INFO_PRODUCT_ID );

        // For best image quality global shutter should use 848x480 resolution, rolling shutter 1280x720
        uint16_t fps = 30;
        uint16_t width = 848;
        uint16_t height = 480;
        if( product_id == RS415_PID || product_id == RS410_PID )
        {
            width = 1280;
            height = 720;
        }
        stream_name_to_default_profile["Depth"] = get_index_of_profile( stream_name_to_profiles.at( "Depth" ),
            realdds::dds_video_stream_profile( fps, realdds::dds_video_encoding::from_rs2( RS2_FORMAT_Z16 ), width, height ) );
        stream_name_to_default_profile["Infrared_1"] = get_index_of_profile( stream_name_to_profiles.at( "Infrared_1" ),
            realdds::dds_video_stream_profile( fps, realdds::dds_video_encoding::from_rs2( RS2_FORMAT_Y8 ), width, height ) );
        stream_name_to_default_profile["Infrared_2"] = get_index_of_profile( stream_name_to_profiles.at( "Infrared_2" ),
            realdds::dds_video_stream_profile( fps, realdds::dds_video_encoding::from_rs2( RS2_FORMAT_Y8 ), width, height ) );

        width = 1280;
        height = 720;
        if( product_id == RS405_PID )
        {
            width = 848;
            height = 480;
        }
        stream_name_to_default_profile["Color"] = get_index_of_profile( stream_name_to_profiles.at( "Color" ),
            realdds::dds_video_stream_profile( fps, realdds::dds_video_encoding::from_rs2( RS2_FORMAT_YUYV ), width, height ) );
    }
}


size_t lrs_device_controller::get_index_of_profile( const realdds::dds_stream_profiles & profiles,
                                                    const realdds::dds_video_stream_profile & profile ) const
{
    for( size_t i = 0; i < profiles.size(); ++i )
    {
        auto dds_vp = std::dynamic_pointer_cast< dds_video_stream_profile >( profiles[i] );
        if( dds_vp->frequency() == profile.frequency()
            && dds_vp->encoding() == profile.encoding()
            && dds_vp->width()  == profile.width()
            && dds_vp->height() == profile.height() )
            return i;
    }

    return 0;
}


size_t lrs_device_controller::get_index_of_profile( const realdds::dds_stream_profiles & profiles,
                                                    const realdds::dds_motion_stream_profile & profile ) const
{
    for( size_t i = 0; i < profiles.size(); ++i )
    {
        if( profiles[i]->frequency() == profile.frequency() )
            return i;
    }

    return 0;
}


bool lrs_device_controller::on_control( std::string const & id, json const & control, json & reply )
{
    static std::map< std::string, bool ( lrs_device_controller::* )( json const &, json & ) > const control_handlers{
        { realdds::topics::control::hw_reset::id, &lrs_device_controller::on_hardware_reset },
        { realdds::topics::control::open_streams::id, &lrs_device_controller::on_open_streams },
        { realdds::topics::control::hwm::id, &lrs_device_controller::on_hwm },
        { realdds::topics::control::dfu_start::id, &lrs_device_controller::on_dfu_start },
        { realdds::topics::control::dfu_apply::id, &lrs_device_controller::on_dfu_apply },
    };
    auto it = control_handlers.find( id );
    if( it == control_handlers.end() )
        return false;

    return (this->*(it->second))( control, reply );
}


bool lrs_device_controller::on_hardware_reset( json const & control, json & reply )
{
    _dds_device_server->broadcast_disconnect();
    _rs_dev.hardware_reset();
    return true;
}


bool lrs_device_controller::on_hwm( json const & control, json & reply )
{
    rs2::debug_protocol dp( _rs_dev );
    if( ! dp )
        throw std::runtime_error( "device does not have a debug protocol implemented" );

    rsutils::string::hexarray data;

    uint32_t opcode;
    if( control.nested( topics::control::hwm::key::opcode ).get_ex( opcode ) )
    {
        // In the presence of 'opcode', we're asked to build the command using optional parameters
        uint32_t param1 = 0, param2 = 0, param3 = 0, param4 = 0;
        control.nested( topics::control::hwm::key::param1 ).get_ex( param1 );
        control.nested( topics::control::hwm::key::param2 ).get_ex( param2 );
        control.nested( topics::control::hwm::key::param3 ).get_ex( param3 );
        control.nested( topics::control::hwm::key::param4 ).get_ex( param4 );

        control.nested( topics::control::hwm::key::data ).get_ex( data );  // optional

        // Build the HWM command
        data = dp.build_command( opcode, param1, param2, param3, param4, data.get_bytes() );

        // And, if told to not actually run it, we return the HWM command
        if( control.nested( topics::control::hwm::key::build_command ).default_value( false ) )
        {
            reply[topics::reply::hwm::key::data] = data;
            return true;
        }
    }
    else
    {
        if( ! control.nested( topics::control::hwm::key::data ).get_ex( data ) )
            throw std::runtime_error( "no 'data' in HWM control" );
    }

    data = dp.send_and_receive_raw_data( data.get_bytes() );
    reply[topics::reply::hwm::key::data] = data;
    return true;
}


struct lrs_device_controller::dfu_support
{
    rs2::device rsdev;
    std::string uid;
    std::weak_ptr< dds_device_server > server;
    std::weak_ptr< lrs_device_controller > controller;
    std::shared_ptr< realdds::dds_topic_reader > reader;
    std::shared_ptr< realdds::topics::blob_msg > image;
    realdds::dds_guid_prefix initiator;

    std::string debug_name() const
    {
        if( auto dev = server.lock() )
            return rsutils::string::from() << "[" << dev->debug_name() << "] ";
        return {};
    }
};


bool lrs_device_controller::on_dfu_start( rsutils::json const & control, rsutils::json & reply )
{
    if( _dfu )
        throw std::runtime_error( "DFU already in progress" );

    // The FW-update-ID is used to recognize the device, even in recovery mode (serial-number is not enough)
    if( ! _rs_dev.supports( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID ) )
        throw std::runtime_error( "device does not support fw-update-id" );

    // Either updatable (regular device) or update_device (recovery) implement the check_fw_compatibility API, from
    // which we can proceed
    if( ! rs2::updatable( _rs_dev ) && ! rs2::update_device( _rs_dev ) )
        throw std::runtime_error( "device is not in an updatable state" );

    _dfu = std::make_shared< dfu_support >();
    _dfu->server = _dds_device_server;
    _dfu->controller = shared_from_this();
    _dfu->rsdev = _rs_dev;
    _dfu->initiator
        = realdds::guid_from_string( reply[realdds::topics::reply::key::sample][0].string_ref() ).guidPrefix;
    _dfu->uid = _rs_dev.get_info( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID );

    // Open a DFU topic and wait for the image on another thread
    auto topic = topics::blob_msg::create_topic( _dds_device_server->participant(),
                                                 _dds_device_server->topic_root() + realdds::topics::DFU_TOPIC_NAME );

    _dfu->reader = std::make_shared< dds_topic_reader_thread >( topic, _dds_device_server->subscriber() );
    _dfu->reader->on_data_available(
        [weak_dfu = std::weak_ptr< dfu_support >( _dfu )]
        {
            topics::blob_msg blob;
            eprosima::fastdds::dds::SampleInfo sample;
            while( auto dfu = weak_dfu.lock() )
            {
                if( ! topics::blob_msg::take_next( *dfu->reader, &blob, &sample ) )
                    break;
                if( ! blob.is_valid() )
                    continue;
                auto const & sender = sample.sample_identity.writer_guid().guidPrefix;
                if( sender != dfu->initiator )
                {
                    LOG_ERROR( dfu->debug_name()
                               << "DFU image received from " << realdds::print_raw_guid_prefix( sender ) << " != "
                               << realdds::print_raw_guid_prefix( dfu->initiator ) << " that started the DFU" );
                }
                else if( dfu->image )
                {
                    LOG_ERROR( dfu->debug_name() << "More than one DFU image received!" );
                }
                else
                {
                    size_t const n_bytes = blob.data().size();
                    auto const crc = rsutils::number::calc_crc32( blob.data().data(), blob.data().size() );
                    LOG_INFO( dfu->debug_name() << "DFU image received, " << n_bytes << " bytes, crc " << crc );

                    // Build a reply
                    rsutils::json j = rsutils::json::object( {
                        { realdds::topics::notification::key::id, realdds::topics::notification::dfu_ready::id },
                        { "size", n_bytes },
                        { "crc", crc } } );

                    try
                    {
                        // Check the image
                        rs2_error * e = nullptr;
                        bool is_compatible = rs2_check_firmware_compatibility( dfu->rsdev.get().get(),
                                                                               blob.data().data(),
                                                                               (int)blob.data().size(),
                                                                               &e );
                        rs2::error::handle( e );

                        if( ! is_compatible )
                            throw std::runtime_error( "image is incompatible" );

                        // Keep it until we get a 'dfu-apply'
                        dfu->image = std::make_shared< topics::blob_msg >( std::move( blob ) );
                    }
                    catch( std::exception const & e )
                    {
                        j[realdds::topics::reply::key::status] = "check-fw-compat";
                        j[realdds::topics::reply::key::explanation] = e.what();
                        LOG_ERROR( dfu->debug_name() << "DFU image check failed: " << e.what() << "; exiting DFU state" );
                        if( auto controller = dfu->controller.lock() )
                            controller->_dfu.reset();  // no longer in DFU state
                    }

                    if( auto server = dfu->server.lock() )
                        server->publish_notification( std::move( j ) );
                }
            }
        } );

    dds_topic_reader::qos rqos( eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS );
    rqos.override_from_json( _dds_device_server->participant()->settings().nested( "device", "dfu" ) );
    _dfu->reader->run( rqos );

    // Start a thread that will cancel if we don't receive an image in time, or if the process somehow takes too long
    std::thread(
        [weak_dfu = std::weak_ptr< dfu_support >( _dfu )]
        {
            std::this_thread::sleep_for( std::chrono::seconds( 5 ) );
            if( auto dfu = weak_dfu.lock() )
            {
                if( ! dfu->image )
                {
                    if( auto controller = dfu->controller.lock() )
                    {
                        LOG_ERROR( dfu->debug_name() << "DFU timed out waiting for image; resetting" );
                        controller->_dfu.reset();  // no longer in DFU state
                    }
                    if( auto server = dfu->server.lock() )
                    {
                        rsutils::json j = rsutils::json::object(
                            { { realdds::topics::notification::key::id, realdds::topics::notification::dfu_ready::id },
                              { realdds::topics::reply::key::status, "error" },
                              { realdds::topics::reply::key::explanation,
                                "timed out waiting for an image; resetting" } } );
                        server->publish_notification( std::move( j ) );
                    }
                    return;
                }
            }
            else
                return;

            // We have an image: sleep some more and cancel the DFU after a while if no dfu-apply is received
            std::this_thread::sleep_for( std::chrono::seconds( 10 ) );
            if( auto dfu = weak_dfu.lock() )
            {
                if( auto controller = dfu->controller.lock() )
                {
                    LOG_ERROR( dfu->debug_name() << "DFU timed out waiting for apply; resetting" );
                    controller->_dfu.reset();  // no longer in DFU state
                }
                if( auto server = dfu->server.lock() )
                {
                    rsutils::json j = rsutils::json::object(
                        { { realdds::topics::notification::key::id, realdds::topics::notification::dfu_ready::id },
                          { realdds::topics::reply::key::status, "error" },
                          { realdds::topics::reply::key::explanation, "timed out; resetting" } } );
                    server->publish_notification( std::move( j ) );
                }
            }
        } )
        .detach();

    return true;  // we handled it
}


bool lrs_device_controller::on_dfu_apply( rsutils::json const & control, rsutils::json & reply )
{
    if( _dfu )
    {
        auto const sender
            = realdds::guid_from_string( reply[realdds::topics::reply::key::sample][0].string_ref() ).guidPrefix;
        if( sender != _dfu->initiator )
            throw std::runtime_error( "only the DFU initiator can apply/cancel" );
    }

    if( control.nested( realdds::topics::control::dfu_apply::key::cancel ).default_value( false ) )
    {
        if( _dfu )
            LOG_DEBUG( _dfu->debug_name() << "DFU cancelled" );
        _dfu.reset();
        return true;  // handled
    }

    if( ! _dfu )
        throw std::runtime_error( "DFU not started" );
    if( ! _dfu->reader )
        throw std::runtime_error( "dfu-apply already received" );

    // Keep the DFU alive once apply starts, even if reset somewhere else - but the reader is no longer needed
    auto dfu = _dfu;
    _dfu->reader.reset();

    if( ! _dfu->image )
        throw std::runtime_error( "no image received" );

    // We want to return a reply right away; we already have an image in hand, so all that needs to happen is to apply
    // it in the background and return a reply when done
    std::thread(
        [dfu]
        {
            auto on_fail = [dfu]( char const * what )
            {
                LOG_ERROR( "Failed to apply DFU image: " << what );
                if( auto server = dfu->server.lock() )
                {
                    rsutils::json j
                        = json::object( { { realdds::topics::reply::key::id, realdds::topics::reply::dfu_apply::id },
                                          { realdds::topics::reply::key::status, "error" } } );
                    if( what )
                        j[realdds::topics::reply::key::explanation] = what;
                    server->publish_notification( std::move( j ) );
                }
            };

            try
            {
                // We already checked FW compatibility; to actually do a signed update, we switch to recovery mode (we need
                // an update_device to do a signed update - but to get one, we need to be in recovery mode):
                rs2::update_device update_device = dfu->rsdev;
                if( ! update_device )
                {
                    LOG_INFO( "Switching device (update ID " << dfu->uid << ") to recovery mode" );
                    if( auto server = dfu->server.lock() )
                        server->broadcast_disconnect( { 3, 0 } );  // timeout for ACKs
                    rs2::updatable( dfu->rsdev ).enter_update_state();
                    // NOTE: the device will go offline; the adapter will see the device disappear, and so will the controller/server!

                    // We need a context but cannot get one from the device... so we can just create a new one:
                    rsutils::json settings;
                    settings["dds"] = false;  // don't want DDS devices
                    rs2::context context( settings.dump() );

                    // Wait for the recovery device
                    rsutils::time::timer timeout( std::chrono::seconds( 60 ) );
                    while( dfu->controller.lock() )
                    {
                        try
                        {
                            auto devs = context.query_devices( RS2_PRODUCT_LINE_ANY_INTEL );
                            for( uint32_t j = 0; j < devs.size(); j++ )
                            {
                                auto d = devs[j];
                                if( d.is< rs2::update_device >() && d.supports( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID )
                                    && dfu->uid == d.get_info( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID ) )
                                {
                                    LOG_DEBUG( "... found DFU recovery device (update ID " << dfu->uid << ")" );
                                    update_device = d;
                                    break;
                                }
                            }
                        }
                        catch( std::exception const & e )
                        {
                            LOG_DEBUG( "... error looking for DFU device: " << e.what() );
                        }
                        catch( ... )
                        {
                            LOG_DEBUG( "... error looking for DFU device" );
                        }

                        if( update_device )
                            break;

                        if( timeout.has_expired() )
                            throw std::runtime_error( "failed to find recovery device" );

                        // Wait a little and retry
                        std::this_thread::sleep_for( std::chrono::milliseconds( 1000 ) );
                    }
                }
                if( update_device )
                {
                    if( auto controller = dfu->controller.lock() )
                    {
                        LOG_INFO( "Updating - DO NOT SHUT DOWN" );
                        int progress = 0;
                        update_device.update(
                            dfu->image->data(),
                            [dfu, progress = int( 0 )]( float percent ) mutable
                            {
                                int new_progress = int( percent * 10 );  // update only every 10%
                                if( new_progress != progress )
                                {
                                    progress = new_progress;
                                    if( auto server = dfu->server.lock() )
                                        server->publish_notification(
                                            json::object( { { realdds::topics::notification::key::id,
                                                              realdds::topics::notification::dfu_apply::id },
                                                            { realdds::topics::notification::dfu_apply::key::progress,
                                                              percent } } ) );
                                }
                            } );
                        // NOTE: the device will go offline again, and should come up normally
                        LOG_INFO( "DFU done" );
                    }
                }
            }
            catch( std::exception const & e )
            {
                on_fail( e.what() );
            }
            catch( ... )
            {
                on_fail( nullptr );
            }

            // Whether successful or not, we're done with the DFU
            if( auto controller = dfu->controller.lock() )
                controller->_dfu.reset();
        } )
        .detach();

    return true;  // handled
}