Chameleon/ptz_tracker_modular/patrol.py

from __future__ import annotations
import logging, time, asyncio
from typing import Optional
from . import config, state
from .geom import ang_diff_signed, normalize360, sector_contains, is_near
from .ptz_io import move_ptz_bounded, stop_ptz, goto_preset_token, read_ptz_azimuth_deg
from .notify import notify_detected

logger = logging.getLogger("PTZTracker")


# ===== DEBUG helpers =====

def _dbg_state(cam_idx: int, msg: str) -> None:
    """
    Унифицированный вывод текущего состояния патруля по камере.
    Важно: пишет на уровне DEBUG, чтобы не засрать лог при INFO.
    """
    st = state.ptz_states.get(cam_idx, {})
    cfg = config.CAMERA_CONFIG.get(cam_idx, {})

    logger.debug(
        "[PATROL][DBG] cam=%s %s | "
        "mode=%s act=%s paused=%s track=%s sync_wait=%s "
        "dir=%s endpoint_latch=%s "
        "leg=[%s..%s] "
        "sector=[%s..%s] "
        "az_deg=%s",
        cam_idx, msg,
        st.get("mode"),
        st.get("patrol_active"),
        st.get("patrol_paused"),
        st.get("tracking_active"),
        st.get("sync_wait"),
        st.get("patrol_dir"),
        st.get("endpoint_latch"),
        st.get("leg_start"),
        st.get("leg_end"),
        cfg.get("sector_min_deg"),
        cfg.get("sector_max_deg"),
        st.get("az_deg"),
    )


def _dbg_skip(cam_idx: int, reason: str) -> None:
    """
    Логируем «почему супервизор не трогает камеру», но с троттлингом,
    чтобы сообщение не сыпалось на каждый тик.
    """
    now = time.time()
    key = f"_patrol_dbg_skip_{cam_idx}"
    last = getattr(state, key, 0.0)
    # не чаще раза в 3 секунды
    if now - last >= 3.0:
        setattr(state, key, now)
        _dbg_state(cam_idx, f"skip: {reason}")


# ===== helpers =====

def _nearest_valid_preset(cam_idx: int) -> Optional[str]:
    cfg = config.CAMERA_CONFIG[cam_idx]
    st  = state.ptz_states[cam_idx]
    az  = st.get("az_deg")
    p1, p2 = cfg.get("preset1"), cfg.get("preset2")
    if not (p1 and p2):
        return p1 or p2
    if az is None:
        return p1

    a1, a2 = cfg.get("preset1_deg"), cfg.get("preset2_deg")
    if isinstance(a1, (int, float)) and isinstance(a2, (int, float)):
        d1 = abs(ang_diff_signed(az, a1))
        d2 = abs(ang_diff_signed(az, a2))
        return p1 if d1 <= d2 else p2
    return p1


def _in_sector(pan: float | None, cfg: dict) -> bool | None:
    """
    Обёртка над sector_contains(angle, dmin, dmax), безопасная к None.
    Возвращает:
      - True/False, если сектор задан и угол валиден;
      - None, если сектор не задан или pan некорректен.
    """
    smin = cfg.get("sector_min_deg")
    smax = cfg.get("sector_max_deg")
    if not (
        isinstance(smin, (int, float))
        and isinstance(smax, (int, float))
        and isinstance(pan, (int, float))
    ):
        return None
    return sector_contains(float(pan), float(smin), float(smax))


def _near_endpoint(pan: float | None, cfg: dict, tol: float = 1.0) -> bool:
    """
    Проверка, близко ли мы к одной из preset1_deg / preset2_deg
    с учётом возможных None и нечисловых значений.
    """
    if not isinstance(pan, (int, float)):
        return False
    a1 = cfg.get("preset1_deg")
    a2 = cfg.get("preset2_deg")
    if isinstance(a1, (int, float)) and is_near(float(pan), float(a1), tol):
        return True
    if isinstance(a2, (int, float)) and is_near(float(pan), float(a2), tol):
        return True
    return False

def return_to_preset(cam_idx: int, reason: str = "") -> None:
    st = state.ptz_states[cam_idx]
    now = time.time()
    logger.info("[RETURN] cam %s -> preset (reason=%s)", cam_idx, reason)

    # Анти-спам: если только что уже возвращали — не дёргать снова
    if (now - float(st.get("last_return_at", 0.0))) < 2.5:
        _dbg_state(cam_idx, "return_to_preset: suppressed by cooldown")
        return

    # Гасим запись/детект и переводим в IDLE
    notify_detected(cam_idx, False, force=True)
    st["mode"] = "IDLE"
    st["tracking_active"] = False
    st["patrol_paused"] = False  # важно: не блокируем future-resume

    # Сброс регуляторов и вспомогательных состояний
    st["last_dx"] = 0.0
    st["last_dy"] = 0.0
    st["ix"] = 0.0
    st["iy"] = 0.0
    st["zoom_int"] = 0.0
    st["zoom_prev_cmd"] = 0.0
    st["zoom_ramp_start"] = 0.0
    st["stable_frames"] = 0
    st["alarm_sent"] = False

    st["zoom_state"] = 0
    st["was_zoomed_in"] = False

    # Сброс логики зума/потерь
    st["zoom_lock"] = False
    st["lock_candidate_since"] = None
    st["loss_since"] = None

    # Убираем «эхо» от последней цели и защёлку у пресета
    st["last_bbox"] = None
    st["endpoint_latch"] = 0  # 0=вне пресетов, 1=P1, 2=P2

    # Останавливаем текущие команды PTZ
    stop_ptz(cam_idx)

    # Выбор ближайшего валидного пресета и установка направления следующей «ноги»
    cfg = config.CAMERA_CONFIG[cam_idx]
    p1, p2 = cfg.get("preset1"), cfg.get("preset2")
    tok = _nearest_valid_preset(cam_idx)
    if tok is not None and p1 and p2:
        st["patrol_dir"] = +1 if tok == p1 else -1

    # Важно: пока камера едет на goto, не слать continuous команды
    settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5))
    dwell  = float(getattr(config, "END_DWELL_SEC", 1.0) or 1.0)
    st["goto_in_progress_until"] = now + settle

    if tok:
        goto_preset_token(cam_idx, tok)

    # Сброс режима патруля на время возврата
    st["patrol_active"] = False
    st["sweep_active"] = False
    st["sweep_pan"] = 0.0
    st["sweep_start_at"] = 0.0
    st["leg_start"] = now
    st["leg_end"] = now + max(1.0, dwell)

    # Анти-спам метка
    st["last_return_at"] = now

    #ВАЖНО: всегда планируем возобновление патруля (и при заданном секторе тоже)
    try:
        _dbg_state(cam_idx, f"return_to_preset: schedule patrol resume after settle+dwell={settle+dwell:.2f}s")
        ensure_patrol_resume(cam_idx, delay=max(0.5, settle + dwell))
    except Exception:
        logger.exception("[PATROL] ensure_patrol_resume failed in return_to_preset")

    _dbg_state(cam_idx, f"return_to_preset done reason={reason}")

