"""MyArmSTS behavior tests with mocked serial."""

from unittest.mock import MagicMock, patch

import pytest

from pymyarm import MyArmSTS
from pymyarm.constants import Instruction, MemAddr, TorqueEnable
from pymyarm.exceptions import InvalidParameterError
from pymyarm.protocol import calculate_checksum


def _resp(servo_id: int, err: int = 0, params=None) -> bytes:
    if params is None:
        params = []
    body = [servo_id, len(params) + 2, err] + params
    chk = calculate_checksum(body)
    return bytes([0xFF, 0xFF] + body + [chk])


@pytest.fixture
def arm_and_port():
    with patch("pymyarm.port.Serial") as serial_cls:
        port = MagicMock()
        port.is_open = True
        port.in_waiting = 64  # Simulate bytes available for read_available
        serial_cls.return_value = port
        arm = MyArmSTS("/dev/ttyUSB0")
        arm.connect()
        yield arm, port


def test_set_servo_enabled(arm_and_port):
    arm, port = arm_and_port
    port.read.return_value = _resp(1)

    ok = arm.set_servo_enabled(1, 1)

    assert ok is True
    frame = port.write.call_args[0][0]
    assert frame[2] == 1
    assert frame[5] == MemAddr.TORQUE_ENABLE
    assert frame[6] == TorqueEnable.ON


def test_set_servo_calibrate(arm_and_port):
    arm, port = arm_and_port
    port.read.return_value = _resp(2)

    ok = arm.set_servo_calibrate(2)

    assert ok is True
    frame = port.write.call_args[0][0]
    assert frame[2] == 2
    assert frame[5] == MemAddr.TORQUE_ENABLE
    assert frame[6] == TorqueEnable.CALIBRATE_CENTER


def test_set_servo_offset_calibrate(arm_and_port):
    arm, port = arm_and_port
    port.read.return_value = _resp(1)

    ok = arm.set_servo_offset_calibrate(1, 3072)

    assert ok is True
    frame = port.write.call_args[0][0]
    assert frame[2] == 1  # servo_id
    assert frame[4] == Instruction.POSITION_CALIBRATE  # 0x0B, not WRITE_DATA
    assert frame[5] == 0x00  # 3072 low byte
    assert frame[6] == 0x0C  # 3072 high byte


def test_set_servo_offset_calibrate_error_response(arm_and_port):
    arm, port = arm_and_port
    port.read.return_value = _resp(1, err=0x04)

    ok = arm.set_servo_offset_calibrate(1, 2048)

    assert ok is False


def test_set_servo_offset_calibrate_rejects_negative(arm_and_port):
    arm, _ = arm_and_port

    with pytest.raises(InvalidParameterError, match="target_position"):
        arm.set_servo_offset_calibrate(1, -1)


def test_set_servo_offset_calibrate_rejects_over_4095(arm_and_port):
    arm, _ = arm_and_port

    with pytest.raises(InvalidParameterError, match="target_position"):
        arm.set_servo_offset_calibrate(1, 4096)


def test_get_servo_offset(arm_and_port):
    arm, port = arm_and_port
    # Return raw value 512 = [0x00, 0x02]
    port.read.return_value = _resp(1, 0, [0x00, 0x02])

    val = arm.get_servo_offset(1)

    assert val == 512
    frame = port.write.call_args[0][0]
    assert frame[4] == Instruction.READ_DATA
    assert frame[5] == MemAddr.OFFSET_POSITION


def test_release_and_focus_joint_dual_motor(arm_and_port):
    arm, port = arm_and_port
    # joint 2 -> motors [2,3]
    port.read.side_effect = [_resp(2), _resp(3), _resp(2), _resp(3)]

    assert arm.release_joint(2, include_dual_motor=True) is True
    assert arm.focus_joint(2, include_dual_motor=True) is True

    sent = [c.args[0] for c in port.write.call_args_list]
    # release: motor2 off, motor3 off
    assert sent[0][2] == 2 and sent[0][6] == TorqueEnable.OFF
    assert sent[1][2] == 3 and sent[1][6] == TorqueEnable.OFF
    # focus: motor2 on, motor3 on
    assert sent[2][2] == 2 and sent[2][6] == TorqueEnable.ON
    assert sent[3][2] == 3 and sent[3][6] == TorqueEnable.ON


