use eyre::Result;
use std::collections::HashMap;
use std::sync::Arc;
use std::time::{Duration, SystemTime};
use tokio::sync::mpsc;
use tokio::sync::RwLock;
use tokio::time;
use tracing::{debug, error, info, trace, warn};

use crate::{
    actuator::{TypedCommandData, TypedFeedbackData},
    actuator_types::ActuatorConfiguration,
    robstride00::{RobStride00, RobStride00Command, RobStride00Feedback, RobStride00Parameter},
    robstride01::{RobStride01, RobStride01Command, RobStride01Feedback, RobStride01Parameter},
    robstride02::{RobStride02, RobStride02Command, RobStride02Feedback, RobStride02Parameter},
    robstride03::{RobStride03, RobStride03Command, RobStride03Feedback, RobStride03Parameter},
    robstride04::{RobStride04, RobStride04Command, RobStride04Feedback, RobStride04Parameter},
    transport::TransportType,
    Actuator, Command, ControlCommand, FeedbackFrame, Frame, Protocol, TxCommand,
};
use crate::{ActuatorType, ObtainIDCommand};

// Store the latest feedback with timestamp
#[derive(Clone)]
pub struct ActuatorState {
    pub feedback: Option<FeedbackFrame>,
    pub last_feedback: SystemTime,
    pub ready: bool,
    pub enabled: bool,
    pub control_config: ControlConfig,
    pub control_command: ControlCommand,
    pub configuration: ActuatorConfiguration,
    pub messages_received: u64,
    pub half_revolutions: i32,
    pub actuator_type: ActuatorType,
}

// Separate configuration from command values
#[derive(Clone)]
pub struct ControlConfig {
    pub kp: f32,
    pub kd: f32,
    pub max_torque: Option<f32>,
    pub max_velocity: Option<f32>,
    pub max_current: Option<f32>,
}

pub struct TransportHandler {
    #[allow(unused)]
    protocol: Protocol,
    tx: mpsc::Sender<TxCommand>,
    #[allow(unused)]
    rx: mpsc::Receiver<TxCommand>,
}

pub struct SupervisorState {
    states: HashMap<u8, ActuatorState>,
    discovered_ids: Vec<u8>,
    last_stats_time: SystemTime,
}

pub struct Supervisor {
    actuators: Arc<RwLock<HashMap<u8, Box<dyn Actuator>>>>,
    transports: Arc<RwLock<HashMap<String, TransportHandler>>>,
    state: Arc<RwLock<SupervisorState>>,
    feedback_timeout: Duration,
}

fn half_revolutions(degrees: f32) -> i32 {
    if degrees < 0.0 {
        (degrees / 180.0).ceil() as i32
    } else {
        (degrees / 180.0).floor() as i32
    }
}

fn normalize_degrees(degrees: f32) -> (f32, i32) {
    let rotations = half_revolutions(degrees);
    let normalized =
        degrees - 360.0 * ((rotations + if rotations >= 0 { 1 } else { -1 }) / 2) as f32;
    (normalized, rotations)
}

fn normalize_radians(radians: f32) -> (f32, i32) {
    let degrees = radians.to_degrees();
    let (normalized_deg, rotations) = normalize_degrees(degrees);
    (normalized_deg.to_radians(), rotations)
}

fn denormalize_degrees(normalized_angle: f32, half_rotations: i32) -> f32 {
    if half_rotations >= 0 {
        normalized_angle + 360.0 * ((half_rotations + 1) / 2) as f32
    } else {
        normalized_angle + 360.0 * (half_rotations / 2) as f32
    }
}
fn denormalize_radians(normalized_radians: f32, half_rotations: i32) -> f32 {
    let normalized_degrees = normalized_radians.to_degrees();
    let original_degrees = denormalize_degrees(normalized_degrees, half_rotations);
    original_degrees.to_radians()
}

impl Supervisor {
    pub fn new(feedback_timeout: Duration) -> Result<Self> {
        let state = Arc::new(RwLock::new(SupervisorState {
            states: HashMap::new(),
            discovered_ids: Vec::new(),
            last_stats_time: SystemTime::now(),
        }));

        Ok(Self {
            actuators: Arc::new(RwLock::new(HashMap::new())),
            transports: Arc::new(RwLock::new(HashMap::new())),
            state,
            feedback_timeout,
        })
    }