def ensure_patrol_resume(cam_idx: int, delay: float = 1.0) -> None:
    st = state.ptz_states[cam_idx]
    cfg = config.CAMERA_CONFIG[cam_idx]
    now = time.time()

    # если идёт трек или стоит пауза — не возобновляем патруль
    if st.get("tracking_active") or st.get("patrol_paused"):
        _dbg_state(cam_idx, "ensure_patrol_resume: skip (tracking_active or patrol_paused)")
        return

    # анти-спам резюмом (важно, иначе будешь вечно в goto)
    RESUME_COOLDOWN_SEC = float(getattr(config, "PATROL_RESUME_COOLDOWN_SEC", 2.0))
    last = float(st.get("patrol_resume_last_at", 0.0) or 0.0)
    if (now - last) < RESUME_COOLDOWN_SEC:
        _dbg_state(cam_idx, "ensure_patrol_resume: suppressed by resume cooldown")
        return
    st["patrol_resume_last_at"] = now

    # если только что был goto — ждём пока доедет
    gip = float(st.get("goto_in_progress_until", 0.0) or 0.0)
    if gip > now:
        delay = max(delay, gip - now)

    # --- Выбираем ближайший валидный пресет ---
    try:
        tok = _nearest_valid_preset(cam_idx)  # "1"/"2"/...
    except Exception:
        tok = None

    p1 = cfg.get("preset1")
    p2 = cfg.get("preset2")

    # --- ВАЖНО: выставляем направление патруля исходя из того, в каком пресете окажемся ---
    # Логика: если стоим на P1 -> едем к P2 (dir=+1), если стоим на P2 -> едем к P1 (dir=-1)
    if tok and p1 and p2:
        if tok == p1:
            st["patrol_dir"] = +1
            st["endpoint_latch"] = 1
        elif tok == p2:
            st["patrol_dir"] = -1
            st["endpoint_latch"] = 2

    # --- Парковка на пресет перед патрулём: ТОЛЬКО если реально не уже на нём ---
    do_goto = False
    if tok:
        az = st.get("az_deg")
        # если умеешь — используй твой _near_endpoint(az,cfg,tol)
        try:
            near = _near_endpoint(az, cfg, tol=float(getattr(config, "PATROL_PARK_TOL_DEG", 1.0)))
        except Exception:
            near = False

        # не дёргаем goto, если уже рядом с пресетом (иначе вечный goto и патруль “умирает”)
        if not near:
            do_goto = True

    if do_goto:
        stop_ptz(cam_idx)
        settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5))
        st["goto_in_progress_until"] = max(float(st.get("goto_in_progress_until", 0.0) or 0.0), now + settle)
        goto_preset_token(cam_idx, tok)
        delay = max(delay, settle + 0.5)

    # --- Планируем патруль ---
    st["patrol_active"] = True
    # чтобы bounded-тактик не скипал из-за mode
    if st.get("mode") not in ("IDLE", "SEARCH"):
        st["mode"] = "IDLE"

    st["leg_extend_total"] = 0.0  # важно для твоего bounded-продления
    st["leg_start"] = now + delay
    st["leg_end"]   = st["leg_start"] + config.PATROL_LEG_SEC

    # sweep режим только если нет сектора
    if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None:
        base = config.SWEEP_PAN_SPEED if st.get("patrol_dir", +1) > 0 else -config.SWEEP_PAN_SPEED
        st["sweep_active"]   = True
        st["sweep_pan"]      = float(cfg.get("pan_sign", 1) or 1) * base
        st["sweep_start_at"] = st["leg_start"] + config.SWEEP_SETTLE_SEC
    else:
        st["sweep_active"]   = False
        st["sweep_pan"]      = 0.0
        st["sweep_start_at"] = 0.0

    _dbg_state(cam_idx, f"ensure_patrol_resume: scheduled leg after delay={delay} tok={tok} dir={st.get('patrol_dir')}")




