use std::{f64::consts::PI, fmt, str::FromStr};

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

pub const DEFAULT_INTERFACE: &str = "can0";
pub const DEFAULT_BITRATE: u32 = 1_000_000;
pub const DEFAULT_MOTOR_ID: u8 = 0x03;
pub const DEFAULT_SECONDARY_MOTOR_ID: u8 = 0x04;
pub const DEFAULT_CONTROL_RATE_HZ: f64 = 100.0;
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 PROFILE_POSITION_SCALE: f64 = POSITION_SCALE;
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 MIT_HELPER_ENABLE: [u8; 8] = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC];
pub const MIT_HELPER_DISABLE: [u8; 8] = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD];
pub const MIT_HELPER_ZERO_POSITION: [u8; 8] = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE];
pub const MIT_HELPER_ENABLE_LEGACY: [u8; 8] = [0xFF; 8];
pub const MIT_HELPER_DISABLE_LEGACY: [u8; 8] = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE];

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[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 name(self) -> &'static str {
        match self {
            Self::DutyCycle => "duty",
            Self::CurrentLoop => "current",
            Self::CurrentBrake => "brake-current",
            Self::VelocityLoop => "velocity",
            Self::PositionLoop => "position",
            Self::SetOrigin => "origin",
            Self::PositionVelocity => "position-velocity",
            Self::MitMode => "mit",
        }
    }

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

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

impl FromStr for ControlMode {
    type Err = String;

