kcan-core 0.1.0

//! No-std CAN protocol primitives for KCAN actuator tooling.
//!
//! This crate intentionally stays small: fixed-size CAN frames, CubeMars Servo
//! Direct payload builders, and RobStride status parsing. Higher-level transport,
//! dashboards, and analytics live in sibling crates or the main `kcan` crate.
//!
//! ```
//! # fn main() -> kcan_core::Result<()> {
//! use kcan_core::{CanId, DirectCommand, direct_command_frame};
//!
//! let id: CanId = "extended:0x301".parse()?;
//! let frame = direct_command_frame(0x03, DirectCommand::Velocity(1_000))?;
//!
//! assert_eq!(id.raw(), 0x301);
//! assert_eq!(frame.to_string(), "extended 0x303 len=4 data=00 00 03 E8");
//! # Ok(())
//! # }
//! ```

#![no_std]
#![forbid(unsafe_code)]

#[cfg(test)]
extern crate std;

use core::{fmt, str::FromStr};

pub const MAX_STANDARD_ID: u16 = 0x07FF;
pub const MAX_EXTENDED_ID: u32 = 0x1FFF_FFFF;
pub const MAX_CAN_DATA_LEN: usize = 8;

pub const DUTY_SCALE: f64 = 100_000.0;
pub const CURRENT_SCALE: f64 = 1_000.0;
pub const POSITION_SCALE: f64 = 10_000.0;
pub const MAX_DUTY_CYCLE: f64 = 0.95;
pub const MAX_CURRENT_AMPS: f64 = 60.0;
pub const MAX_POSITION_DEGREES: f64 = 36_000.0;
pub const MIN_POSITION_DEGREES: f64 = -36_000.0;
pub const MAX_PROTOCOL_ERPM: i32 = 100_000;
pub const MIN_PROTOCOL_ERPM: i32 = -100_000;
pub const MAX_PROFILE_ERPM: i32 = 327_670;
pub const MIN_PROFILE_ERPM: i32 = -327_680;
pub const MAX_PROFILE_ACCELERATION_ERPM_PER_SEC: i32 = 327_670;
pub const MIN_PROFILE_ACCELERATION_ERPM_PER_SEC: i32 = 0;
pub const PROFILE_ERPM_PER_UNIT: i32 = 10;

pub const ROBSTRIDE_MIT_BASE_ID: u32 = 0x200;
pub const ROBSTRIDE_POSITION_BASE_ID: u32 = 0x300;
pub const ROBSTRIDE_SPEED_BASE_ID: u32 = 0x400;
pub const ROBSTRIDE_STATUS_BASE_ID: u32 = 0x500;
pub const ROBSTRIDE_CONFIG_BASE_ID: u32 = 0x600;

pub type Result<T> = core::result::Result<T, Error>;

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Error {
    InvalidCanId(u32),
    InvalidCanIdText,
    PayloadTooLong { len: usize },
    UnknownCanIdKind,
}

impl fmt::Display for Error {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            Self::InvalidCanId(id) => write!(f, "invalid CAN id: 0x{id:X}"),
            Self::InvalidCanIdText => write!(f, "invalid CAN id text"),
            Self::PayloadTooLong { len } => write!(f, "CAN payload too long: {len} bytes"),
            Self::UnknownCanIdKind => write!(f, "unknown CAN id kind"),
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum CanId {
    Standard(u16),
    Extended(u32),
}

impl CanId {
    pub const fn standard(id: u16) -> Result<Self> {
        if id > MAX_STANDARD_ID {
            Err(Error::InvalidCanId(id as u32))
        } else {
            Ok(Self::Standard(id))
        }
    }

    pub const fn extended(id: u32) -> Result<Self> {
        if id > MAX_EXTENDED_ID {
            Err(Error::InvalidCanId(id))
        } else {
            Ok(Self::Extended(id))
        }
    }

    pub const fn raw(self) -> u32 {
        match self {
            Self::Standard(id) => id as u32,
            Self::Extended(id) => id,
        }
    }

    pub const fn is_extended(self) -> bool {
        matches!(self, Self::Extended(_))
    }

    pub const fn kind_name(self) -> &'static str {
        match self {
            Self::Standard(_) => "standard",
            Self::Extended(_) => "extended",
        }
    }
}

impl fmt::Display for CanId {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(f, "{} 0x{:X}", self.kind_name(), self.raw())
    }
}

impl FromStr for CanId {
    type Err = Error;

