haply 0.8.2

//! Device module provides the core functionality for interacting with Haply haptic devices.
//! It handles device communication, state management, and command processing through both
//! HTTP and WebSocket protocols.

use crate::device_model::{
    Command,
    Config,
    ExtensionDataInput,
    ForceInput,
    I3Command,
    Inverse3Device,
    Inverse3Msg,
    PositionInput,
    ProbeAngularPosition,
    ProbeCursorPosition,
    ProbeOrientation,
    ServiceData,
    ServiceMsg,
    ServiceState,
    SetCursorForce,
    SetCursorPosition,
    SetExtensionData,
    VerseGripDevice,
    VerseGripMsg,
    VersionResponse,
    VgCommand,
    WirelessVerseGripDevice,
    CustomVerseGripDevice,
    TimestampedServiceData
};
use crate::http::InverseHttpClient;
use crate::websocket::InverseWebSocketClient;
use log::{error, trace};
use std::sync::Arc;
use tokio::sync::Mutex;
use serde_json::Value;
use serde_json;
use crate::device_model::Vec3D;

#[cfg(test)]
#[path = "unit_tests.rs"]
mod unit_tests;

/// Main struct representing a Haply haptic device.
/// Handles all communication and state management for a connected device.
pub struct HaplyDevice {
    http_client: InverseHttpClient,
    pub ws_client: InverseWebSocketClient,
    /// Cached device state
    pub state: Arc<Mutex<TimestampedServiceData>>,
    /// Cached service cmd msg that will be sent to the device on `send_command` call
    /// 
    /// It's accumulating commands until sent
    pub device_cmd_msg: Arc<Mutex<ServiceMsg>>,
}

impl HaplyDevice {
    /// Creates a new HaplyDevice instance and establishes connections.
    /// Initializes both HTTP and WebSocket connections, sets up state management,
    /// and begins listening for device updates.
    pub async fn new(
        http_base: &str,
        ws_url: &str
    ) -> Result<Self, Box<dyn std::error::Error + Send + Sync>> {
        let http_client = InverseHttpClient::new(http_base);
        let mut ws_client = InverseWebSocketClient::new(ws_url);
        ws_client.connect().await?;

        let initial_state = TimestampedServiceData {
            data: ServiceData::default(),
            timestamp: std::time::Instant::now(),
        };
        let device = Self {
            http_client,
            ws_client: ws_client.clone(),
            state: Arc::new(Mutex::new(initial_state)),
            device_cmd_msg: Arc::new(Mutex::new(ServiceMsg::default())),
        };

        device.start_listening();
        Ok(device)
    }

