ai_fwt_pro/main/xingxing-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, Optional, 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, SentenceType, TTSMessageDTO

from .geo import (
    convert_from_wgs84,
    convert_to_wgs84,
    crosses_alert_line,
    haversine_m,
    normalize_coord_type,
)
from .storage import LineAlert, NavDB


TAG = __name__
logger = setup_logging()

MONITOR_TICK_HZ = 5
MIN_MOVEMENT_M = 1.5
LINE_BUFFER_M = 2.5
TRIGGER_COOLDOWN_S = 15.0
NOTE_MAX_LEN = 120
ALERT_ECHO_TTL_S = 12.0


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


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"]

    try:
        coord_type = normalize_coord_type(payload.get("coord_type"), default="WGS84")
    except ValueError:
        return GnssFix(False, 0.0, 0.0, 0.0)

    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)
        lat_f, lon_f = convert_to_wgs84(lat_f, lon_f, coord_type)
    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(float(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 _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 normalize_note(note: str) -> str:
    normalized = " ".join((note or "").split()).strip()
    if not normalized:
        raise ValueError("note_required")
    if len(normalized) > NOTE_MAX_LEN:
        raise ValueError("note_too_long")
    return normalized


def normalize_latlon(lat: Any, lon: Any) -> Tuple[float, float]:
    lat_f = float(lat)
    lon_f = float(lon)
    if not (-90.0 <= lat_f <= 90.0 and -180.0 <= lon_f <= 180.0):
        raise ValueError("invalid_coordinate")
    return lat_f, lon_f


class NavCopilotService:
    def __init__(self, conn, db: NavDB):
        self.conn = conn
        self.db = db
        self.monitor_task: Optional[asyncio.Task] = None
        self._shared_gps_cache: Optional[Dict[str, Any]] = None
        self._last_fix_wgs84: Optional[Tuple[float, float]] = None
        self._last_trigger_at: Dict[int, float] = {}

    def device_id(self) -> str:
        return str(getattr(self.conn, "device_id", "") or "")

    async def recover_monitoring_if_needed(self) -> Optional[int]:
        device_id = self.device_id()
        if not device_id:
            return None
        count = self.db.count_line_alerts(device_id)
        if count <= 0:
            return None
        await self.ensure_monitoring()
        return count

    def _remember_spoken_alert(self, text: str) -> None:
        if not text:
            return
        now = time.monotonic()
        recent = getattr(self.conn, "_recent_nav_alert_notes", None)
        if not isinstance(recent, dict):
            recent = {}
        recent[text] = now
        expired = [
            note
            for note, spoken_at in recent.items()
            if now - float(spoken_at) > ALERT_ECHO_TTL_S
        ]
        for note in expired:
            recent.pop(note, None)
        self.conn._recent_nav_alert_notes = recent

    def _speak(self, text: str) -> None:
        text = str(text or "").strip()
        if not text:
            return
        try:
            self.conn.client_abort = True
            self.conn.clear_queues()
            self.conn.clearSpeakStatus()
            self.conn.client_abort = False
            self.conn.sentence_id = uuid.uuid4().hex
            self.conn.tts.store_tts_text(self.conn.sentence_id, text)
            self.conn.tts.tts_text_queue.put(
                TTSMessageDTO(
                    sentence_id=self.conn.sentence_id,
                    sentence_type=SentenceType.FIRST,
                    content_type=ContentType.ACTION,
                )
            )
            self.conn.tts.tts_one_sentence(
                self.conn,
                ContentType.TEXT,
                content_detail=text,
            )
            self.conn.tts.tts_text_queue.put(
                TTSMessageDTO(
                    sentence_id=self.conn.sentence_id,
                    sentence_type=SentenceType.LAST,
                    content_type=ContentType.ACTION,
                )
            )
            self._remember_spoken_alert(text)
        except Exception as exc:
            logger.bind(tag=TAG).warning(f"TTS speak failed: {exc}")

    async def _get_fix(self) -> GnssFix:
        server = getattr(self.conn, "server", None)
        device_id = getattr(self.conn, "device_id", None)
        if server is not None and device_id:
            try:
                payload, _ = await server.get_latest_gps(
                    str(device_id), max_age_s=3.0, allow_stale=False
                )
                if payload is not None:
                    return _parse_fix(payload)
            except Exception as exc:
                logger.bind(tag=TAG).debug(f"read live telemetry gps failed: {exc}")

        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)

    def update_shared_gps(self, fix: GnssFix) -> None:
        if not fix.ok:
            return
        self._shared_gps_cache = {
            "ok": True,
            "device_id": getattr(self.conn, "device_id", None),
            "lat": fix.lat,
            "lon": fix.lon,
            "speed_mps": fix.speed_mps,
            "heading_deg": fix.heading_deg,
            "timestamp": fix.t,
            "coord_type": "WGS84",
            "source": "nav_copilot_shared",
        }

    def get_cached_gps(self) -> Optional[Dict[str, Any]]:
        return self._shared_gps_cache

    def is_monitoring_active(self) -> bool:
        return self.monitor_task is not None and not self.monitor_task.done()

    def list_line_alerts(self, device_id: str) -> list[LineAlert]:
        return self.db.list_line_alerts(device_id)

    async def create_line_alert(
        self,
        *,
        device_id: str,
        start_lat: float,
        start_lon: float,
        end_lat: float,
        end_lon: float,
        coord_type: str = "BD09",
        note: str,
    ) -> LineAlert:
        start_lat, start_lon = normalize_latlon(start_lat, start_lon)
        end_lat, end_lon = normalize_latlon(end_lat, end_lon)
        coord_type = normalize_coord_type(coord_type, default="BD09")
        note = normalize_note(note)
        if haversine_m(start_lat, start_lon, end_lat, end_lon) < 1.0:
            raise ValueError("line_too_short")

        alert_id = self.db.create_line_alert(
            device_id=device_id,
            start_lat=start_lat,
            start_lon=start_lon,
            end_lat=end_lat,
            end_lon=end_lon,
            coord_type=coord_type,
            note=note,
        )
        alert = self.db.get_line_alert(alert_id, device_id=device_id)
        assert alert is not None
        if device_id == self.device_id():
            await self.ensure_monitoring()
        return alert

    async def delete_line_alert(self, device_id: str, alert_id: int) -> bool:
        deleted = self.db.delete_line_alert(alert_id, device_id=device_id)
        if deleted and device_id == self.device_id() and self.db.count_line_alerts(device_id) <= 0:
            await self.stop_monitoring()
        return deleted

    async def ensure_monitoring(self) -> bool:
        device_id = self.device_id()
        if not device_id:
            return False
        if self.db.count_line_alerts(device_id) <= 0:
            return False
        if self.is_monitoring_active():
            return True

        self.monitor_task = asyncio.create_task(
            self._monitor_loop(),
            name=f"LineAlertMonitor:{device_id}",
        )
        return True

    async def stop_monitoring(self) -> None:
        task = self.monitor_task
        if task is not None and not task.done():
            task.cancel()
            try:
                await task
            except BaseException:
                pass
        self.monitor_task = None
        self._last_fix_wgs84 = None
        self._last_trigger_at = {}

    async def _monitor_loop(self) -> None:
        device_id = self.device_id()
        logger.bind(tag=TAG).info(f"Line alert monitor started: device_id={device_id}")
        try:
            while not self.conn.stop_event.is_set():
                if self.db.count_line_alerts(device_id) <= 0:
                    break

                fix = await self._get_fix()
                if not fix.ok:
                    await asyncio.sleep(1.0 / MONITOR_TICK_HZ)
                    continue

                self.update_shared_gps(fix)
                current_fix_wgs84 = (fix.lat, fix.lon)

                last_fix_wgs84 = self._last_fix_wgs84
                self._last_fix_wgs84 = current_fix_wgs84
                if last_fix_wgs84 is None:
                    await asyncio.sleep(1.0 / MONITOR_TICK_HZ)
                    continue

                movement_m = haversine_m(
                    last_fix_wgs84[0],
                    last_fix_wgs84[1],
                    current_fix_wgs84[0],
                    current_fix_wgs84[1],
                )
                if movement_m < MIN_MOVEMENT_M:
                    await asyncio.sleep(1.0 / MONITOR_TICK_HZ)
                    continue

                for alert in self.db.list_line_alerts(device_id):
                    prev_alert_lat, prev_alert_lon = convert_from_wgs84(
                        last_fix_wgs84[0],
                        last_fix_wgs84[1],
                        alert.coord_type,
                    )
                    curr_alert_lat, curr_alert_lon = convert_from_wgs84(
                        current_fix_wgs84[0],
                        current_fix_wgs84[1],
                        alert.coord_type,
                    )
                    if not crosses_alert_line(
                        prev_alert_lat,
                        prev_alert_lon,
                        curr_alert_lat,
                        curr_alert_lon,
                        alert.start_lat,
                        alert.start_lon,
                        alert.end_lat,
                        alert.end_lon,
                        buffer_m=LINE_BUFFER_M,
                    ):
                        continue

                    now = time.monotonic()
                    last_trigger_at = self._last_trigger_at.get(alert.id, 0.0)
                    if now - last_trigger_at < TRIGGER_COOLDOWN_S:
                        continue

                    logger.bind(tag=TAG).info(
                        "Line alert triggered: "
                        f"device_id={device_id}, alert_id={alert.id}, note={alert.note!r}"
                    )
                    self._speak(alert.note)
                    self._last_trigger_at[alert.id] = now

                await asyncio.sleep(1.0 / MONITOR_TICK_HZ)
        except asyncio.CancelledError:
            raise
        finally:
            logger.bind(tag=TAG).info(f"Line alert monitor stopped: device_id={device_id}")
            self.monitor_task = None
            self._last_fix_wgs84 = None
            self._last_trigger_at = {}

    def status_text(self) -> str:
        device_id = self.device_id()
        if not device_id:
            return "当前没有可用设备连接。"
        count = self.db.count_line_alerts(device_id)
        if count <= 0:
            return "当前没有通过手机 App 配置的风险线。"
        if self.is_monitoring_active():
            return f"当前设备已启用风险线预警，数量 {count}。"
        return f"当前设备已有风险线 {count} 条，等待开始监控。"


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