mecha10-core 0.8.2

use serde::{Deserialize, Serialize};

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Twist {
    pub timestamp: u64,

    pub linear: [f32; 3],

    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 {
        super::geometry3::magnitude_3d(self.linear)
    }

    /// Returns the angular speed (magnitude) in rad/s
    pub fn angular_speed(&self) -> f32 {
        super::geometry3::magnitude_3d(self.angular)
    }

    /// Clamps velocities to maximum values
    pub fn clamp(&mut self, max_linear: f32, max_angular: f32) {
        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];
        }

        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,
            ];
        }
    }
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct JointState {
    pub timestamp: u64,

    pub names: Vec<String>,

    pub positions: Vec<f32>,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub velocities: Option<Vec<f32>>,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub efforts: Option<Vec<f32>>,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct GripperCommand {
    pub timestamp: u64,

    pub position: f32,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub max_effort: Option<f32>,
}

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::new(1.0, None)
    }

    /// Create a command to close the gripper
    pub fn close() -> Self {
        Self::new(0.0, 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
    }
}