    /// shutdown the device and its connections
    /// This will close the WebSocket connection and clean up resources.
    pub async fn shutdown(&mut self) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
        self.ws_client.disconnect().await
    }

    /// Starts the WebSocket listening loop for device updates.
    /// Creates an asynchronous task that continuously monitors for
    /// device state updates and processes them accordingly.
    fn start_listening(&self) {
        let state_clone = Arc::clone(&self.state);
        let mut ws_client = self.ws_client.clone();
        tokio::spawn(async move {
            let mut first_msg = true;
            ws_client.listen( move |message| {
                let state_clone = Arc::clone(&state_clone);
                async move {
                    update_state_on_message(state_clone, message, &mut first_msg).await;
                }
            }).await;
        });
    }

    /// Retrieves the current state of the device.
    /// Returns a clone of the complete service data containing
    /// state information for all connected devices.
    pub async fn read_state(
        &self
    ) -> Result<ServiceData, Box<dyn std::error::Error + Send + Sync>> {
        trace!("Reading device state");
        Ok(self.state.lock().await.data.clone())
    }

    /// Forces a full state update from the service then returns the current state.
    /// This method sends a command to the service to refresh its state
    /// and then retrieves the updated state.
    pub async fn force_read_full_state(
        &mut self,
        timeout: Option<std::time::Duration>
    ) -> Result<ServiceData, Box<dyn std::error::Error + Send + Sync>> {
        // Start the timer
        let start = std::time::Instant::now();
        self.send_force_full_render().await?;
        if let Some(dur) = timeout {
            loop {
                let elapsed = start.elapsed();
                if elapsed >= dur {
                    error!("Timeout waiting for full state update");
                    return Err("Timeout waiting for full state update".into());
                }
                let last_update = self.state.lock().await;
                if last_update.timestamp >= start {
                    trace!("State updated after force render");
                    break;
                }
                tokio::time::sleep(std::time::Duration::from_millis(10)).await;
            }
        }
        self.read_state().await
    }

    /// Updates force values for one or more devices.
    /// Sends force commands to specified devices, with options for immediate transmission.
    pub async fn update_force(
        &mut self,
        forces_input: Vec<ForceInput>,
        execute_check: Option<bool>,
        send: Option<bool>
    ) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
        // // if all forces are zero set execute to false otherwise true
        // let execute_check = forces_input.iter().any(|force_input| {
        //     force_input.forces.x != 0.0 || force_input.forces.y != 0.0 || force_input.forces.z != 0.0
        // });

        let mut commands = Vec::new();

        let mut set_cursor_force = SetCursorForce {
            values: Vec3D::default(),
            execute: execute_check.unwrap_or(false),
        };

        for force_input in forces_input {
            set_cursor_force.values = force_input.forces.clone();
            let i3_command = I3Command::SetCursorForce(set_cursor_force.clone());
            let inverse3_msg = Inverse3Msg {
                device_id: force_input.device_id.clone(),
                command: i3_command,
            };
            commands.push(Command::I3Command(inverse3_msg));
        }

        self.update_command_msg(commands).await?;

        if send.unwrap_or(false) {
            self.send_command().await?;
        }

        Ok(())
    }

    /// Updates position values for one or more devices.
    /// Sends position commands to specified devices, with options for immediate transmission.
    pub async fn update_position(
        &mut self,
        positions_input: Vec<PositionInput>,
        send: Option<bool>
    ) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
        let mut commands = Vec::new();
        for position_input in positions_input {
            let set_cursor_position = SetCursorPosition {
                values: position_input.positions.clone(),
            };
            let i3_command = I3Command::SetCursorPosition(set_cursor_position);
            let inverse3_msg = Inverse3Msg {
                device_id: position_input.device_id.clone(),
                command: i3_command,
            };
            commands.push(Command::I3Command(inverse3_msg));
        }

        self.update_command_msg(commands).await?;

        if send.unwrap_or(false) {
            self.send_command().await?;
        }

        Ok(())
    }

    /// Updates extension data for wireless verse grip devices.
    /// Sends custom extension data to specified devices, with options for immediate transmission.
    pub async fn update_extension_data(
        &mut self,
        extension_data_inputs: Vec<ExtensionDataInput>,
        send: Option<bool>
    ) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
        let mut commands = Vec::new();
        for extension_data_input in extension_data_inputs {
            let set_extension_data = SetExtensionData {
                extension_data: extension_data_input.extension_data.clone(),
            };
            let vg_command = VgCommand::SetExtensionData(set_extension_data);
            let wireless_verse_grip_msg = VerseGripMsg {
                device_id: extension_data_input.device_id.clone(),
                command: vg_command,
            };
            commands.push(Command::WvgCommand(wireless_verse_grip_msg));
        }

        self.update_command_msg(commands).await?;

        if send.unwrap_or(false) {
            self.send_command().await?;
        }

        Ok(())
    }

    /// Requests cursor position information from specified devices.
    /// Sends probe commands and awaits position data in state updates.
    pub async fn probe_cursor_position(
        &mut self,
        device_ids: Vec<String>,
        send: Option<bool>
    ) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
        let mut commands = Vec::new();
        for device_id in device_ids {
            let probe_cursor_position = ProbeCursorPosition { probe_cursor_position: () };
            let i3_command = I3Command::ProbeCursorPosition(probe_cursor_position);
            let inverse3_msg = Inverse3Msg {
                device_id: device_id.clone(),
                command: i3_command,
            };
            commands.push(Command::I3Command(inverse3_msg));
        }

        self.update_command_msg(commands).await?;

        if send.unwrap_or(false) {
            self.send_command().await?;
        }

        Ok(())
    }

    /// Requests angular position information from specified devices.
    /// Sends probe commands and awaits angular data in state updates.
    pub async fn probe_angular_position(
        &mut self,
        device_ids: Vec<String>,
        send: Option<bool>
    ) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
        let mut commands = Vec::new();
        for device_id in device_ids {
            let probe_angular_position = ProbeAngularPosition::default();
            let i3_command = I3Command::ProbeAngularPosition(probe_angular_position);
            let inverse3_msg = Inverse3Msg {
                device_id: device_id.clone(),
                command: i3_command,
            };
            commands.push(Command::I3Command(inverse3_msg));
        }

        self.update_command_msg(commands).await?;

        if send.unwrap_or(false) {
            self.send_command().await?;
        }

        Ok(())
    }

    /// Requests orientation information from specified devices.
    /// Sends probe commands and awaits orientation data in state updates.
    pub async fn probe_orientation(
        &mut self,
        device_ids: Vec<String>,
        send: Option<bool>
    ) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
        let mut commands = Vec::new();
        for device_id in device_ids {
            let probe_orientation = ProbeOrientation::default();
            let vg_command = VgCommand::ProbeOrientation(probe_orientation);
            let wireless_verse_grip_msg = VerseGripMsg {
                device_id: device_id.clone(),
                command: vg_command,
            };
            commands.push(Command::WvgCommand(wireless_verse_grip_msg));
        }

        self.update_command_msg(commands).await?;

        if send.unwrap_or(false) {
            self.send_command().await?;
        }

        Ok(())
    }

    /// Sends a force full render command to the service
    /// This command is used to trigger a full force rendering update
    pub async fn send_force_full_render(
        &mut self
    ) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
        let command = vec![Command::ForceRenderFullState];
        self.update_command_msg(command).await?;
        self.send_command().await?;
        Ok(())
    }

    /// Internal helper to update the command message cache.
    /// Processes commands and prepares them for transmission.
    pub async fn update_command_msg(
        &mut self,
        commands: Vec<Command>
    ) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
        // retrieve msg for service from internal data
        let mut service_msg = self.device_cmd_msg.lock().await;
        update_entrire_msg(&commands, &mut service_msg)?;
        Ok(())
    }

    /// Sends the cached command message to the device.
    /// Serializes and transmits the command through the WebSocket connection.
    pub async fn send_command(&mut self) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
        // retrieve msg for service from internal data
        let msg = self.device_cmd_msg.lock().await;
        let cmd_out = msg.clone();
        // println!("Sending command message: {:?}", cmd_out);
        // Serialize the message to JSON and send it through the WebSocket stream
        let cmd_text = serde_json::to_string(&cmd_out)?;
        // Convert struct to JSON
        // println!("Sending command: {}", cmd_text);
        self.ws_client.send_message(&cmd_text).await?;
        Ok(())
    }

    /// Lists all available Haply devices.
    /// Queries the HTTP endpoint for connected devices and returns their full configuration data.
    pub async fn list_devices(
        &self
    ) -> Result<Vec<Config>, Box<dyn std::error::Error + Send + Sync>> {
        let configs = self.http_client.get_devices().await?;
        Ok(configs)
    }

    /// Returns all device configs from the cached service state.
    pub async fn get_devices(&self) -> Result<Vec<Config>, Box<dyn std::error::Error + Send + Sync>> {
        let state = self.state.lock().await;
        let mut configs = Vec::new();
        for d in &state.data.inverse3 {
            let config = d.config.clone().ok_or("Missing config for Inverse3 device")?;
            configs.push(Config::DeviceConfig(config));
        }
        for d in &state.data.verse_grip {
            let config = d.config.clone().ok_or("Missing config for Verse Grip device")?;
            configs.push(Config::VGConfig(config));
        }
        for d in &state.data.wireless_verse_grip {
            let config = d.config.clone().ok_or("Missing config for Wireless Verse Grip device")?;
            configs.push(Config::WVGConfig(config));
        }
        for d in &state.data.custom_verse_grip {
            let config = d.config.clone().ok_or("Missing config for Custom Verse Grip device")?;
            configs.push(Config::WVGConfig(config));
        }
        Ok(configs)
    }

    /// Finds a specific device by ID.
    /// Searches through available devices and returns matching device configuration.
    pub async fn get_device_info(
        &self,
        device_id: &str
    ) -> Result<Config, Box<dyn std::error::Error + Send + Sync>> {
        let devices = self.list_devices().await?;
        let device = devices.into_iter().find(|c| {
            match c {
                Config::DeviceConfig(dc) => dc.device_info.id == device_id,
                Config::VGConfig(vgc) => vgc.id == device_id,
                Config::WVGConfig(wvgc) => wvgc.id == device_id,
            }
        });
        match device {
            Some(cfg) => Ok(cfg),
            None => Err(format!("Device {} not found", device_id).into()),
        }
    }

    /// gets the service version from the HTTP client.
    pub async fn get_service_version(
        &self
    ) -> Result<VersionResponse, Box<dyn std::error::Error + Send + Sync>> {
        let version = self.http_client.get_version().await?;
        Ok(version)
    }
}

