use kcan::protocol::{DirectCommand, OriginMode, direct_command_frame};

fn main() -> Result<(), kcan::Error> {
    let motor_id = 0x03;
    let commands = [
        ("duty", DirectCommand::DutyCycle(0.2)),
        ("current", DirectCommand::Current(1.0)),
        ("brake", DirectCommand::BrakeCurrent(0.5)),
        ("velocity", DirectCommand::Velocity(1_000)),
        ("position", DirectCommand::Position(90.0)),
        (
            "position-velocity",
            DirectCommand::PositionVelocity {
                position_degrees: 180.0,
                velocity_erpm: 5_000,
                acceleration_erpm_per_sec: 30_000,
            },
        ),
        ("origin", DirectCommand::SetOrigin(OriginMode::Temporary)),
    ];

    for (name, command) in commands {
        let frame = direct_command_frame(motor_id, command)?;
        println!(
            "{name:<17} 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(" ")
}
