Chameleon/ptz_tracker_modular/toml_persist.py

# toml_persist.py
from __future__ import annotations
import os
import time
import logging
from pathlib import Path
from typing import Any, Dict, List, Optional, Tuple

from . import config
from .geom import ang_diff_signed, normalize360
from .ptz_io import read_ptz_azimuth_deg, goto_preset_token

logger = logging.getLogger("PTZTracker.PERSIST")

def _cameras_toml_path() -> Path:
    """
    Должны писать в тот же файл, который читает config.reload_cameras().
    Приоритет:
      1) env CAMERAS_FILE
      2) cameras.toml рядом с config.py
    """
    env = os.getenv("CAMERAS_FILE")
    if env:
        return Path(env).resolve()

    # рядом с config.py (и тем самым совместимо с config._load_cameras_from_file)
    cfg_dir = Path(getattr(config, "__file__", __file__)).resolve().parent
    return (cfg_dir / "cameras.toml").resolve()


def _load_toml(path: Path) -> Dict[str, Any]:
    if not path.exists():
        return {}
    try:
        import tomllib
        with open(path, "rb") as f:
            return tomllib.load(f)
    except Exception as e:
        logger.error("[PERSIST] TOML read failed: %s", e)
        return {}


def _atomic_write(path: Path, text: str) -> None:
    tmp = path.with_suffix(path.suffix + ".tmp")
    with open(tmp, "w", encoding="utf-8") as f:
        f.write(text)
        f.flush()
        os.fsync(f.fileno())
    os.replace(tmp, path)


def _to_float(val: Any) -> Optional[float]:
    try:
        return float(val)
    except Exception:
        return None


def _render_cameras_table(cams: List[Dict[str, Any]]) -> str:
    lines: List[str] = []
    for c in sorted(cams, key=lambda x: int(x.get("id", 0))):
        lines.append("[[camera]]")
        lines.append(f"id = {int(c['id'])}")
        lines.append(f"ip = \"{c.get('ip','')}\"")
        lines.append(f"username = \"{c.get('username','')}\"")
        lines.append(f"password = \"{c.get('password','')}\"")
        lines.append(f"ptz_channel = {int(c.get('ptz_channel', 1))}")
        if "wiper_channel" in c:
            lines.append(f"wiper_channel = {int(c.get('wiper_channel', 1))}")
        if "preset1" in c and c.get("preset1") not in (None, "None"):
            lines.append(f"preset1 = \"{c.get('preset1')}\"")
        if "preset2" in c and c.get("preset2") not in (None, "None"):
            lines.append(f"preset2 = \"{c.get('preset2')}\"")
        if "sweep_sign" in c:
            lines.append(f"sweep_sign = {int(c.get('sweep_sign', 1))}")
        if "is_ptz" in c:
            lines.append(f"is_ptz = {str(bool(c.get('is_ptz'))).lower()}")
        if "mac" in c and isinstance(c["mac"], str):
            lines.append(f"mac = \"{c['mac']}\"")

        for k in ("north_offset_deg", "preset1_ref_geo_deg", "preset2_ref_geo_deg"):
            if isinstance(c.get(k), (int, float)):
                lines.append(f"{k} = {float(c[k]):.6f}")

        for k in ("preset1_deg_geo", "preset2_deg_geo", "sector_min_deg_geo", "sector_max_deg_geo"):
            if isinstance(c.get(k), (int, float)):
                lines.append(f"{k} = {float(c[k]):.6f}")

        for k in ("preset1_deg", "preset2_deg", "preset1_tilt_deg", "preset2_tilt_deg", "sector_min_deg", "sector_max_deg"):
            if isinstance(c.get(k), (int, float)):
                lines.append(f"{k} = {float(c[k]):.6f}")

        for k in ("north_last_source", "geo_last_source"):
            if isinstance(c.get(k), str):
                lines.append(f"{k} = \"{c[k]}\"")
        for k in ("north_last_at", "geo_last_at"):
            if isinstance(c.get(k), int):
                lines.append(f"{k} = {c[k]}")

        lines.append("")
    return "\n".join(lines).strip() + "\n"


