mecha10-core 0.6.2

// Actuator Message Types

use serde::{Deserialize, Serialize};

/// Twist command for velocity control
///
/// Represents desired linear and angular velocities
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Twist {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Linear velocity (x, y, z) in m/s
    pub linear: [f32; 3],

    /// Angular velocity (x, y, z) in rad/s
    pub angular: [f32; 3],
}

impl Twist {
    /// Create a new Twist command
    pub fn new(linear_x: f32, linear_y: f32, linear_z: f32, angular_x: f32, angular_y: f32, angular_z: f32) -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            linear: [linear_x, linear_y, linear_z],
            angular: [angular_x, angular_y, angular_z],
        }
    }

    /// Create a 2D twist (common for ground robots)
    pub fn new_2d(linear_x: f32, angular_z: f32) -> Self {
        Self::new(linear_x, 0.0, 0.0, 0.0, 0.0, angular_z)
    }

    /// Create a stop command
    pub fn stop() -> Self {
        Self::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
    }

    /// Create a forward command with given speed
    pub fn forward(speed: f32) -> Self {
        Self::new_2d(speed, 0.0)
    }

    /// Create a backward command with given speed
    pub fn backward(speed: f32) -> Self {
        Self::new_2d(-speed, 0.0)
    }

    /// Create a rotate-in-place command with given angular velocity
    pub fn rotate(angular_z: f32) -> Self {
        Self::new_2d(0.0, angular_z)
    }

    /// Returns the linear speed (magnitude) in m/s
    pub fn linear_speed(&self) -> f32 {
        let l = self.linear;
        (l[0] * l[0] + l[1] * l[1] + l[2] * l[2]).sqrt()
    }

    /// Returns the angular speed (magnitude) in rad/s
    pub fn angular_speed(&self) -> f32 {
        let a = self.angular;
        (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]).sqrt()
    }

    /// Clamps velocities to maximum values
    pub fn clamp(&mut self, max_linear: f32, max_angular: f32) {
        // Clamp linear velocity
        let linear_mag = self.linear_speed();
        if linear_mag > max_linear {
            let scale = max_linear / linear_mag;
            self.linear = [self.linear[0] * scale, self.linear[1] * scale, self.linear[2] * scale];
        }

        // Clamp angular velocity
        let angular_mag = self.angular_speed();
        if angular_mag > max_angular {
            let scale = max_angular / angular_mag;
            self.angular = [
                self.angular[0] * scale,
                self.angular[1] * scale,
                self.angular[2] * scale,
            ];
        }
    }
}

/// Joint state (position, velocity, effort)
///
/// Represents the state of articulated joints
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct JointState {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Joint names
    pub names: Vec<String>,

    /// Joint positions in radians or meters
    pub positions: Vec<f32>,

    /// Joint velocities in rad/s or m/s (optional)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub velocities: Option<Vec<f32>>,

    /// Joint efforts (torques or forces) (optional)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub efforts: Option<Vec<f32>>,
}

/// Gripper command
///
/// Simple gripper control (open/close with optional force)
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct GripperCommand {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Position (0.0 = fully closed, 1.0 = fully open)
    pub position: f32,

    /// Maximum effort (optional, 0.0 = no force, 1.0 = maximum force)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub max_effort: Option<f32>,
}

// Helper methods for JointState
impl JointState {
    /// Creates a new empty joint state
    pub fn new() -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            names: Vec::new(),
            positions: Vec::new(),
            velocities: None,
            efforts: None,
        }
    }

    /// Creates a joint state with positions only
    pub fn with_positions(names: Vec<String>, positions: Vec<f32>) -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            names,
            positions,
            velocities: None,
            efforts: None,
        }
    }

    /// Gets the position of a joint by name
    pub fn get_position(&self, name: &str) -> Option<f32> {
        self.names
            .iter()
            .position(|n| n == name)
            .and_then(|i| self.positions.get(i).copied())
    }

    /// Checks if the state is valid (all arrays have consistent lengths)
    pub fn is_valid(&self) -> bool {
        let num = self.names.len();
        if self.positions.len() != num {
            return false;
        }
        if let Some(ref v) = self.velocities {
            if v.len() != num {
                return false;
            }
        }
        if let Some(ref e) = self.efforts {
            if e.len() != num {
                return false;
            }
        }
        true
    }
}

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

impl GripperCommand {
    /// Create a command to open the gripper
    pub fn open() -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            position: 1.0,
            max_effort: None,
        }
    }

    /// Create a command to close the gripper
    pub fn close() -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            position: 0.0,
            max_effort: None,
        }
    }

    /// Create a command with specific position and effort
    pub fn new(position: f32, max_effort: Option<f32>) -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            position: position.clamp(0.0, 1.0),
            max_effort,
        }
    }

    /// Checks if the gripper is commanded to be fully open
    pub fn is_open(&self) -> bool {
        self.position > 0.9
    }

    /// Checks if the gripper is commanded to be fully closed
    pub fn is_closed(&self) -> bool {
        self.position < 0.1
    }

    /// Returns the percentage open (0-100)
    pub fn percent_open(&self) -> f32 {
        self.position * 100.0
    }
}