use kcan::protocols::robstride::RobStrideFeedback;
use kcan::{
    ActuatorConfig, ActuatorFeedback, ActuatorHealth, ActuatorProtocol, ActuatorSnapshot,
    MotorFeedback, MotorModel, RobStrideModel,
};

#[test]
fn actuator_health_tracks_feedback_state() {
    assert_eq!(sample_cubemars(None).health(), ActuatorHealth::Nominal);

    assert_eq!(
        sample_cubemars(Some(MotorFeedback {
            error_code: 2,
            ..cubemars_feedback()
        }))
        .health(),
        ActuatorHealth::Fault
    );

    assert_eq!(
        ActuatorSnapshot {
            config: ActuatorConfig {
                id: 3,
                label: None,
                protocol: ActuatorProtocol::CubeMars(MotorModel::Ak60_6),
            },
            feedback: None,
        }
        .health(),
        ActuatorHealth::Stale
    );
}

#[test]
fn actuator_metrics_are_protocol_specific() {
    let cubemars = sample_cubemars(None);
    let cubemars_metrics = cubemars.metrics();

    assert!(cubemars_metrics.iter().any(|metric| {
        metric.label == "temp" && metric.value == "35" && metric.health == ActuatorHealth::Nominal
    }));
    assert!(cubemars_metrics.iter().any(|metric| {
        metric.label == "fault" && metric.value == "0" && metric.health == ActuatorHealth::Nominal
    }));

    let robstride = ActuatorSnapshot {
        config: ActuatorConfig {
            id: 4,
            label: None,
            protocol: ActuatorProtocol::RobStride(RobStrideModel::Rs01),
        },
        feedback: Some(ActuatorFeedback::RobStride(RobStrideFeedback {
            position_rad: 0.25,
            velocity_rad_s: 0.1,
            torque_nm: 21.0,
            mode: Some(1),
            error_code: Some(0),
        })),
    };
    let robstride_metrics = robstride.metrics();

    assert_eq!(robstride.health(), ActuatorHealth::Nominal);
    assert!(robstride_metrics.iter().any(|metric| {
        metric.label == "torque"
            && metric.value == "21.00"
            && metric.health == ActuatorHealth::Nominal
    }));
}

fn sample_cubemars(feedback: Option<MotorFeedback>) -> ActuatorSnapshot {
    ActuatorSnapshot {
        config: ActuatorConfig {
            id: 3,
            label: Some("left-knee".to_owned()),
            protocol: ActuatorProtocol::CubeMars(MotorModel::Ak60_6),
        },
        feedback: Some(ActuatorFeedback::CubeMars(
            feedback.unwrap_or_else(cubemars_feedback),
        )),
    }
}

fn cubemars_feedback() -> MotorFeedback {
    MotorFeedback {
        reported_motor_id: Some(3),
        position_degrees: 12.5,
        speed_erpm: 800,
        current_amps: 1.2,
        temperature_celsius: 35,
        error_code: 0,
    }
}