pub fn update_entrire_msg(
    commands: &[Command],
    msg: &mut ServiceMsg
) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
    for command in commands {
        if let Err(e) = update_msg_from_command(command.clone(), msg) {
            error!("Error while creating command message: {}", e);
            return Err(e);
        }
    }
    Ok(())
}

/// Updates the device state based on incoming WebSocket messages.
///
/// This function processes JSON messages received from the WebSocket connection.
/// It dynamically detects whether the message is:
/// - The first message (connection info)
/// - A full ServiceData response (with config information)
/// - A ServiceState response (state updates only)
///
/// # Arguments
///
/// * `state` - An `Arc<Mutex<ServiceData>>` that holds the current state of the device.
/// * `message` - A `String` containing the JSON message received from the WebSocket.
/// * `first` - A mutable boolean flag indicating if this is the first message received.
pub async fn update_state_on_message(
    state: Arc<Mutex<TimestampedServiceData>>,
    message: String,
    first: &mut bool
) {
    // parse once into JSON Value
    let v: Value = match serde_json::from_str(&message) {
        Ok(val) => val,
        Err(e) => {
            error!("Invalid JSON from WS: {} | error: {}", message, e);
            return;
        }
    };
    // helper to detect if any device entry contains full config
    fn is_full_payload(v: &Value) -> bool {
        for key in &["inverse3", "verse_grip", "wireless_verse_grip", "custom_verse_grip"] {
            if let Some(arr) = v.get(key).and_then(Value::as_array) {
                if arr.iter().any(|item| item.get("config").is_some()) {
                    return true;
                }
            }
        }
        false
    }
    
    // normal loop: full merge or state-only update
    if is_full_payload(&v) {
        match serde_json::from_value::<ServiceData>(v) {
            Ok(sd) => {
                let mut lock = state.lock().await;
                let _ = update_service_struct(&mut lock.data, &sd.clone());
                // println!("Updated state from full payload: {:#?}", sd);
                // println!("Current cached state: {:#?}", lock.data);
                lock.timestamp = std::time::Instant::now();
                if *first {
                    trace!("Initialized from full payload");
                } else {
                    trace!("Merged full payload");
                }
            }
            Err(e) => {
                // println!("Failed to parse full payload: {}", e);
                if *first {
                    error!("Failed to parse first-message full payload: {}", e);
                } else {
                    error!("Failed to parse full payload: {}", e);
                }
            },
        }
    } else {
        // Try to debug the deserialization issue
        let result = serde_json::from_value::<ServiceState>(v.clone());
        match result {
            Ok(ss) => {
                update_state_from_service_state(state.clone(), ss).await;
                trace!("Applied state-only update");
            }
            Err(e) => {
                println!("Failed to parse state-only payload: {}", e);

                // Print field details for debugging
                if let Some(obj) = v.as_object() {
                    // Try to deserialize specific fields
                    for field in [
                        "inverse3",
                        "verse_grip",
                        "wireless_verse_grip",
                        "custom_verse_grip",
                        "session_id",
                    ].iter() {
                        if let Some(field_val) = obj.get(*field) {
                            println!("Field '{}' exists with type: {}", field, match field_val {
                                serde_json::Value::Null => "null",
                                serde_json::Value::Bool(_) => "boolean",
                                serde_json::Value::Number(_) => "number",
                                serde_json::Value::String(_) => "string",
                                serde_json::Value::Array(_) => "array",
                                serde_json::Value::Object(_) => "object",
                            });
                        } else {
                            println!("Field '{}' is missing", field);
                        }
                    }
                }
            }
        }
    }

    if *first {
        *first = false;
    }
}