    pub fn clone_controller(&self) -> Self {
        Self {
            actuators: self.actuators.clone(),
            transports: self.transports.clone(),
            state: self.state.clone(),
            feedback_timeout: self.feedback_timeout,
        }
    }

    pub async fn add_transport(&self, name: String, transport: TransportType) -> Result<()> {
        info!("Adding transport: {}", name);
        let (tx, mut rx) = mpsc::channel(32);
        let state = self.state.clone();
        let name_clone = name.clone();
        let name_for_log = name_clone.clone();

        // Create callback for frame processing
        let frame_callback: Arc<dyn Fn(u32, Vec<u8>) + Send + Sync + 'static> =
            Arc::new(move |id: u32, data: Vec<u8>| {
                let cmd = Command::from_can_packet(id, data.clone());
                trace!(
                    "Transport callback received: id={:x}, data={:02x?}, cmd={:?}",
                    id,
                    data,
                    cmd
                );

                if let Ok(cmd_frame) = cmd.to_frame() {
                    if let Ok(mut state) = state.try_write() {
                        match cmd_frame {
                            Frame::Feedback(feedback) => {
                                if let Some(actuator_state) =
                                    state.states.get_mut(&feedback.motor_id)
                                {
                                    actuator_state.feedback = Some(feedback.clone());
                                    actuator_state.last_feedback = SystemTime::now();
                                    actuator_state.messages_received += 1;
                                    if actuator_state.messages_received >= 5 {
                                        // robstride lol
                                        // skipping 5 messages to make sure the actuator is ready
                                        actuator_state.ready = true;
                                    }

                                    if !actuator_state.enabled {
                                        // save last pos to move to it on actuator enable
                                        actuator_state.control_command.target_angle =
                                            feedback.angle;
                                    }

                                    actuator_state.half_revolutions =
                                        normalize_radians(feedback.angle).1;
                                }
                            }
                            Frame::ObtainID(ObtainIDCommand { host_id }) => {
                                let motor_id = host_id;
                                if !state.discovered_ids.contains(&motor_id) {
                                    state.discovered_ids.push(motor_id);
                                    info!("Discovered new actuator ID: {}", motor_id);
                                }
                            }
                            Frame::Read(read_command) => {
                                info!(
                                    "param: 0x{:x?} ({:?}) value={:.3}",
                                    read_command.parameter_index,
                                    RobStride04Parameter::from_index(read_command.parameter_index)
                                        .unwrap_or(RobStride04Parameter::Unknown),
                                    read_command.data_as_f32()
                                );
                            }
                            Frame::Fault(fault) => {
                                warn!("Fault: {:?}", fault);
                            }
                            _ => trace!("received: {:?}", cmd_frame),
                        }
                    } else {
                        trace!("Failed to acquire state lock in transport callback");
                    }
                } else {
                    warn!("Failed to parse frame from command: {:?}", cmd);
                }
            });

        let protocol = Protocol::new(transport.clone(), frame_callback);
        debug!("Created protocol for transport: {}", name);

        // Spawn the transport handling task
        let mut protocol_clone = protocol.clone();
        tokio::spawn(async move {
            info!("Starting transport handling task for {}", name_clone);
            loop {
                tokio::select! {
                    // Handle incoming messages
                    recv_result = protocol_clone.recv() => {
                        match recv_result {
                            Ok(_) => trace!("Received message successfully"),
                            Err(e) => {
                                error!("Transport receiver error: {}", e);
                                break;
                            }
                        }
                    }
                    // Handle outgoing messages
                    Some(cmd) = rx.recv() => {
                        trace!("Processing outgoing command: {:?}", cmd);
                        match cmd {
                            TxCommand::Send { id, data } => {
                                if let Err(e) = protocol_clone.send(id, &data).await {
                                    error!("Transport sender error: {}", e);
                                }
                            }
                        }
                    }
                }
            }
        });

        let mut transports = self.transports.write().await;
        transports.insert(
            name,
            TransportHandler {
                protocol,
                tx: tx.clone(),
                rx: mpsc::channel(32).1,
            },
        );
        info!("Transport {} added successfully", name_for_log);

        Ok(())
    }

    pub async fn get_transport_tx(&self, transport_name: &str) -> Result<mpsc::Sender<TxCommand>> {
        let transports = self.transports.read().await;
        let transport = transports
            .get(transport_name)
            .ok_or_else(|| eyre::eyre!("Transport not found: {}", transport_name))?;
        Ok(transport.tx.clone())
    }