    fn from_str(value: &str) -> std::result::Result<Self, Self::Err> {
        match value.trim().to_ascii_lowercase().as_str() {
            "duty" | "duty-cycle" => Ok(Self::DutyCycle),
            "current" | "current-loop" => Ok(Self::CurrentLoop),
            "brake-current" | "current-brake" => Ok(Self::CurrentBrake),
            "velocity" | "velocity-loop" => Ok(Self::VelocityLoop),
            "position" | "position-loop" => Ok(Self::PositionLoop),
            "origin" | "set-origin" => Ok(Self::SetOrigin),
            "position-velocity" | "profile" => Ok(Self::PositionVelocity),
            "mit" | "mit-mode" => Ok(Self::MitMode),
            other => Err(format!("unknown control mode {other:?}")),
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[repr(u8)]
pub enum OriginMode {
    Temporary = 0x00,
    Permanent = 0x01,
}

impl OriginMode {
    pub const fn name(self) -> &'static str {
        match self {
            Self::Temporary => "temporary",
            Self::Permanent => "permanent",
        }
    }

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

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

impl FromStr for OriginMode {
    type Err = String;

    fn from_str(value: &str) -> std::result::Result<Self, Self::Err> {
        match value.trim().to_ascii_lowercase().as_str() {
            "temporary" | "temp" => Ok(Self::Temporary),
            "permanent" | "perm" => Ok(Self::Permanent),
            other => Err(format!("unknown origin mode {other:?}")),
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
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,
    },
}

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

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

    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 {
        debug_assert!(data.len() <= MAX_CAN_DATA_LEN);
        let mut payload = [0_u8; MAX_CAN_DATA_LEN];
        payload[..data.len()].copy_from_slice(data);
        Self {
            data: payload,
            len: data.len() as u8,
        }
    }

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

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) -> Vec<u8> {
        self.payload_bytes().data().to_vec()
    }

    pub fn payload_bytes(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,
                    PROFILE_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 as u8)
            }
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum MotorModel {
    Ak10_9,
    Ak45_10,
    Ak60_6,
    Ak70_10,
    Ak80_6,
    Ak80_8,
    Ak80_9,
    Ak80_64,
    Ak90_6,
    Ak90_9,
    Ak90_12,
    Ak90_30,
    Ak100_9,
    Ak100_15,
    Ak100_25,
    Ak100_33,
    Ak110_9,
    Ak120_12,
    Ak125_9,
    Ak140_10,
    Ak150_9,
}

pub const SUPPORTED_MOTOR_MODELS: [MotorModel; 21] = [
    MotorModel::Ak10_9,
    MotorModel::Ak45_10,
    MotorModel::Ak60_6,
    MotorModel::Ak70_10,
    MotorModel::Ak80_6,
    MotorModel::Ak80_8,
    MotorModel::Ak80_9,
    MotorModel::Ak80_64,
    MotorModel::Ak90_6,
    MotorModel::Ak90_9,
    MotorModel::Ak90_12,
    MotorModel::Ak90_30,
    MotorModel::Ak100_9,
    MotorModel::Ak100_15,
    MotorModel::Ak100_25,
    MotorModel::Ak100_33,
    MotorModel::Ak110_9,
    MotorModel::Ak120_12,
    MotorModel::Ak125_9,
    MotorModel::Ak140_10,
    MotorModel::Ak150_9,
];

impl MotorModel {
    pub const fn name(self) -> &'static str {
        match self {
            Self::Ak10_9 => "AK10-9",
            Self::Ak45_10 => "AK45-10",
            Self::Ak60_6 => "AK60-6",
            Self::Ak70_10 => "AK70-10",
            Self::Ak80_6 => "AK80-6",
            Self::Ak80_8 => "AK80-8",
            Self::Ak80_9 => "AK80-9",
            Self::Ak80_64 => "AK80-64",
            Self::Ak90_6 => "AK90-6",
            Self::Ak90_9 => "AK90-9",
            Self::Ak90_12 => "AK90-12",
            Self::Ak90_30 => "AK90-30",
            Self::Ak100_9 => "AK100-9",
            Self::Ak100_15 => "AK100-15",
            Self::Ak100_25 => "AK100-25",
            Self::Ak100_33 => "AK100-33",
            Self::Ak110_9 => "AK110-9",
            Self::Ak120_12 => "AK120-12",
            Self::Ak125_9 => "AK125-9",
            Self::Ak140_10 => "AK140-10",
            Self::Ak150_9 => "AK150-9",
        }
    }

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

    pub fn from_name(value: &str) -> Option<Self> {
        let compact = value
            .trim()
            .to_ascii_lowercase()
            .replace(['-', '_', ' '], "");

        match compact.as_str() {
            "ak10" | "ak109" => Some(Self::Ak10_9),
            "ak45" | "ak4510" => Some(Self::Ak45_10),
            "ak60" | "ak606" => Some(Self::Ak60_6),
            "ak70" | "ak7010" => Some(Self::Ak70_10),
            "ak80" | "ak806" => Some(Self::Ak80_6),
            "ak808" => Some(Self::Ak80_8),
            "ak809" => Some(Self::Ak80_9),
            "ak8064" => Some(Self::Ak80_64),
            "ak906" => Some(Self::Ak90_6),
            "ak90" | "ak909" => Some(Self::Ak90_9),
            "ak9012" => Some(Self::Ak90_12),
            "ak9030" => Some(Self::Ak90_30),
            "ak100" | "ak1009" => Some(Self::Ak100_9),
            "ak10015" => Some(Self::Ak100_15),
            "ak10025" => Some(Self::Ak100_25),
            "ak10033" => Some(Self::Ak100_33),
            "ak110" | "ak1109" => Some(Self::Ak110_9),
            "ak120" | "ak12012" => Some(Self::Ak120_12),
            "ak125" | "ak1259" => Some(Self::Ak125_9),
            "ak140" | "ak14010" => Some(Self::Ak140_10),
            "ak150" | "ak1509" => Some(Self::Ak150_9),
            _ => None,
        }
    }

    pub const fn spec(self) -> MotorSpec {
        match self {
            Self::Ak10_9 => generic_ak_spec(self, 9),
            Self::Ak45_10 => generic_ak_spec(self, 10),
            Self::Ak60_6 => AK60_6_SPEC,
            Self::Ak70_10 => generic_ak_spec(self, 10),
            Self::Ak80_6 => AK80_6_SPEC,
            Self::Ak80_8 => generic_ak_spec(self, 8),
            Self::Ak80_9 => generic_ak_spec(self, 9),
            Self::Ak80_64 => generic_ak_spec(self, 64),
            Self::Ak90_6 => generic_ak_spec(self, 6),
            Self::Ak90_9 => generic_ak_spec(self, 9),
            Self::Ak90_12 => generic_ak_spec(self, 12),
            Self::Ak90_30 => generic_ak_spec(self, 30),
            Self::Ak100_9 => generic_ak_spec(self, 9),
            Self::Ak100_15 => generic_ak_spec(self, 15),
            Self::Ak100_25 => generic_ak_spec(self, 25),
            Self::Ak100_33 => generic_ak_spec(self, 33),
            Self::Ak110_9 => generic_ak_spec(self, 9),
            Self::Ak120_12 => generic_ak_spec(self, 12),
            Self::Ak125_9 => generic_ak_spec(self, 9),
            Self::Ak140_10 => generic_ak_spec(self, 10),
            Self::Ak150_9 => generic_ak_spec(self, 9),
        }
    }

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

    pub fn mit_id(self, motor_id: u8) -> Result<CanId> {
        if self.uses_extended_mit_layout() {
            ControlMode::MitMode.extended_id(motor_id)
        } else {
            CanId::standard(motor_id.into())
        }
    }

    pub fn helper_id(self, motor_id: u8) -> Result<CanId> {
        if self.uses_extended_mit_layout() {
            CanId::extended(motor_id.into())
        } else {
            CanId::standard(motor_id.into())
        }
    }

    pub fn pack_mit(self, command: MitCommand) -> Result<[u8; 8]> {
        if self.uses_extended_mit_layout() {
            pack_mit_ak60(command, self.spec().mit_limits)
        } else {
            pack_mit_ak80(command, self.spec().mit_limits)
        }
    }

    pub fn parse_feedback(self, data: &[u8]) -> Result<MotorFeedback> {
        if self.uses_extended_mit_layout() {
            parse_ak60_feedback(data)
        } else {
            parse_ak80_feedback(data, self.spec())
        }
    }
}

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

impl FromStr for MotorModel {
    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)]