def save_all_cameras() -> None:
    path = _cameras_toml_path()
    cams = list(config.CAMERA_CONFIG.values())
    text = _render_cameras_table(cams)
    try:
        _atomic_write(path, text)
        logger.info("[PERSIST] cameras.toml saved (%d cameras)", len(cams))
        try:
            config.reload_cameras()
        except Exception as e:
            logger.warning("[PERSIST] reload_cameras failed: %s", e)
    except Exception as e:
        logger.error("[PERSIST] write failed: %s", e)

# ---------- низкоуровневая запись одного поля ----------
def _persist_field(cam_id: int, key: str, value) -> bool:
    """
    Записать камере поле key=value в cameras.toml. True, если действительно изменилось.
    """
    path = _cameras_toml_path()
    data = _load_toml(path)
    cams = data.get("camera") or data.get("cameras") or []
    if not isinstance(cams, list):
        logger.error("[PERSIST] cameras.toml format unexpected: no [[camera]] array")
        return False

    changed = False
    found = False
    for c in cams:
        if not isinstance(c, dict) or "id" not in c:
            continue
        if int(c["id"]) == int(cam_id):
            found = True
            before = c.get(key)
            if before != value:
                c[key] = value
                changed = True
            break

    if not found or not changed:
        return False

    text = _render_cameras_table(cams)
    try:
        _atomic_write(path, text)
        try:
            config.reload_cameras()
        except Exception as e:
                logger.warning("[PERSIST] reload_cameras failed: %s", e)
        return True
    except Exception as e:
        logger.error("[PERSIST] write failed: %s", e)
        return False

# ---------- публичный API: north_offset ----------
def persist_north_offset(cam_id: int, north_deg: float, source: str = "unknown") -> bool:
    """
    Обновляет/добавляет north_offset_deg; ставит метаданные; перезагружает config.
    """
    path = _cameras_toml_path()
    data = _load_toml(path)
    cams = data.get("camera") or data.get("cameras") or []
    if not isinstance(cams, list):
        logger.error("[PERSIST] cameras.toml format unexpected: no [[camera]] array")
        return False

    north_deg = normalize360(float(north_deg))
    changed = False
    found = False
    for c in cams:
        if not isinstance(c, dict) or "id" not in c:
            continue
        if int(c["id"]) == int(cam_id):
            found = True
            old = _to_float(c.get("north_offset_deg"))
            if old is None or abs(old - north_deg) > 1e-6:
                c["north_offset_deg"] = float(north_deg)
                c["north_last_source"] = source
                c["north_last_at"] = int(time.time())
                changed = True
            break

    if not found:
        logger.warning("[PERSIST] camera id=%s not found in cameras.toml; will not add a new block", cam_id)
        return False
    if not changed:
        return False

    text = _render_cameras_table(cams)
    try:
        _atomic_write(path, text)
        logger.info("[PERSIST] cameras.toml updated (cam %s north=%.2f° via %s)", cam_id, north_deg, source)
        try:
            config.reload_cameras()
        except Exception as e:
            logger.warning("[PERSIST] reload_cameras failed: %s", e)
        return True
    except Exception as e:
        logger.error("[PERSIST] write failed: %s", e)
        return False

# ---------- публичный API: массовая запись geo-полей ----------
def persist_geo_fields(cam_id: int, fields: Dict[str, float], source: str = "unknown") -> bool:
    path = _cameras_toml_path()
    data = _load_toml(path)
    cams = data.get("camera") or data.get("cameras") or []
    if not isinstance(cams, list):
        logger.error("[PERSIST] cameras.toml format unexpected: no [[camera]] array")
        return False

    changed = False
    found = False
    for c in cams:
        if not isinstance(c, dict) or "id" not in c:
            continue
        if int(c["id"]) == int(cam_id):
            found = True
            for k, v in fields.items():
                if not isinstance(v, (int, float)):
                    continue
                vv = normalize360(float(v)) if k == "north_offset_deg" else float(v)
                old = _to_float(c.get(k))
                if old is None or abs(old - vv) > 1e-6:
                    c[k] = vv
                    changed = True
            if changed:
                c["geo_last_source"] = source
                c["geo_last_at"] = int(time.time())
            break

    if not found or not changed:
        return False

    text = _render_cameras_table(cams)
    try:
        _atomic_write(path, text)
        logger.info("[PERSIST] cameras.toml updated (cam %s fields=%s via %s)", cam_id, list(fields.keys()), source)
        try:
            config.reload_cameras()
        except Exception as e:
            logger.warning("[PERSIST] reload_cameras failed: %s", e)
        return True
    except Exception as e:
        logger.error("[PERSIST] write failed: %s", e)
        return False