def patrol_start_leg(cam_idx: int, dir_sign: int) -> None:
    st  = state.ptz_states[cam_idx]

    # не стартуем «ногу», если трек или пауза патруля
    if st.get("tracking_active") or st.get("patrol_paused"):
        _dbg_state(cam_idx, "patrol_start_leg: blocked (tracking_active or patrol_paused)")
        return

    cfg = config.CAMERA_CONFIG[cam_idx]
    st["patrol_dir"]    = 1 if dir_sign >= 0 else -1
    st["patrol_active"] = True
    now = time.time()
    st["leg_start"] = now
    st["leg_end"]   = now + config.PATROL_LEG_SEC

    # НИЧЕГО НЕ ДЁРГАЕМ GOTO — патруль дальше ведёт supervisor continuous-ом
    # Просто запускаем sweep (если включен unbounded режим)
    if config.SMOOTH_SWEEP_ENABLE:
        base = config.SWEEP_PAN_SPEED if (st["patrol_dir"] > 0) else -config.SWEEP_PAN_SPEED
        st["sweep_active"]   = True
        st["sweep_pan"]      = float(cfg.get("pan_sign", 1) or 1) * base
        st["sweep_start_at"] = now + config.SWEEP_SETTLE_SEC
    else:
        st["sweep_active"]   = False
        st["sweep_pan"]      = 0.0
        st["sweep_start_at"] = 0.0

    st["shot_pause_until"] = 0.0
    st["shot_next_at"]     = now + config.SWEEP_SETTLE_SEC + 0.35

    logger.info("[PATROL] cam %s leg %s: started (no GOTO)", cam_idx, "+1" if st["patrol_dir"] > 0 else "-1")
    _dbg_state(cam_idx, "patrol_start_leg: started")



def patrol_finish_and_reverse(cam_idx: int) -> None:
    cfg = config.CAMERA_CONFIG[cam_idx]
    st  = state.ptz_states[cam_idx]
    now = time.time()
    _dbg_state(cam_idx, "patrol_finish_and_reverse: begin")

    # Останавливаем sweep-состояния (bounded/unbounded обновится ниже)
    st["sweep_active"] = False
    st["sweep_pan"]    = 0.0

    # ВАЖНО: никакого GOTO на конец. Просто меняем направление ноги и едем continuous-ом.
    st["patrol_dir"] = -st["patrol_dir"]
    st["leg_start"]  = now
    st["leg_end"]    = now + config.PATROL_LEG_SEC

    # для unbounded — включаем sweep обратно
    if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None:
        base = config.SWEEP_PAN_SPEED if (st["patrol_dir"] > 0) else -config.SWEEP_PAN_SPEED
        st["sweep_pan"]      = float(cfg.get("pan_sign", 1) or 1) * base
        st["sweep_active"]   = True
        st["sweep_start_at"] = now + float(max(0.0, config.END_DWELL_SEC))
    else:
        # bounded — sweep не нужен, bounded supervisor сам поведёт
        st["sweep_active"]   = False
        st["sweep_pan"]      = 0.0
        st["sweep_start_at"] = 0.0

    st["mode"] = "IDLE"
    notify_detected(cam_idx, False, force=True)

    _dbg_state(cam_idx, "patrol_finish_and_reverse: reversed leg (no GOTO)")



def patrol_start_leg_bounded(cam_idx: int, dir_sign: int) -> None:
    st  = state.ptz_states[cam_idx]

    if st.get("tracking_active") or st.get("patrol_paused"):
        _dbg_state(cam_idx, "patrol_start_leg_bounded: blocked (tracking_active or patrol_paused)")
        return

    cfg = config.CAMERA_CONFIG[cam_idx]
    if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None:
        _dbg_state(cam_idx, "patrol_start_leg_bounded: no sector -> fallback to unbounded")
        patrol_start_leg(cam_idx, dir_sign)
        return

    st["patrol_dir"]    = 1 if dir_sign >= 0 else -1
    st["patrol_active"] = True
    now = time.time()
    st["leg_start"] = now
    st["leg_end"]   = now + config.PATROL_LEG_SEC

    # bounded режим: движение делает patrol_supervisor_tick_bounded() (continuous),
    # поэтому тут GOTO не используем
    st["sweep_active"]   = False
    st["sweep_pan"]      = 0.0
    st["sweep_start_at"] = 0.0
    st["shot_pause_until"] = 0.0
    st["shot_next_at"]     = now + 1.0

    logger.info("[PATROL/BOUNDED] cam %s leg %s: started (no GOTO)", cam_idx, "+1" if st["patrol_dir"] > 0 else "-1")
    _dbg_state(cam_idx, "patrol_start_leg_bounded: started")

