ai_fwt_pro/main/xiaozhi-server/plugins_func/nav_copilot/service.py

from __future__ import annotations

import asyncio
import json
import time
from dataclasses import dataclass
from typing import Any, Dict, List, Optional, Set, Tuple

from config.logger import setup_logging
from core.providers.tools.device_mcp.mcp_handler import call_mcp_tool
from core.providers.tts.dto.dto import ContentType

from .geo import haversine_m, nearest_point_index
from .storage import Hazard, NavDB, RoutePoint


TAG = __name__
logger = setup_logging()

COUNTER_MAX_VALUE = 256
COUNTER_WRAP_THRESHOLD = 128


_HAZARD_LABELS: Dict[str, str] = {
    "sharp_turn": "急转弯",
    "bump": "大坑",
    "jump": "跳台",
    "slope": "陡坡",
    "water": "涉水",
    "obstacle": "障碍",
    "other": "风险点",
}

_HAZARD_SYNONYMS: Dict[str, str] = {
    "急转弯": "sharp_turn",
    "急弯": "sharp_turn",
    "大坑": "bump",
    "坑": "bump",
    "跳台": "jump",
    "陡坡": "slope",
    "涉水": "water",
    "障碍": "obstacle",
}


@dataclass
class GnssFix:
    ok: bool
    lat: float
    lon: float
    speed_mps: float
    heading_deg: Optional[float] = None
    t: Optional[int] = None


@dataclass
class CounterState:
    ok: bool
    value: int
    absolute: int
    cycle: int
    t: Optional[int] = None


@dataclass(frozen=True)
class HazardMarkResult:
    hazard_id: int
    route_id: int
    mode: str
    counter_value: Optional[int] = None
    counter_absolute: Optional[int] = None
    lat: Optional[float] = None
    lon: Optional[float] = None


@dataclass(frozen=True)
class StopRecordingResult:
    route_id: int
    mode: str
    hazard_count: int
    monitor_started: bool


def _parse_jsonish(raw: Any) -> Optional[dict]:
    if raw is None:
        return None
    if isinstance(raw, str):
        raw = raw.strip()
        if not raw:
            return None
        try:
            raw = json.loads(raw)
        except Exception:
            return None
    if not isinstance(raw, dict):
        return None
    return raw


def _parse_fix(raw: Any) -> GnssFix:
    payload = _parse_jsonish(raw)
    if payload is None:
        return GnssFix(False, 0.0, 0.0, 0.0)

    if "gnss" in payload and isinstance(payload.get("gnss"), dict):
        payload = payload["gnss"]

    ok = bool(payload.get("ok", True))
    has_fix = payload.get("has_fix")
    if has_fix is not None:
        ok = ok and bool(has_fix)

    lat = payload.get("lat", payload.get("latitude"))
    lon = payload.get("lon", payload.get("longitude"))
    try:
        lat_f = float(lat)
        lon_f = float(lon)
    except Exception:
        return GnssFix(False, 0.0, 0.0, 0.0)

    speed_mps = payload.get("speed_mps")
    if speed_mps is None:
        speed_knots = payload.get("speed_knots")
        if speed_knots is not None:
            try:
                speed_mps = float(speed_knots) * 0.514444
            except Exception:
                speed_mps = 0.0
        else:
            speed_kmh = payload.get("speed_kmh")
            if speed_kmh is not None:
                try:
                    speed_mps = float(speed_kmh) / 3.6
                except Exception:
                    speed_mps = 0.0
            else:
                speed_mps = payload.get("speed") or 0.0
    try:
        speed_mps_f = float(speed_mps)
    except Exception:
        speed_mps_f = 0.0

    heading = payload.get("heading_deg", payload.get("course_deg"))
    heading_f = None
    if heading is not None:
        try:
            heading_f = float(heading)
        except Exception:
            heading_f = None

    t = payload.get("t") or payload.get("timestamp") or payload.get("time")
    t_i = None
    if t is not None:
        try:
            t_i = int(t)
        except Exception:
            t_i = None

    fix = payload.get("fix") or payload.get("fix_quality") or payload.get("quality")
    if fix is not None:
        try:
            ok = ok and int(fix) > 0
        except Exception:
            pass

    return GnssFix(ok, lat_f, lon_f, max(0.0, speed_mps_f), heading_f, t_i)


