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

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

pub const ROBSTRIDE_DEFAULT_BITRATE: u32 = 1_000_000;
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;

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum RobStrideModel {
    Rs00,
    Rs01,
    Rs02,
    Rs03,
    Rs04,
    Rs05,
    Rs06,
}

pub const ROBSTRIDE_MODELS: [RobStrideModel; 7] = [
    RobStrideModel::Rs00,
    RobStrideModel::Rs01,
    RobStrideModel::Rs02,
    RobStrideModel::Rs03,
    RobStrideModel::Rs04,
    RobStrideModel::Rs05,
    RobStrideModel::Rs06,
];

impl RobStrideModel {
    pub const fn name(self) -> &'static str {
        match self {
            Self::Rs00 => "RS-00",
            Self::Rs01 => "RS-01",
            Self::Rs02 => "RS-02",
            Self::Rs03 => "RS-03",
            Self::Rs04 => "RS-04",
            Self::Rs05 => "RS-05",
            Self::Rs06 => "RS-06",
        }
    }

    pub const fn spec(self) -> RobStrideSpec {
        match self {
            Self::Rs00 => RobStrideSpec::new(self, 14.0, 315.0, 0.0, 500.0, 0.0, 5.0),
            Self::Rs01 => RobStrideSpec::new(self, 17.0, 315.0, 0.0, 500.0, 0.0, 5.0),
            Self::Rs02 => RobStrideSpec::new(self, 17.0, 410.0, 0.0, 500.0, 0.0, 5.0),
            Self::Rs03 => RobStrideSpec::new(self, 60.0, 195.0, 0.0, 5_000.0, 0.0, 100.0),
            Self::Rs04 => RobStrideSpec::new(self, 120.0, 200.0, 0.0, 5_000.0, 0.0, 100.0),
            Self::Rs05 => RobStrideSpec::new(self, 5.5, 480.0, 0.0, 500.0, 0.0, 5.0),
            Self::Rs06 => RobStrideSpec::new(self, 36.0, 480.0, 0.0, 5_000.0, 0.0, 100.0),
        }
    }

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

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

        match compact.as_str() {
            "rs00" | "rs0" => Some(Self::Rs00),
            "rs01" | "rs1" => Some(Self::Rs01),
            "rs02" | "rs2" => Some(Self::Rs02),
            "rs03" | "rs3" => Some(Self::Rs03),
            "rs04" | "rs4" => Some(Self::Rs04),
            "rs05" | "rs5" => Some(Self::Rs05),
            "rs06" | "rs6" => Some(Self::Rs06),
            _ => None,
        }
    }
}

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

impl FromStr for RobStrideModel {
    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 RobStrideSpec {
    pub model: RobStrideModel,
    pub max_torque_nm: f64,
    pub max_speed_rpm: f64,
    pub kp_min: f64,
    pub kp_max: f64,
    pub kd_min: f64,
    pub kd_max: f64,
}

impl RobStrideSpec {
    pub const fn new(
        model: RobStrideModel,
        max_torque_nm: f64,
        max_speed_rpm: f64,
        kp_min: f64,
        kp_max: f64,
        kd_min: f64,
        kd_max: f64,
    ) -> Self {
        Self {
            model,
            max_torque_nm,
            max_speed_rpm,
            kp_min,
            kp_max,
            kd_min,
            kd_max,
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum RobStrideFrameKind {
    Mit,
    Position,
    Speed,
    Status,
    Config,
}

impl RobStrideFrameKind {
    pub const fn name(self) -> &'static str {
        match self {
            Self::Mit => "mit",
            Self::Position => "position",
            Self::Speed => "speed",
            Self::Status => "status",
            Self::Config => "config",
        }
    }

    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 fn can_id(self, motor_id: u8) -> Result<CanId> {
        CanId::extended(self.base_id() + u32::from(motor_id))
    }
}

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

impl FromStr for RobStrideFrameKind {
    type Err = String;

    fn from_str(value: &str) -> std::result::Result<Self, Self::Err> {
        match value.trim().to_ascii_lowercase().as_str() {
            "mit" => Ok(Self::Mit),
            "position" => Ok(Self::Position),
            "speed" => Ok(Self::Speed),
            "status" => Ok(Self::Status),
            "config" => Ok(Self::Config),
            other => Err(format!("unknown RobStride frame kind {other:?}")),
        }
    }
}

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

impl RobStrideMitCommand {
    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,
        }
    }

