use kcan::protocols::robstride::{
    RobStrideCommand, RobStrideFeedback, RobStrideFrameKind, RobStrideMitCommand, RobStrideModel,
    RobStridePositionCommand, RobStrideRawCommand, RobStrideSpeedCommand, robstride_command_frame,
    robstride_status_query_frame,
};
use kcan::{ActuatorConfig, ActuatorProtocol, CanId, Error};

#[test]
fn robstride_models_expose_specs() {
    let spec = RobStrideModel::Rs04.spec();

    assert_eq!(RobStrideModel::Rs04.name(), "RS-04");
    assert_eq!(RobStrideModel::all().len(), 7);
    assert_eq!(
        RobStrideModel::from_name("rs-01"),
        Some(RobStrideModel::Rs01)
    );
    assert_eq!(
        RobStrideModel::from_name("RS_06"),
        Some(RobStrideModel::Rs06)
    );
    assert_eq!(RobStrideModel::from_name("unknown"), None);
    assert_eq!(RobStrideModel::Rs04.to_string(), "RS-04");
    assert_eq!(
        "rs-01".parse::<RobStrideModel>().unwrap(),
        RobStrideModel::Rs01
    );
    assert_eq!(
        "bad".parse::<RobStrideModel>().unwrap_err(),
        Error::UnknownMotorLabel("bad".to_owned())
    );
    assert_eq!(spec.max_torque_nm, 120.0);
    assert_eq!(spec.kp_max, 5_000.0);
    assert_eq!(spec.kd_max, 100.0);
}

#[test]
fn robstride_frame_kind_uses_documented_base_ids() {
    assert_eq!(RobStrideFrameKind::Mit.name(), "mit");
    assert_eq!(RobStrideFrameKind::Config.to_string(), "config");
    assert_eq!(
        "status".parse::<RobStrideFrameKind>().unwrap(),
        RobStrideFrameKind::Status
    );
    assert!("bad".parse::<RobStrideFrameKind>().is_err());

    assert_eq!(
        RobStrideFrameKind::Mit.can_id(1).unwrap(),
        CanId::Extended(0x201)
    );
    assert_eq!(
        RobStrideFrameKind::Position.can_id(1).unwrap(),
        CanId::Extended(0x301)
    );
    assert_eq!(
        RobStrideFrameKind::Speed.can_id(1).unwrap(),
        CanId::Extended(0x401)
    );
    assert_eq!(
        RobStrideFrameKind::Status.can_id(1).unwrap(),
        CanId::Extended(0x501)
    );
    assert_eq!(
        RobStrideFrameKind::Config.can_id(1).unwrap(),
        CanId::Extended(0x601)
    );
}

#[test]
fn robstride_mit_command_follows_seeed_simple_layout() {
    let frame = robstride_command_frame(
        RobStrideModel::Rs01,
        1,
        RobStrideCommand::Mit(RobStrideMitCommand {
            position_rad: 1.25,
            velocity_rad_s: 0.1,
            kp: 30.0,
            kd: 0.5,
            torque_nm: -1.2,
        }),
    )
    .unwrap();

    assert_eq!(frame.id(), CanId::Extended(0x201));
    assert_eq!(&frame.data()[0..4], &1_250_i32.to_le_bytes());
    assert_eq!(frame.data()[4], 100);
    assert_eq!(frame.data()[5], 150);
    assert_eq!(frame.data()[6], 250);
    assert_eq!(frame.data()[7], (-12_i8) as u8);
}

#[test]
fn robstride_position_command_uses_eight_byte_layout() {
    let frame = robstride_command_frame(
        RobStrideModel::Rs02,
        2,
        RobStrideCommand::Position(RobStridePositionCommand {
            position_rad: -2.0,
            velocity_rad_s: 0.25,
            max_torque_nm: 6.0,
        }),
    )
    .unwrap();

    assert_eq!(frame.id(), CanId::Extended(0x302));
    assert_eq!(&frame.data()[0..4], &(-2_000_i32).to_le_bytes());
    assert_eq!(&frame.data()[4..6], &250_i16.to_le_bytes());
    assert_eq!(&frame.data()[6..8], &60_i16.to_le_bytes());
}

#[test]
fn robstride_speed_status_and_raw_frames_build() {
    let speed = robstride_command_frame(
        RobStrideModel::Rs03,
        3,
        RobStrideCommand::Speed(RobStrideSpeedCommand {
            velocity_rad_s: 1.5,
            max_torque_nm: 12.0,
        }),
    )
    .unwrap();
    let status = robstride_status_query_frame(3).unwrap();
    let raw = robstride_command_frame(
        RobStrideModel::Rs03,
        3,
        RobStrideCommand::Raw(RobStrideRawCommand {
            kind: RobStrideFrameKind::Config,
            data: [1, 2, 3, 4, 5, 6, 7, 8],
        }),
    )
    .unwrap();

    assert_eq!(speed.id(), CanId::Extended(0x403));
    assert_eq!(&speed.data()[0..4], &1_500_i32.to_le_bytes());
    assert_eq!(status.id(), CanId::Extended(0x503));
    assert_eq!(raw.id(), CanId::Extended(0x603));
    assert_eq!(raw.data(), &[1, 2, 3, 4, 5, 6, 7, 8]);
}

#[test]
fn robstride_feedback_parses_documented_numeric_fields() {
    let mut data = [0_u8; 8];
    data[0..4].copy_from_slice(&1_250_i32.to_le_bytes());
    data[4..6].copy_from_slice(&(-500_i16).to_le_bytes());
    data[6..8].copy_from_slice(&42_i16.to_le_bytes());

    let feedback = RobStrideFeedback::parse(&data).unwrap();

    assert_eq!(feedback.position_rad, 1.25);
    assert_eq!(feedback.velocity_rad_s, -0.5);
    assert_eq!(feedback.torque_nm, 4.2);
    assert_eq!(feedback.mode, None);
    assert_eq!(feedback.error_code, None);
    assert!(!feedback.has_error());
}

#[test]
fn actuator_config_formats_protocol_family() {
    let config = ActuatorConfig {
        id: 1,
        label: Some("left-knee".to_owned()),
        protocol: ActuatorProtocol::RobStride(RobStrideModel::Rs01),
    };

    assert_eq!(config.protocol.family_name(), "RobStride/Seeed");
    assert_eq!(config.protocol.model_name(), "RS-01");
    assert_eq!(config.display_name(), "left-knee (0x01)");
}
