kcan 0.1.6

use kcan::protocol::{
    AK60_6_MIT_LIMITS, AK80_6_MIT_LIMITS, ControlMode, DirectCommand, FaultCode, MitCommand,
    MitHelperCommand, MotorModel, OriginMode, degrees_to_radians, direct_command_frame,
    erpm_to_rad_s, float_to_uint, mit_command_frame, mit_helper_frame, pack_mit_command,
    rad_s_to_erpm, radians_to_degrees, uint_to_float,
};
use kcan::{CanId, Error};

fn assert_close(left: f64, right: f64, tolerance: f64) {
    assert!(
        (left - right).abs() <= tolerance,
        "left={left} right={right} tolerance={tolerance}"
    );
}

#[test]
fn float_to_uint_clamps_to_range() {
    assert_eq!(float_to_uint(-999.0, -1.0, 1.0, 12).unwrap(), 0);
    assert_eq!(float_to_uint(999.0, -1.0, 1.0, 12).unwrap(), 4095);
}

#[test]
fn uint_to_float_round_trips_midpoint() {
    let raw = float_to_uint(0.0, -12.56, 12.56, 16).unwrap();
    let value = uint_to_float(raw, -12.56, 12.56, 16).unwrap();
    assert_close(value, 0.0, 0.001);
}

#[test]
fn invalid_bit_width_is_rejected() {
    assert_eq!(
        float_to_uint(0.0, -1.0, 1.0, 0).unwrap_err(),
        Error::InvalidBitWidth(0)
    );
}

#[test]
fn ak60_mit_frame_uses_extended_layout_byte_order() {
    let limits = AK60_6_MIT_LIMITS;
    let p_int = float_to_uint(1.0, limits.position_min_rad, limits.position_max_rad, 16).unwrap();
    let v_int = float_to_uint(
        2.0,
        limits.velocity_min_rad_s,
        limits.velocity_max_rad_s,
        12,
    )
    .unwrap();
    let kp_int = float_to_uint(30.0, limits.kp_min, limits.kp_max, 12).unwrap();
    let kd_int = float_to_uint(1.5, limits.kd_min, limits.kd_max, 12).unwrap();
    let t_int = float_to_uint(3.0, limits.torque_min_nm, limits.torque_max_nm, 12).unwrap();

    let expected = [
        (kp_int >> 4) as u8,
        (((kp_int & 0xF) << 4) | (kd_int >> 8)) as u8,
        (kd_int & 0xFF) as u8,
        (p_int >> 8) as u8,
        (p_int & 0xFF) as u8,
        (v_int >> 4) as u8,
        (((v_int & 0xF) << 4) | (t_int >> 8)) as u8,
        (t_int & 0xFF) as u8,
    ];

    let payload = pack_mit_command(
        MotorModel::Ak60_6,
        MitCommand {
            position_rad: 1.0,
            velocity_rad_s: 2.0,
            kp: 30.0,
            kd: 1.5,
            torque_nm: 3.0,
        },
    )
    .unwrap();

    assert_eq!(payload, expected);
}

#[test]
fn ak80_mit_frame_uses_standard_layout() {
    let payload = pack_mit_command(
        MotorModel::Ak80_6,
        MitCommand {
            position_rad: 0.0,
            velocity_rad_s: 0.0,
            kp: 0.0,
            kd: 0.0,
            torque_nm: 0.0,
        },
    )
    .unwrap();

    assert_eq!(payload.len(), 8);
    assert_eq!(payload[0], 0x7F);
    assert_eq!(payload[1], 0xFF);
}

#[test]
fn mit_command_frame_uses_model_specific_id() {
    let ak60 = mit_command_frame(MotorModel::Ak60_6, 0x03, MitCommand::neutral()).unwrap();
    let ak80 = mit_command_frame(MotorModel::Ak80_6, 0x03, MitCommand::neutral()).unwrap();

    assert_eq!(ak60.id(), CanId::Extended(0x0803));
    assert_eq!(ak80.id(), CanId::Standard(0x0003));
    assert_eq!(ak60.len(), 8);
}

