from line_laser_modbus.codec import (
    build_read_frame,
    build_write_frame,
    decode_pose,
    decode_timed_pose,
    encode_pose,
    encode_timed_pose,
)
from line_laser_modbus.models import Pose6D, TimedPose6D


def test_read_frame_matches_readme_example() -> None:
    frame = build_read_frame(0xD000, 2)
    assert frame.hex(" ").upper() == "08 03 D0 00 00 02 FC 52"


def test_mode_write_frame_matches_readme_example() -> None:
    frame = build_write_frame(0xD000, [0x0003])
    assert frame.hex(" ").upper() == "08 10 D0 00 00 01 02 00 03 5D CC"


def test_pose_float_register_roundtrip() -> None:
    pose = Pose6D(1.25, -2.5, 3.0, 0.0, 45.5, -90.0)
    assert decode_pose(encode_pose(pose)) == pose


def test_timed_pose_register_roundtrip() -> None:
    data = TimedPose6D(1000, Pose6D(1.25, -2.5, 3.0, 0.0, 45.5, -90.0))
    registers = encode_timed_pose(data)

    assert len(registers) == 14
    assert decode_timed_pose(registers) == data


def test_current_pose_read_frame_matches_protocol_example() -> None:
    frame = build_read_frame(0xD00A, 14)
    assert frame.hex(" ").upper() == "08 03 D0 0A 00 0E DC 55"


def test_correction_write_frame_matches_protocol_docx_example() -> None:
    data = TimedPose6D(1000, Pose6D(1.0, 2.0, 3.0, 0.0, 1.0, 2.0))
    frame = build_write_frame(0xD036, encode_timed_pose(data), slave_id=0x01)

    assert (
        frame.hex(" ").upper()
        == "01 10 D0 36 00 0E 1C 00 00 03 E8 3F 80 00 00 40 00 00 00 "
        "40 40 00 00 00 00 00 00 3F 80 00 00 40 00 00 00 D0 72"
    )