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

from __future__ import annotations

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

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

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


TAG = __name__
logger = setup_logging()


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

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

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


def _parse_fix(raw: Any) -> GnssFix:
    if raw is None:
        return GnssFix(False, 0.0, 0.0, 0.0)
    if isinstance(raw, str):
        raw = raw.strip()
        if not raw:
            return GnssFix(False, 0.0, 0.0, 0.0)
        try:
            raw = json.loads(raw)
        except Exception:
            return GnssFix(False, 0.0, 0.0, 0.0)
    if not isinstance(raw, dict):
        return GnssFix(False, 0.0, 0.0, 0.0)

    # Device firmware may wrap fields in {"gnss": {...}}
    if "gnss" in raw and isinstance(raw.get("gnss"), dict):
        raw = raw["gnss"]

    ok = bool(raw.get("ok", True))
    has_fix = raw.get("has_fix")
    if has_fix is not None:
        ok = ok and bool(has_fix)
    lat = raw.get("lat")
    lon = raw.get("lon")
    if lat is None or lon is None:
        lat = raw.get("latitude")
        lon = raw.get("longitude")
    try:
        lat_f = float(lat)
        lon_f = float(lon)
    except Exception:
        return GnssFix(False, 0.0, 0.0, 0.0)

    speed_mps = raw.get("speed_mps")
    if speed_mps is None:
        speed_knots = raw.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 = raw.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 = raw.get("speed") or 0.0
    try:
        speed_mps_f = float(speed_mps)
    except Exception:
        speed_mps_f = 0.0

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

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

    # Optional: fix quality
    fix = raw.get("fix") or raw.get("fix_quality") or raw.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 _pick_gnss_tool_name(conn) -> Optional[str]:
    if not hasattr(conn, "mcp_client") or not conn.mcp_client:
        return None
    # Prefer sanitized name (server side sanitizes tool names).
    preferred = "self_gnss_get_fix"
    if conn.mcp_client.has_tool(preferred):
        return preferred
    # Fallback heuristic
    for name in conn.mcp_client.tools.keys():
        if "gnss" in name.lower() and "fix" in name.lower():
            return name
    return None


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_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 = 1.0

        self.run_task: Optional[asyncio.Task] = None
        self.run_route_id: Optional[int] = None
        self._run_interval_s = 0.2
        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_last_announce_at = 0.0

        self.announce_at_m = 50.0
        self.announce_window_m = 18.0
        self.announce_cooldown_s = 5.0

    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, "{}")
            return _parse_fix(raw)
        except Exception as e:
            logger.bind(tag=TAG).warning(f"GNSS fix failed: {e}")
            return GnssFix(False, 0.0, 0.0, 0.0)

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

    async def start_recording(self, route_name: Optional[str] = None, sample_hz: int = 1) -> int:
        if self.record_task and not self.record_task.done():
            raise RuntimeError("已在录制中")
        if self.run_task and not self.run_task.done():
            raise RuntimeError("正在领航中，不能开始录制")
        if not _pick_gnss_tool_name(self.conn):
            raise RuntimeError("车载端暂未提供定位数据接口，请先在设备端启用定位数据上报。")

        name = (route_name or "").strip() or f"路线 {time.strftime('%Y-%m-%d %H:%M:%S')}"
        route_id = self.db.create_route(name)
        self.record_route_id = route_id
        self._record_seq = 0
        self._record_cum_m = 0.0
        self._record_last_latlon = None
        self._record_last_t = 0
        self._record_interval_s = 1.0 / max(1, int(sample_hz))

        self.record_task = asyncio.create_task(self._record_loop(), name=f"NavRecord:{route_id}")
        return route_id

    async def stop_recording(self) -> Optional[int]:
        rid = self.record_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
        if rid is not None:
            self.db.finish_route(rid)
        return rid

    async def _record_loop(self) -> None:
        assert self.record_route_id is not None
        route_id = self.record_route_id
        logger.bind(tag=TAG).info(f"Nav 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_task = None

    async def mark_hazard(self, hazard_type: str, note: str = "") -> int:
        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("没有可用路线：先说“开始录制路线”")
        if not _pick_gnss_tool_name(self.conn):
            raise RuntimeError("车载端暂未提供定位数据接口，无法读取当前位置。")

        fix = await self._get_fix()
        if not fix.ok:
            raise RuntimeError("当前没有有效GPS定位，标记失败")

        now_t = int(fix.t or time.time())

        # Prefer current recording cum if we are recording
        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 not points:
                raise RuntimeError("路线还没有轨迹点")
            idx, _ = nearest_point_index([(p.lat, p.lon) for p in points], fix.lat, fix.lon)
            seq = points[idx].seq
            cum = points[idx].cum_dist_m

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

        # If we're running on this route, refresh hazards in memory
        if self.run_route_id == route_id and self.run_task and not self.run_task.done():
            self._run_hazards = self.db.load_hazards(route_id)

        return hid

    async def start_run(self, route_id: Optional[int] = None, tick_hz: int = 5) -> int:
        if self.run_task and not self.run_task.done():
            raise RuntimeError("已在领航中")
        if self.record_task and not self.record_task.done():
            raise RuntimeError("正在录制中，不能开始领航")
        if not _pick_gnss_tool_name(self.conn):
            raise RuntimeError("车载端暂未提供定位数据接口，请先在设备端启用定位数据上报。")

        rid = route_id or self.db.get_active_route_id()
        if not rid:
            raise RuntimeError("没有可用路线：先说“开始录制路线”并跑一遍")

        points = self.db.load_route_points(rid)
        hazards = self.db.load_hazards(rid)
        if len(points) < 2:
            raise RuntimeError("路线点太少，无法领航")

        self.run_route_id = rid
        self._run_points = points
        self._run_hazards = hazards
        self._run_last_idx = None
        self._run_announced = set()
        self._run_last_announce_at = 0.0
        self._run_interval_s = 1.0 / max(1, int(tick_hz))

        self.run_task = asyncio.create_task(self._run_loop(), name=f"NavRun:{rid}")
        return rid

    async def stop_run(self) -> Optional[int]:
        rid = 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_points = []
        self._run_hazards = []
        self._run_last_idx = None
        self._run_announced = set()
        return rid

    async def _run_loop(self) -> None:
        assert self.run_route_id is not None
        rid = self.run_route_id
        logger.bind(tag=TAG).info(f"Nav run loop started, route_id={rid}")

        points = self._run_points
        latlon_points = [(p.lat, p.lon) for p in points]
        try:
            while not self.conn.stop_event.is_set():
                fix = await self._get_fix()
                if not fix.ok:
                    await asyncio.sleep(1.0)
                    continue

                idx, _d = nearest_point_index(
                    latlon_points,
                    fix.lat,
                    fix.lon,
                    start_idx=self._run_last_idx,
                    window=120,
                )
                self._run_last_idx = idx
                cur_cum = points[idx].cum_dist_m

                # Find next hazard ahead
                next_h: Optional[Hazard] = None
                next_dist = None
                for h in self._run_hazards:
                    if h.id in self._run_announced:
                        continue
                    d = h.cum_dist_m - cur_cum
                    if d < -15:
                        # already passed
                        self._run_announced.add(h.id)
                        continue
                    if d < 0:
                        continue
                    if next_dist is None or d < next_dist:
                        next_h = h
                        next_dist = d

                if next_h and next_dist is not None:
                    now = time.time()
                    if now - self._run_last_announce_at >= self.announce_cooldown_s:
                        if (self.announce_at_m - self.announce_window_m) <= next_dist <= self.announce_at_m:
                            speed_mps = max(0.5, float(fix.speed_mps or 0.0))
                            speed_kmh = speed_mps * 3.6
                            eta_s = int(round(next_dist / speed_mps))
                            label = _HAZARD_LABELS.get(next_h.type, _HAZARD_LABELS["other"])
                            dist_i = int(round(next_dist))
                            speed_i = int(round(speed_kmh))
                            text = f"注意前方{dist_i}米{label}，当前{speed_i}公里每小时，预计{eta_s}秒抵达。"
                            if next_h.note:
                                text = f"{text}{next_h.note}"
                            self._speak(text)
                            self._run_last_announce_at = now
                            self._run_announced.add(next_h.id)

                await asyncio.sleep(self._run_interval_s)
        finally:
            if self.run_route_id == rid:
                self.run_route_id = None
                self.run_task = None
                self._run_points = []
                self._run_hazards = []
                self._run_last_idx = None
                self._run_announced = set()

    def status_text(self) -> str:
        parts = []
        if self.record_route_id:
            parts.append(f"正在录制路线ID={self.record_route_id}，点数={self._record_seq}")
        if self.run_route_id:
            parts.append(
                f"正在领航路线ID={self.run_route_id}，障碍点={len(self._run_hazards)}"
            )
        if not parts:
            rid = self.db.get_active_route_id()
            if rid:
                parts.append(f"当前激活路线ID={rid}")
            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