pub struct MitLimits {
    pub position_min_rad: f64,
    pub position_max_rad: f64,
    pub velocity_min_rad_s: f64,
    pub velocity_max_rad_s: f64,
    pub torque_min_nm: f64,
    pub torque_max_nm: f64,
    pub kp_min: f64,
    pub kp_max: f64,
    pub kd_min: f64,
    pub kd_max: f64,
}

pub const AK60_6_MIT_LIMITS: MitLimits = MitLimits {
    position_min_rad: -12.56,
    position_max_rad: 12.56,
    velocity_min_rad_s: -60.0,
    velocity_max_rad_s: 60.0,
    torque_min_nm: -12.0,
    torque_max_nm: 12.0,
    kp_min: 0.0,
    kp_max: 500.0,
    kd_min: 0.0,
    kd_max: 5.0,
};

pub const AK80_6_MIT_LIMITS: MitLimits = MitLimits {
    position_min_rad: -12.5,
    position_max_rad: 12.5,
    velocity_min_rad_s: -76.0,
    velocity_max_rad_s: 76.0,
    torque_min_nm: -12.0,
    torque_max_nm: 12.0,
    kp_min: 0.0,
    kp_max: 500.0,
    kd_min: 0.0,
    kd_max: 5.0,
};

pub const AK_DEFAULT_MIT_LIMITS: MitLimits = MitLimits {
    position_min_rad: -12.5,
    position_max_rad: 12.5,
    velocity_min_rad_s: -60.0,
    velocity_max_rad_s: 60.0,
    torque_min_nm: -12.0,
    torque_max_nm: 12.0,
    kp_min: 0.0,
    kp_max: 500.0,
    kd_min: 0.0,
    kd_max: 5.0,
};