    fn from_str(value: &str) -> Result<Self> {
        let value = value.trim();
        let (kind, raw) = if let Some((kind, raw)) = value.split_once(':') {
            (kind.trim(), raw.trim())
        } else {
            let mut parts = value.split_whitespace();
            match (parts.next(), parts.next(), parts.next()) {
                (Some(kind), Some(raw), None) => (kind, raw),
                _ => return Err(Error::InvalidCanIdText),
            }
        };

        let raw = parse_can_id_number(raw)?;
        if matches_kind(kind, &["standard", "std", "s"]) {
            if raw > MAX_STANDARD_ID as u32 {
                return Err(Error::InvalidCanId(raw));
            }
            CanId::standard(raw as u16)
        } else if matches_kind(kind, &["extended", "ext", "e"]) {
            CanId::extended(raw)
        } else {
            Err(Error::UnknownCanIdKind)
        }
    }
}

fn matches_kind(value: &str, candidates: &[&str]) -> bool {
    candidates
        .iter()
        .any(|candidate| value.eq_ignore_ascii_case(candidate))
}

fn parse_can_id_number(value: &str) -> Result<u32> {
    if let Some(hex) = value
        .strip_prefix("0x")
        .or_else(|| value.strip_prefix("0X"))
    {
        u32::from_str_radix(hex, 16).map_err(|_| Error::InvalidCanIdText)
    } else {
        value.parse::<u32>().map_err(|_| Error::InvalidCanIdText)
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct CanFrame {
    id: CanId,
    data: [u8; MAX_CAN_DATA_LEN],
    len: u8,
}

impl CanFrame {
    pub fn new(id: CanId, data: &[u8]) -> Result<Self> {
        if data.len() > MAX_CAN_DATA_LEN {
            return Err(Error::PayloadTooLong { len: data.len() });
        }

        let mut padded = [0_u8; MAX_CAN_DATA_LEN];
        padded[..data.len()].copy_from_slice(data);
        Ok(Self {
            id,
            data: padded,
            len: data.len() as u8,
        })
    }

    pub const fn from_payload(id: CanId, payload: DirectPayload) -> Self {
        Self {
            id,
            data: payload.padded_data(),
            len: payload.len as u8,
        }
    }

    pub const fn id(&self) -> CanId {
        self.id
    }

    pub fn data(&self) -> &[u8] {
        &self.data[..self.len()]
    }

    pub const fn padded_data(&self) -> [u8; MAX_CAN_DATA_LEN] {
        self.data
    }

    pub const fn len(&self) -> usize {
        self.len as usize
    }

    pub const fn is_empty(&self) -> bool {
        self.len == 0
    }
}

impl fmt::Display for CanFrame {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(f, "{} len={} data=", self.id, self.len())?;
        for (index, byte) in self.data().iter().enumerate() {
            if index > 0 {
                f.write_str(" ")?;
            }
            write!(f, "{byte:02X}")?;
        }
        Ok(())
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[repr(u8)]
pub enum ControlMode {
    DutyCycle = 0x00,
    CurrentLoop = 0x01,
    CurrentBrake = 0x02,
    VelocityLoop = 0x03,
    PositionLoop = 0x04,
    SetOrigin = 0x05,
    PositionVelocity = 0x06,
    MitMode = 0x08,
}

impl ControlMode {
    pub const fn extended_id(self, motor_id: u8) -> Result<CanId> {
        CanId::extended(((self as u32) << 8) | motor_id as u32)
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[repr(u8)]
pub enum OriginMode {
    Temporary = 0x00,
    Permanent = 0x01,
}

impl OriginMode {
    pub const fn data(self) -> u8 {
        self as u8
    }
}

#[derive(Debug, Clone, Copy, PartialEq)]
pub enum DirectCommand {
    DutyCycle(f64),
    Current(f64),
    BrakeCurrent(f64),
    Velocity(i32),
    Position(f64),
    SetOrigin(OriginMode),
    PositionVelocity {
        position_degrees: f64,
        velocity_erpm: i32,
        acceleration_erpm_per_sec: i32,
    },
}

impl DirectCommand {
    pub const fn mode(self) -> ControlMode {
        match self {
            Self::DutyCycle(_) => ControlMode::DutyCycle,
            Self::Current(_) => ControlMode::CurrentLoop,
            Self::BrakeCurrent(_) => ControlMode::CurrentBrake,
            Self::Velocity(_) => ControlMode::VelocityLoop,
            Self::Position(_) => ControlMode::PositionLoop,
            Self::SetOrigin(_) => ControlMode::SetOrigin,
            Self::PositionVelocity { .. } => ControlMode::PositionVelocity,
        }
    }

    pub fn payload(self) -> DirectPayload {
        match self {
            Self::DutyCycle(duty) => DirectPayload::from_slice(&scaled_i32_bytes(
                duty,
                -MAX_DUTY_CYCLE,
                MAX_DUTY_CYCLE,
                DUTY_SCALE,
            )),
            Self::Current(current_amps) | Self::BrakeCurrent(current_amps) => {
                DirectPayload::from_slice(&scaled_i32_bytes(
                    current_amps,
                    -MAX_CURRENT_AMPS,
                    MAX_CURRENT_AMPS,
                    CURRENT_SCALE,
                ))
            }
            Self::Velocity(erpm) => DirectPayload::from_slice(
                &erpm
                    .clamp(MIN_PROTOCOL_ERPM, MAX_PROTOCOL_ERPM)
                    .to_be_bytes(),
            ),
            Self::Position(position_degrees) => DirectPayload::from_slice(&scaled_i32_bytes(
                position_degrees,
                MIN_POSITION_DEGREES,
                MAX_POSITION_DEGREES,
                POSITION_SCALE,
            )),
            Self::SetOrigin(origin_mode) => DirectPayload::from_slice(&[origin_mode.data()]),
            Self::PositionVelocity {
                position_degrees,
                velocity_erpm,
                acceleration_erpm_per_sec,
            } => {
                let mut payload = [0_u8; MAX_CAN_DATA_LEN];
                payload[..4].copy_from_slice(&scaled_i32_bytes(
                    position_degrees,
                    MIN_POSITION_DEGREES,
                    MAX_POSITION_DEGREES,
                    POSITION_SCALE,
                ));
                payload[4..6].copy_from_slice(&scaled_i16_units(
                    velocity_erpm,
                    MIN_PROFILE_ERPM,
                    MAX_PROFILE_ERPM,
                    PROFILE_ERPM_PER_UNIT,
                ));
                payload[6..8].copy_from_slice(&scaled_i16_units(
                    acceleration_erpm_per_sec,
                    MIN_PROFILE_ACCELERATION_ERPM_PER_SEC,
                    MAX_PROFILE_ACCELERATION_ERPM_PER_SEC,
                    PROFILE_ERPM_PER_UNIT,
                ));
                DirectPayload::from_array(payload, MAX_CAN_DATA_LEN)
            }
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct DirectPayload {
    data: [u8; MAX_CAN_DATA_LEN],
    len: usize,
}

impl DirectPayload {
    pub const fn len(&self) -> usize {
        self.len
    }

    pub const fn is_empty(&self) -> bool {
        self.len == 0
    }

    pub fn data(&self) -> &[u8] {
        &self.data[..self.len]
    }

    pub const fn padded_data(&self) -> [u8; MAX_CAN_DATA_LEN] {
        self.data
    }

    fn from_slice(data: &[u8]) -> Self {
        let mut payload = [0_u8; MAX_CAN_DATA_LEN];
        payload[..data.len()].copy_from_slice(data);
        Self {
            data: payload,
            len: data.len(),
        }
    }

    const fn from_array(data: [u8; MAX_CAN_DATA_LEN], len: usize) -> Self {
        Self { data, len }
    }
}

pub fn direct_command_frame(motor_id: u8, command: DirectCommand) -> Result<CanFrame> {
    let payload = command.payload();
    Ok(CanFrame::from_payload(
        command.mode().extended_id(motor_id)?,
        payload,
    ))
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum RobStrideFrameKind {
    Mit,
    Position,
    Speed,
    Status,
    Config,
}

impl RobStrideFrameKind {
    pub const fn base_id(self) -> u32 {
        match self {
            Self::Mit => ROBSTRIDE_MIT_BASE_ID,
            Self::Position => ROBSTRIDE_POSITION_BASE_ID,
            Self::Speed => ROBSTRIDE_SPEED_BASE_ID,
            Self::Status => ROBSTRIDE_STATUS_BASE_ID,
            Self::Config => ROBSTRIDE_CONFIG_BASE_ID,
        }
    }

    pub const fn can_id(self, motor_id: u8) -> Result<CanId> {
        CanId::extended(self.base_id() + motor_id as u32)
    }
}

#[derive(Debug, Clone, Copy, PartialEq)]
pub struct RobStrideFeedback {
    pub position_rad: f64,
    pub velocity_rad_s: f64,
    pub torque_nm: f64,
}

impl RobStrideFeedback {
    pub fn parse(data: &[u8]) -> Option<Self> {
        if data.len() < MAX_CAN_DATA_LEN {
            return None;
        }

        let position_raw = i32::from_le_bytes([data[0], data[1], data[2], data[3]]);
        let velocity_raw = i16::from_le_bytes([data[4], data[5]]);
        let torque_raw = i16::from_le_bytes([data[6], data[7]]);

        Some(Self {
            position_rad: position_raw as f64 / 1_000.0,
            velocity_rad_s: velocity_raw as f64 / 1_000.0,
            torque_nm: torque_raw as f64 / 10.0,
        })
    }
}

fn scaled_i32_bytes(value: f64, min: f64, max: f64, scale: f64) -> [u8; 4] {
    let scaled = round_f64(value.clamp(min, max) * scale);
    (scaled.clamp(i32::MIN as f64, i32::MAX as f64) as i32).to_be_bytes()
}

fn round_f64(value: f64) -> f64 {
    if value >= 0.0 {
        (value + 0.5) as i64 as f64
    } else {
        (value - 0.5) as i64 as f64
    }
}

fn scaled_i16_units(value: i32, min: i32, max: i32, units_per_lsb: i32) -> [u8; 2] {
    let units = value.clamp(min, max) / units_per_lsb;
    (units as i16).to_be_bytes()
}

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

    #[test]
    fn direct_payloads_use_protocol_scaling() {
        let current = DirectCommand::Current(1.0).payload();
        assert_eq!(current.data(), &[0x00, 0x00, 0x03, 0xE8]);

        let profile = DirectCommand::PositionVelocity {
            position_degrees: 1.5,
            velocity_erpm: -100,
            acceleration_erpm_per_sec: 200,
        }
        .payload();
        assert_eq!(profile.len(), 8);
        assert_eq!(&profile.data()[..4], &15_000_i32.to_be_bytes());
        assert_eq!(&profile.data()[4..6], &(-10_i16).to_be_bytes());
        assert_eq!(&profile.data()[6..8], &20_i16.to_be_bytes());
    }

    #[test]
    fn direct_frame_uses_natural_length() {
        let frame =
            direct_command_frame(0x03, DirectCommand::SetOrigin(OriginMode::Permanent)).unwrap();

        assert_eq!(frame.id(), CanId::Extended(0x0503));
        assert_eq!(frame.len(), 1);
        assert_eq!(frame.data(), &[0x01]);
    }

    #[test]
    fn can_ids_and_frames_format_and_parse() {
        let standard = CanId::standard(0x123).unwrap();
        let extended = CanId::extended(0x501).unwrap();
        let frame = CanFrame::new(extended, &[0x0A, 0xB0]).unwrap();

        assert_eq!(standard.kind_name(), "standard");
        assert_eq!(extended.kind_name(), "extended");
        assert_eq!(standard.to_string(), "standard 0x123");
        assert_eq!(extended.to_string(), "extended 0x501");
        assert_eq!(frame.to_string(), "extended 0x501 len=2 data=0A B0");
        assert_eq!(standard.to_string().parse::<CanId>().unwrap(), standard);
        assert_eq!(extended.to_string().parse::<CanId>().unwrap(), extended);
        assert_eq!("s:0x123".parse::<CanId>().unwrap(), standard);
        assert_eq!("e:0x501".parse::<CanId>().unwrap(), extended);
        assert_eq!(
            "0x123".parse::<CanId>().unwrap_err(),
            Error::InvalidCanIdText
        );
        assert_eq!(
            "standard 0x800".parse::<CanId>().unwrap_err(),
            Error::InvalidCanId(0x800)
        );
    }

    #[test]
    fn robstride_status_feedback_decodes() {
        let mut data = [0_u8; 8];
        data[..4].copy_from_slice(&1_250_i32.to_le_bytes());
        data[4..6].copy_from_slice(&(-500_i16).to_le_bytes());
        data[6..8].copy_from_slice(&42_i16.to_le_bytes());

        let feedback = RobStrideFeedback::parse(&data).unwrap();

        assert_eq!(RobStrideFrameKind::Status.can_id(1).unwrap().raw(), 0x501);
        assert_eq!(feedback.position_rad, 1.25);
        assert_eq!(feedback.velocity_rad_s, -0.5);
        assert_eq!(feedback.torque_nm, 4.2);
    }
}