kcan 0.3.0

//! Host-side codec for the Knott Dynamics K00/K01 actuator CAN protocol.
//!
//! These helpers build and parse frames; they do not observe the physical actuator state.
//! Prefer the `*_in_mode` and `*_from_mode` checked builders when current mode is known. The
//! device firmware must still validate every command because a host-side mode observation can be
//! stale and active-mode entry also depends on configured protection thresholds.

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

use crate::bus::{CanFrame, CanId};
use crate::error::{Error, Result};

pub const KNOTT_DYNAMICS_DEFAULT_BITRATE: u32 = 1_000_000;
pub const KNOTT_DYNAMICS_MIN_NODE_ID: u8 = 1;
pub const KNOTT_DYNAMICS_MAX_NODE_ID: u8 = 63;
pub const KNOTT_DYNAMICS_READ_COMMAND: u8 = 0x40;
pub const KNOTT_DYNAMICS_WRITE_COMMAND: u8 = 0x20;

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum KnottDynamicsModel {
    K00,
    K01,
}

pub const KNOTT_DYNAMICS_MODELS: [KnottDynamicsModel; 2] =
    [KnottDynamicsModel::K00, KnottDynamicsModel::K01];

impl KnottDynamicsModel {
    pub const fn name(self) -> &'static str {
        match self {
            Self::K00 => "K00",
            Self::K01 => "K01",
        }
    }

    pub const fn size_class(self) -> &'static str {
        match self {
            Self::K00 => "5010-class",
            Self::K01 => "6512-class",
        }
    }

    pub const fn all() -> &'static [Self] {
        &KNOTT_DYNAMICS_MODELS
    }

    pub fn from_name(value: &str) -> Option<Self> {
        let compact = value
            .trim()
            .to_ascii_lowercase()
            .replace(['-', '_', ' '], "");
        match compact.as_str() {
            "k00" | "k0" => Some(Self::K00),
            "k01" | "k1" => Some(Self::K01),
            _ => None,
        }
    }
}

impl fmt::Display for KnottDynamicsModel {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        f.write_str(self.name())
    }
}

impl FromStr for KnottDynamicsModel {
    type Err = Error;

    fn from_str(value: &str) -> std::result::Result<Self, Self::Err> {
        Self::from_name(value).ok_or_else(|| Error::UnknownMotorLabel(value.to_owned()))
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[repr(u16)]
pub enum KnottDynamicsFunction {
    Nmt = 0x0,
    Probe = 0x1,
    Time = 0x2,
    TransmitPdo1 = 0x3,
    ReceivePdo1 = 0x4,
    TransmitPdo2 = 0x5,
    ReceivePdo2 = 0x6,
    TransmitPdo3 = 0x7,
    ReceivePdo3 = 0x8,
    TransmitPdo4 = 0x9,
    ReceivePdo4 = 0xA,
    TransmitSdo = 0xB,
    ReceiveSdo = 0xC,
    Flash = 0xD,
    Heartbeat = 0xE,
}

impl KnottDynamicsFunction {
    pub const fn name(self) -> &'static str {
        match self {
            Self::Nmt => "NMT",
            Self::Probe => "probe",
            Self::Time => "time",
            Self::TransmitPdo1 => "TPDO1",
            Self::ReceivePdo1 => "RPDO1",
            Self::TransmitPdo2 => "TPDO2",
            Self::ReceivePdo2 => "RPDO2",
            Self::TransmitPdo3 => "TPDO3",
            Self::ReceivePdo3 => "RPDO3",
            Self::TransmitPdo4 => "TPDO4",
            Self::ReceivePdo4 => "RPDO4",
            Self::TransmitSdo => "TSDO",
            Self::ReceiveSdo => "RSDO",
            Self::Flash => "flash",
            Self::Heartbeat => "heartbeat",
        }
    }

    pub fn can_id(self, node_id: u8) -> Result<CanId> {
        validate_node_id(node_id)?;
        CanId::standard(((self as u16) << 7) | u16::from(node_id))
    }

    pub const fn from_value(value: u16) -> Option<Self> {
        match value {
            0x0 => Some(Self::Nmt),
            0x1 => Some(Self::Probe),
            0x2 => Some(Self::Time),
            0x3 => Some(Self::TransmitPdo1),
            0x4 => Some(Self::ReceivePdo1),
            0x5 => Some(Self::TransmitPdo2),
            0x6 => Some(Self::ReceivePdo2),
            0x7 => Some(Self::TransmitPdo3),
            0x8 => Some(Self::ReceivePdo3),
            0x9 => Some(Self::TransmitPdo4),
            0xA => Some(Self::ReceivePdo4),
            0xB => Some(Self::TransmitSdo),
            0xC => Some(Self::ReceiveSdo),
            0xD => Some(Self::Flash),
            0xE => Some(Self::Heartbeat),
            _ => None,
        }
    }

    pub const fn from_standard_id(id: u16) -> Option<(Self, u8)> {
        let node_id = (id & 0x7F) as u8;
        if node_id < KNOTT_DYNAMICS_MIN_NODE_ID || node_id > KNOTT_DYNAMICS_MAX_NODE_ID {
            return None;
        }
        match Self::from_value(id >> 7) {
            Some(function) => Some((function, node_id)),
            None => None,
        }
    }
}

impl fmt::Display for KnottDynamicsFunction {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        f.write_str(self.name())
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[repr(u8)]
pub enum KnottDynamicsMode {
    Disabled = 0x00,
    Idle = 0x01,
    Damping = 0x02,
    Calibration = 0x05,
    Current = 0x10,
    Torque = 0x11,
    Velocity = 0x12,
    Position = 0x13,
    VabcOverride = 0x20,
    VAlphaBetaOverride = 0x21,
    VqdOverride = 0x22,
    Debug = 0x80,
}

impl KnottDynamicsMode {
    pub const fn name(self) -> &'static str {
        match self {
            Self::Disabled => "disabled",
            Self::Idle => "idle",
            Self::Damping => "damping",
            Self::Calibration => "calibration",
            Self::Current => "current",
            Self::Torque => "torque",
            Self::Velocity => "velocity",
            Self::Position => "position",
            Self::VabcOverride => "vabc-override",
            Self::VAlphaBetaOverride => "valphabeta-override",
            Self::VqdOverride => "vqd-override",
            Self::Debug => "debug",
        }
    }