#[derive(Debug, Clone, Copy, PartialEq)]
pub struct MotorSpec {
    pub model: MotorModel,
    pub rated_voltage: &'static str,
    pub pole_pairs: u16,
    pub gear_ratio: u16,
    pub rated_torque_nm: f64,
    pub peak_torque_nm: f64,
    pub rated_current_amps: f64,
    pub peak_current_amps: f64,
    pub max_output_speed_rpm: u16,
    pub max_velocity_erpm: i32,
    pub min_velocity_erpm: i32,
    pub mit_position_kp: f64,
    pub mit_position_kd: f64,
    pub mit_velocity_kd: f64,
    pub mit_limits: MitLimits,
}

pub const AK60_6_SPEC: MotorSpec = MotorSpec {
    model: MotorModel::Ak60_6,
    rated_voltage: "24/48V",
    pole_pairs: 14,
    gear_ratio: 6,
    rated_torque_nm: 3.0,
    peak_torque_nm: 9.0,
    rated_current_amps: 3.8,
    peak_current_amps: 11.2,
    max_output_speed_rpm: 640,
    max_velocity_erpm: 48_128,
    min_velocity_erpm: -48_128,
    mit_position_kp: 20.0,
    mit_position_kd: 1.0,
    mit_velocity_kd: 0.2,
    mit_limits: AK60_6_MIT_LIMITS,
};

pub const AK80_6_SPEC: MotorSpec = MotorSpec {
    model: MotorModel::Ak80_6,
    rated_voltage: "48V",
    pole_pairs: 21,
    gear_ratio: 6,
    rated_torque_nm: 6.0,
    peak_torque_nm: 12.0,
    rated_current_amps: 9.7,
    peak_current_amps: 20.0,
    max_output_speed_rpm: 800,
    max_velocity_erpm: 91_444,
    min_velocity_erpm: -91_444,
    mit_position_kp: 1.0,
    mit_position_kd: 1.0,
    mit_velocity_kd: 0.5,
    mit_limits: AK80_6_MIT_LIMITS,
};

const fn generic_ak_spec(model: MotorModel, gear_ratio: u16) -> MotorSpec {
    MotorSpec {
        model,
        rated_voltage: "24/48V",
        pole_pairs: 21,
        gear_ratio,
        rated_torque_nm: 0.0,
        peak_torque_nm: AK_DEFAULT_MIT_LIMITS.torque_max_nm,
        rated_current_amps: 0.0,
        peak_current_amps: 20.0,
        max_output_speed_rpm: 500,
        max_velocity_erpm: 100_000,
        min_velocity_erpm: -100_000,
        mit_position_kp: 1.0,
        mit_position_kd: 1.0,
        mit_velocity_kd: 0.5,
        mit_limits: AK_DEFAULT_MIT_LIMITS,
    }
}

#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct MitCommand {
    pub position_rad: f64,
    pub velocity_rad_s: f64,
    pub kp: f64,
    pub kd: f64,
    pub torque_nm: f64,
}

impl MitCommand {
    pub const fn neutral() -> Self {
        Self {
            position_rad: 0.0,
            velocity_rad_s: 0.0,
            kp: 0.0,
            kd: 0.0,
            torque_nm: 0.0,
        }
    }
}

impl Default for MitCommand {
    fn default() -> Self {
        Self::neutral()
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum MitHelperCommand {
    Enable,
    Disable,
    ZeroPosition,
    EnableLegacy,
    DisableLegacy,
}

impl MitHelperCommand {
    pub const fn name(self) -> &'static str {
        match self {
            Self::Enable => "enable",
            Self::Disable => "disable",
            Self::ZeroPosition => "zero",
            Self::EnableLegacy => "enable-legacy",
            Self::DisableLegacy => "disable-legacy",
        }
    }

    pub const fn data(self) -> [u8; 8] {
        match self {
            Self::Enable => MIT_HELPER_ENABLE,
            Self::Disable => MIT_HELPER_DISABLE,
            Self::ZeroPosition => MIT_HELPER_ZERO_POSITION,
            Self::EnableLegacy => MIT_HELPER_ENABLE_LEGACY,
            Self::DisableLegacy => MIT_HELPER_DISABLE_LEGACY,
        }
    }
}

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

impl FromStr for MitHelperCommand {
    type Err = String;