# ---------- публичный API: preset1_ref_geo_deg ----------
def persist_preset1_ref_geo(cam_id: int, ref_geo_deg: float, source: str = "auto") -> bool:
    """
    Записывает preset1_ref_geo_deg (если изменился) + reload.
    """
    ref_geo_deg = normalize360(float(ref_geo_deg))
    ok = _persist_field(cam_id, "preset1_ref_geo_deg", ref_geo_deg)
    if ok:
        logger.info("[PERSIST] cam %s preset1_ref_geo_deg=%.2f (%s)", cam_id, ref_geo_deg, source)
    return ok

# ---------- утилиты чтения статуса ----------
async def _wait_az_stable(cam: int, timeout: float = 2.5, tol_deg: float = 0.3) -> Optional[float]:
    """
    Ждём пока pan успокоится (изменение <= tol_deg). Возвращаем pan (0..360) или None.
    """
    import asyncio
    loop = asyncio.get_event_loop()
    t0 = loop.time()
    last = None
    stable = 0
    while loop.time() - t0 < timeout:
        a = read_ptz_azimuth_deg(cam)
        if a is None:
            await asyncio.sleep(0.12)
            continue
        a = normalize360(a)
        # подписанная разница
        diff = (a - (last if last is not None else a) + 540.0) % 360.0 - 180.0
        if last is None or abs(diff) > tol_deg:
            stable = 0
        else:
            stable += 1
            if stable >= 1:
                return a
        last = a
        await asyncio.sleep(0.12)
    return last

# ---------- авторасчёт и запись preset1_ref_geo_deg (от самого PTZ) ----------
async def compute_and_persist_preset1_ref_geo(cam_id: int) -> Optional[float]:
    """
    Если у камеры задан `north_offset_deg` и есть `preset1`,
    вычисляет preset1_ref_geo_deg = normalize360(pan_on_preset1 + north_offset_deg),
    сохраняет в cameras.toml и возвращает значение.
    """
    cfg = config.CAMERA_CONFIG.get(cam_id, {})
    north = cfg.get("north_offset_deg")
    p1 = cfg.get("preset1")
    if not isinstance(north, (int, float)) or not p1:
        logger.info("[PERSIST] cam %s: cannot compute preset1_ref_geo_deg (north or preset1 missing)", cam_id)
        return None

    try:
        goto_preset_token(cam_id, str(p1))
        import asyncio
        await asyncio.sleep(0.6)
        pan = await _wait_az_stable(cam_id)
        if pan is None:
            logger.warning("[PERSIST] cam %s: cannot read stable pan on preset1", cam_id)
            return None

        ref_geo = normalize360(float(pan) + float(north))

        changed = persist_geo_fields(
            cam_id,
            {"preset1_ref_geo_deg": ref_geo},
            source="compute_and_persist_preset1_ref_geo",
        )
        if changed:
            logger.info("[PERSIST] cam %s: preset1_ref_geo_deg=%.2f written to cameras.toml", cam_id, ref_geo)
        else:
            logger.info("[PERSIST] cam %s: preset1_ref_geo_deg already up-to-date (%.2f)", cam_id, ref_geo)

        # обновим рантайм для удобства
        cfg["preset1_ref_geo_deg"] = ref_geo
        cfg["preset1_deg_geo"] = ref_geo
        if isinstance(cfg.get("preset2_deg"), (int, float)):
            cfg["preset2_deg_geo"] = normalize360(cfg["preset2_deg"] + float(north))
            span_geo = ang_diff_signed(cfg["preset2_deg_geo"], cfg["preset1_deg_geo"])
            if span_geo >= 0:
                cfg["sector_min_deg_geo"] = normalize360(cfg["preset1_deg_geo"] - config.SECTOR_MARGIN_DEG)
                cfg["sector_max_deg_geo"] = normalize360(cfg["preset2_deg_geo"] + config.SECTOR_MARGIN_DEG)
            else:
                cfg["sector_min_deg_geo"] = normalize360(cfg["preset2_deg_geo"] - config.SECTOR_MARGIN_DEG)
                cfg["sector_max_deg_geo"] = normalize360(cfg["preset1_deg_geo"] + config.SECTOR_MARGIN_DEG)
            _persist_field(cam_id, "preset1_deg_geo", float(cfg["preset1_deg_geo"]))
            _persist_field(cam_id, "preset2_deg_geo", float(cfg["preset2_deg_geo"]))
            _persist_field(cam_id, "sector_min_deg_geo", float(cfg["sector_min_deg_geo"]))
            _persist_field(cam_id, "sector_max_deg_geo", float(cfg["sector_max_deg_geo"]))

        return ref_geo
    except Exception as e:
        logger.warning("[PERSIST] cam %s: compute_and_persist_preset1_ref_geo failed: %s", cam_id, e)
        return None