def patrol_supervisor_tick_bounded() -> None:
    now = time.time()

    dead_band_deg = 0.25
    min_step_cmd = 0.02
    az_none_fallback_sec = 2.0

    # New (configurable)
    end_tol_deg = float(getattr(config, "PATROL_END_TOL_DEG", 1.2))
    extend_step_sec = float(getattr(config, "PATROL_LEG_EXTEND_SEC", 2.0))
    extend_max_sec = float(getattr(config, "PATROL_LEG_EXTEND_MAX_SEC", 20.0))

    max_speed = float(getattr(config, "PATROL_MAX_SPEED", 0.25))
    gain_deg = float(getattr(config, "PATROL_PAN_GAIN_DEG", 45.0))
    scale = float(getattr(config, "PATROL_SPEED_SCALE", 0.70))

    for cam in config.PTZ_CAM_IDS:
        st = state.ptz_states[cam]
        cfg = config.CAMERA_CONFIG[cam]

        # While camera is doing goto — don't send continuous commands
        if now < float(st.get("goto_in_progress_until", 0.0) or 0.0):
            _dbg_skip(cam, "bounded: goto_in_progress")
            continue

        if st.get("tracking_active") or st.get("patrol_paused"):
            _dbg_skip(cam, "bounded: tracking_active or patrol_paused")
            continue

        if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None:
            _dbg_skip(cam, "bounded: no sector_min/max_deg")
            continue

        if not st.get("patrol_active"):
            _dbg_skip(cam, "bounded: patrol_active=False")
            continue

        if st.get("mode") not in ("IDLE", "SEARCH"):
            _dbg_skip(cam, f"bounded: mode={st.get('mode')} not IDLE/SEARCH")
            continue

        az1 = cfg.get("preset1_deg")
        az2 = cfg.get("preset2_deg")

        if not isinstance(az1, (int, float)) or not isinstance(az2, (int, float)):
            logger.warning(
                "[PATROL] cam %s: no preset azimuths -> fallback to smooth sweep",
                cam,
            )
            cfg["sector_min_deg"] = None
            cfg["sector_max_deg"] = None
            continue

        span = abs(ang_diff_signed(az2, az1))
        if span < 1.0:
            logger.warning(
                "[PATROL] cam %s: too small span (%.2f°) -> fallback to smooth sweep",
                cam,
                span,
            )
            cfg["sector_min_deg"] = None
            cfg["sector_max_deg"] = None
            continue

        current_az = st.get("az_deg")
        if current_az is None:
            if st.get("az_none_since") is None:
                st["az_none_since"] = now

            if now - st["az_none_since"] >= az_none_fallback_sec:
                logger.warning(
                    "[PATROL] cam %s: azimuth is None > %.1fs -> temporary unbounded sweep",
                    cam,
                    az_none_fallback_sec,
                )
                cfg["sector_min_deg"] = None
                cfg["sector_max_deg"] = None
            continue

        st["az_none_since"] = None

        # Determine the "end" azimuth for this leg
        patrol_dir = st.get("patrol_dir", +1)
        end_az = normalize360(az2 if patrol_dir > 0 else az1)

        # If leg time is over — don't reverse until we actually reached the end
        if now >= float(st.get("leg_end", 0.0) or 0.0):
            err_to_end = abs(ang_diff_signed(end_az, float(current_az)))

            if err_to_end <= end_tol_deg:
                _dbg_state(
                    cam,
                    f"bounded: leg_end reached AND near end (err={err_to_end:.2f}°) -> reverse",
                )
                patrol_finish_and_reverse(cam)
                continue

            # Extend the leg, but with a cap
            ext_total = float(st.get("leg_extend_total", 0.0) or 0.0)
            if ext_total < extend_max_sec:
                st["leg_end"] = now + extend_step_sec
                st["leg_extend_total"] = ext_total + extend_step_sec
                _dbg_state(
                    cam,
                    (
                        "bounded: leg_end but NOT at end "
                        f"(err={err_to_end:.2f}°) -> extend +{extend_step_sec}s "
                        f"(total={st['leg_extend_total']:.1f}s)"
                    ),
                )
            else:
                _dbg_state(
                    cam,
                    (
                        "bounded: extend limit reached "
                        f"(err={err_to_end:.2f}°) -> reverse anyway"
                    ),
                )
                patrol_finish_and_reverse(cam)
            continue

        # Normal trajectory: target_az by smoothstep/ease
        elapsed = max(0.0, now - st["leg_start"])
        progress = max(
            0.0,
            min(1.0, elapsed / max(1e-6, config.PATROL_LEG_SEC)),
        )
        ease = (3.0 * progress * progress) - (2.0 * progress * progress * progress)

        if patrol_dir > 0:
            target_az = az1 + ang_diff_signed(az2, az1) * ease
        else:
            target_az = az2 + ang_diff_signed(az1, az2) * ease

        target_az = normalize360(target_az)

        err_deg = ang_diff_signed(target_az, float(current_az))
        if abs(err_deg) <= dead_band_deg:
            pan_speed = 0.0
        else:
            pan_speed = scale * (err_deg / max(1e-6, gain_deg))
            pan_speed = max(-max_speed, min(max_speed, pan_speed))

            if abs(pan_speed) < min_step_cmd:
                pan_speed = 0.0

        if pan_speed != 0.0:
            move_ptz_bounded(cam, pan_speed, 0.0, 0.0)
            _dbg_state(
                cam,
                (
                    f"bounded: move pan_speed={pan_speed:.3f} "
                    f"err_deg={err_deg:.3f} target_az={target_az:.2f} end_az={end_az:.2f}"
                ),
            )


def patrol_supervisor_tick() -> None:
    now = time.time()

    for cam in config.PTZ_CAM_IDS:
        st = state.ptz_states[cam]
        cfg = config.CAMERA_CONFIG[cam]

        # While camera is doing goto — don't send continuous commands
        if now < float(st.get("goto_in_progress_until", 0.0) or 0.0):
            _dbg_skip(cam, "unbounded: goto_in_progress")
            continue

        if st.get("tracking_active") or st.get("patrol_paused"):
            _dbg_skip(cam, "unbounded: tracking_active or patrol_paused")
            continue

        # If bounded mode is active (sector is defined) — unbounded tick does nothing
        if cfg.get("sector_min_deg") is not None and cfg.get("sector_max_deg") is not None:
            continue

        if not (
            config.SMOOTH_SWEEP_ENABLE
            and st.get("patrol_active")
            and st.get("sweep_active")
        ):
            _dbg_skip(
                cam,
                (
                    "sweep not active | "
                    f"smooth={config.SMOOTH_SWEEP_ENABLE} "
                    f"patrol={st.get('patrol_active')} "
                    f"sweep={st.get('sweep_active')}"
                ),
            )
            continue

        if st.get("patrol_active") and now >= st.get("leg_end", 0.0):
            _dbg_state(cam, "unbounded: leg_end reached -> reverse")
            patrol_finish_and_reverse(cam)
            continue

        if now < st.get("sweep_start_at", 0.0):
            continue

        if st.get("mode") not in ("IDLE", "SEARCH"):
            _dbg_skip(cam, f"unbounded: mode={st.get('mode')} not IDLE/SEARCH")
            continue

        elapsed = max(0.0, now - st["leg_start"])
        remain = max(0.0, st["leg_end"] - now)

        r_up = min(1.0, elapsed / max(1e-3, config.SWEEP_RAMP_SEC))
        r_dn = min(1.0, remain / max(1e-3, config.SWEEP_RAMP_SEC))
        ramp = min(r_up, r_dn)

        pan = max(-1.0, min(1.0, st.get("sweep_pan", 0.0) * ramp))
        if abs(pan) > 1e-3:
            move_ptz_bounded(cam, pan, 0.0, 0.0)
            _dbg_state(cam, f"unbounded: move pan={pan:.3f} ramp={ramp:.3f}")