    pub async fn scan_bus(
        &mut self,
        host_id: u8,
        transport_name: &str,
        actuator_configs: &[(u8, ActuatorConfiguration)],
    ) -> Result<Vec<u8>> {
        let transport_tx = self.get_transport_tx(transport_name).await?;

        // Clear previously discovered IDs
        {
            let mut state = self.state.write().await;
            state.discovered_ids.clear();
        }

        // Send get_uuid to all possible IDs
        for id in 0..=0xFF {
            // Use desired type if specified, otherwise default to RobStride04
            let actuator: Box<dyn Actuator> = match actuator_configs
                .iter()
                .find(|(desired_id, _)| *desired_id == id)
            {
                Some((_, config)) => match config.actuator_type {
                    ActuatorType::RobStride00 => {
                        Box::new(RobStride00::new(id, host_id, transport_tx.clone()))
                    }
                    ActuatorType::RobStride01 => {
                        Box::new(RobStride01::new(id, host_id, transport_tx.clone()))
                    }
                    ActuatorType::RobStride02 => {
                        Box::new(RobStride02::new(id, host_id, transport_tx.clone()))
                    }
                    ActuatorType::RobStride03 => {
                        Box::new(RobStride03::new(id, host_id, transport_tx.clone()))
                    }
                    ActuatorType::RobStride04 => {
                        Box::new(RobStride04::new(id, host_id, transport_tx.clone()))
                    }
                },
                None => Box::new(RobStride04::new(id, host_id, transport_tx.clone())),
            };
            let _ = actuator.get_uuid().await;
            time::sleep(Duration::from_millis(1)).await;
        }

        // Wait for responses with timeout
        let timeout = Duration::from_millis(100);
        let scan_end = SystemTime::now() + timeout;

        while SystemTime::now() < scan_end {
            // Get a snapshot of discovered IDs
            let discovered_ids = {
                let state = self.state.read().await;
                state.discovered_ids.clone()
            };

            // Process any new IDs
            for id in discovered_ids {
                if !self.actuators.read().await.contains_key(&id) {
                    // Create actuator with desired type or default
                    let (actuator, configuration): (Box<dyn Actuator>, ActuatorConfiguration) =
                        match actuator_configs
                            .iter()
                            .find(|(desired_id, _)| *desired_id == id)
                        {
                            Some((_, config)) => match config.actuator_type {
                                ActuatorType::RobStride00 => (
                                    Box::new(RobStride00::new(id, host_id, transport_tx.clone())),
                                    config.clone(),
                                ),
                                ActuatorType::RobStride01 => (
                                    Box::new(RobStride01::new(id, host_id, transport_tx.clone())),
                                    config.clone(),
                                ),
                                ActuatorType::RobStride02 => (
                                    Box::new(RobStride02::new(id, host_id, transport_tx.clone())),
                                    config.clone(),
                                ),
                                ActuatorType::RobStride03 => (
                                    Box::new(RobStride03::new(id, host_id, transport_tx.clone())),
                                    config.clone(),
                                ),
                                ActuatorType::RobStride04 => (
                                    Box::new(RobStride04::new(id, host_id, transport_tx.clone())),
                                    config.clone(),
                                ),
                            },
                            None => (
                                Box::new(RobStride04::new(id, host_id, transport_tx.clone())),
                                ActuatorConfiguration {
                                    actuator_type: ActuatorType::RobStride04,
                                    max_angle_change: Some(1.0),
                                    max_velocity: None,
                                },
                            ),
                        };
                    self.add_actuator(actuator, configuration).await;
                    debug!(
                        "Added actuator with ID: {} (type: {:?}) on {}",
                        id,
                        actuator_configs
                            .iter()
                            .find(|(desired_id, _)| *desired_id == id)
                            .map(|(_, config)| config.actuator_type)
                            .unwrap_or(ActuatorType::RobStride04),
                        transport_name
                    );
                }
            }
            time::sleep(Duration::from_millis(1)).await;
        }

        let state = self.state.read().await;
        Ok(state.discovered_ids.clone())
    }