    fn from_str(value: &str) -> std::result::Result<Self, Self::Err> {
        match value.trim().to_ascii_lowercase().as_str() {
            "enable" => Ok(Self::Enable),
            "disable" => Ok(Self::Disable),
            "zero" | "zero-position" => Ok(Self::ZeroPosition),
            "enable-legacy" => Ok(Self::EnableLegacy),
            "disable-legacy" => Ok(Self::DisableLegacy),
            other => Err(format!("unknown MIT helper command {other:?}")),
        }
    }
}

#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct MotorFeedback {
    pub reported_motor_id: Option<u8>,
    pub position_degrees: f64,
    pub speed_erpm: i32,
    pub current_amps: f64,
    pub temperature_celsius: i16,
    pub error_code: u8,
}

impl MotorFeedback {
    pub fn fault_code(&self) -> Option<FaultCode> {
        FaultCode::from_code(self.error_code)
    }

    pub const fn has_fault(&self) -> bool {
        self.error_code != 0
    }

    pub const fn error_description(&self) -> &'static str {
        match FaultCode::from_code(self.error_code) {
            Some(fault) => fault.description(),
            None => "Unknown fault",
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[repr(u8)]
pub enum FaultCode {
    None = 0,
    MotorOverTemperature = 1,
    OverCurrent = 2,
    OverVoltage = 3,
    UnderVoltage = 4,
    EncoderFault = 5,
    MosfetOverTemperature = 6,
    MotorLockUp = 7,
}

impl FaultCode {
    pub const fn from_code(code: u8) -> Option<Self> {
        match code {
            0 => Some(Self::None),
            1 => Some(Self::MotorOverTemperature),
            2 => Some(Self::OverCurrent),
            3 => Some(Self::OverVoltage),
            4 => Some(Self::UnderVoltage),
            5 => Some(Self::EncoderFault),
            6 => Some(Self::MosfetOverTemperature),
            7 => Some(Self::MotorLockUp),
            _ => None,
        }
    }

    pub const fn code(self) -> u8 {
        self as u8
    }

    pub const fn description(self) -> &'static str {
        match self {
            Self::None => "No fault",
            Self::MotorOverTemperature => "Motor over-temperature",
            Self::OverCurrent => "Over-current",
            Self::OverVoltage => "Over-voltage",
            Self::UnderVoltage => "Under-voltage",
            Self::EncoderFault => "Encoder fault",
            Self::MosfetOverTemperature => "MOSFET over-temperature",
            Self::MotorLockUp => "Motor lock-up",
        }
    }
}

#[derive(Debug, Clone, PartialEq, Eq)]
pub struct FeedbackFilter {
    extended_ids: Vec<u32>,
    standard_ids: Vec<u16>,
}

impl FeedbackFilter {
    pub fn for_motor(model: MotorModel, motor_id: u8) -> Self {
        if model.uses_extended_mit_layout() {
            Self {
                extended_ids: vec![0x2900 | u32::from(motor_id), 0x2900],
                standard_ids: Vec::new(),
            }
        } else {
            Self {
                extended_ids: Vec::new(),
                standard_ids: vec![u16::from(motor_id)],
            }
        }
    }

    pub fn with_explicit_id(mut self, id: CanId) -> Self {
        match id {
            CanId::Standard(id) => self.standard_ids.push(id),
            CanId::Extended(id) => self.extended_ids.push(id),
        }
        self.dedup();
        self
    }

    pub fn with_legacy_ids(mut self, motor_id: u8) -> Self {
        self.standard_ids.extend([
            u16::from(motor_id),
            u16::from(motor_id.saturating_add(1)),
            0x0080 | u16::from(motor_id),
        ]);
        self.extended_ids.extend([
            u32::from(motor_id),
            u32::from(motor_id.saturating_add(1)),
            0x0080 | u32::from(motor_id),
        ]);
        self.dedup();
        self
    }