/// Updates the device state using data from a ServiceState message (state-only updates)
async fn update_state_from_service_state(
    state: Arc<Mutex<TimestampedServiceData>>,
    state_only: ServiceState
) {

    trace!("Updating device state with partial data");
    let mut state_lock = state.lock().await;
    state_lock.timestamp = std::time::Instant::now();
    // Update or insert inverse3 devices
    for msg in &state_only.inverse3 {
        match state_lock.data.inverse3.iter_mut().find(|d| d.device_id == msg.device_id) {
            Some(dev) => {
                dev.state = msg.state.clone();
                dev.status = msg.status.clone();
                trace!("Updated inverse3 device state: {}", msg.device_id);
            }
            None => {
                // insert new placeholder config
                state_lock.data.inverse3.push(Inverse3Device {
                    device_id: msg.device_id.clone(),
                    config: None,
                    state: msg.state.clone(),
                    status: msg.status.clone(),
                });
                trace!("Added inverse3 device: {}", msg.device_id);
            }
        }
    }

    // Update or insert verse_grip devices
    for msg in &state_only.verse_grip {
        match state_lock.data.verse_grip.iter_mut().find(|d| d.device_id == msg.device_id) {
            Some(dev) => {
                dev.state = msg.state.clone();
                dev.status = msg.status.clone();
                trace!("Updated verse_grip device state: {}", msg.device_id);
            }
            None => {
                state_lock.data.verse_grip.push(VerseGripDevice {
                    device_id: msg.device_id.clone(),
                    config: None,
                    state: msg.state.clone(),
                    status: msg.status.clone(),
                });
                trace!("Added verse_grip device: {}", msg.device_id);
            }
        }
    }

    // Update or insert wireless_verse_grip devices
    for msg in &state_only.wireless_verse_grip {
        match state_lock.data.wireless_verse_grip.iter_mut().find(|d| d.device_id == msg.device_id) {
            Some(dev) => {
                dev.state = msg.state.clone();
                dev.status = msg.status.clone();
                trace!("Updated wireless_verse_grip device state: {}", msg.device_id);
            }
            None => {
                state_lock.data.wireless_verse_grip.push(WirelessVerseGripDevice {
                    device_id: msg.device_id.clone(),
                    config: None,
                    state: msg.state.clone(),
                    status: msg.status.clone(),
                });
                trace!("Added wireless_verse_grip device: {}", msg.device_id);
            }
        }
    }

    // Update or insert custom_verse_grip devices
    for msg in &state_only.custom_verse_grip {
        match state_lock.data.custom_verse_grip.iter_mut().find(|d| d.device_id == msg.device_id) {
            Some(dev) => {
                dev.state = msg.state.clone();
                dev.status = msg.status.clone();
                trace!("Updated custom_verse_grip device state: {}", msg.device_id);
            }
            None => {
                state_lock.data.custom_verse_grip.push(CustomVerseGripDevice {
                    device_id: msg.device_id.clone(),
                    config: None,
                    state: msg.state.clone(),
                    status: msg.status.clone(),
                });
                trace!("Added custom_verse_grip device: {}", msg.device_id);
            }
        }
    }

    // Now remove any devices not present in this partial update:
    state_lock.data.inverse3.retain(|d| {
        state_only.inverse3.iter().any(|msg| msg.device_id == d.device_id)
    });
    state_lock.data.verse_grip.retain(|d| {
        state_only.verse_grip.iter().any(|msg| msg.device_id == d.device_id)
    });
    state_lock.data.wireless_verse_grip.retain(|d| {
        state_only.wireless_verse_grip.iter().any(|msg| msg.device_id == d.device_id)
    });
    state_lock.data.custom_verse_grip.retain(|d| {
        state_only.custom_verse_grip.iter().any(|msg| msg.device_id == d.device_id)
    });

    // TODO: remove as part of full service 3.5 support
    // Hide wireless_verse_grip duplicates of custom_verse_grip devices
    let custom_ids: Vec<String> = state_lock.data.custom_verse_grip.iter()
        .map(|cd| cd.device_id.clone()).collect();
    state_lock.data.wireless_verse_grip.retain(|d| {
        !custom_ids.contains(&d.device_id)
    });

    // Update session_id if needed
    if state_only.session_id != 0 {
        state_lock.data.session_id = state_only.session_id;
    }

    trace!("Updated device state with partial data");
}