def test_get_joint_angle_from_primary_motor(arm_and_port):
    arm, port = arm_and_port
    # servo 1 read position = 2048
    port.read.return_value = _resp(1, 0, [0x00, 0x08])

    angle = arm.get_joint_angle(1)

    assert angle == 0.0


def test_set_joint_angle_joint2_sync_write(arm_and_port):
    arm, port = arm_and_port
    # sync write does not expect response
    ok = arm.set_joint_angle(2, 0.0, speed=50)

    assert ok is True
    frame = port.write.call_args[0][0]
    # broadcast + SYNC_WRITE
    assert frame[2] == 0xFE
    assert frame[4] == 0x83


def test_get_servos_encoder_returns_8_values(arm_and_port):
    arm, _ = arm_and_port
    arm._sts.read_positions = MagicMock(
        return_value={1: 100, 2: 200, 3: 300, 4: 400, 5: 500, 6: 600, 7: 700, 8: 800}
    )

    encoders = arm.get_servos_encoder()

    assert len(encoders) == 8
    assert encoders == [100, 200, 300, 400, 500, 600, 700, 800]


def test_set_servos_encoder_requires_8_values(arm_and_port):
    arm, _ = arm_and_port

    with pytest.raises(InvalidParameterError):
        arm.set_servos_encoder([2048] * 6, speed=50)


def test_set_servos_encoder_writes_all_8_targets(arm_and_port):
    arm, _ = arm_and_port
    arm._sts.write_positions_sync = MagicMock(return_value=True)

    ok = arm.set_servos_encoder([101, 202, 303, 404, 505, 606, 707, 808], speed=50)

    assert ok is True
    motor_targets = arm._sts.write_positions_sync.call_args[0][0]
    assert len(motor_targets) == 8
    assert motor_targets[0] == (1, 101)
    assert motor_targets[-1] == (8, 808)


def test_response_error_code_is_cached(arm_and_port):
    arm, port = arm_and_port
    port.read.return_value = _resp(1, err=0x04)

    ok = arm.set_servo_enabled(1, 1)

    assert ok is False
    assert arm._controller.get_cached_error_code(1) == 0x04


def test_get_robot_error_status_prefer_cached(arm_and_port):
    arm, _ = arm_and_port
    all_ids = arm._config.all_servo_ids(include_gripper=True)
    expected = [servo_id for servo_id in all_ids]

    arm._controller.get_cached_error_code = MagicMock(
        side_effect=lambda servo_id: servo_id
    )
    arm._controller.read_register = MagicMock(
        side_effect=RuntimeError("should not read status")
    )

    result = arm.get_robot_error_status(prefer_cached=True)

    assert result == expected
    arm._controller.read_register.assert_not_called()


def test_sts_write_position_rejects_negative_value(arm_and_port):
    arm, _ = arm_and_port

    with pytest.raises(InvalidParameterError, match="non-negative"):
        arm._sts.write_position(1, -1, 100)


def test_comm_stats_count_no_response_events(arm_and_port):
    arm, _ = arm_and_port
    arm._controller._client.read_available = MagicMock(return_value=b"")

    before = arm.get_comm_stats()
    assert before["no_response_count"] == 0

    _ = arm.get_servos_encoder()
    after = arm.get_comm_stats()

    assert after["no_response_count"] >= 8
    assert after["connected_duration_sec"] >= 0


def test_print_and_reset_comm_stats(arm_and_port, capsys):
    arm, _ = arm_and_port
    arm._controller._client.read_available = MagicMock(return_value=b"")
    _ = arm.get_servos_encoder()

    arm.print_comm_stats()
    output = capsys.readouterr().out
    assert "[comm-stats]" in output

    arm.reset_comm_stats()
    reset_stats = arm.get_comm_stats()
    assert reset_stats["no_response_count"] == 0
