Chameleon/ptz_tracker_modular/status.py

# status.py
from __future__ import annotations

import time
import asyncio
import logging
from typing import Optional, Tuple, List

from . import config, state
from .utils import clipf
from .ptz_io import get_ptz_status  # ожидает cam_idx: int, возвращает (pan_deg|None, tilt_deg|None)

logger = logging.getLogger("PTZTracker")


# ---------------- Публичные утилиты статуса ----------------
def get_cached_angles(cam_id: int) -> Tuple[Optional[float], Optional[float], Optional[float]]:
    """
    Вернёт (pan_deg, tilt_deg, t_epoch) из кэша, если есть (иначе None).
    Кэш наполняется ptz_status_poller().
    """
    cache = getattr(state, "ptz_status", {}).get(cam_id)
    if not cache:
        return None, None, None
    return cache.get("pan"), cache.get("tilt"), cache.get("t")


def format_angles_for_log(pan: Optional[float], tilt: Optional[float]) -> str:
    if pan is None and tilt is None:
        return "pan=NA tilt=NA"
    if pan is None:
        return f"pan=NA tilt={tilt:.2f}"
    if tilt is None:
        return f"pan={pan:.2f} tilt=NA"
    return f"pan={pan:.2f} tilt={tilt:.2f}"


# --------------- Внутренний телеметрический логгер ---------------
def _should_log_telemetry(cam_id: int, now: float, period: float) -> bool:
    """
    Простая защита от спама: логируем статус камеры не чаще, чем раз в period секунд.
    """
    key = f"_last_log_{cam_id}"
    last = getattr(state, key, 0.0)
    if now - last >= period:
        setattr(state, key, now)
        return True
    return False


def _update_runtime_state(cam_id: int, pan: Optional[float], tilt: Optional[float], now: float) -> None:
    """
    Проставляет дубли в state.ptz_states[cam_id] для использования остальным кодом,
    включая воркер из ptz_io.py (он читает az_deg!) и постпроцессинг.

    Ставим:
      - az_deg (совместимость с ptz_io._ptz_worker)
      - pan_deg (то же значение, чтобы везде было одинаково)
      - pan_deg_geo (если в конфиге есть north_offset_deg)
      - tilt_deg (клип -90..+90)
      - last_status_at
    """
    st = state.ptz_states.setdefault(cam_id, {})
    cfg = config.CAMERA_CONFIG.get(cam_id, {})

    if pan is not None:
        pan_n = float(pan) % 360.0
        st["az_deg"] = pan_n          # <-- это читает ptz_io._ptz_worker при bounded-патруле
        st["pan_deg"] = pan_n         # удобный синоним

        north_off = cfg.get("north_offset_deg")
        if north_off is not None:
            try:
                st["pan_deg_geo"] = (pan_n + float(north_off)) % 360.0
            except Exception:
                st["pan_deg_geo"] = pan_n

    if tilt is not None:
        st["tilt_deg"] = clipf(float(tilt), -90.0, 90.0)

    st["last_status_at"] = now


# -------------------- Основной поллер статуса ---------------------
async def ptz_status_poller() -> None:
    """
    Асинхронный опрос PTZ-камер из config.PTZ_CAM_IDS и публикация в state:
      - state.ptz_status[cid] = {"pan": deg|None, "tilt": deg|None, "t": now}
      - state.ptz_states[cid]["az_deg"/"pan_deg"/"pan_deg_geo"/"tilt_deg"/"last_status_at"]

    ВАЖНО: частота задаётся как PTZ_STATUS_HZ **на камеру**.
    То есть при PTZ_STATUS_HZ=4.0 и 3 камерах каждая опрашивается 4 Гц, а не 4/3.

    Редкое логирование: config.PTZ_STATUS_LOG_EVERY_SEC (>0 чтобы включить).
    """
    hz = float(getattr(config, "PTZ_STATUS_HZ", 4.0))
    cam_ids: List[int] = list(getattr(config, "PTZ_CAM_IDS", []))
    n_cams = max(1, len(cam_ids))

    if not cam_ids:
        logger.info("[STATUS] no PTZ cameras to poll (PTZ_CAM_IDS empty)")
        return

    # Интервал между подряд идущими запросами к ОДНОЙ камере:
    # хотим 'hz' опросов в секунду для каждой камеры.
    per_step_interval = 1.0 / max(0.1, hz * n_cams)

    log_period = float(getattr(config, "PTZ_STATUS_LOG_EVERY_SEC", 0.0))

    if not hasattr(state, "ptz_status"):
        state.ptz_status = {}  # type: ignore[attr-defined]

    logger.info(
        "[STATUS] PTZ status poller: %d cam(s), target %.2f Hz per cam (step_interval=%.3fs)",
        len(cam_ids),
        hz,
        per_step_interval,
    )

    i = 0
    try:
        while not getattr(state, "shutdown", False):
            cam_id = cam_ids[i % len(cam_ids)]
            i += 1
            now = time.time()

            # Сетевой запрос делаем в thread pool, чтобы не блокировать event loop
            try:
                pan, tilt = await asyncio.to_thread(get_ptz_status, cam_id)
            except Exception as e:
                logger.warning("[STATUS] cam %d poll error: %s", cam_id, e)
                pan = tilt = None

            # Обновляем кэш
            cache = getattr(state, "ptz_status", {}).get(cam_id, {})
            cache.update({"pan": pan, "tilt": tilt, "t": now})
            state.ptz_status[cam_id] = cache  # type: ignore[attr-defined]

            # Дублируем в ptz_states (для patrol/track/ptz_io и компаса)
            _update_runtime_state(cam_id, pan, tilt, now)

            # Редкое логирование (если включено)
            if log_period > 0 and _should_log_telemetry(cam_id, now, log_period):
                logger.info("[PTZ STATUS] cam %d %s", cam_id, format_angles_for_log(pan, tilt))

            await asyncio.sleep(per_step_interval)

    except asyncio.CancelledError:
        logger.info("[STATUS] PTZ status poller cancelled")
        raise
    finally:
        logger.info("[STATUS] PTZ status poller stopped")


# -------------------- Вспомогательные публичные --------------------
def last_attack_heading_deg(cam_id: int) -> Optional[float]:
    """
    Возвращает последний закэшированный attack_heading_deg (если постпроцессинг его писал).
    Это используется для логов/отладки, не обязательно для патруля.
    """
    st = state.ptz_states.get(cam_id) if hasattr(state, "ptz_states") else None
    if not st:
        return None
    val = st.get("attack_heading_deg")
    return float(val) if isinstance(val, (int, float)) else None


def get_realtime_angles_for_log(cam_id: int) -> str:
    pan, tilt, t = get_cached_angles(cam_id)
    if t is None:
        return f"cam {cam_id} pan=NA tilt=NA"
    age = max(0.0, time.time() - t)
    return f"cam {cam_id} {format_angles_for_log(pan, tilt)} (t-{age:.2f}s)"