kcan 0.1.2

use crate::protocol::{MotorFeedback, MotorModel};
use crate::protocols::robstride::{RobStrideFeedback, RobStrideModel};

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum ActuatorProtocol {
    CubeMars(MotorModel),
    RobStride(RobStrideModel),
}

impl ActuatorProtocol {
    pub const fn family_name(self) -> &'static str {
        match self {
            Self::CubeMars(_) => "CubeMars",
            Self::RobStride(_) => "RobStride/Seeed",
        }
    }

    pub const fn model_name(self) -> &'static str {
        match self {
            Self::CubeMars(model) => model.name(),
            Self::RobStride(model) => model.name(),
        }
    }
}

#[derive(Debug, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct ActuatorConfig {
    pub id: u8,
    pub label: Option<String>,
    pub protocol: ActuatorProtocol,
}

impl ActuatorConfig {
    pub fn display_name(&self) -> String {
        match &self.label {
            Some(label) => format!("{label} (0x{:02X})", self.id),
            None => format!("{} 0x{:02X}", self.protocol.model_name(), self.id),
        }
    }
}

#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum ActuatorFeedback {
    CubeMars(MotorFeedback),
    RobStride(RobStrideFeedback),
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum ActuatorHealth {
    Nominal,
    Warning,
    Fault,
    Stale,
}

impl ActuatorHealth {
    pub const fn label(self) -> &'static str {
        match self {
            Self::Nominal => "NOMINAL",
            Self::Warning => "WARNING",
            Self::Fault => "FAULT",
            Self::Stale => "STALE",
        }
    }
}

#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct ActuatorMetric {
    pub label: String,
    pub value: String,
    pub unit: String,
    pub health: ActuatorHealth,
}

impl ActuatorMetric {
    pub fn new(
        label: impl Into<String>,
        value: impl Into<String>,
        unit: impl Into<String>,
        health: ActuatorHealth,
    ) -> Self {
        Self {
            label: label.into(),
            value: value.into(),
            unit: unit.into(),
            health,
        }
    }
}

#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct ActuatorSnapshot {
    pub config: ActuatorConfig,
    pub feedback: Option<ActuatorFeedback>,
}

impl ActuatorSnapshot {
    pub fn health(&self) -> ActuatorHealth {
        match &self.feedback {
            None => ActuatorHealth::Stale,
            Some(ActuatorFeedback::CubeMars(feedback)) if feedback.has_fault() => {
                ActuatorHealth::Fault
            }
            Some(ActuatorFeedback::RobStride(feedback)) if feedback.has_error() => {
                ActuatorHealth::Fault
            }
            Some(_) => ActuatorHealth::Nominal,
        }
    }

    pub fn metrics(&self) -> Vec<ActuatorMetric> {
        match &self.feedback {
            Some(ActuatorFeedback::CubeMars(feedback)) => vec![
                ActuatorMetric::new(
                    "pos",
                    format!("{:.2}", feedback.position_degrees),
                    "deg",
                    ActuatorHealth::Nominal,
                ),
                ActuatorMetric::new(
                    "speed",
                    feedback.speed_erpm.to_string(),
                    "erpm",
                    ActuatorHealth::Nominal,
                ),
                ActuatorMetric::new(
                    "current",
                    format!("{:.2}", feedback.current_amps),
                    "A",
                    ActuatorHealth::Nominal,
                ),
                ActuatorMetric::new(
                    "temp",
                    feedback.temperature_celsius.to_string(),
                    "C",
                    ActuatorHealth::Nominal,
                ),
                ActuatorMetric::new(
                    "fault",
                    feedback.error_code.to_string(),
                    "",
                    if feedback.has_fault() {
                        ActuatorHealth::Fault
                    } else {
                        ActuatorHealth::Nominal
                    },
                ),
            ],
            Some(ActuatorFeedback::RobStride(feedback)) => vec![
                ActuatorMetric::new(
                    "pos",
                    format!("{:.3}", feedback.position_rad),
                    "rad",
                    ActuatorHealth::Nominal,
                ),
                ActuatorMetric::new(
                    "vel",
                    format!("{:.3}", feedback.velocity_rad_s),
                    "rad/s",
                    ActuatorHealth::Nominal,
                ),
                ActuatorMetric::new(
                    "torque",
                    format!("{:.2}", feedback.torque_nm),
                    "Nm",
                    ActuatorHealth::Nominal,
                ),
                ActuatorMetric::new(
                    "error",
                    feedback.error_code.unwrap_or(0).to_string(),
                    "",
                    if feedback.has_error() {
                        ActuatorHealth::Fault
                    } else {
                        ActuatorHealth::Nominal
                    },
                ),
            ],
            None => vec![ActuatorMetric::new(
                "feedback",
                "missing",
                "",
                ActuatorHealth::Stale,
            )],
        }
    }

    pub fn status_text(&self) -> String {
        match &self.feedback {
            Some(ActuatorFeedback::CubeMars(feedback)) => format!(
                "pos={:.2}deg speed={}erpm current={:.2}A temp={}C fault={}",
                feedback.position_degrees,
                feedback.speed_erpm,
                feedback.current_amps,
                feedback.temperature_celsius,
                feedback.error_code
            ),
            Some(ActuatorFeedback::RobStride(feedback)) => format!(
                "pos={:.3}rad vel={:.3}rad/s torque={:.2}Nm error={}",
                feedback.position_rad,
                feedback.velocity_rad_s,
                feedback.torque_nm,
                feedback.error_code.unwrap_or(0)
            ),
            None => "no feedback".to_owned(),
        }
    }
}