    pub fn matches(&self, frame: &CanFrame) -> bool {
        match frame.id() {
            CanId::Standard(id) => self.standard_ids.contains(&id),
            CanId::Extended(id) => self.extended_ids.contains(&id),
        }
    }

    pub fn extended_ids(&self) -> &[u32] {
        &self.extended_ids
    }

    pub fn standard_ids(&self) -> &[u16] {
        &self.standard_ids
    }

    fn dedup(&mut self) {
        self.extended_ids.sort_unstable();
        self.extended_ids.dedup();
        self.standard_ids.sort_unstable();
        self.standard_ids.dedup();
    }
}

pub fn float_to_uint(value: f64, min: f64, max: f64, bits: u8) -> Result<u32> {
    if max <= min {
        return Err(Error::InvalidRange { min, max });
    }
    if bits == 0 || bits > 32 {
        return Err(Error::InvalidBitWidth(bits));
    }

    let max_int = if bits == 32 {
        u32::MAX
    } else {
        (1_u32 << bits) - 1
    };
    let clamped = value.clamp(min, max);
    if clamped <= min {
        return Ok(0);
    }
    if clamped >= max {
        return Ok(max_int);
    }

    let raw = ((clamped - min) * f64::from(max_int) / (max - min)) as u32;
    Ok(raw.min(max_int))
}

pub fn uint_to_float(raw: u32, min: f64, max: f64, bits: u8) -> Result<f64> {
    if max <= min {
        return Err(Error::InvalidRange { min, max });
    }
    if bits == 0 || bits > 32 {
        return Err(Error::InvalidBitWidth(bits));
    }

    let max_int = if bits == 32 {
        u32::MAX
    } else {
        (1_u32 << bits) - 1
    };
    let raw = raw.min(max_int);
    Ok((f64::from(raw) * (max - min) / f64::from(max_int)) + min)
}

pub fn pack_mit_command(model: MotorModel, command: MitCommand) -> Result<[u8; 8]> {
    model.pack_mit(command)
}

pub fn mit_command_frame(model: MotorModel, motor_id: u8, command: MitCommand) -> Result<CanFrame> {
    CanFrame::new_padded(model.mit_id(motor_id)?, &model.pack_mit(command)?)
}

pub fn mit_helper_frame(
    model: MotorModel,
    motor_id: u8,
    helper: MitHelperCommand,
) -> Result<CanFrame> {
    CanFrame::new_padded(model.helper_id(motor_id)?, &helper.data())
}

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

pub fn erpm_to_rad_s(erpm: i32, spec: MotorSpec) -> f64 {
    f64::from(erpm) * (2.0 * PI) / (60.0 * f64::from(spec.pole_pairs * spec.gear_ratio))
}

pub fn rad_s_to_erpm(rad_s: f64, spec: MotorSpec) -> i32 {
    (rad_s * (60.0 * f64::from(spec.pole_pairs * spec.gear_ratio)) / (2.0 * PI)).round() as i32
}

pub fn degrees_to_radians(degrees: f64) -> f64 {
    degrees * PI / 180.0
}

pub fn radians_to_degrees(radians: f64) -> f64 {
    radians * 180.0 / PI
}

pub fn clamp_erpm(erpm: i32, spec: MotorSpec) -> i32 {
    erpm.clamp(spec.min_velocity_erpm, spec.max_velocity_erpm)
}

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

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()
}

fn pack_mit_ak60(command: MitCommand, limits: MitLimits) -> Result<[u8; 8]> {
    let p_int = float_to_uint(
        command.position_rad,
        limits.position_min_rad,
        limits.position_max_rad,
        16,
    )?;
    let v_int = float_to_uint(
        command.velocity_rad_s,
        limits.velocity_min_rad_s,
        limits.velocity_max_rad_s,
        12,
    )?;
    let kp_int = float_to_uint(command.kp, limits.kp_min, limits.kp_max, 12)?;
    let kd_int = float_to_uint(command.kd, limits.kd_min, limits.kd_max, 12)?;
    let t_int = float_to_uint(
        command.torque_nm,
        limits.torque_min_nm,
        limits.torque_max_nm,
        12,
    )?;

    Ok([
        (kp_int >> 4) as u8,
        (((kp_int & 0xF) << 4) | (kd_int >> 8)) as u8,
        (kd_int & 0xFF) as u8,
        (p_int >> 8) as u8,
        (p_int & 0xFF) as u8,
        (v_int >> 4) as u8,
        (((v_int & 0xF) << 4) | (t_int >> 8)) as u8,
        (t_int & 0xFF) as u8,
    ])
}