/// Merges incoming device state data with existing state.
/// This function ensures that device state is properly updated while maintaining
/// data for devices that haven't sent updates.
pub fn update_service_struct(
    service_data: &mut ServiceData,
    input_data: &ServiceData
) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
    // println!("Updating service data with full payload");
    // println!("Input data: {:?}", input_data);
    // println!("Current service data: {:?}", service_data);
    // Update inverse3 data
    for device in &input_data.inverse3 {
        match service_data.inverse3.iter_mut().find(|d| d.device_id == device.device_id) {
            Some(existing_device) => {
                *existing_device = device.clone();
            }
            None => service_data.inverse3.push(device.clone()),
        }
    }
    // println!("Updated ServiceData: {:?}", service_data);

    // Update verse_grip data
    for device in &input_data.verse_grip {
        match service_data.verse_grip.iter_mut().find(|d| d.device_id == device.device_id) {
            Some(existing_device) => {
                *existing_device = device.clone();
            }
            None => service_data.verse_grip.push(device.clone()),
        }
    }

    // Update wireless_verse_grip data
    for device in &input_data.wireless_verse_grip {
        match service_data.wireless_verse_grip.iter_mut().find(|d| d.device_id == device.device_id) {
            Some(existing_device) => {
                *existing_device = device.clone();
            }
            None => service_data.wireless_verse_grip.push(device.clone()),
        }
    }
    // Update custom_verse_grip data
    for device in &input_data.custom_verse_grip {
        match service_data.custom_verse_grip.iter_mut().find(|d| d.device_id == device.device_id) {
            Some(existing_device) => {
                *existing_device = device.clone();
            }
            None => service_data.custom_verse_grip.push(device.clone()),
        }
    }

    // After merging, prune any devices that disappeared in the new full‐payload:
    service_data.inverse3.retain(|d| {
        input_data.inverse3.iter().any(|nd| nd.device_id == d.device_id)
    });
    service_data.verse_grip.retain(|d| {
        input_data.verse_grip.iter().any(|nd| nd.device_id == d.device_id)
    });
    service_data.wireless_verse_grip.retain(|d| {
        input_data.wireless_verse_grip.iter().any(|nd| nd.device_id == d.device_id)
    });
    service_data.custom_verse_grip.retain(|d| {
        input_data.custom_verse_grip.iter().any(|nd| nd.device_id == d.device_id)
    });

    // TODO: remove as part of full service 3.5 support
    // Hide wireless_verse_grip duplicates of custom_verse_grip devices so consumers
    // only see the custom_verse_grip entry (which carries extension_data).
    let custom_ids: Vec<String> = service_data.custom_verse_grip.iter()
        .map(|cd| cd.device_id.clone()).collect();
    service_data.wireless_verse_grip.retain(|d| {
        !custom_ids.contains(&d.device_id)
    });

    // Update session_id if needed
    if input_data.session_id != 0 {
        service_data.session_id = input_data.session_id;
    }

    Ok(())
}

