"""state_machine_test.py —— 任务级状态机单元测试。

覆盖：完整状态链 WAIT→ARM_TAKEOFF→GOTO→TRACK→APPROACH→RTL、各状态超时、
GOTO→TRACK 的锁定/视觉守卫、TRACK 丢目标退回 GOTO、紧急/返航全局终止、
RTL 终态与 reset。
"""

import pytest

from task_runner.state_machine import (TaskInputs, TaskState, TaskStateConfig,
                                       TaskStateMachine, TaskTransition)


def _inp(**kw) -> TaskInputs:
    return TaskInputs(**kw)


def _sm(cfg=None) -> TaskStateMachine:
    return TaskStateMachine(cfg or TaskStateConfig())


# --- 初始态与 reset ---------------------------------------------------------
def test_initial_state_is_wait():
    assert _sm().state is TaskState.WAIT


def test_reset_returns_to_wait():
    sm = _sm()
    sm.update(_inp(target_injected=True), 1000)
    assert sm.state is TaskState.ARM_TAKEOFF
    sm.reset()
    assert sm.state is TaskState.WAIT


# --- WAIT -------------------------------------------------------------------
def test_wait_without_coords_stays():
    tr = _sm().update(_inp(), 1000)
    assert not tr.occurred
    assert tr.to_state is TaskState.WAIT


def test_wait_receives_coords_goes_arm_takeoff():
    sm = _sm()
    tr = sm.update(_inp(target_injected=True), 1000)
    assert tr.occurred
    assert tr.from_state is TaskState.WAIT
    assert tr.to_state is TaskState.ARM_TAKEOFF
    assert sm.state is TaskState.ARM_TAKEOFF


# --- ARM_TAKEOFF ------------------------------------------------------------
def test_arm_takeoff_reaches_altitude_goes_goto():
    sm = _sm()
    sm.update(_inp(target_injected=True), 1000)
    tr = sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    assert tr.occurred and tr.to_state is TaskState.GOTO


def test_arm_takeoff_timeout_goes_rtl():
    cfg = TaskStateConfig(arm_takeoff_timeout_s=60.0)
    sm = _sm(cfg)
    sm.update(_inp(target_injected=True), 1000)
    # 60s 未到高度 → 超时返航
    tr = sm.update(_inp(target_injected=True), 1000 + 61_000)
    assert tr.occurred and tr.to_state is TaskState.RTL


# --- GOTO -------------------------------------------------------------------
def test_goto_far_away_stays():
    sm = _sm()
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0,
                        target_locked=True), 3000)
    assert not tr.occurred
    assert sm.state is TaskState.GOTO


def test_goto_within_radius_and_locked_goes_track():
    sm = _sm()
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
                        target_locked=True), 3000)
    assert tr.occurred and tr.to_state is TaskState.TRACK


def test_goto_within_radius_but_unlocked_stays():
    sm = _sm()
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
                        target_locked=False), 3000)
    assert not tr.occurred
    assert sm.state is TaskState.GOTO


def test_goto_requires_vision_when_enabled():
    cfg = TaskStateConfig(require_vision=True)
    sm = _sm(cfg)
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    # 有锁定但无视觉第二判据 → 不切换
    tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
                        target_locked=True, vision_has_target=False), 3000)
    assert not tr.occurred
    # 补上视觉 → 切换
    tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
                        target_locked=True, vision_has_target=True), 4000)
    assert tr.occurred and tr.to_state is TaskState.TRACK


def test_goto_loses_coords_goes_rtl():
    sm = _sm()
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    tr = sm.update(_inp(target_injected=False, distance_to_target_m=10.0), 3000)
    assert tr.occurred and tr.to_state is TaskState.RTL


def test_goto_timeout_goes_rtl():
    cfg = TaskStateConfig(goto_timeout_s=120.0)
    sm = _sm(cfg)
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0), 2000 + 121_000)
    assert tr.occurred and tr.to_state is TaskState.RTL


# --- TRACK ------------------------------------------------------------------
def test_track_centered_stable_goes_approach():
    sm = _sm(TaskStateConfig(center_hold_s=3.0))
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
                   target_locked=True), 3000)
    assert sm.state is TaskState.TRACK
    # 居中但未满时长 → 停留
    tr = sm.update(_inp(target_injected=True, target_locked=True,
                        target_centered=True, centered_duration_s=1.0), 4000)
    assert not tr.occurred
    # 居中稳定满 3s → APPROACH
    tr = sm.update(_inp(target_injected=True, target_locked=True,
                        target_centered=True, centered_duration_s=3.0), 5000)
    assert tr.occurred and tr.to_state is TaskState.APPROACH


def test_track_lost_with_coords_goes_back_to_goto():
    sm = _sm()
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
                   target_locked=True), 3000)
    tr = sm.update(_inp(target_injected=True, target_locked=False), 4000)
    assert tr.occurred and tr.to_state is TaskState.GOTO


def test_track_lost_without_coords_goes_rtl():
    sm = _sm()
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
                   target_locked=True), 3000)
    tr = sm.update(_inp(target_injected=False, target_locked=False), 4000)
    assert tr.occurred and tr.to_state is TaskState.RTL


def test_track_timeout_goes_rtl():
    cfg = TaskStateConfig(track_timeout_s=120.0)
    sm = _sm(cfg)
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
                   target_locked=True), 3000)
    tr = sm.update(_inp(target_injected=True, target_locked=True), 3000 + 121_000)
    assert tr.occurred and tr.to_state is TaskState.RTL


# --- APPROACH ---------------------------------------------------------------
def test_approach_hover_done_goes_rtl():
    cfg = TaskStateConfig(center_hold_s=0.0, approach_hover_s=10.0)
    sm = _sm(cfg)
    sm.update(_inp(target_injected=True), 1000)
    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
    sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
                   target_locked=True), 3000)
    sm.update(_inp(target_injected=True, target_locked=True, target_centered=True,
                   centered_duration_s=0.0), 4000)
    assert sm.state is TaskState.APPROACH
    # 悬停未满 10s → 停留
    tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 5000)
    assert not tr.occurred
    # 满 10s → RTL
    tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 10_000)
    assert tr.occurred and tr.to_state is TaskState.RTL


# --- 全局终止（紧急 / 返航）-------------------------------------------------
@pytest.mark.parametrize("state_kw", [
    {},                                                    # WAIT
    {"target_injected": True},                             # 会先转 ARM_TAKEOFF
])
def test_emergency_goes_rtl_from_any_state(state_kw):
    sm = _sm()
    sm.update(_inp(**state_kw), 1000)
    tr = sm.update(_inp(emergency=True), 2000)
    assert tr.occurred and tr.to_state is TaskState.RTL


def test_rtl_requested_goes_rtl():
    sm = _sm()
    tr = sm.update(_inp(rtl_requested=True), 1000)
    assert tr.occurred and tr.to_state is TaskState.RTL


# --- RTL 终态 ---------------------------------------------------------------
def test_rtl_is_terminal():
    sm = _sm()
    sm.update(_inp(rtl_requested=True), 1000)
    assert sm.state is TaskState.RTL
    # 终态不再转移（连紧急/坐标也不动）
    tr = sm.update(_inp(target_injected=True, emergency=True), 2000)
    assert not tr.occurred
    assert sm.state is TaskState.RTL


# --- 转移结果结构 -----------------------------------------------------------
def test_transition_dataclass_defaults():
    tr = TaskTransition()
    assert tr.from_state is TaskState.WAIT
    assert tr.to_state is TaskState.WAIT
    assert tr.occurred is False
    assert tr.reason == ""