def patrol_init_on_startup() -> None:
    for cam in config.PTZ_ORDER:
        dir_sign = config.PATROL_START_DIR.get(cam, +1)
        cfg = config.CAMERA_CONFIG[cam]
        if cfg.get("sector_min_deg") is not None and cfg.get("sector_max_deg") is not None:
            patrol_start_leg_bounded(cam, dir_sign)
        else:
            patrol_start_leg(cam, dir_sign)
        start_token = cfg.get("preset1") if dir_sign >= 0 else cfg.get("preset2")
        dir_txt = "+1" if dir_sign >= 0 else "-1"
        logger.info(
            "[PATROL] cam %s leg %s: goto START %s",
            cam, dir_txt, start_token
        )
        _dbg_state(cam, f"patrol_init_on_startup: dir_sign={dir_sign}")

def patrol_resync_on_endpoint(cam: int, az: float) -> None:
    st = state.ptz_states[cam]; cfg = config.CAMERA_CONFIG[cam]

    # не трогаем ресинк, если трек или пауза патруля
    if st.get("tracking_active") or st.get("patrol_paused"):
        _dbg_state(cam, "patrol_resync_on_endpoint: skip (tracking_active or patrol_paused)")
        return

    # Инициализация защёлки по месту
    if "endpoint_latch" not in st:
        st["endpoint_latch"] = 0  # 0=вне пресетов, 1=P1, 2=P2

    if not st.get("patrol_active"):
        st["endpoint_latch"] = 0
        _dbg_state(cam, "patrol_resync_on_endpoint: patrol_active=False -> reset latch")
        return

    p1a, p2a = cfg.get("preset1_deg"), cfg.get("preset2_deg")
    if p1a is None or p2a is None:
        st["endpoint_latch"] = 0
        _dbg_state(cam, "patrol_resync_on_endpoint: no preset azimuths -> reset latch")
        return

    now = time.time()
    pan_sign = float(cfg.get("pan_sign", 1) or 1)

    def _dir_sign(a_from: float, a_to: float) -> float:
        d = ang_diff_signed(a_to, a_from)
        return 1.0 if d >= 0.0 else -1.0

    # Находимся у P1
    if is_near(az, p1a):
        if st.get("endpoint_latch") == 1:
            return  # уже защёлкнулись на P1 — не перезапускать ногу
        st["endpoint_latch"] = 1
        st["patrol_dir"] = +1
        st["leg_start"] = now
        st["leg_end"] = now + config.PATROL_LEG_SEC
        st["mode"] = "IDLE"
        if config.SMOOTH_SWEEP_ENABLE:
            base_dir = _dir_sign(p1a, p2a)
            st["sweep_active"] = True
            st["sweep_pan"] = pan_sign * config.SWEEP_PAN_SPEED * base_dir
            st["sweep_start_at"] = now + config.SWEEP_SETTLE_SEC
        else:
            st["sweep_active"] = False; st["sweep_pan"] = 0.0; st["sweep_start_at"] = 0.0
        logger.info("[PATROL][RESYNC] cam %s at P1 → leg to P2 (dir=+1)", cam)
        _dbg_state(cam, "patrol_resync_on_endpoint: latch=P1, leg to P2")
        return

    # Находимся у P2
    if is_near(az, p2a):
        if st.get("endpoint_latch") == 2:
            return  # уже защёлкнулись на P2
        st["endpoint_latch"] = 2
        st["patrol_dir"] = -1
        st["leg_start"] = now
        st["leg_end"] = now + config.PATROL_LEG_SEC
        st["mode"] = "IDLE"
        if config.SMOOTH_SWEEP_ENABLE:
            base_dir = _dir_sign(p2a, p1a)
            st["sweep_active"] = True
            st["sweep_pan"] = pan_sign * config.SWEEP_PAN_SPEED * base_dir
            st["sweep_start_at"] = now + config.SWEEP_SETTLE_SEC
        else:
            st["sweep_active"] = False; st["sweep_pan"] = 0.0; st["sweep_start_at"] = 0.0
        logger.info("[PATROL][RESYNC] cam %s at P2 → leg to P1 (dir=-1)", cam)
        _dbg_state(cam, "patrol_resync_on_endpoint: latch=P2, leg to P1")
        return

    # Ушли с пресета — снимаем защёлку
    st["endpoint_latch"] = 0
    _dbg_state(cam, "patrol_resync_on_endpoint: left presets -> latch=0")


# ===== SYNC patrol =====

def is_in_sync_group(cam: int) -> Optional[str]:
    return config.sync_group_by_cam.get(cam)


def cohort_dir_for_cam(cam: int, group_dir: int) -> int:
    sign = config.cohort_sign_by_cam.get(cam, +1)
    return +1 if sign * int(group_dir) >= 0 else -1


def _start_token_for_dir(cfg: dict, dir_sign: int) -> Optional[str]:
    p1, p2 = cfg.get("preset1"), cfg.get("preset2")
    if not (p1 and p2): return None
    return p1 if dir_sign >= 0 else p2