# ---------- авторасчёт рефов от кромок панорамы и запись ----------
def _pano_edges_geo(pano_id: int) -> Optional[Tuple[float, float, float]]:
    """
    Возвращает (center_geo, left_geo, right_geo) для панорамы.
    Требует: north_offset_deg (пано) и bullet_hfov_deg/hfov_deg (пано).
    center_geo берём из runtime, если там есть pan_deg; иначе считаем центр по north_offset.
    """
    pcfg = config.CAMERA_CONFIG.get(pano_id, {})
    north = pcfg.get("north_offset_deg")
    hfov = pcfg.get("bullet_hfov_deg") or pcfg.get("hfov_deg") or 90.0
    if not isinstance(north, (int, float)):
        return None
    pan_abs = None
    try:
        from . import state
        st = getattr(state, "ptz_states", {}).get(pano_id) or {}
        pan_abs = st.get("pan_deg")
    except Exception:
        pan_abs = None
    if pan_abs is None:
        pan_abs = 0.0
    center_geo = normalize360(float(pan_abs) + float(north))
    left_geo = normalize360(center_geo - float(hfov) * 0.5)
    right_geo = normalize360(center_geo + float(hfov) * 0.5)
    return center_geo, left_geo, right_geo

async def compute_ref_from_pano_edges_and_persist(ptz_id: int, bind: str = "preset1=left,preset2=right") -> Optional[Tuple[float, float]]:
    """
    Находит «спаренную» панораму (тот же IP, channel=2), берёт её north_offset_deg и hfov,
    вычисляет гео-азимуты левой/правой кромки и сохраняет в cameras.toml:
        preset1_ref_geo_deg и preset2_ref_geo_deg.
    bind: схема привязки (default: preset1->left, preset2->right).
    Возвращает (p1_ref, p2_ref) или None.
    """
    # 1) попытка найти панораму по PTZ_TO_PAN
    pano_id = config.PTZ_TO_PAN.get(ptz_id)
    if pano_id is None:
        # резервный поиск по IP/каналу
        cfg = config.CAMERA_CONFIG.get(ptz_id, {})
        ip = cfg.get("ip")
        if ip:
            for cid, c in config.CAMERA_CONFIG.items():
                if c.get("ip") == ip and int(c.get("ptz_channel", 0)) == 2:
                    pano_id = cid
                    break
    if pano_id is None:
        logger.info("[PERSIST] cam %s: paired panorama not found", ptz_id)
        return None

    edges = _pano_edges_geo(pano_id)
    if edges is None:
        logger.info("[PERSIST] cam %s: panorama %s has no north_offset/hfov — cannot compute", ptz_id, pano_id)
        return None
    _, left_geo, right_geo = edges

    ptz_cfg = config.CAMERA_CONFIG.get(ptz_id, {})
    p1 = ptz_cfg.get("preset1"); p2 = ptz_cfg.get("preset2")
    if not (p1 and p2):
        logger.info("[PERSIST] cam %s: no preset1/preset2 — skip", ptz_id)
        return None

    b = bind.lower().replace(" ", "")
    if "preset1=right" in b:
        p1_ref, p2_ref = right_geo, left_geo
    else:
        p1_ref, p2_ref = left_geo, right_geo

    fields = {
        "preset1_ref_geo_deg": float(p1_ref),
        "preset2_ref_geo_deg": float(p2_ref),
    }
    changed = persist_geo_fields(ptz_id, fields, source="pano_edges_autobind")
    if changed:
        logger.info("[PERSIST] cam %s: wrote %s into cameras.toml", ptz_id, list(fields.keys()))
        try:
            ptz_cfg.update(fields)  # обновим рантайм
        except Exception:
            pass
    else:
        logger.info("[PERSIST] cam %s: %s already up-to-date", ptz_id, list(fields.keys()))
    return p1_ref, p2_ref