use kcan::protocols::robstride::{
    RobStrideCommand, RobStrideMitCommand, RobStrideModel, RobStridePositionCommand,
    RobStrideSpeedCommand, robstride_command_frame, robstride_status_query_frame,
};

fn main() -> Result<(), kcan::Error> {
    let model = RobStrideModel::Rs01;
    let motor_id = 0x01;
    let frames = [
        ("status", robstride_status_query_frame(motor_id)?),
        (
            "mit",
            robstride_command_frame(
                model,
                motor_id,
                RobStrideCommand::Mit(RobStrideMitCommand {
                    position_rad: 0.25,
                    velocity_rad_s: 0.05,
                    kp: 20.0,
                    kd: 0.4,
                    torque_nm: 0.5,
                }),
            )?,
        ),
        (
            "position",
            robstride_command_frame(
                model,
                motor_id,
                RobStrideCommand::Position(RobStridePositionCommand {
                    position_rad: 0.5,
                    velocity_rad_s: 0.1,
                    max_torque_nm: 3.0,
                }),
            )?,
        ),
        (
            "speed",
            robstride_command_frame(
                model,
                motor_id,
                RobStrideCommand::Speed(RobStrideSpeedCommand {
                    velocity_rad_s: 0.2,
                    max_torque_nm: 2.0,
                }),
            )?,
        ),
    ];

    for (name, frame) in frames {
        println!(
            "{name:<8} id={:?} len={} data={}",
            frame.id(),
            frame.len(),
            hex(frame.data())
        );
    }

    Ok(())
}

fn hex(data: &[u8]) -> String {
    data.iter()
        .map(|byte| format!("{byte:02X}"))
        .collect::<Vec<_>>()
        .join(" ")
}