/// Processes individual command messages and updates the service message structure.
/// Handles different types of commands (I3Commands, VgCommands, WvgCommands)
/// and ensures they are properly formatted for transmission to the device.
pub fn update_msg_from_command(
    command: Command,
    msg: &mut ServiceMsg
) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
    match command {
        Command::I3Command(inverse3_msg) => {
            let id = &inverse3_msg.device_id;
            trace!("I3Commands received for ID: {:?}", id);
            // Find and modify the existing device with the matching ID
            if let Some(device) = msg.inverse3.iter_mut().find(|d| d.device_id == *id) {
                match &inverse3_msg.command {
                    I3Command::SetCursorForce(set_cursor_force) => {
                        device.command = I3Command::SetCursorForce(set_cursor_force.clone());
                    }
                    I3Command::SetCursorPosition(set_cursor_position) => {
                        device.command = I3Command::SetCursorPosition(
                            set_cursor_position.clone()
                        );
                    }
                    I3Command::ProbeCursorPosition(probe_cursor_position) => {
                        device.command = I3Command::ProbeCursorPosition(
                            probe_cursor_position.clone()
                        );
                    }
                    I3Command::ProbeAngularPosition(probe_angular_position) => {
                        device.command = I3Command::ProbeAngularPosition(
                            probe_angular_position.clone()
                        );
                    }
                }
            } else {
                msg.inverse3.push(inverse3_msg.clone());
            }
        }
        Command::VgCommand(verse_grip_msg) => {
            let id = &verse_grip_msg.device_id;
            trace!("VgCommands received for ID: {:?}", id);
            // Find and modify the existing device with the matching ID
            if let Some(device) = msg.verse_grip.iter_mut().find(|d| d.device_id == *id) {
                device.command = verse_grip_msg.command.clone();
            } else {
                msg.verse_grip.push(verse_grip_msg.clone());
            }
        }
        Command::WvgCommand(wireless_verse_grip_msg) => {
            let id = &wireless_verse_grip_msg.device_id;
            trace!("WirelessVgCommands received for ID: {:?}", id);
            // Find and modify the existing device with the matching ID
            if let Some(device) = msg.wireless_verse_grip.iter_mut().find(|d| d.device_id == *id) {
                match &wireless_verse_grip_msg.command {
                    VgCommand::SetExtensionData(set_extension_data) => {
                        device.command = VgCommand::SetExtensionData(set_extension_data.clone());
                    }
                    VgCommand::ProbeOrientation(_) => {
                        device.command = VgCommand::ProbeOrientation(ProbeOrientation {
                            probe_orientation: (),
                        });
                    }
                }
            } else {
                msg.wireless_verse_grip.push(wireless_verse_grip_msg.clone());
            }
            // do the same for custom verse grip
            if let Some(device) = msg.custom_verse_grip.iter_mut().find(|d| d.device_id == *id) {
                match &wireless_verse_grip_msg.command {
                    VgCommand::SetExtensionData(set_extension_data) => {
                        device.command = VgCommand::SetExtensionData(set_extension_data.clone());
                    }
                    VgCommand::ProbeOrientation(_) => {
                        device.command = VgCommand::ProbeOrientation(ProbeOrientation {
                            probe_orientation: (),
                        });
                    }
                }
            } else {
                msg.custom_verse_grip.push(wireless_verse_grip_msg.clone());
            }
        }
        Command::ForceRenderFullState => {
            // No modification needed for ServiceMsg
        }
    }
    Ok(())
}