kcan 0.1.0

#![forbid(unsafe_code)]

//! # kcan
//!
//! `kcan` is a Rust CAN module from **Knott Dynamics** for low-level actuator
//! command framing and helpers used in robotics applications, including humanoid
//! platforms.
//!
//! It provides a tiny, dependency-free core for describing CAN frames and a small
//! set of actuator-oriented command helpers.

use std::collections::VecDeque;

/// Maximum number of data bytes in a classic CAN payload.
pub const KCAN_DATA_BYTES: usize = 8;

/// Standard CAN base ID used by the seed helper commands.
pub const KCAN_BASE_CMD_ID: u32 = 0x200;

/// Maximum standard CAN identifier value.
pub const KCAN_STANDARD_ID_MAX: u32 = 0x7ff;

/// Canonical CAN frame shape used by this crate.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct KCanFrame {
    id: u32,
    len: u8,
    data: [u8; KCAN_DATA_BYTES],
}

impl KCanFrame {
    /// Create a new frame.
    ///
    /// Payload values longer than 8 bytes are truncated. This keeps the API small
    /// while still giving explicit control over the exact 8-byte frame body.
    pub fn new(id: u32, payload: &[u8]) -> Self {
        let mut data = [0u8; KCAN_DATA_BYTES];
        let len = payload.len().min(KCAN_DATA_BYTES);
        data[..len].copy_from_slice(&payload[..len]);

        Self {
            id,
            len: len as u8,
            data,
        }
    }

    /// Frame identifier (arbitration ID).
    pub const fn id(&self) -> u32 {
        self.id
    }

    /// Data length code (0..=8).
    pub const fn dlc(&self) -> u8 {
        self.len
    }

    /// Full frame bytes (up to `dlc`).
    pub fn payload(&self) -> &[u8] {
        &self.data[..self.len as usize]
    }

    /// Whether this uses standard (11-bit) CAN ID space.
    pub const fn is_standard_id(&self) -> bool {
        self.id <= KCAN_STANDARD_ID_MAX
    }

    /// Whether this uses extended CAN ID space.
    pub const fn is_extended_id(&self) -> bool {
        !self.is_standard_id()
    }
}

/// Actuator command families for common humanoid/robotic drives.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[repr(u8)]
pub enum KCanActuatorMode {
    /// Move to an absolute position target.
    Position = 0x01,
    /// Set a target velocity.
    Velocity = 0x02,
    /// Set a target torque.
    Torque = 0x03,
    /// Enable actuator output stage.
    Enable = 0x04,
    /// Disable actuator output stage.
    Disable = 0x05,
    /// Poll a node for status.
    QueryStatus = 0x06,
}

impl KCanActuatorMode {
    /// ID used by an actuator with this mode and node.
    pub const fn frame_id(self, node_id: u8) -> u32 {
        KCAN_BASE_CMD_ID + node_id as u32 + (self as u32)
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum KCanDecodeError {
    /// The frame does not contain enough bytes to decode a command.
    TooShort,
    /// The mode byte is unknown to this crate.
    UnknownMode,
}

/// Compact actuator message used by Rust controller code.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct KCanActuatorCommand {
    /// Numeric actuator or joint node ID.
    pub node_id: u8,
    /// Desired command mode.
    pub mode: KCanActuatorMode,
    /// Signed payload value. Units are left to higher-level control code.
    pub value: i16,
}

impl KCanActuatorCommand {
    /// Create a position command.
    pub const fn position(node_id: u8, position: i16) -> Self {
        Self {
            node_id,
            mode: KCanActuatorMode::Position,
            value: position,
        }
    }

    /// Create a velocity command.
    pub const fn velocity(node_id: u8, velocity: i16) -> Self {
        Self {
            node_id,
            mode: KCanActuatorMode::Velocity,
            value: velocity,
        }
    }

    /// Create a torque command.
    pub const fn torque(node_id: u8, torque: i16) -> Self {
        Self {
            node_id,
            mode: KCanActuatorMode::Torque,
            value: torque,
        }
    }

    /// Create an enable command.
    pub const fn enable(node_id: u8) -> Self {
        Self {
            node_id,
            mode: KCanActuatorMode::Enable,
            value: 1,
        }
    }

    /// Create a disable command.
    pub const fn disable(node_id: u8) -> Self {
        Self {
            node_id,
            mode: KCanActuatorMode::Disable,
            value: 0,
        }
    }

    /// Encode the command into a frame.
    pub fn encode(&self) -> KCanFrame {
        let value = self.value.to_be_bytes();
        let payload = [
            self.mode as u8,
            self.node_id,
            value[0],
            value[1],
            0,
            0,
            0,
            0,
        ];

        KCanFrame::new(self.mode.frame_id(self.node_id), &payload)
    }