def _parse_counter_state(raw: Any) -> CounterState:
    payload = _parse_jsonish(raw)
    if payload is None:
        return CounterState(False, 0, 0, 0)

    if "counter" in payload and isinstance(payload.get("counter"), dict):
        payload = payload["counter"]

    ok = bool(payload.get("ok", True))
    has_value = payload.get("has_value")
    if has_value is not None:
        ok = ok and bool(has_value)

    value = (
        payload.get("value")
        or payload.get("counter_value")
        or payload.get("counter")
        or payload.get("raw")
    )
    absolute = payload.get("absolute") or payload.get("counter_absolute")
    cycle = payload.get("cycle", 0)
    t = payload.get("t") or payload.get("timestamp") or payload.get("time")

    try:
        value_i = int(value)
    except Exception:
        return CounterState(False, 0, 0, 0)
    if value_i < 1 or value_i > COUNTER_MAX_VALUE:
        return CounterState(False, 0, 0, 0)

    try:
        absolute_i = int(absolute) if absolute is not None else value_i
    except Exception:
        absolute_i = value_i
    try:
        cycle_i = int(cycle)
    except Exception:
        cycle_i = 0
    try:
        t_i = int(t) if t is not None else None
    except Exception:
        t_i = None

    return CounterState(ok, value_i, max(value_i, absolute_i), max(0, cycle_i), t_i)


def _pick_tool_name(conn, preferred: str, keywords: Tuple[str, ...]) -> Optional[str]:
    if not hasattr(conn, "mcp_client") or not conn.mcp_client:
        return None
    if conn.mcp_client.has_tool(preferred):
        return preferred
    for name in conn.mcp_client.tools.keys():
        lowered = name.lower()
        if all(keyword in lowered for keyword in keywords):
            return name
    return None


def _pick_gnss_tool_name(conn) -> Optional[str]:
    return _pick_tool_name(conn, "self_gnss_get_fix", ("gnss", "fix"))


def _pick_counter_tool_name(conn) -> Optional[str]:
    return _pick_tool_name(conn, "self_nav_counter_get_state", ("counter", "state"))


def _pick_nav_alert_tool_name(conn) -> Optional[str]:
    return _pick_tool_name(conn, "self_nav_alert_play", ("nav", "alert", "play"))