def sync_mark_wait(cam: int) -> None:
    st = state.ptz_states[cam]
    st["patrol_active"] = False
    st["sweep_active"]  = False
    st["sweep_pan"]     = 0.0
    st["mode"]          = "IDLE"
    st["sync_wait"]     = True
    notify_detected(cam, False, force=True)
    _dbg_state(cam, "sync_mark_wait: waiting for cohort leg")


def sync_park_to_start(cam: int, next_dir: int) -> None:
    st = state.ptz_states[cam]
    # в синхро-режиме тоже не дёргать PTZ, если трек/пауза
    if st.get("tracking_active") or st.get("patrol_paused"):
        _dbg_state(cam, "sync_park_to_start: blocked (tracking_active or patrol_paused)")
        return
    cfg = config.CAMERA_CONFIG[cam]
    tok = _start_token_for_dir(cfg, next_dir)
    stop_ptz(cam)
    if tok:
        goto_preset_token(cam, tok)
    sync_mark_wait(cam)
    _dbg_state(cam, f"sync_park_to_start: parked to start preset={tok}, next_dir={next_dir}")


def sync_activate_leg(cam: int, dir_sign: int, start_at: float, end_at: float) -> None:
    st  = state.ptz_states[cam]
    # не активировать «ногу», если трек/пауза
    if st.get("tracking_active") or st.get("patrol_paused"):
        _dbg_state(cam, "sync_activate_leg: blocked (tracking_active or patrol_paused)")
        return
    st["patrol_dir"]     = 1 if dir_sign >= 0 else -1
    st["patrol_active"]  = True
    st["leg_start"]      = start_at
    st["leg_end"]        = end_at
    st["sweep_active"]   = False
    st["sweep_pan"]      = 0.0
    st["sweep_start_at"] = 0.0
    st["sync_wait"]      = False
    _dbg_state(cam, f"sync_activate_leg: dir_sign={dir_sign}, leg=[{start_at}..{end_at}]")


async def sync_patrol_coordinator_loop():
    while True:
        t_now = time.time()
        try:
            for gname, coh in (config.SYNC_COHORTS or {}).items():
                stg = state.sync_state[gname]
                if t_now < stg.get("next_start_at", 0.0):
                    continue
                group_dir = stg["dir"]
                next_start = t_now + config.SYNC_BARRIER_SEC
                for cam in (coh.get("plus", []) + coh.get("minus", [])):
                    st = state.ptz_states[cam]
                    if st.get("mode") == "TRACK":
                        _dbg_skip(cam, f"sync: skip park, mode=TRACK (group={gname})")
                        continue
                    # не парковать, если трек/пауза
                    if st.get("tracking_active") or st.get("patrol_paused"):
                        _dbg_skip(cam, f"sync: skip park (tracking_active or patrol_paused), group={gname}")
                        continue
                    want_dir = cohort_dir_for_cam(cam, group_dir)
                    sync_park_to_start(cam, want_dir)

                await asyncio.sleep(max(0.0, next_start - time.time()))

                leg_start = time.time()
                leg_end   = leg_start + config.PATROL_LEG_SEC
                for cam in (coh.get("plus", []) + coh.get("minus", [])):
                    st = state.ptz_states[cam]
                    if st.get("mode") == "TRACK":
                        _dbg_skip(cam, f"sync: skip leg activate, mode=TRACK (group={gname})")
                        continue
                    # не активировать, если трек/пауза
                    if st.get("tracking_active") or st.get("patrol_paused"):
                        _dbg_skip(cam, f"sync: skip leg activate (tracking_active or patrol_paused), group={gname}")
                        continue
                    want_dir = cohort_dir_for_cam(cam, group_dir)
                    sync_activate_leg(cam, want_dir, leg_start, leg_end)

                dwell = max(0.0, float(config.SYNC_DWELL_SEC))
                stg["leg_idx"]      += 1
                stg["leg_start"]     = leg_start
                stg["leg_end"]       = leg_end
                stg["next_start_at"] = leg_end + dwell
                stg["dir"]           = -group_dir

            await asyncio.sleep(0.05)
        except Exception as e:
            logger.error("[SYNC] coordinator error: %s", e)
            await asyncio.sleep(0.2)