    pub const fn from_value(value: u8) -> Option<Self> {
        match value {
            0x00 => Some(Self::Disabled),
            0x01 => Some(Self::Idle),
            0x02 => Some(Self::Damping),
            0x05 => Some(Self::Calibration),
            0x10 => Some(Self::Current),
            0x11 => Some(Self::Torque),
            0x12 => Some(Self::Velocity),
            0x13 => Some(Self::Position),
            0x20 => Some(Self::VabcOverride),
            0x21 => Some(Self::VAlphaBetaOverride),
            0x22 => Some(Self::VqdOverride),
            0x80 => Some(Self::Debug),
            _ => None,
        }
    }

    pub const fn can_requestable(self) -> bool {
        !matches!(self, Self::Calibration)
    }

    pub const fn is_active(self) -> bool {
        matches!(
            self,
            Self::Damping
                | Self::Calibration
                | Self::Current
                | Self::Torque
                | Self::Velocity
                | Self::Position
                | Self::VabcOverride
                | Self::VAlphaBetaOverride
                | Self::VqdOverride
        )
    }

    /// Checks the firmware's mode-transition graph, but not protection configuration.
    pub const fn can_transition_to(self, requested: Self) -> bool {
        if !requested.can_requestable() {
            return false;
        }
        if requested as u8 == self as u8 {
            return true;
        }
        match requested {
            Self::Disabled | Self::Idle | Self::Debug => true,
            _ if requested.is_active() => {
                matches!(self, Self::Idle)
                    || (self.is_active() && !matches!(self, Self::Calibration))
            }
            _ => false,
        }
    }
}

impl fmt::Display for KnottDynamicsMode {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        f.write_str(self.name())
    }
}

impl FromStr for KnottDynamicsMode {
    type Err = String;

    fn from_str(value: &str) -> std::result::Result<Self, Self::Err> {
        match value.trim().to_ascii_lowercase().as_str() {
            "disabled" | "disable" => Ok(Self::Disabled),
            "idle" => Ok(Self::Idle),
            "damping" | "damp" => Ok(Self::Damping),
            "calibration" | "calibrate" => Ok(Self::Calibration),
            "current" => Ok(Self::Current),
            "torque" => Ok(Self::Torque),
            "velocity" => Ok(Self::Velocity),
            "position" => Ok(Self::Position),
            "vabc" | "vabc-override" => Ok(Self::VabcOverride),
            "valphabeta" | "alpha-beta" | "valphabeta-override" => Ok(Self::VAlphaBetaOverride),
            "vqd" | "vqd-override" => Ok(Self::VqdOverride),
            "debug" => Ok(Self::Debug),
            other => Err(format!("unknown Knott Dynamics mode {other:?}")),
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[repr(u32)]
pub enum KnottDynamicsFault {
    General = 0x0001,
    EmergencyStop = 0x0002,
    Initialization = 0x0004,
    Calibration = 0x0008,
    PowerStage = 0x0010,
    InvalidMode = 0x0020,
    WatchdogTimeout = 0x0040,
    BusOvervoltage = 0x0080,
    Overcurrent = 0x0100,
    Overtemperature = 0x0200,
    CanReceive = 0x0400,
    CanTransmit = 0x0800,
    I2c = 0x1000,
    Encoder = 0x2000,
    Configuration = 0x4000,
    BusUndervoltage = 0x8000,
}

pub const KNOTT_DYNAMICS_FAULTS: [KnottDynamicsFault; 16] = [
    KnottDynamicsFault::General,
    KnottDynamicsFault::EmergencyStop,
    KnottDynamicsFault::Initialization,
    KnottDynamicsFault::Calibration,
    KnottDynamicsFault::PowerStage,
    KnottDynamicsFault::InvalidMode,
    KnottDynamicsFault::WatchdogTimeout,
    KnottDynamicsFault::BusOvervoltage,
    KnottDynamicsFault::Overcurrent,
    KnottDynamicsFault::Overtemperature,
    KnottDynamicsFault::CanReceive,
    KnottDynamicsFault::CanTransmit,
    KnottDynamicsFault::I2c,
    KnottDynamicsFault::Encoder,
    KnottDynamicsFault::Configuration,
    KnottDynamicsFault::BusUndervoltage,
];

impl KnottDynamicsFault {
    pub const fn mask(self) -> u32 {
        self as u32
    }

    pub const fn description(self) -> &'static str {
        match self {
            Self::General => "general fault",
            Self::EmergencyStop => "emergency stop",
            Self::Initialization => "initialization failure",
            Self::Calibration => "calibration failure",
            Self::PowerStage => "power-stage failure",
            Self::InvalidMode => "invalid mode or transition",
            Self::WatchdogTimeout => "command watchdog timeout",
            Self::BusOvervoltage => "bus overvoltage",
            Self::Overcurrent => "overcurrent",
            Self::Overtemperature => "overtemperature",
            Self::CanReceive => "CAN receive/protocol fault",
            Self::CanTransmit => "CAN transmit fault",
            Self::I2c => "I2C fault",
            Self::Encoder => "encoder data fault",
            Self::Configuration => "configuration record fault",
            Self::BusUndervoltage => "bus undervoltage",
        }
    }

    pub const fn all() -> &'static [Self] {
        &KNOTT_DYNAMICS_FAULTS
    }
}

#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct KnottDynamicsFaults(u32);

impl KnottDynamicsFaults {
    pub const fn from_bits(bits: u32) -> Self {
        Self(bits)
    }

