robot-command-center/backend/monitoring/telemetry.py

from __future__ import annotations

import json
import math
import threading
import time
from datetime import UTC, datetime
from typing import Any

from .common import GEOSTREAM_JSON_PATH, GEOSTREAM_STALE_SECONDS, utc_iso_now
from .control import ControlArbiter, NativeUdpControlIngress, OmniSocketControlSender
from .video import OmniSocketVideoReceiver


class GpsDataService:
    def get_latest(self) -> dict[str, Any]:
        payload = self._read_geostream_payload()
        if payload is not None:
            payload["source_mode"] = "geostream-json"
            payload["updated_at"] = utc_iso_now()
            return payload

        return self._build_simulated_payload()

    def _read_geostream_payload(self) -> dict[str, Any] | None:
        if not GEOSTREAM_JSON_PATH.exists():
            return None

        age_seconds = time.time() - GEOSTREAM_JSON_PATH.stat().st_mtime
        if age_seconds > GEOSTREAM_STALE_SECONDS:
            return None

        try:
            with GEOSTREAM_JSON_PATH.open("r", encoding="utf-8") as file:
                return json.load(file)
        except (OSError, json.JSONDecodeError):
            return None

    def _build_simulated_payload(self) -> dict[str, Any]:
        tick = time.time() / 12.0
        latitude = 31.2304 + math.sin(tick) * 0.0014
        longitude = 121.4737 + math.cos(tick) * 0.0018

        return {
            "has_fix": True,
            "utc_time": datetime.now(UTC).strftime("%H:%M:%S"),
            "latitude": round(latitude, 6),
            "longitude": round(longitude, 6),
            "satellites": 14 + int((math.sin(tick * 0.7) + 1.0) * 2),
            "altitude_m": round(6.5 + math.cos(tick * 0.5) * 1.2, 2),
            "coordinate_system": "WGS84",
            "source_sentence": "SIMULATED",
            "raw_coordinate_format": "decimal degrees",
            "source_mode": "simulated",
            "updated_at": utc_iso_now(),
        }


class NetworkTelemetryService:
    def __init__(
        self,
        video_receiver: OmniSocketVideoReceiver,
        control_sender: OmniSocketControlSender,
        control_arbiter: ControlArbiter,
        native_ingress: NativeUdpControlIngress,
    ) -> None:
        self._video_receiver = video_receiver
        self._control_sender = control_sender
        self._control_arbiter = control_arbiter
        self._native_ingress = native_ingress
        self._rate_lock = threading.Lock()
        self._last_rate_sample: tuple[float, int, int] | None = None

    def _compute_rates(self, send_bytes: int, recv_bytes: int) -> tuple[float, float]:
        now = time.monotonic()
        with self._rate_lock:
            previous = self._last_rate_sample
            self._last_rate_sample = (now, send_bytes, recv_bytes)

        if previous is None:
            return 0.0, 0.0

        prev_time, prev_send, prev_recv = previous
        elapsed = now - prev_time
        if elapsed <= 0.0:
            return 0.0, 0.0

        tx_kbps = max(0.0, ((send_bytes - prev_send) * 8.0) / elapsed / 1000.0)
        rx_kbps = max(0.0, ((recv_bytes - prev_recv) * 8.0) / elapsed / 1000.0)
        return tx_kbps, rx_kbps

    def get_latest(self) -> dict[str, Any]:
        self._video_receiver.ensure_started()
        self._control_arbiter.ensure_started()
        self._native_ingress.ensure_started()

        video_app = self._video_receiver.session_stats()
        control_app = self._control_sender.session_stats()
        video_kcp = self._video_receiver.session_kcp_stats()
        control_kcp = self._control_sender.session_kcp_stats()
        arbiter_status = self._control_arbiter.get_status()
        ingress_status = self._native_ingress.get_status()
        sender_status = self._control_sender.get_status()

        total_send_bytes = int(video_app.get("send_bytes", 0)) + int(control_app.get("send_bytes", 0))
        total_recv_bytes = int(video_app.get("recv_bytes", 0)) + int(control_app.get("recv_bytes", 0))
        tx_kbps, rx_kbps = self._compute_rates(total_send_bytes, total_recv_bytes)

        video_connected = int(video_app.get("connected", 0))
        control_connected = int(control_app.get("connected", 0))
        connected_sessions = video_connected + control_connected

        primary_kcp = control_kcp if control_connected else video_kcp
        latency_ms = primary_kcp.get("srtt_ms")
        jitter_ms = primary_kcp.get("srttvar_ms")

        if connected_sessions > 0:
            peer_status = "online"
        elif sender_status.get("backend_ready"):
            peer_status = "idle"
        else:
            peer_status = "backend-unavailable"

        return {
            "peer_status": peer_status,
            "latency_ms": latency_ms,
            "jitter_ms": jitter_ms,
            "packet_loss_pct": None,
            "tx_kbps": round(tx_kbps, 3),
            "rx_kbps": round(rx_kbps, 3),
            "transport": "OmniSocket / kcp",
            "source_mode": "omnisocket-live" if connected_sessions > 0 else "omnisocket-idle",
            "updated_at": utc_iso_now(),
            "active_control_source": arbiter_status["active_source"],
            "control_lease_remaining_ms": arbiter_status["control_lease_remaining_ms"],
            "combined": {
                "connected_sessions": connected_sessions,
                "send_bytes": total_send_bytes,
                "recv_bytes": total_recv_bytes,
                "tx_kbps": round(tx_kbps, 3),
                "rx_kbps": round(rx_kbps, 3),
            },
            "sessions": {
                "video": {
                    "app": video_app,
                    "kcp": video_kcp,
                },
                "control": {
                    "app": control_app,
                    "kcp": control_kcp,
                },
            },
            "ingress": {
                "native_udp": ingress_status,
            },
            "control": {
                "arbiter": arbiter_status,
                "sender": sender_status,
            },
        }