    /// Decode a command from a frame payload.
    pub fn decode(frame: &KCanFrame) -> Result<Self, KCanDecodeError> {
        let bytes = frame.payload();
        if bytes.len() < 4 {
            return Err(KCanDecodeError::TooShort);
        }

        let mode = match bytes[0] {
            0x01 => KCanActuatorMode::Position,
            0x02 => KCanActuatorMode::Velocity,
            0x03 => KCanActuatorMode::Torque,
            0x04 => KCanActuatorMode::Enable,
            0x05 => KCanActuatorMode::Disable,
            0x06 => KCanActuatorMode::QueryStatus,
            _ => return Err(KCanDecodeError::UnknownMode),
        };

        let value = i16::from_be_bytes([bytes[2], bytes[3]]);
        Ok(Self {
            node_id: bytes[1],
            mode,
            value,
        })
    }
}

/// Simple in-memory queue for quick command scheduling in tests and samples.
#[derive(Debug, Default)]
pub struct KCanMailbox {
    outbound: VecDeque<KCanFrame>,
    inbound: VecDeque<KCanFrame>,
}

impl KCanMailbox {
    /// Create a new empty mailbox.
    pub fn new() -> Self {
        Self {
            outbound: VecDeque::new(),
            inbound: VecDeque::new(),
        }
    }

    /// Queue an outgoing frame.
    pub fn send(&mut self, frame: KCanFrame) {
        self.outbound.push_back(frame);
    }

    /// Inject a received frame.
    pub fn receive(&mut self, frame: KCanFrame) {
        self.inbound.push_back(frame);
    }

    /// Push a high-level command onto the outbound queue.
    pub fn send_command(&mut self, command: KCanActuatorCommand) {
        self.send(command.encode());
    }

    /// Pop next outgoing frame.
    pub fn pop_outbound(&mut self) -> Option<KCanFrame> {
        self.outbound.pop_front()
    }

    /// Pop next inbound frame.
    pub fn pop_inbound(&mut self) -> Option<KCanFrame> {
        self.inbound.pop_front()
    }

    /// Parse inbound frames into actuator commands.
    pub fn pop_inbound_command(&mut self) -> Option<KCanActuatorCommand> {
        self.pop_inbound().and_then(|frame| KCanActuatorCommand::decode(&frame).ok())
    }
}

#[cfg(test)]
mod tests {
    use super::{
        KCanActuatorCommand, KCanActuatorMode, KCanDecodeError, KCanFrame, KCanMailbox,
        KCAN_DATA_BYTES, KCAN_STANDARD_ID_MAX,
    };

    #[test]
    fn frame_truncates_long_payload_and_reads() {
        let payload = [1u8, 2, 3, 4, 5, 6, 7, 8, 9, 10];
        let frame = KCanFrame::new(0x42, &payload);

        assert_eq!(frame.id(), 0x42);
        assert!(frame.is_standard_id());
        assert!(!frame.is_extended_id());
        assert_eq!(frame.dlc(), 8);
        assert_eq!(frame.payload().len(), KCAN_DATA_BYTES);
        assert_eq!(frame.payload()[0], 1);
        assert_eq!(frame.payload()[7], 8);
    }

    #[test]
    fn frame_id_range_flags() {
        let ext = KCanFrame::new(KCAN_STANDARD_ID_MAX + 1, &[0]);
        assert!(!ext.is_standard_id());
        assert!(ext.is_extended_id());
    }

    #[test]
    fn actuator_command_roundtrip() {
        let command = KCanActuatorCommand::position(0x12, 512);
        let frame = command.encode();

        assert_eq!(frame.id(), KCanActuatorMode::Position.frame_id(0x12));
        assert_eq!(frame.payload()[0], KCanActuatorMode::Position as u8);

        let decoded = KCanActuatorCommand::decode(&frame).expect("decode");
        assert_eq!(decoded, command);
        assert_eq!(decoded.value, 512);
    }

    #[test]
    fn decode_rejects_short_frame() {
        let frame = KCanFrame::new(0x321, &[0x01, 0x22]);
        let err = KCanActuatorCommand::decode(&frame).unwrap_err();
        assert!(matches!(err, KCanDecodeError::TooShort));
    }

    #[test]
    fn mailbox_push_and_pop() {
        let mut mailbox = KCanMailbox::new();
        mailbox.send_command(KCanActuatorCommand::enable(0x05));
        mailbox.receive(KCanActuatorCommand::torque(0x05, -128).encode());

        let sent = mailbox.pop_outbound().expect("outbound");
        assert_eq!(sent.id(), KCanActuatorMode::Enable.frame_id(0x05));

        let received = mailbox.pop_inbound_command().expect("received cmd");
        assert_eq!(received, KCanActuatorCommand::torque(0x05, -128));
    }
}