    pub fn pack(self, spec: RobStrideSpec) -> [u8; 8] {
        let mut data = [0_u8; 8];
        data[..4].copy_from_slice(&scaled_i32_le(
            self.position_rad,
            -100_000.0,
            100_000.0,
            1_000.0,
        ));
        data[4] = scaled_i8_byte(
            self.velocity_rad_s,
            -255.0 / 1_000.0,
            255.0 / 1_000.0,
            1_000.0,
        );
        data[5] = scaled_u8(self.kp, spec.kp_min, spec.kp_max, 5.0);
        data[6] = scaled_u8(self.kd, spec.kd_min, spec.kd_max, 500.0);
        data[7] = scaled_i8_byte(
            self.torque_nm,
            -spec.max_torque_nm,
            spec.max_torque_nm,
            10.0,
        );
        data
    }
}

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

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

impl RobStridePositionCommand {
    pub fn pack(self, spec: RobStrideSpec) -> [u8; 8] {
        let mut data = [0_u8; 8];
        data[..4].copy_from_slice(&scaled_i32_le(
            self.position_rad,
            -100_000.0,
            100_000.0,
            1_000.0,
        ));
        data[4..6].copy_from_slice(&scaled_i16_le(
            self.velocity_rad_s,
            -1_000.0,
            1_000.0,
            1_000.0,
        ));
        data[6..8].copy_from_slice(&scaled_i16_le(
            self.max_torque_nm,
            0.0,
            spec.max_torque_nm,
            10.0,
        ));
        data
    }
}

#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct RobStrideSpeedCommand {
    pub velocity_rad_s: f64,
    pub max_torque_nm: f64,
}

impl RobStrideSpeedCommand {
    pub fn pack(self, spec: RobStrideSpec) -> [u8; 8] {
        let mut data = [0_u8; 8];
        data[..4].copy_from_slice(&scaled_i32_le(
            self.velocity_rad_s,
            -1_000.0,
            1_000.0,
            1_000.0,
        ));
        data[4..6].copy_from_slice(&scaled_i16_le(
            self.max_torque_nm,
            0.0,
            spec.max_torque_nm,
            10.0,
        ));
        data
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct RobStrideRawCommand {
    pub kind: RobStrideFrameKind,
    pub data: [u8; 8],
}

#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum RobStrideCommand {
    Mit(RobStrideMitCommand),
    Position(RobStridePositionCommand),
    Speed(RobStrideSpeedCommand),
    StatusQuery,
    Config(RobStrideRawCommand),
    Raw(RobStrideRawCommand),
}

impl RobStrideCommand {
    pub const fn kind(self) -> RobStrideFrameKind {
        match self {
            Self::Mit(_) => RobStrideFrameKind::Mit,
            Self::Position(_) => RobStrideFrameKind::Position,
            Self::Speed(_) => RobStrideFrameKind::Speed,
            Self::StatusQuery => RobStrideFrameKind::Status,
            Self::Config(_) => RobStrideFrameKind::Config,
            Self::Raw(raw) => raw.kind,
        }
    }

    pub fn payload(self, spec: RobStrideSpec) -> [u8; 8] {
        match self {
            Self::Mit(command) => command.pack(spec),
            Self::Position(command) => command.pack(spec),
            Self::Speed(command) => command.pack(spec),
            Self::StatusQuery => [0_u8; 8],
            Self::Config(raw) | Self::Raw(raw) => raw.data,
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct RobStrideFeedback {
    pub position_rad: f64,
    pub velocity_rad_s: f64,
    pub torque_nm: f64,
    pub mode: Option<u8>,
    pub error_code: Option<u8>,
}

impl RobStrideFeedback {
    pub fn parse(data: &[u8]) -> Option<Self> {
        if data.len() < 8 {
            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: f64::from(position_raw) / 1_000.0,
            velocity_rad_s: f64::from(velocity_raw) / 1_000.0,
            torque_nm: f64::from(torque_raw) / 10.0,
            mode: data.get(8).copied(),
            error_code: data.get(9).copied(),
        })
    }

    pub const fn has_error(&self) -> bool {
        matches!(self.error_code, Some(code) if code != 0)
    }
}

pub fn robstride_command_frame(
    model: RobStrideModel,
    motor_id: u8,
    command: RobStrideCommand,
) -> Result<CanFrame> {
    let spec = model.spec();
    CanFrame::new_padded(command.kind().can_id(motor_id)?, &command.payload(spec))
}

pub fn robstride_status_query_frame(motor_id: u8) -> Result<CanFrame> {
    CanFrame::new_padded(RobStrideFrameKind::Status.can_id(motor_id)?, &[0_u8; 8])
}

fn scaled_i32_le(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_le_bytes()
}

fn scaled_i16_le(value: f64, min: f64, max: f64, scale: f64) -> [u8; 2] {
    let scaled = (value.clamp(min, max) * scale).round();
    (scaled.clamp(f64::from(i16::MIN), f64::from(i16::MAX)) as i16).to_le_bytes()
}

fn scaled_i8_byte(value: f64, min: f64, max: f64, scale: f64) -> u8 {
    let scaled = (value.clamp(min, max) * scale).round();
    let signed = scaled.clamp(f64::from(i8::MIN), f64::from(i8::MAX)) as i8;
    signed as u8
}

fn scaled_u8(value: f64, min: f64, max: f64, scale: f64) -> u8 {
    let scaled = (value.clamp(min, max) * scale).round();
    scaled.clamp(0.0, f64::from(u8::MAX)) as u8
}