    pub async fn run(&mut self, interval: Duration) -> Result<()> {
        info!("Starting supervisor");
        let mut interval = time::interval(interval);

        loop {
            tokio::select! {
                _ = interval.tick() => {
                    // Get both locks at once to avoid deadlocks
                    let (state_guard, actuators_guard) = tokio::join!(
                        self.state.read(),
                        self.actuators.read()
                    );

                    // Store the length before dropping the guard
                    let num_actuators = actuators_guard.len();

                    // Process actuators
                    for (id, actuator) in actuators_guard.iter() {
                        if let Some(actuator_state) = state_guard.states.get(id) {
                            if actuator_state.enabled {
                                if actuator_state.ready {
                                    let feedback = match actuator_state.feedback.as_ref() {
                                        Some(f) => f,
                                        None => {
                                            warn!("No feedback available for actuator {}, skipping control {:?}", id, actuator_state.control_command);
                                            continue;
                                        }
                                    };
                                    let mut command_valid = true;

                                    // Check angle change limit if configured
                                    if let Some(max_angle_change) = actuator_state.configuration.max_angle_change {
                                        let angle_diff = (actuator_state.control_command.target_angle - feedback.angle).abs();
                                        if angle_diff > max_angle_change {
                                            error!(
                                                "Actuator {} angle change too large: {:.3} > {:.3}",
                                                id, angle_diff, max_angle_change
                                            );
                                            command_valid = false;
                                        }
                                    }

                                    // Check velocity limit if configured
                                    if let Some(max_velocity) = actuator_state.configuration.max_velocity {
                                        if actuator_state.control_command.target_velocity.abs() > max_velocity {
                                            error!(
                                                "Actuator {} velocity too large: {:.3} > {:.3}",
                                                id,
                                                actuator_state.control_command.target_velocity.abs(),
                                                max_velocity
                                            );
                                            command_valid = false;
                                        }
                                    }

                                    if command_valid {
                                        if let Err(e) = actuator.control(actuator_state.control_command.clone()).await {
                                            error!("Failed to control actuator {}: {}", id, e);
                                        }
                                    }
                                } else {
                                    warn!("Actuator {} is not ready, {:?}", id, actuator_state.control_command);
                                }
                            } else {
                                if let Err(e) = actuator.get_feedback().await {
                                    error!("Failed to get feedback from actuator {}: {}", id, e);
                                }
                            }
                        } else {
                            warn!("No state found for actuator {}", id);
                        }
                    }

                    // Drop the read locks before acquiring write lock
                    drop(actuators_guard);
                    drop(state_guard);

                    // Handle timeouts and stats with write lock
                    let mut state = self.state.write().await;
                    let mut actuators = self.actuators.write().await;

                    for (id, actuator_state) in state.states.iter_mut() {
                        if actuator_state.last_feedback.elapsed()? > self.feedback_timeout {
                            error!("Feedback timeout for actuator {}", id);
                            actuator_state.enabled = false;

                            if let Some(actuator) = actuators.get_mut(id) {
                                if let Err(e) = actuator.disable(false).await {
                                    error!("Failed to disable actuator {} after timeout: {}", id, e);
                                }
                            }
                        }
                    }

                    if state.last_stats_time.elapsed()? > Duration::from_secs(5) {
                        let total_messages: u64 = state.states.values()
                            .map(|state| state.messages_received)
                            .sum();
                        info!(
                            "Messages received: {} (avg {:.1} Hz), len={}",
                            total_messages,
                            total_messages as f32 / (5.0 * num_actuators as f32),
                            num_actuators
                        );
                        for state in state.states.values_mut() {
                            state.messages_received = 0;
                        }
                        state.last_stats_time = SystemTime::now();
                    }
                }
            }
        }
    }

    pub async fn add_actuator(
        &self,
        actuator: Box<dyn Actuator>,
        configuration: ActuatorConfiguration,
    ) {
        let id = actuator.id();
        let actuator_type = actuator.actuator_type();
        self.actuators.write().await.insert(id, actuator);
        self.state.write().await.states.insert(
            id,
            ActuatorState {
                feedback: None,
                last_feedback: SystemTime::now(),
                ready: false,
                enabled: false,
                control_config: ControlConfig {
                    kp: 0.0,
                    kd: 0.0,
                    max_torque: None,
                    max_velocity: None,
                    max_current: None,
                },
                control_command: ControlCommand {
                    target_angle: 0.0,
                    target_velocity: 0.0,
                    kp: 0.0,
                    kd: 0.0,
                    torque: 0.0,
                },
                configuration,
                messages_received: 0,
                half_revolutions: 0,
                actuator_type,
            },
        );
    }