#[test]
fn command_modes_have_stable_names_and_parsers() {
    assert_eq!(ControlMode::DutyCycle.name(), "duty");
    assert_eq!(ControlMode::VelocityLoop.to_string(), "velocity");
    assert_eq!(
        "current-loop".parse::<ControlMode>().unwrap(),
        ControlMode::CurrentLoop
    );
    assert_eq!(
        "profile".parse::<ControlMode>().unwrap(),
        ControlMode::PositionVelocity
    );
    assert!("unknown".parse::<ControlMode>().is_err());

    assert_eq!(OriginMode::Temporary.to_string(), "temporary");
    assert_eq!("perm".parse::<OriginMode>().unwrap(), OriginMode::Permanent);
    assert!("bad".parse::<OriginMode>().is_err());

    assert_eq!(MitHelperCommand::ZeroPosition.to_string(), "zero");
    assert_eq!(
        "zero-position".parse::<MitHelperCommand>().unwrap(),
        MitHelperCommand::ZeroPosition
    );
    assert_eq!(
        "enable-legacy".parse::<MitHelperCommand>().unwrap(),
        MitHelperCommand::EnableLegacy
    );
    assert!("bad".parse::<MitHelperCommand>().is_err());
}

#[test]
fn helper_frame_uses_raw_motor_id() {
    let frame = mit_helper_frame(MotorModel::Ak60_6, 0x03, MitHelperCommand::ZeroPosition).unwrap();

    assert_eq!(frame.id(), CanId::Extended(0x03));
    assert_eq!(
        frame.data(),
        &[0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE]
    );
}

#[test]
fn feedback_decodes_ak60_status_frame() {
    let data = [0x00, 0x64, 0x00, 0x0A, 0xFF, 0x9C, 0x19, 0x02];
    let feedback = MotorModel::Ak60_6.parse_feedback(&data).unwrap();

    assert_close(feedback.position_degrees, 10.0, f64::EPSILON);
    assert_eq!(feedback.speed_erpm, 100);
    assert_close(feedback.current_amps, -1.0, f64::EPSILON);
    assert_eq!(feedback.temperature_celsius, 25);
    assert_eq!(feedback.error_code, 2);
    assert_eq!(feedback.fault_code(), Some(FaultCode::OverCurrent));
    assert!(feedback.has_fault());
    assert_eq!(feedback.error_description(), "Over-current");
}

#[test]
fn fault_codes_map_known_errors() {
    assert_eq!(FaultCode::from_code(0), Some(FaultCode::None));
    assert_eq!(FaultCode::from_code(7), Some(FaultCode::MotorLockUp));
    assert_eq!(FaultCode::from_code(99), None);
    assert_eq!(FaultCode::UnderVoltage.code(), 4);
    assert_eq!(FaultCode::UnderVoltage.description(), "Under-voltage");
}

#[test]
fn direct_duty_frame_uses_extended_mode_id_and_scaled_i32() {
    let frame = direct_command_frame(0x03, DirectCommand::DutyCycle(0.5)).unwrap();

    assert_eq!(frame.id(), CanId::Extended(0x0003));
    assert_eq!(frame.len(), 4);
    assert_eq!(&frame.data()[..4], &50_000_i32.to_be_bytes());
}

#[test]
fn direct_current_and_brake_frames_use_milliamp_scaling() {
    let current = direct_command_frame(0x03, DirectCommand::Current(2.5)).unwrap();
    let brake = direct_command_frame(0x03, DirectCommand::BrakeCurrent(-1.5)).unwrap();

    assert_eq!(current.id(), CanId::Extended(0x0103));
    assert_eq!(&current.data()[..4], &2_500_i32.to_be_bytes());
    assert_eq!(brake.id(), CanId::Extended(0x0203));
    assert_eq!(&brake.data()[..4], &(-1_500_i32).to_be_bytes());
}

#[test]
fn direct_velocity_frame_clamps_protocol_range() {
    let frame = direct_command_frame(0x03, DirectCommand::Velocity(999_999)).unwrap();

    assert_eq!(frame.id(), CanId::Extended(0x0303));
    assert_eq!(frame.len(), 4);
    assert_eq!(&frame.data()[..4], &100_000_i32.to_be_bytes());
}

#[test]
fn direct_position_frame_uses_degree_scaling() {
    let frame = direct_command_frame(0x03, DirectCommand::Position(10.0)).unwrap();

    assert_eq!(frame.id(), CanId::Extended(0x0403));
    assert_eq!(frame.len(), 4);
    assert_eq!(&frame.data()[..4], &100_000_i32.to_be_bytes());
}