    pub const fn bits(self) -> u32 {
        self.0
    }

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

    pub const fn contains(self, fault: KnottDynamicsFault) -> bool {
        self.0 & fault.mask() != 0
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum KnottDynamicsParameterType {
    U32,
    I32,
    F32,
}

impl KnottDynamicsParameterType {
    pub const fn name(self) -> &'static str {
        match self {
            Self::U32 => "u32",
            Self::I32 => "i32",
            Self::F32 => "f32",
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum KnottDynamicsParameterAccess {
    ReadOnly,
    ReadWrite,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum KnottDynamicsWriteRequirement {
    AnyMode,
    ValidModeTransition,
    DisabledOrIdle,
    VabcOverride,
    VAlphaBetaOverride,
    VqdOverride,
}

impl KnottDynamicsWriteRequirement {
    pub const fn description(self) -> &'static str {
        match self {
            Self::AnyMode => "any mode accepted by firmware",
            Self::ValidModeTransition => "a valid firmware mode transition",
            Self::DisabledOrIdle => "disabled or idle mode with PWM disabled",
            Self::VabcOverride => "VABC override mode",
            Self::VAlphaBetaOverride => "V-alpha/beta override mode",
            Self::VqdOverride => "V-q/d override mode",
        }
    }

    pub const fn permits(self, current_mode: KnottDynamicsMode) -> bool {
        match self {
            Self::AnyMode | Self::ValidModeTransition => true,
            Self::DisabledOrIdle => matches!(
                current_mode,
                KnottDynamicsMode::Disabled | KnottDynamicsMode::Idle
            ),
            Self::VabcOverride => matches!(current_mode, KnottDynamicsMode::VabcOverride),
            Self::VAlphaBetaOverride => {
                matches!(current_mode, KnottDynamicsMode::VAlphaBetaOverride)
            }
            Self::VqdOverride => matches!(current_mode, KnottDynamicsMode::VqdOverride),
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct KnottDynamicsParameterDescriptor {
    pub parameter: KnottDynamicsParameter,
    pub name: &'static str,
    pub parameter_type: KnottDynamicsParameterType,
    pub access: KnottDynamicsParameterAccess,
}

macro_rules! define_parameters {
    ($($variant:ident = $id:literal, $name:literal, $kind:ident, $access:ident;)+) => {
        #[derive(Debug, Clone, Copy, PartialEq, Eq)]
        #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
        #[repr(u16)]
        pub enum KnottDynamicsParameter {
            $($variant = $id,)+
        }

        pub const KNOTT_DYNAMICS_PARAMETERS: &[KnottDynamicsParameterDescriptor] = &[
            $(KnottDynamicsParameterDescriptor {
                parameter: KnottDynamicsParameter::$variant,
                name: $name,
                parameter_type: KnottDynamicsParameterType::$kind,
                access: KnottDynamicsParameterAccess::$access,
            },)+
        ];

        impl TryFrom<u16> for KnottDynamicsParameter {
            type Error = Error;

            fn try_from(value: u16) -> std::result::Result<Self, Self::Error> {
                match value {
                    $($id => Ok(Self::$variant),)+
                    _ => Err(Error::UnknownParameter(format!("0x{value:03X}"))),
                }
            }
        }
    };
}

define_parameters! {
    DeviceId = 0x000, "device_id", U32, ReadWrite;
    FirmwareVersion = 0x004, "firmware_version", U32, ReadOnly;
    WatchdogTimeoutMs = 0x008, "watchdog_timeout_ms", U32, ReadWrite;
    FastFrameFrequencyHz = 0x00C, "fast_frame_frequency_hz", U32, ReadWrite;
    Mode = 0x010, "mode", U32, ReadWrite;
    ErrorBits = 0x014, "error_bits", U32, ReadOnly;
    PositionUpdateCounter = 0x018, "position_update_counter", U32, ReadOnly;
    GearRatio = 0x01C, "gear_ratio", F32, ReadWrite;
    PositionKp = 0x020, "position_kp", F32, ReadWrite;
    PositionKi = 0x024, "position_ki", F32, ReadWrite;
    VelocityKp = 0x028, "velocity_kp", F32, ReadWrite;
    VelocityKi = 0x02C, "velocity_ki", F32, ReadWrite;
    TorqueLimit = 0x030, "torque_limit", F32, ReadWrite;
    VelocityLimit = 0x034, "velocity_limit", F32, ReadWrite;
    PositionLimitLower = 0x038, "position_limit_lower", F32, ReadWrite;
    PositionLimitUpper = 0x03C, "position_limit_upper", F32, ReadWrite;
    PositionOffset = 0x040, "position_offset", F32, ReadWrite;
    TorqueTarget = 0x044, "torque_target", F32, ReadWrite;
    TorqueMeasured = 0x048, "torque_measured", F32, ReadOnly;
    TorqueSetpoint = 0x04C, "torque_setpoint", F32, ReadOnly;
    VelocityTarget = 0x050, "velocity_target", F32, ReadWrite;
    VelocityMeasured = 0x054, "velocity_measured", F32, ReadOnly;
    VelocitySetpoint = 0x058, "velocity_setpoint", F32, ReadOnly;
    PositionTarget = 0x05C, "position_target", F32, ReadWrite;
    PositionMeasured = 0x060, "position_measured", F32, ReadOnly;
    PositionSetpoint = 0x064, "position_setpoint", F32, ReadOnly;
    PositionIntegrator = 0x068, "position_integrator", F32, ReadOnly;
    VelocityIntegrator = 0x06C, "velocity_integrator", F32, ReadOnly;
    TorqueFilterAlpha = 0x070, "torque_filter_alpha", F32, ReadWrite;
    CurrentLimit = 0x074, "current_limit", F32, ReadWrite;
    CurrentKp = 0x078, "current_kp", F32, ReadWrite;
    CurrentKi = 0x07C, "current_ki", F32, ReadWrite;
    PhaseACurrent = 0x080, "phase_a_current", F32, ReadOnly;
    PhaseBCurrent = 0x084, "phase_b_current", F32, ReadOnly;
    PhaseCCurrent = 0x088, "phase_c_current", F32, ReadOnly;
    PhaseAVoltageTarget = 0x08C, "phase_a_voltage_target", F32, ReadWrite;
    PhaseBVoltageTarget = 0x090, "phase_b_voltage_target", F32, ReadWrite;
    PhaseCVoltageTarget = 0x094, "phase_c_voltage_target", F32, ReadWrite;
    AlphaCurrent = 0x098, "alpha_current", F32, ReadOnly;
    BetaCurrent = 0x09C, "beta_current", F32, ReadOnly;
    AlphaVoltageTarget = 0x0A0, "alpha_voltage_target", F32, ReadWrite;
    BetaVoltageTarget = 0x0A4, "beta_voltage_target", F32, ReadWrite;
    QVoltageTarget = 0x0A8, "q_voltage_target", F32, ReadWrite;
    DVoltageTarget = 0x0AC, "d_voltage_target", F32, ReadWrite;
    QVoltageSetpoint = 0x0B0, "q_voltage_setpoint", F32, ReadOnly;
    DVoltageSetpoint = 0x0B4, "d_voltage_setpoint", F32, ReadOnly;
    QCurrentTarget = 0x0B8, "q_current_target", F32, ReadWrite;
    DCurrentTarget = 0x0BC, "d_current_target", F32, ReadWrite;
    QCurrentMeasured = 0x0C0, "q_current_measured", F32, ReadOnly;
    DCurrentMeasured = 0x0C4, "d_current_measured", F32, ReadOnly;
    QCurrentSetpoint = 0x0C8, "q_current_setpoint", F32, ReadOnly;
    DCurrentSetpoint = 0x0CC, "d_current_setpoint", F32, ReadOnly;
    QCurrentIntegrator = 0x0D0, "q_current_integrator", F32, ReadOnly;
    DCurrentIntegrator = 0x0D4, "d_current_integrator", F32, ReadOnly;
    AdcPhaseARaw = 0x0D8, "adc_phase_a_raw", U32, ReadOnly;
    AdcPhaseBRaw = 0x0DC, "adc_phase_b_raw", U32, ReadOnly;
    AdcPhaseCRaw = 0x0E0, "adc_phase_c_raw", U32, ReadOnly;
    AdcBusRaw = 0x0E4, "adc_bus_raw", U32, ReadOnly;
    CurrentOffsetA = 0x0E8, "current_offset_a", U32, ReadOnly;
    CurrentOffsetB = 0x0EC, "current_offset_b", U32, ReadOnly;
    CurrentOffsetC = 0x0F0, "current_offset_c", U32, ReadOnly;
    UndervoltageThreshold = 0x0F4, "undervoltage_threshold", F32, ReadWrite;
    OvervoltageThreshold = 0x0F8, "overvoltage_threshold", F32, ReadWrite;
    BusVoltageFilterAlpha = 0x0FC, "bus_voltage_filter_alpha", F32, ReadWrite;
    BusVoltageMeasured = 0x100, "bus_voltage_measured", F32, ReadOnly;
    MotorPolePairs = 0x104, "motor_pole_pairs", U32, ReadWrite;
    MotorTorqueConstant = 0x108, "motor_torque_constant", F32, ReadWrite;
    MotorPhaseOrder = 0x10C, "motor_phase_order", I32, ReadWrite;
    MotorCalibrationCurrent = 0x110, "motor_calibration_current", F32, ReadWrite;
    EncoderUpdateCount = 0x11C, "encoder_update_count", U32, ReadOnly;
    EncoderCpr = 0x120, "encoder_cpr", I32, ReadWrite;
    EncoderPositionOffset = 0x124, "encoder_position_offset", F32, ReadWrite;
    EncoderVelocityFilterAlpha = 0x128, "encoder_velocity_filter_alpha", F32, ReadWrite;
    EncoderPositionRaw = 0x12C, "encoder_position_raw", U32, ReadOnly;
    EncoderTurnCount = 0x130, "encoder_turn_count", I32, ReadOnly;
    EncoderPosition = 0x134, "encoder_position", F32, ReadOnly;
    EncoderVelocity = 0x138, "encoder_velocity", F32, ReadOnly;
    EncoderFluxOffset = 0x13C, "encoder_flux_offset", F32, ReadOnly;
    EncoderFluxTableIndex = 0x140, "encoder_flux_table_index", U32, ReadWrite;
    EncoderFluxTableValue = 0x144, "encoder_flux_table_value", F32, ReadOnly;
}

impl KnottDynamicsParameter {
    pub fn descriptor(self) -> &'static KnottDynamicsParameterDescriptor {
        KNOTT_DYNAMICS_PARAMETERS
            .iter()
            .find(|descriptor| descriptor.parameter == self)
            .expect("all Knott Dynamics parameters have descriptors")
    }

    pub const fn write_requirement(self) -> KnottDynamicsWriteRequirement {
        match self {
            Self::Mode => KnottDynamicsWriteRequirement::ValidModeTransition,
            Self::DeviceId
            | Self::MotorPolePairs
            | Self::MotorTorqueConstant
            | Self::MotorPhaseOrder
            | Self::MotorCalibrationCurrent
            | Self::EncoderCpr
            | Self::EncoderFluxTableIndex => KnottDynamicsWriteRequirement::DisabledOrIdle,
            Self::PhaseAVoltageTarget | Self::PhaseBVoltageTarget | Self::PhaseCVoltageTarget => {
                KnottDynamicsWriteRequirement::VabcOverride
            }
            Self::AlphaVoltageTarget | Self::BetaVoltageTarget => {
                KnottDynamicsWriteRequirement::VAlphaBetaOverride
            }
            Self::QVoltageTarget | Self::DVoltageTarget => {
                KnottDynamicsWriteRequirement::VqdOverride
            }
            _ => KnottDynamicsWriteRequirement::AnyMode,
        }
    }
}

impl fmt::Display for KnottDynamicsParameter {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        f.write_str(self.descriptor().name)
    }
}

impl FromStr for KnottDynamicsParameter {
    type Err = Error;

    fn from_str(value: &str) -> std::result::Result<Self, Self::Err> {
        let value = value.trim();
        let normalized = value.to_ascii_lowercase().replace(['-', ' '], "_");
        if let Some(descriptor) = KNOTT_DYNAMICS_PARAMETERS
            .iter()
            .find(|descriptor| descriptor.name == normalized)
        {
            return Ok(descriptor.parameter);
        }

        let numeric = value
            .strip_prefix("0x")
            .or_else(|| value.strip_prefix("0X"))
            .map(|hex| u16::from_str_radix(hex, 16))
            .unwrap_or_else(|| value.parse::<u16>());
        match numeric {
            Ok(raw) => Self::try_from(raw),
            Err(_) => Err(Error::UnknownParameter(value.to_owned())),
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum KnottDynamicsParameterValue {
    U32(u32),
    I32(i32),
    F32(f32),
}

impl KnottDynamicsParameterValue {
    pub const fn parameter_type(self) -> KnottDynamicsParameterType {
        match self {
            Self::U32(_) => KnottDynamicsParameterType::U32,
            Self::I32(_) => KnottDynamicsParameterType::I32,
            Self::F32(_) => KnottDynamicsParameterType::F32,
        }
    }

    pub const fn to_le_bytes(self) -> [u8; 4] {
        match self {
            Self::U32(value) => value.to_le_bytes(),
            Self::I32(value) => value.to_le_bytes(),
            Self::F32(value) => value.to_le_bytes(),
        }
    }

    const fn from_le_bytes(parameter_type: KnottDynamicsParameterType, bytes: [u8; 4]) -> Self {
        match parameter_type {
            KnottDynamicsParameterType::U32 => Self::U32(u32::from_le_bytes(bytes)),
            KnottDynamicsParameterType::I32 => Self::I32(i32::from_le_bytes(bytes)),
            KnottDynamicsParameterType::F32 => Self::F32(f32::from_le_bytes(bytes)),
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum KnottDynamicsFlashCommand {
    Store,
    Load,
}

impl KnottDynamicsFlashCommand {
    pub const fn byte(self) -> u8 {
        match self {
            Self::Store => 1,
            Self::Load => 2,
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum KnottDynamicsFeedbackKind {
    PositionVelocity,
    PositionTorque,
    PeriodicPositionVelocity,
}

#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct KnottDynamicsFeedback {
    pub node_id: u8,
    pub kind: KnottDynamicsFeedbackKind,
    pub position_rad: f64,
    pub velocity_rad_s: Option<f64>,
    pub torque_nm: Option<f64>,
    pub mode: Option<KnottDynamicsMode>,
    pub faults: Option<KnottDynamicsFaults>,
}

impl KnottDynamicsFeedback {
    pub const fn with_status(
        mut self,
        mode: Option<KnottDynamicsMode>,
        faults: Option<KnottDynamicsFaults>,
    ) -> Self {
        self.mode = mode;
        self.faults = faults;
        self
    }

    pub const fn has_fault(self) -> Option<bool> {
        match self.faults {
            Some(faults) => Some(!faults.is_empty()),
            None => None,
        }
    }
}

/// Low-level NMT builder. Prefer [`knott_dynamics_nmt_frame_from_mode`] when current mode is known.
pub fn knott_dynamics_nmt_frame(
    target_node: Option<u8>,
    mode: KnottDynamicsMode,
) -> Result<CanFrame> {
    if !mode.can_requestable() {
        return Err(Error::UnsupportedMode(mode as u32));
    }
    let target_node = match target_node {
        Some(node_id) => {
            validate_node_id(node_id)?;
            node_id
        }
        None => 0,
    };
    CanFrame::new(CanId::standard(0)?, &[mode as u8, target_node])
}

/// Builds an NMT frame after checking the supplied current mode against the firmware transition
/// graph. Active-mode protection thresholds remain a device-side check.
pub fn knott_dynamics_nmt_frame_from_mode(
    target_node: Option<u8>,
    current_mode: KnottDynamicsMode,
    requested_mode: KnottDynamicsMode,
) -> Result<CanFrame> {
    if !requested_mode.can_requestable() {
        return Err(Error::UnsupportedMode(requested_mode as u32));
    }
    if !current_mode.can_transition_to(requested_mode) {
        return Err(Error::InvalidModeTransition {
            from: current_mode as u8,
            to: requested_mode as u8,
        });
    }
    knott_dynamics_nmt_frame(target_node, requested_mode)
}

/// Builds the canonical PWM-off request. `None` targets all nodes; `Some(id)` targets one node.
pub fn knott_dynamics_disable_frame(target_node: Option<u8>) -> Result<CanFrame> {
    knott_dynamics_nmt_frame(target_node, KnottDynamicsMode::Disabled)
}

pub fn knott_dynamics_probe_frame(node_id: u8) -> Result<CanFrame> {
    CanFrame::new(KnottDynamicsFunction::Probe.can_id(node_id)?, &[])
}

/// Builds an opaque RPDO1 command. A valid RPDO1 services the device watchdog.
pub fn knott_dynamics_opaque_watchdog_frame(node_id: u8, data: [u8; 8]) -> Result<CanFrame> {
    CanFrame::new(KnottDynamicsFunction::ReceivePdo1.can_id(node_id)?, &data)
}

/// Builds an RPDO2 target. Values are required to be finite; device configuration owns limits.
pub fn knott_dynamics_position_velocity_frame(
    node_id: u8,
    position_rad: f32,
    velocity_rad_s: f32,
) -> Result<CanFrame> {
    two_f32_frame(
        KnottDynamicsFunction::ReceivePdo2,
        node_id,
        position_rad,
        velocity_rad_s,
        "position_rad",
        "velocity_rad_s",
    )
}

/// Builds an RPDO3 target. Values are required to be finite; device configuration owns limits.
pub fn knott_dynamics_position_torque_frame(
    node_id: u8,
    position_rad: f32,
    torque_nm: f32,
) -> Result<CanFrame> {
    two_f32_frame(
        KnottDynamicsFunction::ReceivePdo3,
        node_id,
        position_rad,
        torque_nm,
        "position_rad",
        "torque_nm",
    )
}

/// Services the command watchdog without changing the previous target. This is not a stop frame.
pub fn knott_dynamics_heartbeat_frame(node_id: u8) -> Result<CanFrame> {
    CanFrame::new(KnottDynamicsFunction::Heartbeat.can_id(node_id)?, &[])
}

/// Low-level flash builder. Prefer [`knott_dynamics_flash_frame_in_mode`].
pub fn knott_dynamics_flash_frame(
    node_id: u8,
    command: KnottDynamicsFlashCommand,
) -> Result<CanFrame> {
    CanFrame::new(
        KnottDynamicsFunction::Flash.can_id(node_id)?,
        &[command.byte()],
    )
}

/// Builds a flash command only when the supplied mode is one in which firmware permits storage.
/// The caller must obtain current mode immediately before use and remain responsible for races.
pub fn knott_dynamics_flash_frame_in_mode(
    node_id: u8,
    current_mode: KnottDynamicsMode,
    command: KnottDynamicsFlashCommand,
) -> Result<CanFrame> {
    if !matches!(
        current_mode,
        KnottDynamicsMode::Disabled | KnottDynamicsMode::Idle
    ) {
        return Err(Error::UnsafeOperation {
            operation: "flash operation",
            required: "disabled or idle mode with PWM disabled",
            actual_mode: current_mode as u8,
        });
    }
    knott_dynamics_flash_frame(node_id, command)
}

pub fn knott_dynamics_parameter_read_frame(
    node_id: u8,
    parameter: KnottDynamicsParameter,
) -> Result<CanFrame> {
    let id = parameter as u16;
    CanFrame::new(
        KnottDynamicsFunction::ReceiveSdo.can_id(node_id)?,
        &[KNOTT_DYNAMICS_READ_COMMAND, id as u8, (id >> 8) as u8, 0],
    )
}

/// Builds an SDO write with type and standalone value validation.
///
/// Prefer [`knott_dynamics_parameter_write_frame_in_mode`] for state-dependent parameters.
pub fn knott_dynamics_parameter_write_frame(
    node_id: u8,
    parameter: KnottDynamicsParameter,
    value: KnottDynamicsParameterValue,
) -> Result<CanFrame> {
    validate_knott_dynamics_parameter_write(parameter, value)?;

    let id = parameter as u16;
    let value = value.to_le_bytes();
    CanFrame::new(
        KnottDynamicsFunction::ReceiveSdo.can_id(node_id)?,
        &[
            KNOTT_DYNAMICS_WRITE_COMMAND,
            id as u8,
            (id >> 8) as u8,
            0,
            value[0],
            value[1],
            value[2],
            value[3],
        ],
    )
}

/// Builds an SDO write after checking both value constraints and the supplied current mode.
/// Cross-parameter relationships and races remain device-side checks.
pub fn knott_dynamics_parameter_write_frame_in_mode(
    node_id: u8,
    current_mode: KnottDynamicsMode,
    parameter: KnottDynamicsParameter,
    value: KnottDynamicsParameterValue,
) -> Result<CanFrame> {
    validate_knott_dynamics_parameter_write_in_mode(current_mode, parameter, value)?;
    knott_dynamics_parameter_write_frame(node_id, parameter, value)
}

pub fn validate_knott_dynamics_parameter_write(
    parameter: KnottDynamicsParameter,
    value: KnottDynamicsParameterValue,
) -> Result<()> {
    let descriptor = parameter.descriptor();
    if descriptor.access == KnottDynamicsParameterAccess::ReadOnly {
        return Err(Error::ReadOnlyParameter(descriptor.name));
    }
    let actual = value.parameter_type();
    if actual != descriptor.parameter_type {
        return Err(Error::ParameterTypeMismatch {
            parameter: descriptor.name,
            expected: descriptor.parameter_type.name(),
            actual: actual.name(),
        });
    }
    if let KnottDynamicsParameterValue::F32(value) = value {
        require_finite(value, descriptor.name)?;
    }
    if parameter == KnottDynamicsParameter::Mode {
        let KnottDynamicsParameterValue::U32(raw_mode) = value else {
            unreachable!("the parameter type check above requires a u32 mode")
        };
        let mode = u8::try_from(raw_mode)
            .ok()
            .and_then(KnottDynamicsMode::from_value)
            .filter(|mode| mode.can_requestable())
            .ok_or(Error::UnsupportedMode(raw_mode))?;
        debug_assert_ne!(mode, KnottDynamicsMode::Calibration);
    }
    validate_parameter_range(parameter, value)
}

pub fn validate_knott_dynamics_parameter_write_in_mode(
    current_mode: KnottDynamicsMode,
    parameter: KnottDynamicsParameter,
    value: KnottDynamicsParameterValue,
) -> Result<()> {
    validate_knott_dynamics_parameter_write(parameter, value)?;
    let requirement = parameter.write_requirement();
    if !requirement.permits(current_mode) {
        return Err(Error::UnsafeOperation {
            operation: parameter.descriptor().name,
            required: requirement.description(),
            actual_mode: current_mode as u8,
        });
    }
    if parameter == KnottDynamicsParameter::Mode {
        let KnottDynamicsParameterValue::U32(requested_mode) = value else {
            unreachable!("mode value type was validated")
        };
        let requested_mode =
            KnottDynamicsMode::from_value(requested_mode as u8).expect("mode value was validated");
        if !current_mode.can_transition_to(requested_mode) {
            return Err(Error::InvalidModeTransition {
                from: current_mode as u8,
                to: requested_mode as u8,
            });
        }
    }
    Ok(())
}

pub fn parse_knott_dynamics_probe_response(frame: &CanFrame) -> Result<Option<u8>> {
    let CanId::Standard(id) = frame.id() else {
        return Ok(None);
    };
    if !(u16::from(KNOTT_DYNAMICS_MIN_NODE_ID)..=u16::from(KNOTT_DYNAMICS_MAX_NODE_ID))
        .contains(&id)
    {
        return Ok(None);
    }
    require_exact_len(frame, 1)?;
    let node_id = id as u8;
    if frame.data()[0] != node_id {
        return Ok(None);
    }
    Ok(Some(node_id))
}

pub fn parse_knott_dynamics_feedback(frame: &CanFrame) -> Result<Option<KnottDynamicsFeedback>> {
    let CanId::Standard(id) = frame.id() else {
        return Ok(None);
    };
    let Some((function, node_id)) = KnottDynamicsFunction::from_standard_id(id) else {
        return Ok(None);
    };
    let kind = match function {
        KnottDynamicsFunction::TransmitPdo2 => KnottDynamicsFeedbackKind::PositionVelocity,
        KnottDynamicsFunction::TransmitPdo3 => KnottDynamicsFeedbackKind::PositionTorque,
        KnottDynamicsFunction::TransmitPdo4 => KnottDynamicsFeedbackKind::PeriodicPositionVelocity,
        _ => return Ok(None),
    };
    require_exact_len(frame, 8)?;
    let position_rad = parse_f32(&frame.data()[0..4], "position feedback")?;
    let second = parse_f32(&frame.data()[4..8], "velocity/torque feedback")?;

    Ok(Some(KnottDynamicsFeedback {
        node_id,
        kind,
        position_rad: f64::from(position_rad),
        velocity_rad_s: matches!(
            kind,
            KnottDynamicsFeedbackKind::PositionVelocity
                | KnottDynamicsFeedbackKind::PeriodicPositionVelocity
        )
        .then_some(f64::from(second)),
        torque_nm: (kind == KnottDynamicsFeedbackKind::PositionTorque).then_some(f64::from(second)),
        mode: None,
        faults: None,
    }))
}

pub fn parse_knott_dynamics_opaque_watchdog_response(
    node_id: u8,
    frame: &CanFrame,
) -> Result<Option<[u8; 8]>> {
    let expected_id = KnottDynamicsFunction::TransmitPdo1.can_id(node_id)?;
    if frame.id() != expected_id {
        return Ok(None);
    }
    require_exact_len(frame, 8)?;
    Ok(Some(
        frame
            .data()
            .try_into()
            .expect("eight-byte payload was checked"),
    ))
}

pub fn parse_knott_dynamics_parameter_response(
    node_id: u8,
    parameter: KnottDynamicsParameter,
    frame: &CanFrame,
) -> Result<KnottDynamicsParameterValue> {
    let expected_id = KnottDynamicsFunction::TransmitSdo.can_id(node_id)?;
    if frame.id() != expected_id {
        return Err(Error::InvalidCanId(frame.id().raw()));
    }
    require_exact_len(frame, 4)?;
    Ok(KnottDynamicsParameterValue::from_le_bytes(
        parameter.descriptor().parameter_type,
        frame
            .data()
            .try_into()
            .expect("four-byte payload was checked"),
    ))
}

pub fn is_knott_dynamics_frame_shape(frame: &CanFrame) -> bool {
    let CanId::Standard(id) = frame.id() else {
        return false;
    };

    if id == 0 {
        return frame.len() == 2;
    }
    if id <= KNOTT_DYNAMICS_MAX_NODE_ID as u16 {
        return frame.len() == 1 && frame.data()[0] == id as u8;
    }

    let Some((function, _)) = KnottDynamicsFunction::from_standard_id(id) else {
        return false;
    };
    match function {
        KnottDynamicsFunction::Probe | KnottDynamicsFunction::Heartbeat => frame.is_empty(),
        KnottDynamicsFunction::TransmitPdo1
        | KnottDynamicsFunction::ReceivePdo1
        | KnottDynamicsFunction::TransmitPdo2
        | KnottDynamicsFunction::ReceivePdo2
        | KnottDynamicsFunction::TransmitPdo3
        | KnottDynamicsFunction::ReceivePdo3
        | KnottDynamicsFunction::TransmitPdo4 => frame.len() == 8,
        KnottDynamicsFunction::TransmitSdo => frame.len() == 4,
        KnottDynamicsFunction::ReceiveSdo => frame.len() == 4 || frame.len() == 8,
        KnottDynamicsFunction::Flash => frame.len() == 1,
        KnottDynamicsFunction::Nmt
        | KnottDynamicsFunction::Time
        | KnottDynamicsFunction::ReceivePdo4 => false,
    }
}

pub fn validate_node_id(node_id: u8) -> Result<()> {
    if (KNOTT_DYNAMICS_MIN_NODE_ID..=KNOTT_DYNAMICS_MAX_NODE_ID).contains(&node_id) {
        Ok(())
    } else {
        Err(Error::InvalidNodeId(node_id))
    }
}

fn two_f32_frame(
    function: KnottDynamicsFunction,
    node_id: u8,
    first: f32,
    second: f32,
    first_name: &'static str,
    second_name: &'static str,
) -> Result<CanFrame> {
    require_finite(first, first_name)?;
    require_finite(second, second_name)?;
    let mut data = [0_u8; 8];
    data[..4].copy_from_slice(&first.to_le_bytes());
    data[4..].copy_from_slice(&second.to_le_bytes());
    CanFrame::new(function.can_id(node_id)?, &data)
}

fn parse_f32(data: &[u8], name: &'static str) -> Result<f32> {
    let value = f32::from_le_bytes(data.try_into().expect("four-byte slice supplied"));
    require_finite(value, name)?;
    Ok(value)
}

fn require_finite(value: f32, name: &'static str) -> Result<()> {
    if value.is_finite() {
        Ok(())
    } else {
        Err(Error::NonFiniteValue(name))
    }
}

fn require_exact_len(frame: &CanFrame, expected: usize) -> Result<()> {
    if frame.len() == expected {
        Ok(())
    } else {
        Err(Error::InvalidPayloadLength {
            expected,
            actual: frame.len(),
        })
    }
}

fn validate_parameter_range(
    parameter: KnottDynamicsParameter,
    value: KnottDynamicsParameterValue,
) -> Result<()> {
    use KnottDynamicsParameter as Parameter;
    use KnottDynamicsParameterValue as Value;

    let constraint = match (parameter, value) {
        (Parameter::DeviceId, Value::U32(value)) if !(1..=63).contains(&value) => Some("1..63"),
        (Parameter::WatchdogTimeoutMs, Value::U32(0)) => {
            Some("a nonzero timeout in milliseconds; zero disables the command watchdog")
        }
        (Parameter::FastFrameFrequencyHz, Value::U32(value)) if value > 10_000 => {
            Some("0..10000 Hz")
        }
        (Parameter::GearRatio, Value::F32(0.0)) => Some("a finite nonzero gear ratio"),
        (
            Parameter::TorqueLimit
            | Parameter::VelocityLimit
            | Parameter::CurrentLimit
            | Parameter::UndervoltageThreshold
            | Parameter::OvervoltageThreshold
            | Parameter::MotorTorqueConstant
            | Parameter::MotorCalibrationCurrent,
            Value::F32(value),
        ) if value <= 0.0 => Some("a finite value greater than zero"),
        (
            Parameter::TorqueFilterAlpha
            | Parameter::BusVoltageFilterAlpha
            | Parameter::EncoderVelocityFilterAlpha,
            Value::F32(value),
        ) if !(0.0..=1.0).contains(&value) => Some("a finite value in 0..=1"),
        (Parameter::CurrentKp | Parameter::CurrentKi, Value::F32(value)) if value < 0.0 => {
            Some("a finite nonnegative value")
        }
        (Parameter::MotorPolePairs, Value::U32(value)) if !(1..=32).contains(&value) => {
            Some("1..32 pole pairs")
        }
        (Parameter::MotorPhaseOrder, Value::I32(value)) if !matches!(value, -1 | 1) => {
            Some("-1 or +1")
        }
        (Parameter::EncoderCpr, Value::I32(value)) if !matches!(value, -4096 | 4096) => {
            Some("-4096 or +4096")
        }
        (Parameter::EncoderFluxTableIndex, Value::U32(value)) if value > 127 => Some("0..127"),
        _ => None,
    };

    match constraint {
        Some(constraint) => Err(Error::InvalidParameterValue {
            parameter: parameter.descriptor().name,
            constraint,
        }),
        None => Ok(()),
    }
}