    pub async fn enable(&mut self, id: u8) -> Result<()> {
        let mut actuators = self.actuators.write().await;
        let actuator = actuators
            .get_mut(&id)
            .ok_or_else(|| eyre::eyre!("Actuator not found"))?;
        actuator.enable().await?;

        let mut state = self.state.write().await;
        if let Some(actuator_state) = state.states.get_mut(&id) {
            actuator_state.enabled = true;
        }
        Ok(())
    }

    pub async fn disable(&mut self, id: u8, clear_fault: bool) -> Result<()> {
        let mut actuators = self.actuators.write().await;
        let actuator = actuators
            .get_mut(&id)
            .ok_or_else(|| eyre::eyre!("Actuator not found"))?;
        actuator.disable(clear_fault).await?;

        let mut state = self.state.write().await;
        if let Some(actuator_state) = state.states.get_mut(&id) {
            actuator_state.enabled = false;
        }
        Ok(())
    }

    pub async fn configure(&mut self, id: u8, config: ControlConfig) -> Result<()> {
        // First update the state
        {
            let mut state = self.state.write().await;
            let actuator_state = state
                .states
                .get_mut(&id)
                .ok_or_else(|| eyre::eyre!("Actuator state not found"))?;

            let cmd = match actuator_state.actuator_type {
                ActuatorType::RobStride00 => RobStride00Command {
                    kp: config.kp,
                    kd: config.kd,
                    ..Default::default()
                }
                .to_control_command(),
                ActuatorType::RobStride01 => RobStride01Command {
                    kp: config.kp,
                    kd: config.kd,
                    ..Default::default()
                }
                .to_control_command(),
                ActuatorType::RobStride02 => RobStride02Command {
                    kp: config.kp,
                    kd: config.kd,
                    ..Default::default()
                }
                .to_control_command(),
                ActuatorType::RobStride03 => RobStride03Command {
                    kp: config.kp,
                    kd: config.kd,
                    ..Default::default()
                }
                .to_control_command(),
                ActuatorType::RobStride04 | _ => RobStride04Command {
                    kp: config.kp,
                    kd: config.kd,
                    ..Default::default()
                }
                .to_control_command(),
            };

            // Update control config
            actuator_state.control_config = config.clone();

            // Update control command gains
            actuator_state.control_command.kp = cmd.kp;
            actuator_state.control_command.kd = cmd.kd;
        }

        // Then update the actuator limits
        {
            let mut actuators = self.actuators.write().await;
            let actuator = actuators
                .get_mut(&id)
                .ok_or_else(|| eyre::eyre!("Actuator not found"))?;

            // Set limits if provided
            if let Some(max_torque) = config.max_torque {
                actuator.set_max_torque(max_torque).await?;
            }
            if let Some(max_velocity) = config.max_velocity {
                actuator.set_max_velocity(max_velocity).await?;
            }
            if let Some(max_current) = config.max_current {
                actuator.set_max_current(max_current).await?;
            }
        }

        Ok(())
    }

    pub async fn command(
        &mut self,
        id: u8,
        position: f32,
        velocity: f32,
        torque: f32,
    ) -> Result<()> {
        // Acquire the lock for actuators
        let actuators = self.actuators.read().await;
        let actuator = actuators
            .get(&id)
            .ok_or_else(|| eyre::eyre!("Actuator not found"))?;

        //

        // Construct the control command based on actuator type
        let cmd = match actuator.actuator_type() {
            ActuatorType::RobStride00 => RobStride00Command {
                target_angle_rad: position,
                target_velocity_rads: velocity,
                torque_nm: torque,
                ..Default::default()
            }
            .to_control_command(),
            ActuatorType::RobStride01 => RobStride01Command {
                target_angle_rad: position,
                target_velocity_rads: velocity,
                torque_nm: torque,
                ..Default::default()
            }
            .to_control_command(),
            ActuatorType::RobStride02 => RobStride02Command {
                target_angle_rad: position,
                target_velocity_rads: velocity,
                torque_nm: torque,
                ..Default::default()
            }
            .to_control_command(),
            ActuatorType::RobStride03 => RobStride03Command {
                target_angle_rad: position,
                target_velocity_rads: velocity,
                torque_nm: torque,
                ..Default::default()
            }
            .to_control_command(),
            ActuatorType::RobStride04 => RobStride04Command {
                target_angle_rad: position,
                target_velocity_rads: velocity,
                torque_nm: torque,
                ..Default::default()
            }
            .to_control_command(),
        };

        // Acquire the lock for state
        let mut state = self.state.write().await;
        let actuator_state = state
            .states
            .get_mut(&id)
            .ok_or_else(|| eyre::eyre!("Actuator state not found"))?;

        // Update the actuator state with the new command
        actuator_state.control_command.target_angle = cmd.target_angle;
        actuator_state.control_command.target_velocity = cmd.target_velocity;
        actuator_state.control_command.torque = cmd.torque;

        Ok(())
    }