#[test]
fn direct_origin_frame_uses_origin_mode_byte() {
    let frame =
        direct_command_frame(0x03, DirectCommand::SetOrigin(OriginMode::Permanent)).unwrap();

    assert_eq!(frame.id(), CanId::Extended(0x0503));
    assert_eq!(frame.len(), 1);
    assert_eq!(frame.data()[0], 0x01);
}

#[test]
fn direct_position_velocity_frame_uses_profile_layout() {
    let frame = direct_command_frame(
        0x03,
        DirectCommand::PositionVelocity {
            position_degrees: 1.5,
            velocity_erpm: -100,
            acceleration_erpm_per_sec: 200,
        },
    )
    .unwrap();

    assert_eq!(frame.id(), CanId::Extended(0x0603));
    assert_eq!(&frame.data()[..4], &15_000_i32.to_be_bytes());
    assert_eq!(&frame.data()[4..6], &(-10_i16).to_be_bytes());
    assert_eq!(&frame.data()[6..8], &20_i16.to_be_bytes());
}

#[test]
fn direct_payload_bytes_match_frame_payload_without_vec() {
    let command = DirectCommand::PositionVelocity {
        position_degrees: 1.5,
        velocity_erpm: -100,
        acceleration_erpm_per_sec: 200,
    };
    let payload = command.payload_bytes();
    let frame = direct_command_frame(0x03, command).unwrap();

    assert_eq!(payload.len(), 8);
    assert!(!payload.is_empty());
    assert_eq!(payload.data(), frame.data());
    assert_eq!(payload.data(), command.payload().as_slice());
    assert_eq!(payload.padded_data(), frame.padded_data());

    let origin = DirectCommand::SetOrigin(OriginMode::Temporary).payload_bytes();
    assert_eq!(origin.len(), 1);
    assert_eq!(origin.data(), &[0x00]);
    assert_eq!(origin.padded_data()[1], 0x00);
}

#[test]
fn model_limits_follow_cube_mars_ranges() {
    assert_eq!(AK60_6_MIT_LIMITS.velocity_max_rad_s, 60.0);
    assert_eq!(AK60_6_MIT_LIMITS.torque_max_nm, 12.0);
    assert_eq!(AK80_6_MIT_LIMITS.position_min_rad, -12.5);
    assert_eq!(AK80_6_MIT_LIMITS.position_max_rad, 12.5);

    assert_eq!(MotorModel::Ak60_6.spec().max_velocity_erpm, 48_128);
    assert_eq!(MotorModel::Ak80_6.spec().max_velocity_erpm, 91_444);
}

#[test]
fn cubemars_supported_models_pack_neutral_mit_frames() {
    assert!(MotorModel::all().len() > 10);
    assert_eq!(MotorModel::from_name("ak90-9"), Some(MotorModel::Ak90_9));
    assert_eq!(
        MotorModel::from_name("AK100_33"),
        Some(MotorModel::Ak100_33)
    );
    assert_eq!(MotorModel::from_name("unknown"), None);
    assert_eq!(MotorModel::Ak60_6.to_string(), "AK60-6");
    assert_eq!("ak60-6".parse::<MotorModel>().unwrap(), MotorModel::Ak60_6);
    assert_eq!(
        "AK100_33".parse::<MotorModel>().unwrap(),
        MotorModel::Ak100_33
    );
    assert_eq!(
        "unknown".parse::<MotorModel>().unwrap_err(),
        Error::UnknownMotorLabel("unknown".to_owned())
    );

    for model in MotorModel::all() {
        assert!(!model.name().is_empty());
        let frame = mit_command_frame(*model, 0x03, MitCommand::neutral()).unwrap();
        assert_eq!(frame.len(), 8);

        if model.uses_extended_mit_layout() {
            assert_eq!(frame.id(), CanId::Extended(0x0803));
        } else {
            assert_eq!(frame.id(), CanId::Standard(0x0003));
        }
    }
}

#[test]
fn unit_conversions_round_trip() {
    let spec = MotorModel::Ak60_6.spec();
    let rad_s = erpm_to_rad_s(7_840, spec);
    assert_eq!(rad_s_to_erpm(rad_s, spec), 7_840);
    assert_close(radians_to_degrees(degrees_to_radians(90.0)), 90.0, 1e-12);
}