"""Tests for MotionFeedbackSentinel (Closed-Loop Motion Feedback).
===================================================================
Kiểm thử cảm biến phản hồi chuyển động vòng lặp kín:
- Xác minh nhận diện di chuyển qua RAM và Optical.
- Xác minh kích hoạt cảnh báo khi nhân vật chôn chân.
- Xác minh khóa bất biến INV-MOTION-IMMOBILE sau 3 lần kẹt liên tiếp.
"""

import pytest
import numpy as np
import time

from src.assistant_tool.navigation.motion_feedback_sentinel import (
    MotionFeedbackSentinel,
    ImmobileCharacterError,
)


def test_motion_sentinel_normal_ram_movement():
    """Kiểm thử khi nhân vật di chuyển bình thường theo dữ liệu RAM."""
    logs = []
    sentinel = MotionFeedbackSentinel(min_move_threshold_u=1.0, log_callback=logs.append)

    # Lấy mẫu trước khi di chuyển
    sentinel.sample_pre_move(pos_xyz=(100.0, 200.0, 0.0))
    time.sleep(0.01)

    # Nhân vật đã di chuyển tới (115.0, 200.0, 0.0) -> Delta = 15.0u
    res = sentinel.sample_post_move(pos_xyz=(115.0, 200.0, 0.0))

    assert res.is_moving is True
    assert res.is_stalled is False
    assert res.stall_count == 0
    assert res.delta_xyz == pytest.approx(15.0, rel=1e-3)
    assert "RAM_CONFIRMED" in res.diagnostic_message


def test_motion_sentinel_stalled_ram_movement():
    """Kiểm thử phát hiện chôn chân khi gửi phím nhưng tọa độ RAM không đổi."""
    logs = []
    sentinel = MotionFeedbackSentinel(
        min_move_threshold_u=1.0,
        max_consecutive_stalls=3,
        log_callback=logs.append,
    )

    # Lần 1: Bấm phím nhưng tọa độ đứng yên
    sentinel.sample_pre_move(pos_xyz=(100.0, 200.0, 0.0))
    time.sleep(0.01)
    res1 = sentinel.sample_post_move(pos_xyz=(100.0, 200.0, 0.0))

    assert res1.is_moving is False
    assert res1.is_stalled is True
    assert res1.stall_count == 1
    assert "RAM_STALLED" in res1.diagnostic_message
    assert len(logs) == 1

    # Lần 2: Tiếp tục đứng yên
    sentinel.sample_pre_move(pos_xyz=(100.0, 200.0, 0.0))
    time.sleep(0.01)
    res2 = sentinel.sample_post_move(pos_xyz=(100.2, 200.1, 0.0))  # Delta = 0.22u < 1.0u

    assert res2.is_moving is False
    assert res2.is_stalled is True
    assert res2.stall_count == 2

    # Lần 3: Đứng yên lần thứ 3 -> BẮT BUỘC ném ImmobileCharacterError
    sentinel.sample_pre_move(pos_xyz=(100.2, 200.1, 0.0))
    time.sleep(0.01)
    with pytest.raises(ImmobileCharacterError) as exc_info:
        sentinel.sample_post_move(pos_xyz=(100.2, 200.1, 0.0))

    assert "INV-MOTION-IMMOBILE-FATAL" in str(exc_info.value)
    assert "Nhân vật bị chôn chân hoàn toàn" in str(exc_info.value)


def test_motion_sentinel_optical_fallback():
    """Kiểm thử chế độ dự phòng qua thị giác quang học khi mất dữ liệu RAM."""
    sentinel = MotionFeedbackSentinel(min_optical_shift_px=3.0)

    frame1 = np.zeros((100, 100), dtype=np.uint8)
    frame2 = np.zeros((100, 100), dtype=np.uint8)
    frame2[20:40, 20:40] = 255  # Tạo biến thiên điểm ảnh quang học

    # 1. Có biến thiên quang học
    sentinel.sample_pre_move(pos_xyz=None, frame_crop=frame1)
    res_opt = sentinel.sample_post_move(pos_xyz=None, frame_crop=frame2)

    assert res_opt.is_moving is True
    assert "OPTICAL_CONFIRMED" in res_opt.diagnostic_message

    # 2. Hai frame y hệt nhau -> Optical Stalled
    sentinel.sample_pre_move(pos_xyz=None, frame_crop=frame1)
    res_stalled = sentinel.sample_post_move(pos_xyz=None, frame_crop=frame1)

    assert res_stalled.is_moving is False
    assert res_stalled.is_stalled is True
    assert "OPTICAL_STALLED" in res_stalled.diagnostic_message