mecha10-core 0.8.0

//! Teleoperation message types
//!
//! This module contains message types for teleoperation control from multiple sources
//! (CLI, dashboard, game controllers, etc.)

use crate::messages::Message;
use serde::{Deserialize, Serialize};

/// The command payload a `TeleopInput` carries. `Drivetrain` is the original differential-drive
/// velocity command; `Actuator` generalizes the same envelope (source/priority/timestamp/
/// emergency_stop) to drive a named set of joints to target angles, so a single teleop node can
/// front either a mobile base or an arm/pan-tilt actuator without a second message type.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub enum TeleopCommand {
    Drivetrain {
        linear: f32,

        angular: f32,
    },

    Actuator {
        names: Vec<String>,

        /// Target angles in radians, one per entry in `names`, following `JointState`'s
        /// convention.
        positions: Vec<f32>,
    },
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct TeleopInput {
    pub source: String,

    pub priority: u8,

    pub command: TeleopCommand,

    pub timestamp: u64,

    pub emergency_stop: bool,
}

impl Message for TeleopInput {}

impl TeleopInput {
    /// Create a new drivetrain teleop input with standard priority
    pub fn new(source: impl Into<String>, linear: f32, angular: f32, timestamp: u64) -> Self {
        Self {
            source: source.into(),
            priority: 50,
            command: TeleopCommand::Drivetrain { linear, angular },
            timestamp,
            emergency_stop: false,
        }
    }

    /// Create a new actuator (joint) teleop input with standard priority
    pub fn new_actuator(source: impl Into<String>, names: Vec<String>, positions: Vec<f32>, timestamp: u64) -> Self {
        Self {
            source: source.into(),
            priority: 50,
            command: TeleopCommand::Actuator { names, positions },
            timestamp,
            emergency_stop: false,
        }
    }

    /// Create an emergency stop command
    pub fn emergency_stop(source: impl Into<String>, timestamp: u64) -> Self {
        Self {
            source: source.into(),
            priority: 255,
            command: TeleopCommand::Drivetrain {
                linear: 0.0,
                angular: 0.0,
            },
            timestamp,
            emergency_stop: true,
        }
    }

    /// Set the priority level
    pub fn with_priority(mut self, priority: u8) -> Self {
        self.priority = priority;
        self
    }

    /// Get age in milliseconds
    pub fn age_ms(&self, current_time: u64) -> u64 {
        let age_us = current_time.saturating_sub(self.timestamp);
        age_us / 1000
    }
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct TeleopStatus {
    pub active_source: String,

    pub last_command_age_ms: u64,

    pub safety_engaged: bool,

    /// Which kind of command this status reflects - `"drivetrain"` or `"actuator"`.
    pub mode: String,

    pub current_linear: f32,

    pub current_angular: f32,

    /// `(joint_name, angle_rad)` pairs - the actuator-mode equivalent of
    /// `current_linear`/`current_angular`. Empty in drivetrain mode.
    pub current_positions: Vec<(String, f32)>,

    /// The names this instance's `ActuatorControllerConfig.joint_names` resolved to at init -
    /// unlike `current_positions`, populated immediately (not just once a command has arrived),
    /// so a UI can learn which joints to offer controls for before sending its first command.
    /// Empty in drivetrain mode.
    #[serde(default)]
    pub joint_names: Vec<String>,

    /// `(joint_name, min_angle_rad, max_angle_rad)` triples - the actuator-mode equivalent of
    /// `max_linear_vel`/`max_angular_vel`, resolved from the project's `mecha10.json`
    /// `actuator.joints` (`mecha10_core::config::joint_limits`) for each of `joint_names`, same
    /// as `joint_names` itself: populated immediately at init, not just once a command arrives,
    /// and only for a joint a limit is actually configured for. Empty in drivetrain mode.
    #[serde(default)]
    pub joint_limits: Vec<(String, f32, f32)>,

    pub total_commands: u64,

    pub active_priority: u8,

    pub max_linear_vel: f32,

    pub max_angular_vel: f32,
}

impl Message for TeleopStatus {}

impl TeleopStatus {
    /// Create a new status with default values
    pub fn new() -> Self {
        Self {
            active_source: "none".to_string(),
            last_command_age_ms: 0,
            safety_engaged: false,
            mode: "drivetrain".to_string(),
            current_linear: 0.0,
            current_angular: 0.0,
            current_positions: Vec::new(),
            joint_names: Vec::new(),
            joint_limits: Vec::new(),
            total_commands: 0,
            active_priority: 0,
            max_linear_vel: 0.0,
            max_angular_vel: 0.0,
        }
    }