fn pack_mit_ak80(command: MitCommand, limits: MitLimits) -> Result<[u8; 8]> {
    let p_int = float_to_uint(
        command.position_rad,
        limits.position_min_rad,
        limits.position_max_rad,
        16,
    )?;
    let v_int = float_to_uint(
        command.velocity_rad_s,
        limits.velocity_min_rad_s,
        limits.velocity_max_rad_s,
        12,
    )?;
    let kp_int = float_to_uint(command.kp, limits.kp_min, limits.kp_max, 12)?;
    let kd_int = float_to_uint(command.kd, limits.kd_min, limits.kd_max, 12)?;
    let t_int = float_to_uint(
        command.torque_nm,
        limits.torque_min_nm,
        limits.torque_max_nm,
        12,
    )?;

    Ok([
        (p_int >> 8) as u8,
        (p_int & 0xFF) as u8,
        (v_int >> 4) as u8,
        (((v_int & 0x0F) << 4) | ((kp_int >> 8) & 0x0F)) as u8,
        (kp_int & 0xFF) as u8,
        (kd_int >> 4) as u8,
        (((kd_int & 0x0F) << 4) | ((t_int >> 8) & 0x0F)) as u8,
        (t_int & 0xFF) as u8,
    ])
}

fn parse_ak60_feedback(data: &[u8]) -> Result<MotorFeedback> {
    if data.len() < 8 {
        return Err(Error::PayloadTooShort {
            expected: 8,
            actual: data.len(),
        });
    }

    let position_raw = i16::from_be_bytes([data[0], data[1]]);
    let speed_raw = i16::from_be_bytes([data[2], data[3]]);
    let current_raw = i16::from_be_bytes([data[4], data[5]]);
    let temperature_raw = i8::from_be_bytes([data[6]]);

    Ok(MotorFeedback {
        reported_motor_id: None,
        position_degrees: f64::from(position_raw) * 0.1,
        speed_erpm: i32::from(speed_raw) * 10,
        current_amps: f64::from(current_raw) * 0.01,
        temperature_celsius: i16::from(temperature_raw),
        error_code: data[7],
    })
}

fn parse_ak80_feedback(data: &[u8], spec: MotorSpec) -> Result<MotorFeedback> {
    if data.len() < 7 {
        return Err(Error::PayloadTooShort {
            expected: 7,
            actual: data.len(),
        });
    }

    let motor_id = data[0];
    let p_int = (u32::from(data[1]) << 8) | u32::from(data[2]);
    let v_int = (u32::from(data[3]) << 4) | u32::from(data[4] >> 4);
    let i_int = (u32::from(data[4] & 0x0F) << 8) | u32::from(data[5]);
    let temp_raw = data[6];
    let error_code = data.get(7).copied().unwrap_or(0);

    let position_rad = uint_to_float(
        p_int,
        spec.mit_limits.position_min_rad,
        spec.mit_limits.position_max_rad,
        16,
    )?;
    let velocity_rad_s = uint_to_float(
        v_int,
        spec.mit_limits.velocity_min_rad_s,
        spec.mit_limits.velocity_max_rad_s,
        12,
    )?;
    let current_amps = uint_to_float(i_int, -12.0, 12.0, 12)?;

    Ok(MotorFeedback {
        reported_motor_id: Some(motor_id),
        position_degrees: radians_to_degrees(position_rad),
        speed_erpm: rad_s_to_erpm(velocity_rad_s, spec),
        current_amps,
        temperature_celsius: i16::from(temp_raw) - 40,
        error_code,
    })
}
