"""runner_test.py —— 任务编排器单元测试（mock px4/bridge/ground/drone，无硬件）。

覆盖：输入采集（注入坐标新鲜/超龄、起飞到位、锁定、居中累加）、
状态进入动作（ARM_TAKEOFF 触发起飞、GOTO 切 L2、TRACK 切 L1）。
"""

import asyncio
from dataclasses import replace

import pytest

from pod_bridge.config import BridgeConfig
from pod_bridge.ground_link import GroundTarget
from pod_bridge.px4_gateway import OwnState
from pod_bridge.protocol import Telemetry
from task_runner.runner import TaskRunner
from task_runner.state_machine import (TaskState, TaskStateConfig,
                                       TaskStateMachine)


# --- mocks ------------------------------------------------------------------
class FakePx4:
    def __init__(self, state=None):
        self._state = state or OwnState()
        self.telemetry_started = 0
        self.offboard_started = False
        self.brake_calls = []

    def own_state(self):
        return replace(self._state)

    async def start_telemetry(self):
        self.telemetry_started += 1

    async def stop_telemetry(self):
        pass

    async def start_offboard(self):
        self.offboard_started = True
        return True

    async def stop_offboard(self):
        pass

    async def send_velocity_heading(self, out, t=None):
        return True

    async def send_brake_hover(self, t=None):
        self.brake_calls.append(t)
        return True


class FakeBridge:
    def __init__(self):
        self.started = False
        self.stopped = False
        self.law = None              # set_heading_servo_enabled 最后值
        self.steps = 0
        self._locked = False
        self._telemetry = None

    async def start(self):
        self.started = True
        return True

    async def stop(self):
        self.stopped = True

    def set_heading_servo_enabled(self, enabled):
        self.law = enabled

    def pod_locked(self, now_ms):
        return self._locked

    async def step(self, now_ms):
        self.steps += 1

    @property
    def latest_telemetry(self):
        return self._telemetry


class FakeGround:
    def __init__(self, target=None):
        self._target = target

    def latest(self):
        return self._target or GroundTarget()


class FakeDrone:
    def __init__(self):
        self.armed = False
        self.took_off = False
        self.takeoff_alt = 0.0
        self.rtl = False
        self.landed = False
        self.disarmed = False

    class _Action:
        def __init__(self, d):
            self._d = d

        async def arm(self):
            self._d.armed = True

        async def set_takeoff_altitude(self, a):
            self._d.takeoff_alt = a

        async def takeoff(self):
            self._d.took_off = True

        async def return_to_launch(self):
            self._d.rtl = True

        async def land(self):
            self._d.landed = True

        async def disarm(self):
            self._d.disarmed = True

    @property
    def action(self):
        return FakeDrone._Action(self)


def _state(now_ms, lat=0.0, lon=0.0, rel_alt=0.0):
    return OwnState(latitude_deg=lat, longitude_deg=lon,
                    relative_altitude_m=rel_alt, fix_type=3, num_satellites=12,
                    has_position=True, has_attitude=True, position_valid=True,
                    time_ms=now_ms, position_time_ms=now_ms)


def _make_runner(px4=None, bridge=None, ground=None, drone=None,
                 sm_cfg=None, vision_fn=None, pod_control=None):
    cfg = BridgeConfig()
    cfg.ground_inject.enabled = True
    cfg.ground_inject.timeout_ms = 5000
    return TaskRunner(
        cfg, sm_cfg or TaskStateConfig(),
        px4 or FakePx4(_state(1000)),
        bridge or FakeBridge(),
        TaskStateMachine(sm_cfg or TaskStateConfig()),
        ground or FakeGround(),
        drone or FakeDrone(),
        vision_fn=vision_fn,
        now_ms_fn=lambda: 1000,
        pod_control=pod_control,
    )


# --- 输入采集 ---------------------------------------------------------------
def test_collect_injected_fresh_sets_distance():
    ground = FakeGround(GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
                                     time_ms=1000))
    px4 = FakePx4(_state(1000))
    r = _make_runner(px4=px4, ground=ground)

    inp = r._collect_inputs(1000)
    assert inp.target_injected is True
    assert inp.distance_to_target_m == pytest.approx(111.19, rel=1e-2)  # ~111m
    assert inp.takeoff_reached is False   # rel_alt=0 < 5


def test_collect_injected_stale_is_invalid():
    ground = FakeGround(GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
                                     time_ms=1000))
    r = _make_runner(ground=ground)  # timeout=5000

    inp = r._collect_inputs(7000)  # 6000ms 差值 > 5000 → 超龄
    assert inp.target_injected is False
    assert inp.distance_to_target_m == float("inf")


def test_collect_takeoff_reached():
    px4 = FakePx4(_state(1000, rel_alt=5.0))
    sm_cfg = TaskStateConfig(takeoff_altitude_m=5.0)
    r = _make_runner(px4=px4, sm_cfg=sm_cfg)

    assert r._collect_inputs(1000).takeoff_reached is True