    /// Check if teleop is actively controlling
    pub fn is_active(&self) -> bool {
        !self.safety_engaged && self.active_source != "none"
    }
}

impl Default for TeleopStatus {
    fn default() -> Self {
        Self::new()
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn new_builds_a_drivetrain_command() {
        let input = TeleopInput::new("gamepad", 1.0, -0.5, 123);
        assert_eq!(input.source, "gamepad");
        assert_eq!(input.priority, 50);
        assert_eq!(input.timestamp, 123);
        assert!(!input.emergency_stop);
        match input.command {
            TeleopCommand::Drivetrain { linear, angular } => {
                assert_eq!(linear, 1.0);
                assert_eq!(angular, -0.5);
            }
            TeleopCommand::Actuator { .. } => panic!("expected a Drivetrain command"),
        }
    }

    #[test]
    fn new_actuator_builds_an_actuator_command() {
        let input = TeleopInput::new_actuator(
            "dashboard",
            vec!["shoulder".to_string(), "elbow".to_string()],
            vec![0.1, 0.2],
            456,
        );
        match input.command {
            TeleopCommand::Actuator { names, positions } => {
                assert_eq!(names, vec!["shoulder".to_string(), "elbow".to_string()]);
                assert_eq!(positions, vec![0.1, 0.2]);
            }
            TeleopCommand::Drivetrain { .. } => panic!("expected an Actuator command"),
        }
    }

    #[test]
    fn emergency_stop_is_a_zeroed_drivetrain_command_with_max_priority() {
        let input = TeleopInput::emergency_stop("safety", 789);
        assert_eq!(input.priority, 255);
        assert!(input.emergency_stop);
        match input.command {
            TeleopCommand::Drivetrain { linear, angular } => {
                assert_eq!(linear, 0.0);
                assert_eq!(angular, 0.0);
            }
            TeleopCommand::Actuator { .. } => panic!("expected a Drivetrain command"),
        }
    }

    #[test]
    fn with_priority_overrides_the_default() {
        let input = TeleopInput::new("gamepad", 0.0, 0.0, 0).with_priority(10);
        assert_eq!(input.priority, 10);
    }

    #[test]
    fn age_ms_converts_microseconds_to_milliseconds() {
        let input = TeleopInput::new("gamepad", 0.0, 0.0, 1_000);
        assert_eq!(input.age_ms(5_000), 4);
    }

    #[test]
    fn teleop_command_round_trips_through_json() {
        let drivetrain = TeleopCommand::Drivetrain {
            linear: 1.0,
            angular: 2.0,
        };
        let json = serde_json::to_string(&drivetrain).expect("serializable");
        let parsed: TeleopCommand = serde_json::from_str(&json).expect("deserializable");
        match parsed {
            TeleopCommand::Drivetrain { linear, angular } => {
                assert_eq!(linear, 1.0);
                assert_eq!(angular, 2.0);
            }
            TeleopCommand::Actuator { .. } => panic!("expected a Drivetrain command"),
        }

        let actuator = TeleopCommand::Actuator {
            names: vec!["wrist".to_string()],
            positions: vec![0.75],
        };
        let json = serde_json::to_string(&actuator).expect("serializable");
        let parsed: TeleopCommand = serde_json::from_str(&json).expect("deserializable");
        match parsed {
            TeleopCommand::Actuator { names, positions } => {
                assert_eq!(names, vec!["wrist".to_string()]);
                assert_eq!(positions, vec![0.75]);
            }
            TeleopCommand::Drivetrain { .. } => panic!("expected an Actuator command"),
        }
    }

    #[test]
    fn status_default_is_drivetrain_mode_with_no_positions() {
        let status = TeleopStatus::new();
        assert_eq!(status.mode, "drivetrain");
        assert!(status.current_positions.is_empty());
        assert_eq!(status.active_source, "none");
        assert!(!status.is_active());
    }
}