    pub async fn set_id(&mut self, id: u8, new_id: u8) -> Result<()> {
        let mut actuators = self.actuators.write().await;
        let actuator = actuators
            .get_mut(&id)
            .ok_or_else(|| eyre::eyre!("Actuator not found"))?;
        actuator.set_id(new_id).await
    }

    pub async fn get_uuid(&mut self, id: u8) -> Result<()> {
        let mut actuators = self.actuators.write().await;
        let actuator = actuators
            .get_mut(&id)
            .ok_or_else(|| eyre::eyre!("Actuator not found"))?;
        actuator.get_uuid().await
    }

    pub async fn control(&mut self, id: u8, cmd: ControlCommand) -> Result<()> {
        let mut actuators = self.actuators.write().await;
        let actuator = actuators
            .get_mut(&id)
            .ok_or_else(|| eyre::eyre!("Actuator not found"))?;
        actuator.control(cmd).await
    }

    pub async fn get_feedback(&self, id: u8) -> Result<Option<(FeedbackFrame, SystemTime)>> {
        let state = self.state.read().await;
        Ok(state.states.get(&id).and_then(|actuator_state| {
            actuator_state.feedback.clone().map(|mut feedback| {
                let typed_feedback: Box<dyn TypedFeedbackData> = match actuator_state.actuator_type
                {
                    ActuatorType::RobStride00 => {
                        Box::new(RobStride00Feedback::from_feedback_frame(feedback.clone()))
                    }
                    ActuatorType::RobStride01 => {
                        Box::new(RobStride01Feedback::from_feedback_frame(feedback.clone()))
                    }
                    ActuatorType::RobStride02 => {
                        Box::new(RobStride02Feedback::from_feedback_frame(feedback.clone()))
                    }
                    ActuatorType::RobStride03 => {
                        Box::new(RobStride03Feedback::from_feedback_frame(feedback.clone()))
                    }
                    ActuatorType::RobStride04 => {
                        Box::new(RobStride04Feedback::from_feedback_frame(feedback.clone()))
                    }
                };

                feedback.angle = typed_feedback.angle_rad();
                feedback.velocity = typed_feedback.velocity_rads();
                feedback.torque = typed_feedback.torque_nm();

                feedback.angle = normalize_radians(feedback.angle).0;
                (feedback, actuator_state.last_feedback)
            })
        }))
    }

    pub async fn zero(&mut self, id: u8) -> Result<()> {
        let mut actuators = self.actuators.write().await;
        let actuator = actuators
            .get_mut(&id)
            .ok_or_else(|| eyre::eyre!("Actuator not found"))?;
        actuator.set_zero().await
    }

    pub async fn change_id(&mut self, id: u8, new_id: u8) -> Result<()> {
        // TODO order of ops?
        // Change ID in the actuator first
        let mut actuators = self.actuators.write().await;
        let actuator = actuators
            .get_mut(&id)
            .ok_or_else(|| eyre::eyre!("Actuator not found"))?;
        actuator.set_id(new_id).await?;

        // Move the actuator to the new ID in the actuators HashMap
        let actuator = actuators
            .remove(&id)
            .ok_or_else(|| eyre::eyre!("Actuator not found while changing ID"))?;
        actuators.insert(new_id, actuator);
        drop(actuators); // Release the lock

        // Move the state to the new ID
        let mut state = self.state.write().await;
        if let Some(actuator_state) = state.states.remove(&id) {
            state.states.insert(new_id, actuator_state);
        }

        Ok(())
    }
}