def test_collect_locked_from_bridge():
    bridge = FakeBridge()
    bridge._locked = True
    r = _make_runner(bridge=bridge)

    assert r._collect_inputs(1000).target_locked is True


def test_collect_vision_second_judgement():
    r = _make_runner(vision_fn=lambda: True)
    assert r._collect_inputs(1000).vision_has_target is True

    r2 = _make_runner(vision_fn=lambda: False)
    assert r2._collect_inputs(1000).vision_has_target is False


# --- 居中累加 ---------------------------------------------------------------
def test_centered_accumulates_duration():
    bridge = FakeBridge()
    bridge._telemetry = (Telemetry(off_target_x_deg=0.1, off_target_y_deg=0.1), 1000)
    sm_cfg = TaskStateConfig(center_threshold_deg=1.0)
    r = _make_runner(bridge=bridge, sm_cfg=sm_cfg)

    assert r._update_centered(1000) == (True, 0.0)      # 开始居中
    assert r._update_centered(3000) == (True, 2.0)      # 持续 2s
    assert r._update_centered(3500) == (True, 2.5)      # 持续 2.5s


def test_centered_resets_on_large_offset():
    bridge = FakeBridge()
    bridge._telemetry = (Telemetry(off_target_x_deg=0.1, off_target_y_deg=0.1), 1000)
    r = _make_runner(bridge=bridge)

    r._update_centered(1000)
    bridge._telemetry = (Telemetry(off_target_x_deg=5.0, off_target_y_deg=0.1), 2000)
    assert r._update_centered(2000) == (False, 0.0)     # 脱靶量大 → 清零


def test_centered_no_telemetry_returns_false():
    r = _make_runner()  # bridge.latest_telemetry is None
    assert r._update_centered(1000) == (False, 0.0)


# --- 状态进入动作 -----------------------------------------------------------
def test_on_enter_arm_takeoff_arms_and_takes_off():
    drone = FakeDrone()
    r = _make_runner(drone=drone, sm_cfg=TaskStateConfig(takeoff_altitude_m=5.0))
    asyncio.run(r._on_enter(TaskState.ARM_TAKEOFF, 1000))
    assert drone.armed is True
    assert drone.took_off is True
    assert drone.takeoff_alt == pytest.approx(5.0)


def test_on_enter_goto_switches_to_l2():
    bridge = FakeBridge()
    r = _make_runner(bridge=bridge)
    asyncio.run(r._on_enter(TaskState.GOTO, 1000))
    assert bridge.started is True
    assert bridge.law is False  # L2 地理引导


def test_on_enter_track_switches_to_l1():
    bridge = FakeBridge()
    r = _make_runner(bridge=bridge)
    asyncio.run(r._on_enter(TaskState.TRACK, 1000))
    assert bridge.law is True  # L1 航向伺服


def test_on_enter_rtl_stops_bridge_and_returns():
    bridge = FakeBridge()
    drone = FakeDrone()
    r = _make_runner(bridge=bridge, drone=drone)
    asyncio.run(r._on_enter(TaskState.RTL, 1000))
    assert bridge.stopped is True
    assert drone.rtl is True


# --- 持续动作 ---------------------------------------------------------------
def test_tick_goto_steps_bridge():
    bridge = FakeBridge()
    r = _make_runner(bridge=bridge)
    asyncio.run(r._tick(1000))
    # WAIT 状态不 step；切到 GOTO 才 step
    r._sm._state = TaskState.GOTO
    asyncio.run(r._tick(1000))
    assert bridge.steps == 1


def test_tick_approach_brakes():
    px4 = FakePx4(_state(1000))
    r = _make_runner(px4=px4)
    r._sm._state = TaskState.APPROACH
    asyncio.run(r._tick(1000))
    assert len(px4.brake_calls) == 1


# --- 吊舱控制集成（真机）----------------------------------------------------
class FakePodControl:
    def __init__(self):
        self.armed = False
        self.stopped = False

    def arm_detection(self):
        self.armed = True

    def stop_tracking(self):
        self.stopped = True


def test_on_enter_goto_arms_detection():
    pod_control = FakePodControl()
    r = _make_runner(pod_control=pod_control)
    asyncio.run(r._on_enter(TaskState.GOTO, 1000))
    assert pod_control.armed is True
    assert pod_control.stopped is False


def test_on_enter_rtl_stops_tracking_before_teardown():
    pod_control = FakePodControl()
    r = _make_runner(pod_control=pod_control)
    asyncio.run(r._on_enter(TaskState.RTL, 1000))
    assert pod_control.stopped is True


def test_no_pod_control_is_safe():
    """未注入 pod_control（SITL）→ GOTO/RTL 进入动作不触发吊舱控制、不抛异常。"""
    r = _make_runner(pod_control=None)
    asyncio.run(r._on_enter(TaskState.GOTO, 1000))
    asyncio.run(r._on_enter(TaskState.RTL, 1000))