async def track_return_watchdog(cam_idx: int) -> None:
    """
    Watchdog:
    - TRACK: следит за выходом за сектор и лимитом времени трекинга
    - IDLE/SEARCH:
        * после окончания TRACK: один раз паркует в пресет (post-track park once) и возобновляет патруль
        * если патруль активен: следит за "вылетом" из сектора и плохим tilt/zoom → паркует в пресет и возобновляет
        * если патруль НЕ активен: возвращает к пресетам, если камера долго вне пресетов
    """
    import time
    from . import state, config
    from .patrol import ensure_patrol_resume
    from .sector import sector_contains  # если у тебя это в другом модуле — поправь импорт
    # _near_endpoint и return_to_preset должны быть доступны в текущем модуле/области
    # (как у тебя в версии выше)

    st = state.ptz_states[cam_idx]
    cfg = config.CAMERA_CONFIG.get(cam_idx, {})

    # ====== TRACK (во время трекинга) ======
    TRACK_OFFSECTOR_GRACE_SEC = float(getattr(config, "TRACK_OFFSECTOR_GRACE_SEC", 3.0))
    TRACK_MAX_UNINTERRUPTED_SEC = float(getattr(config, "TRACK_MAX_UNINTERRUPTED_SEC", 30.0))

    # ====== POST-TRACK (после потери цели) ======
    POST_TRACK_RETURN_SEC = float(getattr(config, "POST_TRACK_RETURN_SEC", 1.4))  # ждать после потери цели перед парковкой
    POST_TRACK_GUARD_SEC  = float(getattr(config, "POST_TRACK_GUARD_SEC", 4.0))   # сколько времени блокируем повторные "парковки" после post-track park

    # ====== IDLE/SEARCH (без трекинга) ======
    IDLE_OFFPRESET_GRACE_SEC = float(getattr(config, "IDLE_OFFPRESET_GRACE_SEC", 7.0))

    # если патруль активен и камера "вылетела" из сектора — сколько ждать до парковки
    PATROL_OFFSECTOR_RETURN_SEC = float(getattr(config, "PATROL_OFFSECTOR_RETURN_SEC", 6.0))

    # tilt-контроль в патруле
    PATROL_TILT_TOL_DEG = float(getattr(config, "PATROL_TILT_TOL_DEG", 4.0))
    PATROL_BAD_TILT_GRACE_SEC = float(getattr(config, "PATROL_BAD_TILT_GRACE_SEC", 2.5))

    # zoom-сброс в патруле (если нет детекций)
    PATROL_ZOOM_RESET_SEC = float(getattr(config, "PATROL_ZOOM_RESET_SEC", 9.0))
    PATROL_RECOVER_AFTER_PARK_SEC = float(getattr(config, "PATROL_RECOVER_AFTER_PARK_SEC", 1.0))

    # защита от спама парковкой
    PARK_COOLDOWN_SEC = float(getattr(config, "WATCHDOG_PARK_COOLDOWN_SEC", 6.0))
    # короткая пауза после парковки, чтобы статус успел обновиться (иначе можно триггерить повторно на старых данных)
    PARK_IGNORE_SEC = float(getattr(config, "WATCHDOG_PARK_IGNORE_SEC", 1.6))

    # ===== helpers =====
    def _norm_tilt_cfg(v: float) -> float:
        """
        Нормализация tilt из cameras.toml/конфига.
        Часто tilt хранится в десятых/сотых градуса, а status отдаёт градусы.
        """
        av = abs(v)
        if 900 < av <= 90000:
            return v / 100.0
        if 90 < av <= 9000:
            return v / 10.0
        return v

    def _get_tilt_deg() -> float | None:
        v = st.get("tilt_deg")
        if isinstance(v, (int, float)):
            return float(v)
        v = st.get("tilt")
        if isinstance(v, (int, float)):
            return float(v)
        return None

    def _expected_patrol_tilt_deg() -> float | None:
        """
        Возвращает ожидаемый tilt патруля в градусах или None если данных нет/мусор.
        Жёстко режем мусор вроде -138, -150 и т.п., чтобы не ломать патруль.
        """
        # 1) явный patrol_tilt_deg
        v = cfg.get("patrol_tilt_deg")
        if isinstance(v, (int, float)):
            vv = _norm_tilt_cfg(float(v))
            if abs(vv) <= 90.0:
                return vv
            return None

        # 2) fallback: tilt пресетов
        for key in ("preset1_tilt_deg", "preset2_tilt_deg"):
            v = cfg.get(key)
            if isinstance(v, (int, float)):
                vv = _norm_tilt_cfg(float(v))
                if abs(vv) <= 90.0:
                    return vv
        return None

    def _in_sector_safe(az: float | None) -> bool | None:
        smin = cfg.get("sector_min_deg")
        smax = cfg.get("sector_max_deg")
        if not (isinstance(az, (int, float)) and isinstance(smin, (int, float)) and isinstance(smax, (int, float))):
            return None
        return sector_contains(float(az), float(smin), float(smax))

    def _reset_accumulators():
        st.pop("offsector_since", None)
        st.pop("offpreset_since", None)
        st.pop("patrol_offsector_since", None)
        st.pop("patrol_bad_tilt_since", None)
        st.pop("patrol_zoom_bad_since", None)

    def _throttled_park(reason: str) -> bool:
        """
        Единая точка парковки: уважает goto_in_progress + cooldown + ignore window.
        Возвращает True если реально начали парковку.
        """
        now = time.time()

        # не мешаем выполнению goto
        if now < float(st.get("goto_in_progress_until", 0.0) or 0.0):
            return False

        # "тихий период" после предыдущей парковки — даём статусу обновиться
        if now < float(st.get("watchdog_ignore_until", 0.0) or 0.0):
            return False

        last = float(st.get("watchdog_last_park_at", 0.0) or 0.0)
        if (now - last) < PARK_COOLDOWN_SEC:
            return False

        st["watchdog_last_park_at"] = now
        st["watchdog_ignore_until"] = now + PARK_IGNORE_SEC

        # сбросить накопители, чтобы сразу после goto не сработало снова
        _reset_accumulators()

        # важное: подготовить патруль к нормальному старту после парковки
        st["mode"] = "IDLE"
        st["endpoint_latch"] = 0
        st["leg_extend_total"] = 0.0

        return_to_preset(cam_idx, reason=reason)
        ensure_patrol_resume(cam_idx, delay=PATROL_RECOVER_AFTER_PARK_SEC)
        return True

    # ===== loop =====
    while True:
        await asyncio.sleep(0.2)
        now = time.time()

        # пока едем на goto — ничего не трогаем
        if now < float(st.get("goto_in_progress_until", 0.0) or 0.0):
            _reset_accumulators()
            # post-track таймер не трогаем, но "парковать" не будем пока goto идёт
            continue

        # если в ignore window — тоже не дёргаем (статус ещё может быть старый)
        if now < float(st.get("watchdog_ignore_until", 0.0) or 0.0):
            _reset_accumulators()
            continue

        az = st.get("az_deg")
        mode = st.get("mode")

        # ---- фикс: детектируем переход TRACK -> не TRACK ----
        last_mode = st.get("last_mode")
        if last_mode == "TRACK" and mode != "TRACK":
            st["post_track_since"] = now
            st["post_track_park_done"] = False
        st["last_mode"] = mode

        # ====== TRACK ======
        if mode == "TRACK":
            if "track_start_at" not in st:
                st["track_start_at"] = now

            # во время трека не запускаем post-track паркинг
            st.pop("post_track_since", None)
            st["post_track_park_done"] = False
            st.pop("post_track_guard_until", None)

            # контроль выхода из сектора во время TRACK
            if isinstance(az, (int, float)):
                in_sector = _in_sector_safe(float(az))
                near_endpoint = _near_endpoint(az, cfg, tol=1.0)

                if in_sector is False and not near_endpoint:
                    if "offsector_since" not in st:
                        st["offsector_since"] = now
                    elif now - st["offsector_since"] > TRACK_OFFSECTOR_GRACE_SEC:
                        st.pop("offsector_since", None)
                        _throttled_park("track_offsector_timeout")
                        continue
                else:
                    st.pop("offsector_since", None)

            # лимит непрерывного трекинга
            if now - float(st.get("track_start_at", now)) > TRACK_MAX_UNINTERRUPTED_SEC:
                st.pop("track_start_at", None)
                _throttled_park("track_max_duration")
                continue

        else:
            st.pop("track_start_at", None)
            st.pop("offsector_since", None)

        # ====== IDLE / SEARCH ======
        if mode in ("IDLE", "SEARCH"):
            patrol_active = bool(st.get("patrol_active"))

            # ---- (0) Post-track park ONCE ----
            pts = st.get("post_track_since")
            guard_until = float(st.get("post_track_guard_until", 0.0) or 0.0)
            if isinstance(pts, (int, float)) and not st.get("post_track_park_done", False):
                # не даём другим правилам сразу после трека дёргать парковку много раз
                if now < guard_until:
                    _reset_accumulators()
                    continue

                if (now - float(pts)) >= POST_TRACK_RETURN_SEC:
                    started = _throttled_park("track_end_park")
                    if started:
                        st["post_track_park_done"] = True
                        st["post_track_guard_until"] = now + POST_TRACK_GUARD_SEC
                    continue

            # ---- 1) если патруль активен: самовосстановление по сектору/tilt/zoom ----
            if patrol_active:
                # (A) если камера реально вылетела из сектора — вернуть
                in_sector = _in_sector_safe(float(az)) if isinstance(az, (int, float)) else None
                near_endpoint = _near_endpoint(az, cfg, tol=1.0)

                if in_sector is False and not near_endpoint:
                    if "patrol_offsector_since" not in st:
                        st["patrol_offsector_since"] = now
                    elif now - st["patrol_offsector_since"] >= PATROL_OFFSECTOR_RETURN_SEC:
                        st.pop("patrol_offsector_since", None)
                        _throttled_park("patrol_offsector")
                        continue
                else:
                    st.pop("patrol_offsector_since", None)

                # (B) tilt “застрял” (после трека/ручного)
                exp_tilt = _expected_patrol_tilt_deg()
                cur_tilt = _get_tilt_deg()

                # если exp_tilt мусор — просто не проверяем tilt
                if (exp_tilt is not None) and (cur_tilt is not None):
                    bad_tilt = abs(float(cur_tilt) - float(exp_tilt)) > PATROL_TILT_TOL_DEG
                    if bad_tilt:
                        if "patrol_bad_tilt_since" not in st:
                            st["patrol_bad_tilt_since"] = now
                        elif now - st["patrol_bad_tilt_since"] >= PATROL_BAD_TILT_GRACE_SEC:
                            st.pop("patrol_bad_tilt_since", None)
                            _throttled_park("patrol_bad_tilt")
                            continue
                    else:
                        st.pop("patrol_bad_tilt_since", None)
                else:
                    st.pop("patrol_bad_tilt_since", None)

                # (C) zoom “застрял”: если давно нет цели, а zoom_state/lock не нулевые — парк в пресет
                last_seen = float(st.get("last_seen", 0.0) or 0.0)
                no_target_for = (now - last_seen) if last_seen > 0 else 1e9

                zoom_stuck = (
                    bool(st.get("zoom_lock", False))
                    or (abs(float(st.get("zoom_prev_cmd", 0.0) or 0.0)) > 1e-3)
                    or (int(st.get("zoom_state", 0) or 0) != 0)
                    or (abs(float(st.get("zoom_int", 0.0) or 0.0)) > 0.02)
                )

                if (no_target_for >= PATROL_ZOOM_RESET_SEC) and zoom_stuck:
                    if "patrol_zoom_bad_since" not in st:
                        st["patrol_zoom_bad_since"] = now
                    elif now - st["patrol_zoom_bad_since"] >= 0.8:
                        st.pop("patrol_zoom_bad_since", None)
                        _throttled_park("patrol_zoom_reset")
                        continue
                else:
                    st.pop("patrol_zoom_bad_since", None)

                # при активном патруле offpreset-таймер не нужен
                st.pop("offpreset_since", None)
                continue

            # ---- 2) если патруля нет: классическое “idle away from presets too long” ----
            near_preset = _near_endpoint(az, cfg, tol=1.0)
            if near_preset:
                st.pop("offpreset_since", None)
            else:
                if "offpreset_since" not in st:
                    st["offpreset_since"] = now
                elif now - st["offpreset_since"] > IDLE_OFFPRESET_GRACE_SEC:
                    st.pop("offpreset_since", None)
                    _throttled_park("idle_offpreset_timeout")
                    continue

        else:
            st.pop("offpreset_since", None)
            st.pop("patrol_offsector_since", None)
            st.pop("patrol_bad_tilt_since", None)
            st.pop("patrol_zoom_bad_since", None)