class NavCopilotService:
    def __init__(self, conn, db: NavDB):
        self.conn = conn
        self.db = db

        self.record_task: Optional[asyncio.Task] = None
        self.record_route_id: Optional[int] = None
        self.record_mode: Optional[str] = None
        self._record_seq = 0
        self._record_cum_m = 0.0
        self._record_last_latlon: Optional[Tuple[float, float]] = None
        self._record_last_t = 0
        self._record_interval_s = 0.1
        self._record_counter_start_absolute: Optional[int] = None
        self._record_counter_last_absolute: Optional[int] = None
        self._record_counter_hazards: List[Hazard] = []
        self._record_counter_announced: Set[Tuple[int, int, str]] = set()
        self._record_last_announce_at = 0.0

        self.run_task: Optional[asyncio.Task] = None
        self.run_route_id: Optional[int] = None
        self.run_mode: Optional[str] = None
        self._run_interval_s = 0.1
        self._run_points: List[RoutePoint] = []
        self._run_hazards: List[Hazard] = []
        self._run_last_idx: Optional[int] = None
        self._run_announced: Set[int] = set()
        self._run_gps_last_announce_at: Dict[int, float] = {}
        self._run_counter_announced: Set[Tuple[int, int, str]] = set()
        self._run_last_announce_at = 0.0

        self.gps_alert_radius_m = 3.0
        self.gps_alert_repeat_cooldown_s = 2.0
        self.announce_at_m = 50.0
        self.announce_window_m = 18.0
        self.announce_cooldown_s = 5.0
        self.counter_announce_ahead = 6
        self.counter_announce_cooldown_s = 1.5

    async def recover_monitoring_if_needed(self) -> Optional[int]:
        if self.run_route_id is not None:
            return self.run_route_id
        if not _pick_gnss_tool_name(self.conn):
            return None

        route_id = self.db.get_monitoring_route_id()
        if not route_id:
            return None

        hazards = self.db.load_hazards(route_id)
        if not hazards:
            logger.bind(tag=TAG).warning(
                f"Route {route_id} was marked as monitoring but has no hazards; clearing monitoring state"
            )
            self.db.set_monitoring_route(None)
            return None

        route_mode = self.db.get_route_mode(route_id)
        if route_mode != "gps":
            logger.bind(tag=TAG).warning(
                f"Route {route_id} monitoring recovery only supports gps mode, current mode={route_mode}; clearing monitoring state"
            )
            self.db.set_monitoring_route(None)
            return None

        try:
            await self.start_run(route_id=route_id, tick_hz=10, mode=route_mode)
        except RuntimeError as exc:
            logger.bind(tag=TAG).warning(
                f"Failed to recover monitoring route {route_id}: {exc}"
            )
            return None

        logger.bind(tag=TAG).info(
            f"Recovered GPS monitoring after reconnect, route_id={route_id}, hazards={len(hazards)}"
        )
        return route_id

    def _counter_input_hint(self) -> str:
        return (
            "请确认 STM32 已接到 RXD1/GND，波特率 115200，"
            "并且按带分隔符的文本格式发送，例如 1\\n 2\\n 3\\n ... 256\\n。"
        )

    def _speak(self, text: str) -> None:
        try:
            self.conn.tts.tts_one_sentence(
                self.conn,
                ContentType.TEXT,
                content_detail=text,
            )
        except Exception as exc:
            logger.bind(tag=TAG).warning(f"TTS speak failed: {exc}")

    async def _play_local_hazard_alert(self, hazard: Hazard) -> bool:
        tool_name = _pick_nav_alert_tool_name(self.conn)
        if not tool_name:
            return False
        try:
            await call_mcp_tool(
                self.conn,
                self.conn.mcp_client,
                tool_name,
                json.dumps(
                    {
                        "hazard_type": hazard.type,
                        "note": hazard.note or "",
                    },
                    ensure_ascii=False,
                ),
                timeout=5,
            )
            return True
        except Exception as exc:
            logger.bind(tag=TAG).warning(f"Local hazard alert failed: {exc}")
            return False

    async def _announce_gps_hazard(self, hazard: Hazard) -> None:
        if await self._play_local_hazard_alert(hazard):
            return
        label = _HAZARD_LABELS.get(hazard.type, _HAZARD_LABELS["other"])
        text = f"注意{label}"
        if hazard.note:
            text = f"{text}，{hazard.note}"
        self._speak(text)

    def _normalize_mode(self, mode: str | None) -> str:
        normalized = (mode or "gps").strip().lower()
        if normalized not in {"auto", "gps", "counter"}:
            return "gps"
        return normalized

    def _resolve_mode(self, requested_mode: str | None, route_id: int | None = None) -> str:
        mode = self._normalize_mode(requested_mode)
        if route_id:
            stored_mode = self.db.get_route_mode(route_id)
            if stored_mode in {"gps", "counter"}:
                return stored_mode
        if mode == "gps":
            if not _pick_gnss_tool_name(self.conn):
                raise RuntimeError("设备端没有可用的 GNSS 定位工具")
            return "gps"
        if _pick_gnss_tool_name(self.conn):
            return "gps"
        raise RuntimeError("设备端没有可用的 GNSS 定位工具")

    async def _get_fix(self) -> GnssFix:
        tool_name = _pick_gnss_tool_name(self.conn)
        if not tool_name:
            return GnssFix(False, 0.0, 0.0, 0.0)
        try:
            raw = await call_mcp_tool(
                self.conn,
                self.conn.mcp_client,
                tool_name,
                "{}",
                timeout=5,
            )
            return _parse_fix(raw)
        except Exception as exc:
            logger.bind(tag=TAG).warning(f"GNSS fix failed: {exc}")
            return GnssFix(False, 0.0, 0.0, 0.0)

    async def _get_counter_state(self) -> CounterState:
        tool_name = _pick_counter_tool_name(self.conn)
        if not tool_name:
            return CounterState(False, 0, 0, 0)
        try:
            raw = await call_mcp_tool(
                self.conn,
                self.conn.mcp_client,
                tool_name,
                "{}",
                timeout=5,
            )
            return _parse_counter_state(raw)
        except Exception as exc:
            logger.bind(tag=TAG).warning(f"Counter state failed: {exc}")
            return CounterState(False, 0, 0, 0)

    def _counter_distance(self, current_value: int, target_value: int) -> int:
        delta = target_value - current_value
        if delta < 0:
            delta += COUNTER_MAX_VALUE
        return delta

    def _prune_counter_announced(
        self,
        announced: Set[Tuple[int, int, str]],
        current_cycle: int,
    ) -> None:
        stale = {
            item for item in announced if item[1] < max(0, current_cycle - 1)
        }
        announced.difference_update(stale)

    def _maybe_announce_counter_hazard(
        self,
        sample: CounterState,
        hazards: List[Hazard],
        announced: Set[Tuple[int, int, str]],
        *,
        last_announce_at: float,
    ) -> float:
        if not sample.ok or not hazards:
            return last_announce_at

        self._prune_counter_announced(announced, sample.cycle)

        for hazard in hazards:
            if hazard.counter_value <= 0:
                continue
            arrive_key = (hazard.id, sample.cycle, "arrived")
            if sample.value != hazard.counter_value or arrive_key in announced:
                continue

            label = _HAZARD_LABELS.get(hazard.type, _HAZARD_LABELS["other"])
            text = f"当前{label}"
            if hazard.note:
                text = f"{text}，{hazard.note}"
            self._speak(text)
            announced.add(arrive_key)
            return time.time()

        next_hazard: Optional[Hazard] = None
        next_distance: Optional[int] = None
        for hazard in hazards:
            if hazard.counter_value <= 0:
                continue
            distance = self._counter_distance(sample.value, hazard.counter_value)
            if distance == 0:
                continue
            ahead_key = (hazard.id, sample.cycle, "ahead")
            if ahead_key in announced:
                continue
            if next_distance is None or distance < next_distance:
                next_hazard = hazard
                next_distance = distance

        if next_hazard is None or next_distance is None:
            return last_announce_at
        if not (1 <= next_distance <= self.counter_announce_ahead):
            return last_announce_at

        now = time.time()
        if now - last_announce_at < self.counter_announce_cooldown_s:
            return last_announce_at

        label = _HAZARD_LABELS.get(next_hazard.type, _HAZARD_LABELS["other"])
        text = f"注意{label}"
        if next_hazard.counter_value > 0:
            text = f"{text}，目标点 {next_hazard.counter_value}"
        if next_hazard.note:
            text = f"{text}，{next_hazard.note}"
        self._speak(text)
        announced.add((next_hazard.id, sample.cycle, "ahead"))
        return now

    def _pick_counter_point(self, points: List[RoutePoint], sample: CounterState) -> RoutePoint:
        if not points:
            raise RuntimeError("路线里还没有计数点")
        return min(
            points,
            key=lambda point: min(
                self._counter_distance(sample.value, point.counter_value or 1),
                self._counter_distance(point.counter_value or 1, sample.value),
            ),
        )

    async def start_recording(
        self,
        route_name: Optional[str] = None,
        sample_hz: int = 10,
        mode: str | None = None,
    ) -> int:
        if self.record_route_id is not None:
            raise RuntimeError("当前已经在记录路线")
        if self.run_route_id is not None:
            raise RuntimeError("当前正在预警，不能同时开始记录")

        route_mode = self._resolve_mode(mode)
        name = (route_name or "").strip() or f"路线 {time.strftime('%Y-%m-%d %H:%M:%S')}"
        route_id = self.db.create_route(name, route_mode)

        self.record_route_id = route_id
        self.record_mode = route_mode
        self._record_seq = 0
        self._record_cum_m = 0.0
        self._record_last_latlon = None
        self._record_last_t = 0
        self._record_counter_start_absolute = None
        self._record_counter_last_absolute = None
        self._record_counter_hazards = []
        self._record_counter_announced = set()
        self._record_last_announce_at = 0.0
        hz = max(1, int(sample_hz or 10))
        if route_mode == "counter":
            hz = max(5, hz)
        self._record_interval_s = 1.0 / hz

        if route_mode == "counter":
            self.record_task = None
        else:
            self.record_task = asyncio.create_task(
                self._record_gps_loop(),
                name=f"NavRecordGps:{route_id}",
            )
        return route_id

    async def stop_recording(self) -> Optional[StopRecordingResult]:
        route_id = self.record_route_id
        if route_id is None:
            return None

        route_mode = self.record_mode or self.db.get_route_mode(route_id)
        if self.record_task and not self.record_task.done():
            self.record_task.cancel()
            try:
                await self.record_task
            except BaseException:
                pass

        self.record_task = None
        self.record_route_id = None
        self.record_mode = None
        self._record_counter_hazards = []
        self._record_counter_announced = set()
        self.db.finish_route(route_id)

        hazard_count = len(self.db.load_hazards(route_id))
        monitor_started = False
        logger.bind(tag=TAG).info(
            f"Stop recording route_id={route_id}, mode={route_mode}, hazards={hazard_count}"
        )
        if hazard_count > 0:
            await self.start_run(route_id=route_id, tick_hz=10, mode=route_mode)
            monitor_started = True

        return StopRecordingResult(
            route_id=route_id,
            mode=route_mode,
            hazard_count=hazard_count,
            monitor_started=monitor_started,
        )

    async def _record_gps_loop(self) -> None:
        assert self.record_route_id is not None
        route_id = self.record_route_id
        logger.bind(tag=TAG).info(f"GPS record loop started, route_id={route_id}")
        try:
            while not self.conn.stop_event.is_set():
                fix = await self._get_fix()
                now_t = int(fix.t or time.time())
                if fix.ok:
                    latlon = (fix.lat, fix.lon)
                    should_add = False
                    if self._record_last_latlon is None:
                        should_add = True
                    else:
                        moved_m = haversine_m(
                            self._record_last_latlon[0],
                            self._record_last_latlon[1],
                            latlon[0],
                            latlon[1],
                        )
                        if moved_m >= 1.0:
                            should_add = True
                        if (now_t - self._record_last_t) >= 2:
                            should_add = True

                    if should_add:
                        if self._record_last_latlon is not None:
                            self._record_cum_m += haversine_m(
                                self._record_last_latlon[0],
                                self._record_last_latlon[1],
                                latlon[0],
                                latlon[1],
                            )
                        self._record_last_latlon = latlon
                        self._record_last_t = now_t
                        self._record_seq += 1

                        self.db.add_point(
                            route_id=route_id,
                            seq=self._record_seq,
                            t=now_t,
                            lat=latlon[0],
                            lon=latlon[1],
                            speed_mps=fix.speed_mps,
                            cum_dist_m=self._record_cum_m,
                        )

                await asyncio.sleep(self._record_interval_s)
        finally:
            if self.record_route_id == route_id:
                try:
                    self.db.finish_route(route_id)
                except Exception:
                    pass
                self.record_route_id = None
                self.record_mode = None
            self.record_task = None

    async def _record_counter_loop(self) -> None:
        assert self.record_route_id is not None
        route_id = self.record_route_id
        logger.bind(tag=TAG).info(f"Counter record loop started, route_id={route_id}")
        try:
            while not self.conn.stop_event.is_set():
                sample = await self._get_counter_state()
                now_t = int(sample.t or time.time())
                if sample.ok:
                    if self._record_counter_start_absolute is None:
                        self._record_counter_start_absolute = sample.absolute

                    should_add = (
                        self._record_counter_last_absolute is None
                        or sample.absolute != self._record_counter_last_absolute
                    )
                    if should_add:
                        self._record_seq += 1
                        self._record_cum_m = float(
                            max(0, sample.absolute - self._record_counter_start_absolute)
                        )
                        self._record_counter_last_absolute = sample.absolute
                        self._record_last_t = now_t
                        self.db.add_point(
                            route_id=route_id,
                            seq=self._record_seq,
                            t=now_t,
                            lat=0.0,
                            lon=0.0,
                            speed_mps=0.0,
                            cum_dist_m=self._record_cum_m,
                            counter_value=sample.value,
                            counter_absolute=sample.absolute,
                        )

                    self._record_last_announce_at = self._maybe_announce_counter_hazard(
                        sample,
                        self._record_counter_hazards,
                        self._record_counter_announced,
                        last_announce_at=self._record_last_announce_at,
                    )

                await asyncio.sleep(self._record_interval_s)
        finally:
            if self.record_route_id == route_id:
                try:
                    self.db.finish_route(route_id)
                except Exception:
                    pass
                self.record_route_id = None
                self.record_mode = None
            self.record_task = None
            self._record_counter_hazards = []
            self._record_counter_announced = set()

    async def mark_hazard(self, hazard_type: str, note: str = "") -> HazardMarkResult:
        hazard_type = (hazard_type or "").strip() or "other"
        if hazard_type in _HAZARD_SYNONYMS:
            hazard_type = _HAZARD_SYNONYMS[hazard_type]
        if hazard_type not in _HAZARD_LABELS:
            hazard_type = "other"

        route_id = self.record_route_id or self.run_route_id or self.db.get_active_route_id()
        if not route_id:
            raise RuntimeError("没有可用路线，请先开始记录")
        route_mode = "gps"
        now_t = int(time.time())

        if False and route_mode == "counter":
            sample = await self._get_counter_state()
            if not sample.ok:
                fix = await self._get_fix()
                if fix.ok:
                    logger.bind(tag=TAG).warning(
                        f"Counter route {route_id} has no live counter sample but GPS is available; switching route to gps mode"
                    )
                    self.db.update_route_mode(route_id, "gps")
                    route_mode = "gps"
                    if self.record_route_id == route_id:
                        self.record_mode = "gps"
                        if self.record_task is None or self.record_task.done():
                            self._record_interval_s = 0.1
                            self.record_task = asyncio.create_task(
                                self._record_gps_loop(),
                                name=f"NavRecordGps:{route_id}",
                            )
                    if self.run_route_id == route_id:
                        self.run_mode = "gps"
            if route_mode == "counter" and not sample.ok:
                raise RuntimeError(
                    "当前没有可用的串口计数值，无法标记风险点。"
                    + self._counter_input_hint()
                )

            if route_mode == "counter":
                points = self.db.load_route_points(route_id)
                if points:
                    point = self._pick_counter_point(points, sample)
                    seq = point.seq
                    cum = point.cum_dist_m
                else:
                    seq = len(self.db.load_hazards(route_id)) + 1
                    cum = float(max(0, seq - 1))

                hazard_id = self.db.add_hazard(
                    route_id=route_id,
                    seq=seq,
                    t=int(sample.t or now_t),
                    lat=0.0,
                    lon=0.0,
                    cum_dist_m=cum,
                    hazard_type=hazard_type,
                    note=note or "",
                    counter_value=sample.value,
                    counter_absolute=sample.absolute,
                )

                if self.record_route_id == route_id and self.record_mode == "counter":
                    self._record_counter_hazards = self.db.load_hazards(route_id)
                if self.run_route_id == route_id and self.run_mode == "counter":
                    self._run_hazards = self.db.load_hazards(route_id)
                return HazardMarkResult(
                    hazard_id=hazard_id,
                    route_id=route_id,
                    mode="counter",
                    counter_value=sample.value,
                    counter_absolute=sample.absolute,
                )

        fix = await self._get_fix()
        if not fix.ok:
            raise RuntimeError("当前没有有效 GPS 定位，无法标记风险点")

        if self.record_route_id == route_id and self._record_last_latlon is not None:
            seq = self._record_seq
            cum = self._record_cum_m
        else:
            points = self.db.load_route_points(route_id)
            if points:
                idx, _ = nearest_point_index([(point.lat, point.lon) for point in points], fix.lat, fix.lon)
                seq = points[idx].seq
                cum = points[idx].cum_dist_m
            else:
                seq = self._record_seq if self._record_seq > 0 else len(self.db.load_hazards(route_id)) + 1
                cum = self._record_cum_m

        hazard_id = self.db.add_hazard(
            route_id=route_id,
            seq=seq,
            t=int(fix.t or now_t),
            lat=fix.lat,
            lon=fix.lon,
            cum_dist_m=cum,
            hazard_type=hazard_type,
            note=note or "",
        )

        if self.run_route_id == route_id and self.run_mode == "gps":
            self._run_hazards = self.db.load_hazards(route_id)
        return HazardMarkResult(
            hazard_id=hazard_id,
            route_id=route_id,
            mode="gps",
            lat=fix.lat,
            lon=fix.lon,
        )

    async def start_run(
        self,
        route_id: Optional[int] = None,
        tick_hz: int = 10,
        mode: str | None = None,
    ) -> int:
        if self.run_route_id is not None:
            raise RuntimeError("当前已经在预警")
        if self.record_route_id is not None:
            raise RuntimeError("当前正在记录路线，不能同时开始预警")

        route_id = route_id or self.db.get_active_route_id()
        if not route_id:
            raise RuntimeError("没有可用路线，请先开始记录路线")

        route_mode = self._resolve_mode(mode, route_id=route_id)
        points = self.db.load_route_points(route_id)
        hazards = self.db.load_hazards(route_id)
        if route_mode == "gps" and not hazards:
            raise RuntimeError("这条 GPS 路线还没有任何标记点")
        if route_mode == "counter" and not hazards:
            raise RuntimeError("这条计数路线还没有任何标记点")

        logger.bind(tag=TAG).info(
            f"Start run requested, route_id={route_id}, mode={route_mode}, hazards={len(hazards)}, points={len(points)}"
        )
        self.run_route_id = route_id
        self.run_mode = route_mode
        self.db.set_active_route(route_id)
        self.db.set_monitoring_route(route_id)
        self._run_points = points
        self._run_hazards = hazards
        self._run_last_idx = None
        self._run_announced = set()
        self._run_gps_last_announce_at = {}
        self._run_counter_announced = set()
        self._run_last_announce_at = 0.0
        hz = max(1, int(tick_hz or 10))
        if route_mode == "counter":
            hz = max(5, hz)
        self._run_interval_s = 1.0 / hz

        if route_mode == "counter":
            self.run_task = asyncio.create_task(
                self._run_counter_loop(),
                name=f"NavRunCounter:{route_id}",
            )
        else:
            self.run_task = asyncio.create_task(
                self._run_gps_loop(),
                name=f"NavRunGps:{route_id}",
            )
        return route_id

    async def stop_run(self) -> Optional[int]:
        route_id = self.run_route_id
        if self.run_task and not self.run_task.done():
            self.run_task.cancel()
            try:
                await self.run_task
            except BaseException:
                pass
        self.run_task = None
        self.run_route_id = None
        self.run_mode = None
        self.db.set_monitoring_route(None)
        self._run_points = []
        self._run_hazards = []
        self._run_last_idx = None
        self._run_announced = set()
        self._run_gps_last_announce_at = {}
        self._run_counter_announced = set()
        return route_id

    async def _run_gps_loop(self) -> None:
        assert self.run_route_id is not None
        route_id = self.run_route_id
        logger.bind(tag=TAG).info(
            f"GPS run loop started, route_id={route_id}, hazards={len(self._run_hazards)}, radius_m={self.gps_alert_radius_m}, repeat_cooldown_s={self.gps_alert_repeat_cooldown_s}"
        )

        try:
            while not self.conn.stop_event.is_set():
                fix = await self._get_fix()
                if not fix.ok:
                    await asyncio.sleep(1.0)
                    continue

                for hazard in self._run_hazards:
                    if hazard.lat == 0.0 and hazard.lon == 0.0:
                        continue
                    distance = haversine_m(fix.lat, fix.lon, hazard.lat, hazard.lon)
                    if distance <= self.gps_alert_radius_m:
                        now = time.monotonic()
                        last_announce_at = self._run_gps_last_announce_at.get(hazard.id, 0.0)
                        if now - last_announce_at < self.gps_alert_repeat_cooldown_s:
                            continue
                        logger.bind(tag=TAG).info(
                            f"GPS hazard triggered: route_id={route_id}, hazard_id={hazard.id}, distance_m={distance:.3f}, fix=({fix.lat:.6f},{fix.lon:.6f}), hazard=({hazard.lat:.6f},{hazard.lon:.6f})"
                        )
                        await self._announce_gps_hazard(hazard)
                        self._run_gps_last_announce_at[hazard.id] = now

                await asyncio.sleep(self._run_interval_s)
        finally:
            if self.run_route_id == route_id:
                self.run_route_id = None
                self.run_mode = None
                self.run_task = None
                self._run_points = []
                self._run_hazards = []
                self._run_last_idx = None
                self._run_announced = set()
                self._run_gps_last_announce_at = {}
                self._run_counter_announced = set()

    async def _run_counter_loop(self) -> None:
        assert self.run_route_id is not None
        route_id = self.run_route_id
        logger.bind(tag=TAG).info(f"Counter run loop started, route_id={route_id}")
        try:
            while not self.conn.stop_event.is_set():
                sample = await self._get_counter_state()
                if sample.ok:
                    self._run_last_announce_at = self._maybe_announce_counter_hazard(
                        sample,
                        self._run_hazards,
                        self._run_counter_announced,
                        last_announce_at=self._run_last_announce_at,
                    )
                await asyncio.sleep(self._run_interval_s)
        finally:
            if self.run_route_id == route_id:
                self.run_route_id = None
                self.run_mode = None
                self.run_task = None
                self._run_points = []
                self._run_hazards = []
                self._run_last_idx = None
                self._run_announced = set()
                self._run_gps_last_announce_at = {}
                self._run_counter_announced = set()

    def status_text(self) -> str:
        parts: List[str] = []
        if self.record_route_id:
            if self.record_mode == "counter":
                parts.append(
                    f"正在准备录制计数路线 ID={self.record_route_id}，已标记风险点={len(self.db.load_hazards(self.record_route_id))}"
                )
            else:
                parts.append(
                    f"正在记录路线 ID={self.record_route_id}，模式={self.record_mode or 'unknown'}，点数={self._record_seq}"
                )
        if self.run_route_id:
            if self.run_mode == "counter":
                parts.append(
                    f"正在监听预警路线 ID={self.run_route_id}，风险点={len(self._run_hazards)}"
                )
            else:
                parts.append(
                    f"正在预警路线 ID={self.run_route_id}，模式={self.run_mode or 'unknown'}，风险点={len(self._run_hazards)}"
                )
        if not parts:
            route_id = self.db.get_monitoring_route_id()
            if route_id:
                parts.append(
                    f"数据库显示正在预警路线 ID={route_id}，模式={self.db.get_route_mode(route_id)}，等待本次连接恢复执行"
                )
            route_id = self.db.get_active_route_id()
            if route_id:
                parts.append(
                    f"当前激活路线 ID={route_id}，模式={self.db.get_route_mode(route_id)}"
                )
            else:
                parts.append("当前还没有路线，请先开始记录")
        return "；".join(parts)


def get_service(conn) -> NavCopilotService:
    if not hasattr(conn, "nav_copilot_service") or conn.nav_copilot_service is None:
        conn.nav_copilot_service = NavCopilotService(conn, NavDB.default())
    return conn.nav_copilot_service