import sys
from pathlib import Path
import cv2
import numpy as np
import time
from collections import deque
import torch
from ultralytics import YOLO

import cbam_register  # noqa: F401
# Keep compatibility with checkpoints that reference __main__.CBAM.
ChannelAttentionDyn = cbam_register.ChannelAttentionDyn
SpatialAttention = cbam_register.SpatialAttention
CBAM = cbam_register.CBAM
from config import *
from helpers import *
from trackers import Kalman8D
from trackers_hybrid import HybridTracker
from trackers_safe import SafeKalman8D
from yolo_worker import YOLOWorker
from autogaze_runner import AutoGazeROIWorker
from camera_motion import (
    estimate_global_affine,
    estimate_global_affine_ex,
    apply_affine_to_point,
    affine_is_plausible,
)
from motion_saliency import MotionSaliency
from stationary_killer import StationaryKiller
from decision_logger import TrackingDecisionLogger
from guidance import ScreenGuidanceController
from template_matching import tm_update_template, tm_search
from track_score_policy import track_passes_score_gate
from target_handoff import (
    compute_fast_handoff_hits,
    evaluate_stale_lock,
    pick_guidance_override_box,
    should_override_guidance,
    update_guidance_override_latch,
)

# Allow importing ByteTrack implementation from parent TEST directory.
PARENT_TEST = Path(__file__).resolve().parent.parent
if str(PARENT_TEST) not in sys.path:
    sys.path.insert(0, str(PARENT_TEST))

try:
    from bytetrack_min_aggressive import BYTETracker
except Exception:
    from bytetrack_min_aggressive import BYTETracker


def build_unique_out_video_path(base_path: str) -> str:
    base = Path(base_path)
    suffix = base.suffix or ".mp4"
    stem = base.stem if base.suffix else base.name
    parent = base.parent if str(base.parent) not in ("", ".") else Path.cwd()
    parent.mkdir(parents=True, exist_ok=True)

    stamp = time.strftime("%Y%m%d_%H%M%S")
    candidate = parent / f"{stem}_{stamp}{suffix}"
    attempt = 1
    while candidate.exists():
        candidate = parent / f"{stem}_{stamp}_{attempt:02d}{suffix}"
        attempt += 1
    return str(candidate)


# ─── Motion saliency re-weighting ────────────────────────────
# Параметры тюнинга (подбираются по логам):
MS_BOOST_WEIGHT = 0.6     # сила буста для движущихся детектов
MS_FLOOR        = 0.12    # ниже этого ms_score — confidence штрафуется
MS_FACTOR_MIN   = 0.15    # clamp factor снизу (до -85% confidence для статики)
MS_FACTOR_MAX   = 1.80    # clamp factor сверху


def reweight_dets_by_motion(dets, motion_sal):
    """
    Корректирует confidence детектов в зависимости от motion
    saliency внутри bbox каждого детекта.

    Движущиеся цели получают буст, статичные (на горизонте,
    ЛЭП, деревьях) — штраф. Защищает от wrong target lock на
    статических объектах сцены.
    """
    if motion_sal is None or motion_sal.saliency is None or not dets:
        return dets

    out = []
    for d in dets:
        box = d[:4]
        conf = float(d[4])
        ms_score = motion_sal.score_box(box)
        delta = ms_score - float(MS_FLOOR)
        factor = 1.0 + float(MS_BOOST_WEIGHT) * delta
        factor = max(float(MS_FACTOR_MIN), min(float(MS_FACTOR_MAX), factor))
        new_conf = float(max(0.0, min(1.0, conf * factor)))
        new_d = d.copy() if isinstance(d, np.ndarray) else np.array(d, dtype=np.float32)
        new_d[4] = new_conf
        out.append(new_d)
    return out





def pick_soft_yolo_handoff_det(
    dets,
    ref_box,
    ew,
    eh,
    *,
    min_score,
    dist_diag,
    dist_min,
    iou_floor,
    max_area_ratio,
    max_aspect_ratio,
):
    """Pick a YOLO detection that is spatially consistent with the current lock.

    This is intentionally independent from ByteTrack id. It lets Kalman/KLT
    accept a fresh detector measurement when ByteTrack keeps creating new ids
    for a tiny fast target.
    """
    if ref_box is None or not dets:
        return None, 0.0

    ref = clip_box(ref_box, ew, eh)
    rc = box_center(ref)
    rw, rh = box_wh(ref)
    rdiag = max(1.0, float(np.hypot(float(rw), float(rh))))
    rarea = max(1.0, float(box_area(ref)))
    max_dist = max(float(dist_min), float(dist_diag) * rdiag)

    best_box = None
    best_conf = 0.0
    best_score = -1e9
    for det in dets:
        arr = np.asarray(det, dtype=np.float32).reshape(-1)
        if arr.size < 5:
            continue
        conf = float(arr[4])
        if conf < float(min_score):
            continue
        b = clip_box(arr[:4], ew, eh)
        if box_area(b) <= 1.0:
            continue

        cc = box_center(b)
        dist = float(np.linalg.norm(cc - rc))
        ov = float(iou(b, ref))
        if dist > max_dist and ov < float(iou_floor):
            continue

        carea = max(1.0, float(box_area(b)))
        area_ratio = max(carea / rarea, rarea / carea)
        if float(max_area_ratio) > 0.0 and area_ratio > float(max_area_ratio):
            continue

        ar_ratio = safe_ratio(box_ar(b), box_ar(ref))
        if float(max_aspect_ratio) > 0.0 and ar_ratio > float(max_aspect_ratio):
            continue

        # Prefer overlap/near-center, but keep confidence meaningful.
        score = (2.5 * ov) + (0.6 * conf) + (1.0 / (1.0 + dist)) - (0.015 * dist / rdiag)
        if score > best_score:
            best_score = score
            best_box = b
            best_conf = conf

    return best_box, best_conf


def match_fresh_track_to_box(fresh_tracks, box, ew, eh, *, min_iou, max_center_dist):
    if box is None or not fresh_tracks:
        return None
    bc = box_center(box)
    best_track = None
    best_score = -1e9
    for t in fresh_tracks:
        tb = clip_box(t.tlbr, ew, eh)
        ov = float(iou(tb, box))
        dist = float(np.linalg.norm(box_center(tb) - bc))
        if ov < float(min_iou) and dist > float(max_center_dist):
            continue
        score = (3.0 * ov) + (0.5 * float(t.score)) + (1.0 / (1.0 + dist))
        if score > best_score:
            best_score = score
            best_track = t
    return best_track


def candidate_box_is_similar(candidate_box, prev_box, ew, eh, *, dist_min, dist_diag):
    if candidate_box is None or prev_box is None:
        return False
    cand = clip_box(candidate_box, ew, eh)
    prev = clip_box(prev_box, ew, eh)
    pc = box_center(prev)
    cc = box_center(cand)
    pw, ph = box_wh(prev)
    cw, ch = box_wh(cand)
    diag = max(1.0, float(np.hypot(max(float(pw), float(cw)), max(float(ph), float(ch)))))
    limit = max(float(dist_min), float(dist_diag) * diag)
    return float(np.linalg.norm(cc - pc)) <= limit



def make_box_from_center_wh(cx, cy, bw, bh, ew, eh):
    bw = max(2.0, float(bw))
    bh = max(2.0, float(bh))
    return clip_box([float(cx) - 0.5 * bw, float(cy) - 0.5 * bh,
                     float(cx) + 0.5 * bw, float(cy) + 0.5 * bh], ew, eh)


def make_union_roi_from_boxes(boxes, ew, eh, *, margin, min_side):
    valid = []
    for b in boxes or []:
        if b is None:
            continue
        bb = clip_box(b, ew, eh)
        if box_area(bb) > 1.0:
            valid.append(bb)
    if not valid:
        return None
    arr = np.asarray(valid, dtype=np.float32)
    x1 = float(np.min(arr[:, 0])) - float(margin)
    y1 = float(np.min(arr[:, 1])) - float(margin)
    x2 = float(np.max(arr[:, 2])) + float(margin)
    y2 = float(np.max(arr[:, 3])) + float(margin)
    side = max(float(min_side), x2 - x1, y2 - y1)
    cx = 0.5 * (x1 + x2)
    cy = 0.5 * (y1 + y2)
    return clip_box([cx - 0.5 * side, cy - 0.5 * side, cx + 0.5 * side, cy + 0.5 * side], ew, eh)


def _trajectory_velocity_from_history(obs_hist, fallback_vx, fallback_vy):
    if obs_hist is None or len(obs_hist) < 2:
        return float(fallback_vx), float(fallback_vy), 0.0, 0.0
    recent = list(obs_hist)[-min(len(obs_hist), int(max(2, TRAJ_HISTORY_LOOKBACK))):]
    p0 = np.asarray(recent[0]["center"], dtype=np.float32)
    p1 = np.asarray(recent[-1]["center"], dtype=np.float32)
    t0 = float(recent[0]["ts"])
    t1 = float(recent[-1]["ts"])
    dt_hist = max(1e-3, t1 - t0)
    hv = (p1 - p0) / dt_hist
    vx = float(TRAJ_VEL_HIST_WEIGHT) * float(hv[0]) + (1.0 - float(TRAJ_VEL_HIST_WEIGHT)) * float(fallback_vx)
    vy = float(TRAJ_VEL_HIST_WEIGHT) * float(hv[1]) + (1.0 - float(TRAJ_VEL_HIST_WEIGHT)) * float(fallback_vy)
    ax = 0.0
    ay = 0.0
    if len(recent) >= 4:
        mid = len(recent) // 2
        pa = np.asarray(recent[0]["center"], dtype=np.float32)
        pb = np.asarray(recent[mid]["center"], dtype=np.float32)
        pc = np.asarray(recent[-1]["center"], dtype=np.float32)
        ta = float(recent[0]["ts"])
        tb = float(recent[mid]["ts"])
        tc = float(recent[-1]["ts"])
        v1 = (pb - pa) / max(1e-3, tb - ta)
        v2 = (pc - pb) / max(1e-3, tc - tb)
        acc = (v2 - v1) / max(1e-3, tc - ta)
        ax = float(np.clip(acc[0], -float(TRAJ_MAX_ACCEL_PX_S2), float(TRAJ_MAX_ACCEL_PX_S2)))
        ay = float(np.clip(acc[1], -float(TRAJ_MAX_ACCEL_PX_S2), float(TRAJ_MAX_ACCEL_PX_S2)))
    return vx, vy, ax, ay


def build_maneuver_hypotheses(obs_hist, ref_box, kf, miss_streak, dt, ew, eh):
    """Build short-horizon trajectory hypotheses for detector loss."""
    if ref_box is None or kf is None or (not getattr(kf, "initialized", False)):
        return []
    ref = clip_box(ref_box, ew, eh)
    if box_area(ref) <= 1.0:
        return []
    cx, cy = box_center(ref)
    bw, bh = box_wh(ref)
    vx_kf = float(kf.x[4, 0]) if getattr(kf, "x", None) is not None else 0.0
    vy_kf = float(kf.x[5, 0]) if getattr(kf, "x", None) is not None else 0.0
    vx, vy, ax, ay = _trajectory_velocity_from_history(obs_hist, vx_kf, vy_kf)
    speed = float(np.hypot(vx, vy))
    horizon = float(dt) * float(max(1, int(miss_streak) + int(TRAJ_HORIZON_MISS_OFFSET)))
    horizon = float(np.clip(horizon, float(TRAJ_HORIZON_MIN_SEC), float(TRAJ_HORIZON_MAX_SEC)))
    grow = min(1.0 + float(TRAJ_BOX_GROW_PER_MISS) * float(max(0, int(miss_streak))), float(TRAJ_BOX_GROW_MAX))
    bw2 = float(bw) * grow
    bh2 = float(bh) * grow

    hypotheses = []
    def add(label, px, py, weight):
        b = make_box_from_center_wh(px, py, bw2, bh2, ew, eh)
        hypotheses.append({"label": str(label), "box": b, "center": box_center(b), "weight": float(weight)})

    add("cv", cx + vx * horizon, cy + vy * horizon, 1.00)
    if TRAJ_USE_ACCEL:
        add("ca", cx + vx * horizon + 0.5 * ax * horizon * horizon,
            cy + vy * horizon + 0.5 * ay * horizon * horizon, 0.95)
    add("damp", cx + 0.55 * vx * horizon, cy + 0.55 * vy * horizon, 0.72)

    if speed >= float(TRAJ_MIN_SPEED_FOR_MANEUVER):
        ux = vx / max(speed, 1e-6)
        uy = vy / max(speed, 1e-6)
        px1, py1 = -uy, ux
        px2, py2 = uy, -ux
        lat = min(float(TRAJ_LATERAL_ACCEL_MAX), max(float(TRAJ_LATERAL_ACCEL_MIN), speed * float(TRAJ_LATERAL_ACCEL_SPEED_GAIN)))
        shift = 0.5 * lat * horizon * horizon
        add("turnL", cx + vx * horizon + px1 * shift, cy + vy * horizon + py1 * shift, 0.82)
        add("turnR", cx + vx * horizon + px2 * shift, cy + vy * horizon + py2 * shift, 0.82)
        vert = min(float(TRAJ_VERTICAL_ACCEL_MAX), max(float(TRAJ_VERTICAL_ACCEL_MIN), speed * float(TRAJ_VERTICAL_ACCEL_SPEED_GAIN)))
        vshift = 0.5 * vert * horizon * horizon
        add("up", cx + vx * horizon, cy + vy * horizon - vshift, 0.65)
        add("down", cx + vx * horizon, cy + vy * horizon + vshift, 0.65)

    filtered = []
    for h in hypotheses:
        hc = np.asarray(h["center"], dtype=np.float32)
        if any(float(np.linalg.norm(hc - np.asarray(old["center"], dtype=np.float32))) < float(TRAJ_DUP_CENTER_DIST) for old in filtered):
            continue
        filtered.append(h)
        if len(filtered) >= int(TRAJ_MAX_HYPOTHESES):
            break
    return filtered


def pick_det_near_trajectory_hypotheses(dets, hypotheses, ew, eh, *, min_score, dist_min, dist_diag):
    if not dets or not hypotheses:
        return None, 0.0, "-", -1e9
    best_box = None
    best_conf = 0.0
    best_label = "-"
    best_score = -1e9
    for det in dets:
        arr = np.asarray(det, dtype=np.float32).reshape(-1)
        if arr.size < 5:
            continue
        conf = float(arr[4])
        if conf < float(min_score):
            continue
        b = clip_box(arr[:4], ew, eh)
        if box_area(b) <= 1.0:
            continue
        bc = box_center(b)
        for h in hypotheses:
            hb = clip_box(h["box"], ew, eh)
            hc = np.asarray(h["center"], dtype=np.float32)
            hw, hh = box_wh(hb)
            hdiag = max(1.0, float(np.hypot(float(hw), float(hh))))
            lim = max(float(dist_min), float(dist_diag) * hdiag)
            dist = float(np.linalg.norm(bc - hc))
            ov = float(iou(b, hb))
            if dist > lim and ov < 0.01:
                continue
            score = (float(h.get("weight", 1.0)) * 0.35) + conf + 2.0 * ov + (1.0 / (1.0 + dist)) - 0.01 * (dist / hdiag)
            if score > best_score:
                best_score = score
                best_box = b
                best_conf = conf
                best_label = str(h.get("label", "traj"))
    return best_box, best_conf, best_label, best_score

def pick_yolo_reanchor_candidate(
    dets,
    *,
    ew,
    eh,
    pred_ref,
    prev_candidate_box,
    fresh_tracks,
    min_score,
    repeat_dist_min,
    repeat_dist_diag,
    pred_dist_min,
    pred_dist_diag,
    max_area_ratio,
    max_aspect_ratio,
    osd_reject,
    osd_min_score,
    trajectory_hypotheses=None,
    traj_dist_min=0.0,
    traj_dist_diag=0.0,
):
    """Pick a detector candidate for stale-KLT re-anchoring.

    This path is intentionally separate from the normal KLT-anchor guard.
    It is only allowed to adopt a far box after a short M/N persistence check,
    so a single false positive cannot pull Kalman away from a real lock.
    """
    if not dets:
        return None, 0.0, None, False, False

    pred = clip_box(pred_ref, ew, eh) if pred_ref is not None else None
    pred_c = box_center(pred) if pred is not None else None
    pred_diag = 40.0
    pred_area = 1.0
    if pred is not None:
        pw, ph = box_wh(pred)
        pred_diag = max(1.0, float(np.hypot(float(pw), float(ph))))
        pred_area = max(1.0, float(box_area(pred)))

    best_box = None
    best_conf = 0.0
    best_track = None
    best_same = False
    best_near_pred = False
    best_score = -1e9

    for det in dets:
        arr = np.asarray(det, dtype=np.float32).reshape(-1)
        if arr.size < 5:
            continue
        conf = float(arr[4])
        if conf < float(min_score):
            continue
        b = clip_box(arr[:4], ew, eh)
        if box_area(b) <= 1.0:
            continue

        c = box_center(b)
        if osd_reject and in_osd_zone(float(c[0]), float(c[1]), ew, eh) and conf < float(osd_min_score):
            continue

        if pred is not None:
            carea = max(1.0, float(box_area(b)))
            area_ratio = max(carea / pred_area, pred_area / carea)
            if float(max_area_ratio) > 0.0 and area_ratio > float(max_area_ratio):
                continue
            ar_ratio = safe_ratio(box_ar(b), box_ar(pred))
            if float(max_aspect_ratio) > 0.0 and ar_ratio > float(max_aspect_ratio):
                continue

        near_pred = False
        near_traj = False
        traj_bonus = 0.0
        dist_pred = 0.0
        if pred_c is not None:
            dist_pred = float(np.linalg.norm(c - pred_c))
            pred_limit = max(float(pred_dist_min), float(pred_dist_diag) * pred_diag)
            near_pred = bool(dist_pred <= pred_limit)

        if trajectory_hypotheses:
            best_traj_score = -1e9
            for h in trajectory_hypotheses:
                hb = clip_box(h["box"], ew, eh)
                hc = np.asarray(h["center"], dtype=np.float32)
                hw, hh = box_wh(hb)
                hdiag = max(1.0, float(np.hypot(float(hw), float(hh))))
                lim = max(float(traj_dist_min), float(traj_dist_diag) * hdiag)
                dtraj = float(np.linalg.norm(c - hc))
                ovtraj = float(iou(b, hb))
                if dtraj <= lim or ovtraj >= 0.01:
                    near_traj = True
                    best_traj_score = max(best_traj_score, (2.0 * ovtraj) + (1.0 / (1.0 + dtraj)) + 0.25 * float(h.get("weight", 1.0)))
            if near_traj:
                traj_bonus = max(0.0, best_traj_score)

        same_prev = candidate_box_is_similar(
            b,
            prev_candidate_box,
            ew,
            eh,
            dist_min=float(repeat_dist_min),
            dist_diag=float(repeat_dist_diag),
        )

        fresh_track = match_fresh_track_to_box(
            fresh_tracks,
            b,
            ew,
            eh,
            min_iou=float(SOFT_YOLO_TRACK_IOU),
            max_center_dist=max(float(SOFT_YOLO_TRACK_DIST_MIN), float(SOFT_YOLO_TRACK_DIST_DIAG) * pred_diag),
        )

        # For the first far candidate we still keep memory, but we do not adopt
        # it until it repeats. Give repeated/fresh/near-pred boxes priority.
        score = 1.0 * conf
        if same_prev:
            score += 0.80
        if fresh_track is not None:
            score += 0.35 + 0.25 * float(fresh_track.score)
        if near_pred:
            score += 0.25 + (1.0 / (1.0 + dist_pred))
        if near_traj:
            score += 0.45 + traj_bonus
        if pred_c is not None and (not near_traj):
            score -= 0.0025 * dist_pred

        if score > best_score:
            best_score = score
            best_box = b
            best_conf = conf
            best_track = fresh_track
            best_same = bool(same_prev)
            best_near_pred = bool(near_pred or near_traj)

    return best_box, best_conf, best_track, best_same, best_near_pred



def klt_anchor_accepts_box(
    candidate_box,
    anchor_box,
    ew,
    eh,
    *,
    dist_diag,
    dist_min,
    iou_floor,
    max_area_ratio,
    max_aspect_ratio,
):
    """Reject one-frame ByteTrack/YOLO jumps when KLT still has a strong anchor."""
    if candidate_box is None or anchor_box is None:
        return True, "no_anchor"

    cand = clip_box(candidate_box, ew, eh)
    anch = clip_box(anchor_box, ew, eh)
    if box_area(cand) <= 1.0 or box_area(anch) <= 1.0:
        return False, "empty_box"

    ac = box_center(anch)
    cc = box_center(cand)
    aw, ah = box_wh(anch)
    adiag = max(1.0, float(np.hypot(float(aw), float(ah))))
    max_dist = max(float(dist_min), float(dist_diag) * adiag)
    dist = float(np.linalg.norm(cc - ac))
    ov = float(iou(cand, anch))

    if dist > max_dist and ov < float(iou_floor):
        return False, f"far_from_klt:dist={dist:.1f}>lim={max_dist:.1f},iou={ov:.3f}"

    aarea = max(1.0, float(box_area(anch)))
    carea = max(1.0, float(box_area(cand)))
    area_ratio = max(carea / aarea, aarea / carea)
    if float(max_area_ratio) > 0.0 and area_ratio > float(max_area_ratio):
        return False, f"area_jump:{area_ratio:.1f}"

    ar_ratio = safe_ratio(box_ar(cand), box_ar(anch))
    if float(max_aspect_ratio) > 0.0 and ar_ratio > float(max_aspect_ratio):
        return False, f"aspect_jump:{ar_ratio:.1f}"

    return True, "ok"

def main():
    print("Loading YOLO...")
    model = YOLO(MODEL_PATH)

    if torch.cuda.is_available():
        model.to(f"cuda:{DEVICE}")

    if torch.cuda.is_available():
        # Warmup на обоих размерах — критично для dynamic CBAM
        # чтобы MLP построились на обоих scales до реального инференса.
        dummy_roi = np.zeros((IMG_SIZE_ROI, IMG_SIZE_ROI, 3), dtype=np.uint8)
        dummy_full = np.zeros((IMG_SIZE_FULL, IMG_SIZE_FULL, 3), dtype=np.uint8)
        with torch.inference_mode():
            for _ in range(3):
                _ = model(
                    dummy_roi,
                    conf=YOLO_CONF_EFFECTIVE,
                    imgsz=IMG_SIZE_ROI,
                    verbose=False,
                    max_det=MAX_DET,
                    device=DEVICE,
                    half=USE_HALF
                )
            for _ in range(2):
                _ = model(
                    dummy_full,
                    conf=YOLO_CONF_EFFECTIVE,
                    imgsz=IMG_SIZE_FULL,
                    verbose=False,
                    max_det=MAX_DET,
                    device=DEVICE,
                    half=USE_HALF
                )
        print(f"Model warmed up at {IMG_SIZE_ROI} and {IMG_SIZE_FULL}")

    cap, source_kind = open_source(SOURCE, CAP_BACKEND)
    if not cap.isOpened():
        print(f"Capture open failed: {SOURCE}")
        return

    try:
        cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
    except Exception:
        pass

    print(f"Opened source: {SOURCE} ({source_kind})")

    input_fps = float(cap.get(cv2.CAP_PROP_FPS))
    if (not np.isfinite(input_fps)) or (input_fps <= 1.0):
        input_fps = 0.0

    if TARGET_OUT_FPS > 0:
        target_out_fps = float(TARGET_OUT_FPS)
    elif source_kind == "file" and input_fps > 0.0:
        target_out_fps = input_fps
    else:
        target_out_fps = 0.0

    if VIDEO_REALTIME and target_out_fps > 0.0:
        print(f"Pacing output at ~{target_out_fps:.2f} FPS")
    else:
        print("Pacing disabled (show as fast as processing allows)")

    if SHOW_OUTPUT:
        cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL)

    writer = None
    if SAVE_INFER_VIDEO:
        ow = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
        oh = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
        fpsw = float(cap.get(cv2.CAP_PROP_FPS))
        if (not np.isfinite(fpsw)) or fpsw <= 1.0:
            fpsw = target_out_fps if target_out_fps > 0 else 30.0
        fourcc = cv2.VideoWriter_fourcc(*"mp4v")
        out_video_path = build_unique_out_video_path(OUT_VIDEO_PATH)
        writer = cv2.VideoWriter(out_video_path, fourcc, fpsw, (ow, oh))
        if not writer.isOpened():
            print(f"WARN: video writer open failed: {out_video_path}")
            writer = None
        else:
            print(f"Saving inference video to: {out_video_path}")

    yolo_worker = YOLOWorker(model)
    yolo_worker.start()
    autogaze_worker = AutoGazeROIWorker()
    autogaze_worker.start()
    print(autogaze_worker.status_line())
    track_logger = TrackingDecisionLogger(
        TRACK_LOG_ENABLE,
        TRACK_LOG_PATH,
        TRACK_SUMMARY_PATH,
        flush_every=TRACK_LOG_FLUSH_EVERY,
    )
    print(track_logger.status_line())
    guidance_ctrl = ScreenGuidanceController()
    print(guidance_ctrl.status_line())

    kf = SafeKalman8D()
    klt = HybridTracker()

    bt = BYTETracker(
        track_high_thresh=BT_HIGH,
        track_low_thresh=BT_LOW,
        new_track_thresh=BT_NEW,
        match_thresh=BT_MATCH_IOU,
        track_buffer=BT_BUFFER,
        min_hits=BT_MIN_HITS,
    )
    target_id = None
    target_absent_frames = 0
    switch_candidate_id = None
    switch_candidate_hits = 0

    locked_box_eff = None
    confirmed = False
    hit_streak = 0
    miss_streak = 0
    acquire_score = 0
    acquire_miss = 0
    ref_hist = None

    nominal_dt = 1.0 / float(input_fps if input_fps > 1.0 else INPUT_FPS_FALLBACK)
    prev_frame_ts = None
    frame_id = 0
    last_det_frame = -999
    last_fullscan_frame = -999
    last_klt_init_frame = -999

    yolo_no_det = 0
    last_yolo_ok_ts = -1e9

    perf_hist = deque(maxlen=120)
    yolo_hist = deque(maxlen=120)

    traj = deque(maxlen=max(10, int(TRAIL_SECONDS * (input_fps if input_fps > 0 else 60.0))))
    traj_frame_i = 0
    guidance_override_memory_box_eff = None
    guidance_override_memory_ttl = 0

    prev_gray_global = None
    template_gray = None

    # --- Motion saliency ---
    motion_sal = MotionSaliency()
    print(motion_sal.status_line())
    ms_outliers = np.zeros((0, 2), dtype=np.float32)

    # --- Stationary clutter killer ---
    killer = StationaryKiller()
    print(killer.status_line())
    motion_roi_eff = None
    motion_active_zones = 0
    motion_mask = None
    flash_roi_eff = None
    flash_ttl = 0
    wavelet_roi_eff = None
    wavelet_roi_ttl = 0
    wavelet_roi_peak = 0.0
    wavelet_track_hist = {}
    wavelet_track_last_seen = {}
    track_center_hist = {}
    track_center_last_seen = {}
    preacq_hist = deque(maxlen=max(1, int(PREACQ_WINDOW)))
    preacq_hits = 0
    autogaze_cooldown = 0
    wavelet_cooldown = 0
    adaptive_chase_stage = "far"

    # M/N memory for safe YOLO re-anchor when KLT/Kalman are stale.
    yolo_reanchor_memory_box_eff = None
    yolo_reanchor_memory_hits = 0
    yolo_reanchor_memory_last_frame = -999
    yolo_reanchor_last_reason = ""

    # Short history of accepted target centers for multi-hypothesis prediction.
    trajectory_obs_hist = deque(maxlen=max(6, int(TRAJ_HISTORY_LEN)))

    print("Start")

    while True:
        ret, frame_orig = cap.read()
        loop_ts = time.perf_counter()
        if not ret:
            break

        frame_eff, sx, sy = get_effective_frame(frame_orig)
        eh, ew = frame_eff.shape[:2]
        frame_ts, dt_source = get_frame_timestamp_seconds(cap, source_kind, frame_id, input_fps, loop_ts)
        dt = sanitize_dt(frame_ts, prev_frame_ts, nominal_dt)
        prev_frame_ts = frame_ts

        if autogaze_cooldown > 0:
            autogaze_cooldown -= 1
        if wavelet_cooldown > 0:
            wavelet_cooldown -= 1

        if autogaze_cooldown <= 0:
            autogaze_worker.submit(frame_eff, frame_id)
        autogaze_roi_eff = None
        autogaze_rois_eff = []
        autogaze_info = None
        gray_now = cv2.cvtColor(frame_eff, cv2.COLOR_BGR2GRAY)
        target_track = None
        pred_box_eff = None
        best_score = None
        have_yolo = False
        yolo_mode = "IDLE"
        yolo_raw_count = 0
        merged_part_count = 0
        infer_ms = 0.0
        klt_valid = False
        speed = 0.0
        det_every = 0
        approach_active = False
        close_force_fullscan = False
        fast_maneuver_guard = False
        wavelet_active = False
        best_wavelet_energy = 0.0
        best_wavelet_bonus = 0.0
        best_wavelet_hits = 0
        best_dist = 0.0
        best_residual_ok = False
        guidance_candidate_box_eff = None
        guidance_candidate_track_id = None
        guidance_raw_yolo_box_eff = None
        guidance_override_box_eff = None
        used_prediction_hold = False
        stale_lock_active = False
        stale_lock_reason = ""
        fast_handoff_active = False
        guidance_reset_event = ""
        guidance_force_neutral = False
        soft_yolo_box_eff = None
        soft_yolo_score = 0.0
        soft_yolo_track = None
        soft_yolo_adopted = False
        yolo_reanchor_box_eff = None
        yolo_reanchor_score = 0.0
        yolo_reanchor_track = None
        yolo_reanchor_adopted = False
        yolo_reanchor_near_pred = False
        yolo_reanchor_reason = ""
        trajectory_hypotheses = []
        trajectory_roi_eff = None
        trajectory_primary_box_eff = None
        trajectory_det_label = "-"
        trajectory_reanchor_used = False
        A = None
        ms_outliers = np.zeros((0, 2), dtype=np.float32)
        if prev_gray_global is not None and USE_CAM_MOTION_COMP:
            A, _ms_inliers, ms_outliers = estimate_global_affine_ex(
                prev_gray_global, gray_now
            )
            if A is not None and (not affine_is_plausible(A)):
                A = None
                ms_outliers = np.zeros((0, 2), dtype=np.float32)
            if A is not None and kf.initialized:
                cx0 = float(kf.x[0, 0])
                cy0 = float(kf.x[1, 0])
                ncx, ncy = apply_affine_to_point(A, cx0, cy0)
                kf.x[0, 0] = ncx
                kf.x[1, 0] = ncy
                if locked_box_eff is not None:
                    x1, y1, x2, y2 = locked_box_eff
                    p1x, p1y = apply_affine_to_point(A, float(x1), float(y1))
                    p2x, p2y = apply_affine_to_point(A, float(x2), float(y2))
                    locked_box_eff[:] = np.array([p1x, p1y, p2x, p2y], dtype=np.float32)
                    locked_box_eff[:] = clip_box(locked_box_eff, ew, eh)

        motion_roi_eff = None
        motion_active_zones = 0
        motion_mask = None
        if flash_ttl > 0:
            flash_ttl -= 1
        else:
            flash_roi_eff = None
        if wavelet_roi_ttl > 0:
            wavelet_roi_ttl -= 1
        else:
            wavelet_roi_eff = None
            wavelet_roi_peak = 0.0
        if MOTION_ZONE_ENABLE and prev_gray_global is not None:
            motion_mask = build_motion_mask(
                prev_gray_global,
                gray_now,
                affine=A if USE_CAM_MOTION_COMP else None
            )
            motion_roi_eff, motion_active_zones = motion_zones_to_roi(motion_mask, ew, eh)

        # --- Update motion saliency map ---
        if prev_gray_global is not None:
            motion_sal.update(
                gray_now,
                prev_gray_global,
                A,
                kind="affine" if A is not None else "none",
                outlier_pts=ms_outliers,
            )

        prev_gray_global = gray_now

        iter_start = time.perf_counter()

        pred_box_eff = None
        if kf.initialized:
            kf_q_scale = 1.0
            if KALMAN_Q_DT_BOOST_ENABLE and dt > float(KALMAN_Q_DT_BOOST_START_SEC):
                kf_q_scale += float(KALMAN_Q_DT_BOOST_SLOPE) * (dt - float(KALMAN_Q_DT_BOOST_START_SEC))
                kf_q_scale = min(kf_q_scale, float(KALMAN_Q_DT_BOOST_MAX))
            kf.predict(dt, q_scale=kf_q_scale)
            vmax = float(KALMAN_CLAMP_V_PX_S)
            kf.x[4, 0] = float(np.clip(kf.x[4, 0], -vmax, vmax))
            kf.x[5, 0] = float(np.clip(kf.x[5, 0], -vmax, vmax))
            pred_box_eff = clip_box(kf.to_box(), ew, eh)

        unc = 0.0
        if TURN_SAFE_ENABLE and pred_box_eff is not None:
            unc = kf.uncertainty()

        speed = 0.0
        if kf.initialized:
            vx = float(kf.x[4, 0])
            vy = float(kf.x[5, 0])
            speed = (vx * vx + vy * vy) ** 0.5
            unc = kf.uncertainty()

        pred_area_eff = 0.0
        if pred_box_eff is not None:
            pred_area_eff = box_area(pred_box_eff)
        elif locked_box_eff is not None:
            pred_area_eff = box_area(locked_box_eff)
        if ADAPTIVE_CHASE_ENABLE:
            hyst = float(max(0.0, ADAPTIVE_STAGE_AREA_HYST))
            mid_on = float(ADAPTIVE_STAGE_MID_AREA)
            close_on = float(APPROACH_CLOSE_AREA)
            if adaptive_chase_stage == "close":
                if pred_area_eff < (close_on - hyst):
                    adaptive_chase_stage = "mid" if pred_area_eff >= (mid_on - hyst) else "far"
            elif adaptive_chase_stage == "mid":
                if pred_area_eff >= (close_on + hyst):
                    adaptive_chase_stage = "close"
                elif pred_area_eff < (mid_on - hyst):
                    adaptive_chase_stage = "far"
            else:
                if pred_area_eff >= (close_on + hyst):
                    adaptive_chase_stage = "close"
                elif pred_area_eff >= (mid_on + hyst):
                    adaptive_chase_stage = "mid"
        else:
            adaptive_chase_stage = "close" if (APPROACH_MODE_ENABLE and (pred_area_eff >= float(APPROACH_FORCE_DET_AREA))) else "far"

        approach_active = APPROACH_MODE_ENABLE and (adaptive_chase_stage in ("mid", "close"))
        approach_force_det_every = None
        approach_kf_roi_scale = 1.0
        approach_max_area_ratio = float(TURN_MAX_AREA_RATIO)
        approach_near_dist_diag = float(APPROACH_NEAR_DIST_DIAG)
        approach_hsv_min_scale = 1.0
        approach_score_weight = 0.0
        approach_score_clip = 0.0
        approach_switch_extra_miss = 0
        approach_switch_extra_hits = 0
        if approach_active:
            if adaptive_chase_stage == "mid":
                approach_force_det_every = int(max(1, APPROACH_MID_FORCE_DET_EVERY))
                approach_kf_roi_scale = float(APPROACH_MID_KF_ROI_SCALE)
                approach_max_area_ratio = float(APPROACH_MID_MAX_AREA_RATIO)
                approach_near_dist_diag = float(APPROACH_MID_NEAR_DIST_DIAG)
                approach_hsv_min_scale = float(APPROACH_MID_HSV_MIN_SCALE)
                approach_score_weight = float(APPROACH_MID_SCORE_WEIGHT)
                approach_score_clip = float(APPROACH_MID_SCORE_CLIP)
                approach_switch_extra_miss = int(max(0, APPROACH_MID_SWITCH_EXTRA_MISS))
                approach_switch_extra_hits = int(max(0, APPROACH_MID_SWITCH_EXTRA_HITS))
            else:
                approach_force_det_every = int(max(1, APPROACH_FORCE_DET_EVERY))
                approach_kf_roi_scale = float(APPROACH_KF_ROI_SCALE)
                approach_max_area_ratio = float(APPROACH_MAX_AREA_RATIO)
                approach_near_dist_diag = float(APPROACH_NEAR_DIST_DIAG)
                approach_hsv_min_scale = float(APPROACH_HSV_MIN_SCALE)
                approach_score_weight = float(APPROACH_SCORE_WEIGHT)
                approach_score_clip = float(APPROACH_SCORE_CLIP)
                approach_switch_extra_miss = int(max(0, APPROACH_CLOSE_SWITCH_EXTRA_MISS))
                approach_switch_extra_hits = int(max(0, APPROACH_CLOSE_SWITCH_EXTRA_HITS))

        klt_valid = False
        if confirmed and locked_box_eff is not None:
            if klt.prev_gray is None or klt.pts is None or len(klt.pts) < KLT_REINIT_MIN_POINTS:
                klt.init(frame_eff, locked_box_eff)
                last_klt_init_frame = frame_id

            klt_box = klt.update(frame_eff)
            if klt_box is not None:
                klt_box = clip_box(klt_box, ew, eh)
                klt_valid = (klt.quality >= KLT_OK_Q) and (klt.good_count >= KLT_OK_PTS)
                if klt_valid:
                    cx, cy = box_center(klt_box)
                    bw, bh = box_wh(klt_box)
                    kf.update([cx, cy, bw, bh])
                    locked_box_eff = klt_box

        if (
            TRAJ_PREDICT_ENABLE
            and confirmed
            and kf.initialized
            and (miss_streak >= int(TRAJ_PREDICT_MISS_GE))
        ):
            traj_ref_box = pred_box_eff if pred_box_eff is not None else locked_box_eff
            trajectory_hypotheses = build_maneuver_hypotheses(
                trajectory_obs_hist, traj_ref_box, kf, miss_streak, dt, ew, eh
            )
            if trajectory_hypotheses:
                trajectory_primary_box_eff = trajectory_hypotheses[0]["box"]
                trajectory_roi_eff = make_union_roi_from_boxes(
                    [h["box"] for h in trajectory_hypotheses], ew, eh,
                    margin=TRAJ_ROI_MARGIN, min_side=TRAJ_ROI_MIN_SIDE
                )

        wavelet_roi_allowed = (
            (wavelet_cooldown <= 0)
            and WAVELET_ROI_ENABLE and (
                ((not confirmed) and (miss_streak >= int(WAVELET_ROI_MISS_GE)))
                or (confirmed and (miss_streak >= int(WAVELET_ROI_CONF_MISS_GE)))
            )
        )
        if wavelet_roi_allowed:
            wv_every = int(max(1, WAVELET_ROI_EVERY_N))
            compute_wavelet_now = (wavelet_roi_ttl <= 0) or ((frame_id % wv_every) == 0)
            if compute_wavelet_now:
                wv_t0 = time.perf_counter()
                wave_pred_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff
                wroi, wpeak = wavelet_hot_roi(
                    gray_now,
                    ew,
                    eh,
                    motion_mask=motion_mask,
                    pred_ref=wave_pred_ref,
                    margin=WAVELET_ROI_MARGIN,
                    min_side=WAVELET_ROI_MIN_SIDE,
                    pred_scale=WAVELET_ROI_PRED_SCALE,
                    min_peak_ratio=WAVELET_ROI_MIN_PEAK_RATIO,
                )
                wv_ms = (time.perf_counter() - wv_t0) * 1000.0
                if wv_ms > float(MODULE_TIME_BUDGET_MS):
                    wavelet_cooldown = max(int(wavelet_cooldown), int(WAVELET_COOLDOWN_FRAMES))
                if wroi is not None:
                    wavelet_roi_eff = wroi
                    wavelet_roi_peak = float(wpeak)
                    wavelet_roi_ttl = int(max(int(wavelet_roi_ttl), int(max(1, WAVELET_ROI_TTL))))

        base = 6 if confirmed else RECOVER_FORCED_DET_EVERY
        fast_bonus = int(clamp(speed / 28.0, 0, 4))
        unc_bonus = int(clamp(unc / 160.0, 0, 4))
        miss_bonus = int(clamp(miss_streak, 0, 3))
        klt_bonus = 2 if (confirmed and not klt_valid) else 0
        det_every = clamp(base - (fast_bonus + unc_bonus + miss_bonus + klt_bonus), 1, 10)
        if confirmed and approach_active and (approach_force_det_every is not None):
            det_every = min(int(det_every), int(approach_force_det_every))

        yolo_fresh = (frame_ts - last_yolo_ok_ts) <= YOLO_FRESH_SEC
        if YOLO_FORCE_DET_WHEN_WEAK and confirmed and (not klt_valid) and (not yolo_fresh):
            det_every = 1

        run_det = (frame_id - last_det_frame) >= det_every
        need_fullscan = (not confirmed) and (frame_id - last_fullscan_frame) >= RECOVER_FULLSCAN_EVERY

        # Close-stage detector policy:
        # older builds forced FULL-CLOSE on every detector pass while ch=close.
        # That is safe but noisy: ByteTrack sees every false positive on the screen,
        # even when KLT/Kalman are locked on the real target. Now close mode uses
        # ROI-KF/ROI-TRAJ most of the time and falls back to FULL-CLOSE only when
        # the lock is weak or on a periodic health-check.
        close_periodic_due = bool(
            CLOSE_PERIODIC_FULLSCAN_ENABLE
            and confirmed
            and (adaptive_chase_stage == "close")
            and ((frame_id - last_fullscan_frame) >= int(CLOSE_PERIODIC_FULLSCAN_EVERY))
        )
        close_force_fullscan = bool(
            confirmed
            and CLOSE_FORCE_FULLSCAN
            and (adaptive_chase_stage == "close")
            and (
                (miss_streak >= int(CLOSE_FULLSCAN_MISS_GE))
                or (CLOSE_FULLSCAN_WHEN_KLT_INVALID and (not klt_valid))
                or (target_absent_frames >= int(CLOSE_FULLSCAN_TARGET_ABSENT_GE))
                or close_periodic_due
            )
        )

        if TURN_SAFE_ENABLE and TURN_SAFE_FORCE_FULLSCAN and confirmed and (miss_streak >= FULLSCAN_WHEN_MISS_GE):
            need_fullscan = True
        if (
            confirmed
            and (miss_streak >= int(MOTION_CONF_MISS_GE))
            and ((frame_id - last_fullscan_frame) >= int(CONF_MISS_FORCE_FULLSCAN_EVERY))
        ):
            need_fullscan = True
        if (not confirmed) and (miss_streak >= int(RECOVER_FORCE_FULLSCAN_MISS_GE)):
            need_fullscan = True
        if close_force_fullscan:
            need_fullscan = True

        autogaze_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff
        autogaze_roi_eff, autogaze_rois_eff, autogaze_info = autogaze_worker.get_latest(
            frame_id,
            ew,
            eh,
            pred_ref_box=autogaze_ref,
        )
        autogaze_roi_allowed = (
            (autogaze_cooldown <= 0)
            and autogaze_roi_eff is not None
            and (
                ((not confirmed) and (miss_streak >= int(AUTOGAZE_MISS_GE)))
                or (confirmed and (miss_streak >= int(AUTOGAZE_CONF_MISS_GE)))
            )
        )
        if (
            autogaze_info is not None
            and int(autogaze_info.get("age", -1)) == 0
            and float(autogaze_info.get("infer_ms", 0.0)) > float(MODULE_TIME_BUDGET_MS)
        ):
            autogaze_cooldown = max(int(autogaze_cooldown), int(AUTOGAZE_COOLDOWN_FRAMES))
            autogaze_roi_allowed = False

        if run_det:
            roi_box = None
            mode = "FULL"
            force_motion_first = (not confirmed) and (miss_streak >= int(RECOVER_FORCE_MOTION_FIRST_MISS_GE))
            allow_kf_roi = confirmed or (miss_streak < int(RECOVER_ROI_KF_DISABLE_MISS_GE))
            prefer_motion_confirmed = (
                MOTION_CONF_ENABLE
                and confirmed
                and (miss_streak >= int(MOTION_CONF_MISS_GE))
                and (motion_roi_eff is not None)
                and ((not MOTION_CONF_KLT_INVALID_ONLY) or (not klt_valid))
            )
            if (
                (not need_fullscan)
                and prefer_motion_confirmed
            ):
                roi_box = motion_roi_eff.copy()
                mode = "ROI-MOTION-C"
            elif (
                (not need_fullscan)
                and FLASH_ROI_ENABLE
                and (flash_ttl > 0)
                and (flash_roi_eff is not None)
                and ((not confirmed) or (target_id is None) or (not klt_valid) or (miss_streak >= 1))
            ):
                roi_box = clip_box(flash_roi_eff, ew, eh)
                mode = "ROI-FLASH"
            elif (
                (not need_fullscan)
                and autogaze_roi_allowed
            ):
                roi_box = clip_box(autogaze_roi_eff, ew, eh)
                mode = "ROI-GAZE"
            elif (
                (not need_fullscan)
                and wavelet_roi_allowed
                and WAVELET_ROI_ENABLE
                and (wavelet_roi_ttl > 0)
                and (wavelet_roi_eff is not None)
            ):
                roi_box = clip_box(wavelet_roi_eff, ew, eh)
                mode = "ROI-WAVE"
            elif (not need_fullscan) and force_motion_first and MOTION_ZONE_ENABLE and (motion_roi_eff is not None):
                roi_box = motion_roi_eff.copy()
                mode = "ROI-MOTION"
            elif (
                TRAJ_ROI_ENABLE
                and (not need_fullscan)
                and confirmed
                and (miss_streak >= int(TRAJ_ROI_MISS_GE))
                and (trajectory_roi_eff is not None)
            ):
                roi_box = clip_box(trajectory_roi_eff, ew, eh)
                mode = "ROI-TRAJ"
            elif (not need_fullscan) and kf.initialized and allow_kf_roi:
                cx = float(kf.x[0, 0])
                cy = float(kf.x[1, 0])
                vx = float(kf.x[4, 0])
                vy = float(kf.x[5, 0])
                vnorm = (vx * vx + vy * vy) ** 0.5 + 1e-6
                ux, uy = vx / vnorm, vy / vnorm

                # Адаптивный ROI: для мелких целей сохраняем
                # запас на ошибку Kalman + контекст для YOLO.
                # Для bbox 15×10 ROI ≈ 280px (после resize 640
                # объект занимает ~15% — оптимум для YOLO recall).
                # Для bbox 30×20 ROI ≈ 550px (почти не сужается).
                box_diag = 0.0
                if locked_box_eff is not None:
                    bw = float(locked_box_eff[2] - locked_box_eff[0])
                    bh = float(locked_box_eff[3] - locked_box_eff[1])
                    box_diag = (bw * bw + bh * bh) ** 0.5
                if box_diag >= 8.0:
                    target_roi_side = box_diag / 0.065
                    target_roi_side = max(260.0, min(520.0, target_roi_side))
                else:
                    target_roi_side = float(BASE_RADIUS)

                radius_speed = BASE_RADIUS + SPEED_RADIUS_FACTOR * vnorm
                radius_speed = clamp(radius_speed, BASE_RADIUS, MAX_RADIUS)
                # Берём максимум — даём запас на смещение Kalman
                radius = max(radius_speed, target_roi_side * 0.5)
                radius = min(radius, MAX_RADIUS)
                radius = max(radius, BASE_RADIUS)

                fx = ux * radius * (FORWARD_BIAS - 1.0)
                fy = uy * radius * (FORWARD_BIAS - 1.0)

                scx = cx + fx * 0.35
                scy = cy + fy * 0.35
                rx = radius * FORWARD_BIAS
                ry = radius * SIDE_BIAS
                if approach_active:
                    rx *= float(approach_kf_roi_scale)
                    ry *= float(approach_kf_roi_scale)

                roi_box = clip_box([scx - rx, scy - ry, scx + rx, scy + ry], ew, eh)
                mode = "ROI-KF"
            elif (not need_fullscan) and (not confirmed) and MOTION_ZONE_ENABLE and (motion_roi_eff is not None):
                roi_box = motion_roi_eff.copy()
                mode = "ROI-MOTION"

            if need_fullscan:
                roi_box = None
                mode = "FULL-CLOSE" if close_force_fullscan else "FULL"
                last_fullscan_frame = frame_id

            yolo_worker.submit(frame_eff, roi_box, mode, frame_ts)
            last_det_frame = frame_id

        yolo = yolo_worker.try_get()
        dets_eff = []
        infer_ms = 0.0
        used_roi = False
        have_yolo = False
        yolo_mode = "NONE"
        yolo_raw_count = 0
        yolo_ts = None
        merged_part_count = 0
        if yolo is not None:
            have_yolo = True
            dets_eff, yolo_ts, yolo_mode, infer_ms, used_roi = yolo
            # Motion-aware re-weighting: буст движущимся детектам,
            # штраф статичным (горизонт, ЛЭП, деревья). Защита
            # от wrong target lock на статических объектах.
            dets_eff = reweight_dets_by_motion(dets_eff, motion_sal)
            yolo_raw_count = len(dets_eff)
            if RECOVER_FILTER_ENABLE and (not confirmed) and (yolo_mode in ("ROI-MOTION", "ROI-WAVE", "ROI-MOTION-C", "ROI-GAZE")) and yolo_raw_count > 0:
                raw_dets = dets_eff
                filtered = [d for d in raw_dets if recover_det_is_valid(d, motion_mask)]
                if len(filtered) > 0:
                    dets_eff = filtered
                else:
                    raw_sorted = sorted(raw_dets, key=lambda dd: float(dd[4]), reverse=True)
                    dets_eff = raw_sorted[:min(3, len(raw_sorted))]
            merge_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff
            if confirmed and (merge_ref is not None) and len(dets_eff) >= int(max(2, PART_MERGE_MIN_PARTS)):
                dets_eff, merged_part_count = merge_close_part_dets(dets_eff, merge_ref, ew, eh)
            if FLASH_ROI_ENABLE and len(dets_eff) > 0:
                best_d = max(dets_eff, key=lambda dd: float(dd[4]))
                if float(best_d[4]) >= float(FLASH_ROI_MIN_SCORE):
                    flash_roi_eff = make_focus_roi_from_box(
                        best_d[:4],
                        ew,
                        eh,
                        margin=FLASH_ROI_MARGIN,
                        min_side=FLASH_ROI_MIN_SIDE,
                    )
                    flash_ttl = int(max(1, FLASH_ROI_TTL))
            yolo_hist.append(infer_ms)

            if len(dets_eff) == 0:
                yolo_no_det += 1
            else:
                yolo_no_det = 0
                if yolo_ts is not None:
                    last_yolo_ok_ts = float(yolo_ts)
                else:
                    last_yolo_ok_ts = frame_ts

        preacq_det = None
        if PREACQ_ENABLE and (not confirmed):
            pre_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff
            if have_yolo:
                preacq_det = pick_preacq_det(dets_eff, pre_ref, ew, eh)
                preacq_hist.append(1 if preacq_det is not None else 0)
            else:
                preacq_hist.append(0)
            preacq_hits = int(sum(preacq_hist))

            if preacq_det is not None:
                acquire_score = int(clamp(acquire_score + int(PREACQ_ACQ_BONUS), 0, 999))
                acquire_miss = 0

                if PREACQ_INIT_KF_ON_MN and preacq_hits >= int(PREACQ_MIN_HITS):
                    pb = clip_box(preacq_det[:4], ew, eh)
                    if not kf.initialized:
                        kf.init_from_box(pb)
                    else:
                        cxp, cyp = box_center(pb)
                        bwp, bhp = box_wh(pb)
                        kf.update([cxp, cyp, bwp, bhp])
                    locked_box_eff = pb
                    hit_streak = max(int(hit_streak), 1)
                    flash_roi_eff = make_focus_roi_from_box(
                        pb, ew, eh, margin=FLASH_ROI_MARGIN, min_side=FLASH_ROI_MIN_SIDE
                    )
                    flash_ttl = int(max(int(flash_ttl), int(max(1, FLASH_ROI_TTL))))
        else:
            preacq_hist.clear()
            preacq_hits = 0

        if DEBUG and dets_eff and (frame_id % 30 == 0):
            print("sample det:", dets_eff[0])

        if have_yolo and dets_eff:
            guidance_raw_yolo_box_eff = pick_best_det(dets_eff, ew, eh)

        if have_yolo:
            dets_np = np.array(dets_eff, dtype=np.float32) if dets_eff else np.zeros((0, 5), dtype=np.float32)
        else:
            dets_np = None
        tracks = bt.update(dets_np, dt=dt)
        tracks_for_select = [t for t in tracks if t.time_since_update <= BT_MAX_PREDICT_AGE]
        fresh_tracks = [t for t in tracks_for_select if t.time_since_update == 0]
        if SWITCH_STATIC_GUARD_ENABLE:
            hist_window = max(2, int(SWITCH_TRACK_HIST_WINDOW))
            stale_thr = max(12, 3 * hist_window)
            if track_center_last_seen:
                stale_ids = [
                    tid for tid, last_f in track_center_last_seen.items()
                    if (frame_id - int(last_f)) > int(stale_thr)
                ]
                for tid in stale_ids:
                    track_center_last_seen.pop(tid, None)
                    track_center_hist.pop(tid, None)
            for t in fresh_tracks:
                tid = int(t.track_id)
                b_hist = clip_box(t.tlbr, ew, eh)
                cc = box_center(b_hist)
                hist = track_center_hist.get(tid)
                if hist is None or hist.maxlen != hist_window:
                    hist = deque(maxlen=hist_window)
                    track_center_hist[tid] = hist
                hist.append(np.array([float(cc[0]), float(cc[1])], dtype=np.float32))
                track_center_last_seen[tid] = int(frame_id)

        chosen = None
        target_track = None
        pred_ref = pred_box_eff if pred_box_eff is not None else (locked_box_eff if locked_box_eff is not None else None)
        pred_diag = float(np.linalg.norm(box_wh(pred_ref))) if pred_ref is not None else 40.0

        if SOFT_YOLO_HANDOFF_ENABLE and confirmed and have_yolo and dets_eff:
            soft_ref = locked_box_eff if (klt_valid and locked_box_eff is not None) else pred_ref
            if soft_ref is not None and (
                klt_valid
                or miss_streak >= int(SOFT_YOLO_MIN_MISS)
                or target_absent_frames >= int(SOFT_YOLO_MIN_ABSENT)
            ):
                soft_has_strong_klt_anchor = bool(
                    klt_valid
                    and locked_box_eff is not None
                    and float(klt.quality) >= float(SOFT_YOLO_KLT_MIN_Q)
                )
                soft_yolo_box_eff, soft_yolo_score = pick_soft_yolo_handoff_det(
                    dets_eff,
                    soft_ref,
                    ew,
                    eh,
                    min_score=float(SOFT_YOLO_MIN_SCORE),
                    dist_diag=float(SOFT_YOLO_KLT_DIST_DIAG if soft_has_strong_klt_anchor else SOFT_YOLO_LOST_DIST_DIAG),
                    dist_min=float(SOFT_YOLO_KLT_DIST_MIN if soft_has_strong_klt_anchor else SOFT_YOLO_LOST_DIST_MIN),
                    iou_floor=float(SOFT_YOLO_KLT_IOU_FLOOR if soft_has_strong_klt_anchor else SOFT_YOLO_IOU_FLOOR),
                    max_area_ratio=float(SOFT_YOLO_KLT_MAX_AREA_RATIO if soft_has_strong_klt_anchor else SOFT_YOLO_MAX_AREA_RATIO),
                    max_aspect_ratio=float(SOFT_YOLO_MAX_ASPECT_RATIO),
                )
                if soft_yolo_box_eff is not None:
                    match_dist = max(float(SOFT_YOLO_TRACK_DIST_MIN), float(SOFT_YOLO_TRACK_DIST_DIAG) * pred_diag)
                    soft_yolo_track = match_fresh_track_to_box(
                        fresh_tracks,
                        soft_yolo_box_eff,
                        ew,
                        eh,
                        min_iou=float(SOFT_YOLO_TRACK_IOU),
                        max_center_dist=match_dist,
                    )
                    if (soft_yolo_track is not None) and ((target_id is None) or (soft_yolo_track.track_id != target_id)):
                        guidance_candidate_box_eff = soft_yolo_box_eff
                        guidance_candidate_track_id = int(soft_yolo_track.track_id)
                    elif target_id is not None:
                        guidance_candidate_box_eff = soft_yolo_box_eff

        if target_id is not None:
            for t in tracks_for_select:
                if t.track_id == target_id:
                    target_track = t
                    break
            if target_track is None:
                target_absent_frames += 1
            else:
                if have_yolo and target_track.time_since_update > 0 and len(fresh_tracks) > 0:
                    target_track = None
                    target_absent_frames = TARGET_SWITCH_MISS_FRAMES
                else:
                    target_absent_frames = 0
        else:
            target_absent_frames = 0
        current_target_track = target_track
        current_target_has_fresh_update = bool(
            current_target_track is not None and current_target_track.time_since_update == 0
        )
        current_target_motion_ok = False
        if (
            current_target_track is not None
            and EGO_RESIDUAL_GATE_ENABLE
            and motion_mask is not None
        ):
            current_target_motion_ok = track_residual_motion_ok(
                current_target_track,
                motion_mask,
                ew,
                eh,
            )

        # If KLT/Kalman are holding an old point while YOLO repeatedly sees a
        # candidate elsewhere, allow a controlled re-anchor. This fixes stale
        # KLT locks without reintroducing one-frame false-positive jumps.
        reanchor_context = bool(
            YOLO_REANCHOR_ENABLE
            and confirmed
            and have_yolo
            and dets_eff
            and (
                miss_streak >= int(YOLO_REANCHOR_MISS_GE)
                or (not current_target_has_fresh_update)
                or target_absent_frames >= int(YOLO_REANCHOR_ABSENT_GE)
            )
        )
        if reanchor_context:
            reanchor_ref = pred_box_eff if pred_box_eff is not None else locked_box_eff
            (
                yolo_reanchor_box_eff,
                yolo_reanchor_score,
                yolo_reanchor_track,
                yolo_reanchor_same,
                yolo_reanchor_near_pred,
            ) = pick_yolo_reanchor_candidate(
                dets_eff,
                ew=ew,
                eh=eh,
                pred_ref=reanchor_ref,
                prev_candidate_box=yolo_reanchor_memory_box_eff,
                fresh_tracks=fresh_tracks,
                min_score=float(YOLO_REANCHOR_MIN_SCORE),
                repeat_dist_min=float(YOLO_REANCHOR_REPEAT_DIST_MIN),
                repeat_dist_diag=float(YOLO_REANCHOR_REPEAT_DIST_DIAG),
                pred_dist_min=float(YOLO_REANCHOR_PRED_DIST_MIN),
                pred_dist_diag=float(YOLO_REANCHOR_PRED_DIST_DIAG),
                max_area_ratio=float(YOLO_REANCHOR_MAX_AREA_RATIO),
                max_aspect_ratio=float(YOLO_REANCHOR_MAX_ASPECT_RATIO),
                osd_reject=bool(YOLO_REANCHOR_OSD_REJECT),
                osd_min_score=float(YOLO_REANCHOR_OSD_MIN_SCORE),
                trajectory_hypotheses=trajectory_hypotheses,
                traj_dist_min=float(TRAJ_REANCHOR_DIST_MIN),
                traj_dist_diag=float(TRAJ_REANCHOR_DIST_DIAG),
            )
            if (
                TRAJ_REANCHOR_ENABLE
                and (yolo_reanchor_box_eff is None)
                and trajectory_hypotheses
            ):
                (traj_box, traj_score, trajectory_det_label, _traj_pick_score) = pick_det_near_trajectory_hypotheses(
                    dets_eff, trajectory_hypotheses, ew, eh,
                    min_score=float(TRAJ_REANCHOR_MIN_SCORE),
                    dist_min=float(TRAJ_REANCHOR_DIST_MIN),
                    dist_diag=float(TRAJ_REANCHOR_DIST_DIAG),
                )
                if traj_box is not None:
                    yolo_reanchor_box_eff = traj_box
                    yolo_reanchor_score = float(traj_score)
                    yolo_reanchor_near_pred = True
                    yolo_reanchor_track = match_fresh_track_to_box(
                        fresh_tracks, yolo_reanchor_box_eff, ew, eh,
                        min_iou=float(SOFT_YOLO_TRACK_IOU),
                        max_center_dist=max(float(SOFT_YOLO_TRACK_DIST_MIN), float(SOFT_YOLO_TRACK_DIST_DIAG) * 40.0),
                    )

            if yolo_reanchor_box_eff is not None:
                recent_same = bool(
                    yolo_reanchor_same
                    and ((frame_id - int(yolo_reanchor_memory_last_frame)) <= int(YOLO_REANCHOR_WINDOW))
                )
                if recent_same:
                    yolo_reanchor_memory_hits += 1
                else:
                    yolo_reanchor_memory_hits = 1
                yolo_reanchor_memory_box_eff = yolo_reanchor_box_eff.copy()
                yolo_reanchor_memory_last_frame = int(frame_id)

                req_reanchor_hits = int(YOLO_REANCHOR_HITS)
                if yolo_reanchor_track is not None:
                    req_reanchor_hits = min(req_reanchor_hits, int(YOLO_REANCHOR_TRACK_HITS))
                if (
                    yolo_reanchor_near_pred
                    and float(yolo_reanchor_score) >= float(YOLO_REANCHOR_NEAR_PRED_ONE_SHOT_SCORE)
                ):
                    req_reanchor_hits = 1

                if yolo_reanchor_memory_hits >= req_reanchor_hits:
                    yolo_reanchor_reason = (
                        f"yolo_reanchor:hits={yolo_reanchor_memory_hits}/{req_reanchor_hits},"
                        f"score={float(yolo_reanchor_score):.2f},"
                        f"nearPred={int(yolo_reanchor_near_pred)},"
                        f"traj={trajectory_det_label},"
                        f"trk={int(yolo_reanchor_track is not None)}"
                    )
                    guidance_candidate_box_eff = yolo_reanchor_box_eff
                    if yolo_reanchor_track is not None:
                        guidance_candidate_track_id = int(yolo_reanchor_track.track_id)
            elif (frame_id - int(yolo_reanchor_memory_last_frame)) > int(YOLO_REANCHOR_WINDOW):
                yolo_reanchor_memory_box_eff = None
                yolo_reanchor_memory_hits = 0
        elif (frame_id - int(yolo_reanchor_memory_last_frame)) > int(YOLO_REANCHOR_WINDOW):
            yolo_reanchor_memory_box_eff = None
            yolo_reanchor_memory_hits = 0

        motion_switch_mode = confirmed and (yolo_mode in ("ROI-MOTION-C", "ROI-WAVE"))
        fast_maneuver_guard = (
            SWITCH_FAST_MANEUVER_ENABLE
            and confirmed
            and (speed >= float(SWITCH_FAST_MANEUVER_SPEED))
        )
        switch_miss_need = int(TARGET_SWITCH_MISS_FRAMES)
        if motion_switch_mode:
            switch_miss_need = max(switch_miss_need, int(MOTION_CONF_SWITCH_MISS_FRAMES))
        if approach_active:
            switch_miss_need += int(approach_switch_extra_miss)
        if fast_maneuver_guard:
            switch_miss_need += int(max(0, SWITCH_FAST_MANEUVER_EXTRA_MISS))
        stale_switch_ready = bool(
            STALE_LOCK_BREAK_ENABLE
            and confirmed
            and (target_track is None)
            and (len(fresh_tracks) > 0)
            and (
                (miss_streak >= int(STALE_LOCK_BREAK_MIN_MISS))
                or (not klt_valid)
                or (float(klt.quality) < float(STALE_LOCK_BREAK_KLT_QUALITY))
            )
        )
        can_switch = target_track is None and (
            (target_id is None)
            or (not confirmed)
            or (target_absent_frames >= switch_miss_need)
            or stale_switch_ready
        )

        candidate_tracks = fresh_tracks if (have_yolo and len(fresh_tracks) > 0) else tracks_for_select
        eligible_tracks = candidate_tracks
        if (not confirmed) and RECOVER_FILTER_ENABLE and (yolo_mode in ("ROI-MOTION", "ROI-WAVE", "ROI-MOTION-C", "ROI-GAZE")):
            eligible_tracks = [t for t in candidate_tracks if t.hits >= int(RECOVER_TARGET_MIN_HITS)]
            if len(eligible_tracks) == 0:
                eligible_tracks = candidate_tracks

        weak_reacq_guard = (
            WEAK_REACQ_GUARD_ENABLE
            and confirmed
            and (not klt_valid)
            and (miss_streak >= int(WEAK_REACQ_MISS_GE))
        )
        if weak_reacq_guard:
            eligible_tracks = [
                t for t in eligible_tracks
                if (t.hits >= int(WEAK_REACQ_MIN_HITS)) and (float(t.score) >= float(WEAK_REACQ_MIN_SCORE))
            ]
            if EGO_RESIDUAL_GATE_ENABLE and miss_streak >= int(EGO_RESIDUAL_MISS_GE) and motion_mask is not None:
                motion_tracks = [t for t in eligible_tracks if track_residual_motion_ok(t, motion_mask, ew, eh)]
                if len(motion_tracks) > 0:
                    eligible_tracks = motion_tracks

        wavelet_active = (
            (wavelet_cooldown <= 0)
            and WAVELET_ASSIST_ENABLE
            and (miss_streak >= int(WAVELET_ACTIVE_MISS_GE))
            and ((not WAVELET_ONLY_UNCONFIRMED) or (not confirmed))
        )
        if wavelet_active and WAVELET_ONLY_RECOVER_PHASE:
            wavelet_active = ((not confirmed) or (miss_streak > 0))
        best_wavelet_energy = 0.0
        best_wavelet_bonus = 0.0
        best_wavelet_hits = 0

        if WAVELET_MN_ENABLE and wavelet_track_last_seen:
            stale = [
                tid for tid, last_f in wavelet_track_last_seen.items()
                if (frame_id - int(last_f)) > int(max(12, 3 * int(WAVELET_MN_WINDOW)))
            ]
            for tid in stale:
                wavelet_track_last_seen.pop(tid, None)
                wavelet_track_hist.pop(tid, None)

        if can_switch and len(eligible_tracks) > 0:
            best = None
            best_score = -1e9
            best_hist = None
            for t in eligible_tracks:
                b = clip_box(t.tlbr, ew, eh)
                c = box_center(b)
                approach_bonus = 0.0
                is_switch_candidate = confirmed and (target_id is not None) and (t.track_id != target_id)
                if not track_passes_score_gate(
                    track_score=float(t.score),
                    confirmed=confirmed,
                    is_switch_candidate=is_switch_candidate,
                    weak_reacq_guard=weak_reacq_guard,
                    acquire_floor=float(TRACK_SCORE_MIN_ACQUIRE),
                    reacquire_floor=float(TRACK_SCORE_MIN_REACQUIRE),
                    switch_floor=float(TRACK_SCORE_MIN_SWITCH),
                ):
                    continue
                residual_ok = False
                if is_switch_candidate and EGO_RESIDUAL_GATE_ENABLE and motion_mask is not None:
                    residual_ok = track_residual_motion_ok(t, motion_mask, ew, eh)

                dist = 0.0
                i = 0.0
                if pred_ref is not None:
                    pc = box_center(pred_ref)
                    dist = float(np.linalg.norm(c - pc))
                    max_dist = max(40.0, TARGET_REACQ_DIST_FACTOR * pred_diag)
                    if confirmed and (target_id is None):
                        noid_max = max(float(CONFIRMED_NO_ID_NEAR_MIN), float(CONFIRMED_NO_ID_NEAR_FACTOR) * pred_diag)
                        max_dist = min(max_dist, noid_max)
                    if not confirmed:
                        near_max = max(float(RECOVER_NEAR_DIST_MIN), float(RECOVER_NEAR_DIST_FACTOR) * pred_diag)
                        max_dist = min(max_dist, near_max)
                    if weak_reacq_guard:
                        weak_max = max(float(WEAK_REACQ_MAX_DIST_MIN), float(WEAK_REACQ_MAX_DIST_FACTOR) * pred_diag)
                        max_dist = min(max_dist, weak_max)
                    if dist > max_dist:
                        continue
                    i = iou(b, pred_ref)

                    pred_area = box_area(pred_ref)
                    cand_area = box_area(b)
                    ar_ratio = safe_ratio(box_ar(b), box_ar(pred_ref))
                    grow_ratio = cand_area / max(pred_area, 1e-3)
                    shrink_ratio = pred_area / max(cand_area, 1e-3)
                    allow_grow_ratio = float(TURN_MAX_AREA_RATIO)
                    if approach_active and cand_area >= pred_area:
                        close_growth = (
                            dist <= (float(approach_near_dist_diag) * pred_diag)
                            or (cand_area >= float(APPROACH_CLOSE_AREA))
                        )
                        if close_growth:
                            allow_grow_ratio = max(allow_grow_ratio, float(approach_max_area_ratio))
                            if grow_ratio > 1.0:
                                approach_bonus = float(
                                    clamp(
                                        float(approach_score_weight) * np.log2(grow_ratio),
                                        0.0,
                                        float(approach_score_clip),
                                    )
                                )
                    if ar_ratio > TURN_MAX_ASPECT_RATIO:
                        continue
                    if cand_area >= pred_area:
                        if grow_ratio > allow_grow_ratio:
                            continue
                    else:
                        if shrink_ratio > TURN_MAX_AREA_RATIO:
                            continue
                    if target_id is not None and t.track_id != target_id:
                        if i < TARGET_SWITCH_IOU_FLOOR and dist > (1.2 * pred_diag):
                            continue

                if (
                    is_switch_candidate
                    and SWITCH_TRAJ_GATE_ENABLE
                    and (miss_streak >= int(SWITCH_TRAJ_MISS_GE))
                    and (pred_ref is not None)
                    and kf.initialized
                ):
                    vx = float(kf.x[4, 0])
                    vy = float(kf.x[5, 0])
                    vnorm = float(np.hypot(vx, vy))
                    if vnorm >= float(SWITCH_TRAJ_MIN_SPEED):
                        dvec = c - box_center(pred_ref)
                        dnorm = float(np.linalg.norm(dvec))
                        if dnorm > 1e-3:
                            ux = vx / vnorm
                            uy = vy / vnorm
                            cos_v = float((dvec[0] * ux + dvec[1] * uy) / dnorm)
                            perp = float(abs(dvec[0] * uy - dvec[1] * ux))
                            perp_lim = max(float(SWITCH_TRAJ_PERP_MIN), float(SWITCH_TRAJ_PERP_DIAG) * pred_diag)
                            traj_ok = (cos_v >= float(SWITCH_TRAJ_COS_MIN)) and (perp <= perp_lim)
                            if (not traj_ok) and (not (SWITCH_TRAJ_REQUIRE_RESIDUAL_BYPASS and residual_ok)):
                                continue

                if is_switch_candidate and SWITCH_STATIC_GUARD_ENABLE:
                    hist = track_center_hist.get(int(t.track_id))
                    min_hist = max(3, int(max(2, int(SWITCH_TRACK_HIST_WINDOW)) // 2))
                    if hist is not None and len(hist) >= min_hist:
                        disp = float(np.linalg.norm(hist[-1] - hist[0]))
                        disp_lim = max(float(SWITCH_STATIC_MIN_DISP), float(SWITCH_STATIC_MIN_DIAG) * pred_diag)
                        if (disp < disp_lim) and (not (SWITCH_STATIC_REQUIRE_RESIDUAL_BYPASS and residual_ok)):
                            continue

                if motion_switch_mode and is_switch_candidate:
                    if int(t.hits) < int(MOTION_CONF_SWITCH_MIN_HITS):
                        continue
                    if float(t.score) < float(MOTION_CONF_SWITCH_MIN_SCORE):
                        continue
                    if pred_ref is not None:
                        if (i < float(MOTION_CONF_SWITCH_IOU_FLOOR)) and (dist > (float(MOTION_CONF_SWITCH_DIST_DIAG) * pred_diag)):
                            continue
                    if (
                        MOTION_CONF_SWITCH_REQUIRE_RESIDUAL
                        and EGO_RESIDUAL_GATE_ENABLE
                        and motion_mask is not None
                        and (not residual_ok)
                    ):
                        continue
                if (
                    confirmed
                    and (target_id is not None)
                    and (t.track_id != target_id)
                    and OSD_SWITCH_BLOCK_ENABLE
                    and REJECT_OSD_ZONES
                ):
                    in_osd = in_osd_zone(float(c[0]), float(c[1]), ew, eh)
                    if in_osd:
                        if pred_ref is None:
                            continue
                        if dist > (float(OSD_SWITCH_BLOCK_DIST_DIAG) * pred_diag):
                            continue

                app = 0.0
                cand_hist = None
                if USE_HSV_GATE and ref_hist is not None:
                    cand_hist = compute_hsv_hist(frame_eff, b)
                    app = hsv_sim(ref_hist, cand_hist)
                    min_app = float(HSV_GATE_MIN_SIM)
                    if approach_active and pred_ref is not None:
                        pred_area = box_area(pred_ref)
                        cand_area = box_area(b)
                        grow_ratio = cand_area / max(pred_area, 1e-3)
                        if (cand_area >= float(APPROACH_CLOSE_AREA)) or (grow_ratio >= float(APPROACH_HSV_RELAX_GROW_RATIO)):
                            min_app *= float(approach_hsv_min_scale)
                    if app < min_app:
                        continue

                wv_energy = 0.0
                wv_bonus = 0.0
                wv_hits = 0
                if wavelet_active:
                    wv_energy = wavelet_energy_haar(
                        gray_now,
                        b,
                        margin=WAVELET_BOX_MARGIN,
                        min_side=WAVELET_MIN_SIDE,
                    )
                    motion_ok = (
                        EGO_RESIDUAL_GATE_ENABLE
                        and motion_mask is not None
                        and track_residual_motion_ok(t, motion_mask, ew, eh)
                    )
                    tiny_box = box_area(b) <= float(WAVELET_GATE_TINY_AREA_MAX)
                    if tiny_box and (miss_streak >= int(WAVELET_GATE_MISS_GE)):
                        if (wv_energy < float(WAVELET_GATE_MIN_ENERGY)) and (not motion_ok):
                            continue

                    if WAVELET_MN_ENABLE:
                        tid = int(t.track_id)
                        hist = wavelet_track_hist.get(tid)
                        if hist is None:
                            hist = deque(maxlen=max(1, int(WAVELET_MN_WINDOW)))
                            wavelet_track_hist[tid] = hist
                        wv_pass = (wv_energy >= float(WAVELET_GATE_MIN_ENERGY)) or motion_ok
                        hist.append(1 if wv_pass else 0)
                        wavelet_track_last_seen[tid] = int(frame_id)
                        wv_hits = int(sum(hist))
                        if tiny_box and (miss_streak >= int(WAVELET_MN_MISS_GE)) and (wv_hits < int(WAVELET_MN_MIN_HITS)) and (not motion_ok):
                            continue

                    wv_norm = (wv_energy - float(WAVELET_ENERGY_BASE)) / max(1e-6, float(WAVELET_ENERGY_BASE))
                    wv_bonus = float(clamp(float(WAVELET_SCORE_WEIGHT) * wv_norm, -float(WAVELET_SCORE_CLIP), float(WAVELET_SCORE_CLIP)))
                    if WAVELET_MN_ENABLE and int(WAVELET_MN_WINDOW) > 0:
                        mn_ratio = float(wv_hits) / float(max(1, int(WAVELET_MN_WINDOW)))
                        wv_bonus += 0.08 * mn_ratio

                s = (2.2 * i) + (1.0 / (1.0 + dist)) + (0.25 * float(t.score)) + (0.9 * app) + wv_bonus + approach_bonus
                if pred_ref is None:
                    s = (0.35 * float(t.score)) + (0.0005 * box_area(b)) + (0.9 * app) + wv_bonus
                if target_id is not None and t.track_id == target_id:
                    s += TARGET_STICKY_SCORE_BONUS

                if s > best_score:
                    best_score = s
                    best = t
                    best_hist = cand_hist
                    best_wavelet_energy = wv_energy
                    best_wavelet_bonus = wv_bonus
                    best_wavelet_hits = wv_hits
                    best_dist = float(dist)
                    best_residual_ok = bool(residual_ok)

            if best is not None and best_score >= TARGET_PICK_MIN_SCORE:
                if (
                    confirmed
                    and (target_id is not None)
                    and (best.track_id != target_id)
                    and (best.time_since_update == 0)
                ):
                    guidance_candidate_box_eff = clip_box(best.tlbr, ew, eh)
                    guidance_candidate_track_id = int(best.track_id)
                if (
                    STALE_LOCK_BREAK_ENABLE
                    and confirmed
                    and (target_id is not None)
                    and (best.track_id != target_id)
                ):
                    klt_iou_now = 0.0
                    if (klt.box is not None) and (locked_box_eff is not None):
                        klt_iou_now = float(iou(klt.box, locked_box_eff))
                    handoff_decision = evaluate_stale_lock(
                        confirmed=confirmed,
                        have_fresh_candidate=(best.time_since_update == 0),
                        target_has_fresh_update=current_target_has_fresh_update,
                        candidate_dist_px=float(best_dist),
                        pred_diag_px=float(pred_diag),
                        miss_streak=int(miss_streak),
                        klt_valid=bool(klt_valid),
                        klt_quality=float(klt.quality),
                        klt_iou=klt_iou_now,
                        candidate_motion_ok=bool(best_residual_ok),
                        target_motion_ok=bool(current_target_motion_ok),
                        stale_break_dist_diag=float(STALE_LOCK_BREAK_DIST_DIAG),
                        stale_break_min_miss=int(STALE_LOCK_BREAK_MIN_MISS),
                        stale_break_klt_quality=float(STALE_LOCK_BREAK_KLT_QUALITY),
                        stale_break_klt_iou=float(STALE_LOCK_BREAK_KLT_IOU),
                    )
                    stale_lock_active = bool(handoff_decision.stale_lock_active)
                    stale_lock_reason = handoff_decision.reason_text()

                if confirmed and weak_reacq_guard and ((target_id is None) or (best.track_id != target_id)):
                    if switch_candidate_id == best.track_id:
                        switch_candidate_hits += 1
                    else:
                        switch_candidate_id = best.track_id
                        switch_candidate_hits = 1
                    if switch_candidate_hits < int(WEAK_REACQ_ADOPT_HITS):
                        best = None
                elif target_id is not None and confirmed and best.track_id != target_id:
                    req_switch_hits = compute_fast_handoff_hits(
                        stale_lock_active=bool(FAST_HANDOFF_ENABLE and stale_lock_active),
                        motion_switch_mode=bool(motion_switch_mode),
                        approach_extra_hits=int(approach_switch_extra_hits) if approach_active else 0,
                        fast_maneuver_extra_hits=int(max(0, SWITCH_FAST_MANEUVER_EXTRA_HITS)) if fast_maneuver_guard else 0,
                        default_switch_hits=int(TARGET_SWITCH_CONFIRM_HITS),
                        fast_handoff_hits=int(FAST_HANDOFF_CONFIRM_HITS),
                        motion_switch_hits=int(MOTION_CONF_SWITCH_CONFIRM_HITS),
                    )
                    fast_handoff_active = bool(
                        FAST_HANDOFF_ENABLE
                        and stale_lock_active
                        and (req_switch_hits == int(FAST_HANDOFF_CONFIRM_HITS))
                    )
                    if switch_candidate_id == best.track_id:
                        switch_candidate_hits += 1
                    else:
                        switch_candidate_id = best.track_id
                        switch_candidate_hits = 1
                    if switch_candidate_hits < req_switch_hits:
                        best = None
                else:
                    switch_candidate_id = None
                    switch_candidate_hits = 0

            if best is not None and best_score >= TARGET_PICK_MIN_SCORE:
                target_track = best
                if USE_HSV_GATE:
                    ref_hist = blend_hist(ref_hist, best_hist, alpha=0.85)
                target_absent_frames = 0

        if target_track is None and target_id is None and len(eligible_tracks) > 0:
            eligible_tracks = [
                t for t in eligible_tracks
                if track_passes_score_gate(
                    track_score=float(t.score),
                    confirmed=confirmed,
                    is_switch_candidate=False,
                    weak_reacq_guard=weak_reacq_guard,
                    acquire_floor=float(TRACK_SCORE_MIN_ACQUIRE),
                    reacquire_floor=float(TRACK_SCORE_MIN_REACQUIRE),
                    switch_floor=float(TRACK_SCORE_MIN_SWITCH),
                )
            ]
            picked = None
            if len(eligible_tracks) == 0:
                picked = None
            elif pred_ref is None:
                picked = max(eligible_tracks, key=lambda tt: (float(tt.score), box_area(tt.tlbr)))
            elif not confirmed:
                pc = box_center(pred_ref)
                near_lim = max(float(RECOVER_NEAR_DIST_MIN), float(RECOVER_NEAR_DIST_FACTOR) * pred_diag)
                near_tracks = []
                for tt in eligible_tracks:
                    cc = box_center(clip_box(tt.tlbr, ew, eh))
                    if float(np.linalg.norm(cc - pc)) <= near_lim:
                        near_tracks.append(tt)
                if len(near_tracks) > 0:
                    picked = max(near_tracks, key=lambda tt: (float(tt.score), box_area(tt.tlbr)))
            else:
                if not weak_reacq_guard:
                    pc = box_center(pred_ref)
                    near_lim = max(float(CONFIRMED_NO_ID_NEAR_MIN), float(CONFIRMED_NO_ID_NEAR_FACTOR) * pred_diag)
                    near_tracks = []
                    for tt in eligible_tracks:
                        cc = box_center(clip_box(tt.tlbr, ew, eh))
                        if float(np.linalg.norm(cc - pc)) <= near_lim:
                            near_tracks.append(tt)
                    if len(near_tracks) > 0:
                        picked = max(near_tracks, key=lambda tt: (float(tt.score), box_area(tt.tlbr)))

            if picked is not None:
                target_track = picked
                target_absent_frames = 0
                switch_candidate_id = None
                switch_candidate_hits = 0

        if (
            SOFT_YOLO_HANDOFF_ENABLE
            and confirmed
            and (soft_yolo_box_eff is not None)
            and (
                target_track is None
                or miss_streak >= int(SOFT_YOLO_MIN_MISS)
                or target_absent_frames >= int(SOFT_YOLO_MIN_ABSENT)
            )
        ):
            if soft_yolo_track is not None:
                target_track = soft_yolo_track
                target_absent_frames = 0
            else:
                chosen = soft_yolo_box_eff
                target_absent_frames = 0
            switch_candidate_id = None
            switch_candidate_hits = 0
            soft_yolo_adopted = True

        if (
            YOLO_REANCHOR_ENABLE
            and confirmed
            and (yolo_reanchor_box_eff is not None)
            and yolo_reanchor_reason
        ):
            # Bypass the normal KLT-anchor rejection only after M/N detector
            # persistence says the old KLT anchor is stale.
            if yolo_reanchor_track is not None:
                target_track = yolo_reanchor_track
                target_absent_frames = 0
            else:
                chosen = yolo_reanchor_box_eff
                target_absent_frames = 0
            switch_candidate_id = None
            switch_candidate_hits = 0
            soft_yolo_adopted = False
            yolo_reanchor_adopted = True
            trajectory_reanchor_used = bool(trajectory_det_label != "-")
            yolo_reanchor_last_reason = yolo_reanchor_reason

        suppress_target_id_update = False
        klt_anchor_reject_reason = ""
        if (
            BT_KLT_ANCHOR_GUARD_ENABLE
            and (not yolo_reanchor_adopted)
            and confirmed
            and klt_valid
            and locked_box_eff is not None
            and float(klt.quality) >= float(BT_KLT_ANCHOR_MIN_Q)
        ):
            proposed_box_for_anchor = None
            if target_track is not None:
                proposed_box_for_anchor = clip_box(target_track.tlbr, ew, eh)
            elif chosen is not None:
                proposed_box_for_anchor = clip_box(chosen, ew, eh)

            if proposed_box_for_anchor is not None:
                anchor_ok, klt_anchor_reject_reason = klt_anchor_accepts_box(
                    proposed_box_for_anchor,
                    locked_box_eff,
                    ew,
                    eh,
                    dist_diag=float(BT_KLT_ANCHOR_DIST_DIAG),
                    dist_min=float(BT_KLT_ANCHOR_DIST_MIN),
                    iou_floor=float(BT_KLT_ANCHOR_IOU_FLOOR),
                    max_area_ratio=float(BT_KLT_ANCHOR_MAX_AREA_RATIO),
                    max_aspect_ratio=float(BT_KLT_ANCHOR_MAX_ASPECT_RATIO),
                )
                if not anchor_ok:
                    # KLT is still confidently sitting on the physical target.
                    # Do not let a one-frame ByteTrack/Yolo false positive drag
                    # Kalman/guidance away. Keep the KLT anchor as measurement.
                    target_track = None
                    soft_yolo_adopted = False
                    target_absent_frames = 0
                    switch_candidate_id = None
                    switch_candidate_hits = 0
                    if BT_KLT_ANCHOR_HOLD_ON_REJECT:
                        chosen = clip_box(locked_box_eff, ew, eh)
                    else:
                        chosen = None
                    if DEBUG and (frame_id % max(1, int(BT_KLT_ANCHOR_DEBUG_EVERY)) == 0):
                        print(f"[klt_anchor_guard] reject BT/Yolo box frame={frame_id} reason={klt_anchor_reject_reason}")
                elif (
                    BT_ID_STABILIZE_WHEN_KLT_VALID
                    and target_track is not None
                    and target_id is not None
                    and int(target_track.track_id) != int(target_id)
                ):
                    # ByteTrack often creates a new id every frame for tiny FPV targets.
                    # Use its box as a detector measurement, but do not treat this as a
                    # physical target switch while KLT is strong.
                    suppress_target_id_update = True
                    target_absent_frames = 0
                    switch_candidate_id = None
                    switch_candidate_hits = 0

        prev_target_id = target_id
        if target_track is not None:
            if not suppress_target_id_update:
                target_id = target_track.track_id
            chosen = clip_box(target_track.tlbr, ew, eh)
            switch_candidate_id = None
            switch_candidate_hits = 0

        chosen_valid = chosen is not None
        if chosen_valid:
            if not kf.initialized:
                kf.init_from_box(chosen)
            else:
                cx, cy = box_center(chosen)
                bw, bh = box_wh(chosen)
                if pred_box_eff is not None:
                    pcx, pcy = box_center(pred_box_eff)
                    dist = float(np.hypot(cx - pcx, cy - pcy))
                    diag = float(np.linalg.norm(box_wh(pred_box_eff)))
                    if dist > 10.0 * max(15.0, diag):
                        chosen = None

                if chosen is None and kf.initialized and TM_ENABLE and template_gray is not None:
                    pcx, pcy = box_center(pred_box_eff) if pred_box_eff is not None else (kf.x[0, 0], kf.x[1, 0])
                    tm = tm_search(gray_now, template_gray, (pcx, pcy), miss_streak)
                    if tm is not None:
                        mcx, mcy, _, tw, th = tm
                        if pred_box_eff is not None:
                            bw, bh = box_wh(pred_box_eff)
                        else:
                            bw, bh = float(tw), float(th)
                        kf.update([float(mcx), float(mcy), float(bw), float(bh)])

                if chosen is not None:
                    kf.update([cx, cy, bw, bh])

            chosen_valid = chosen is not None

        if chosen_valid:
            locked_box_eff = chosen
            hit_streak += 1
            miss_streak = 0
            if not confirmed:
                bonus = int(ACQUIRE_HIT_BONUS)
                if target_track is not None and float(target_track.score) >= float(BT_HIGH):
                    bonus += 1
                acquire_score = int(clamp(acquire_score + bonus, 0, 999))
                acquire_miss = 0

            if not confirmed and ((hit_streak >= CONFIRM_HITS) or (acquire_score >= ACQUIRE_CONFIRM_SCORE)):
                confirmed = True
                if locked_box_eff is not None:
                    klt.init(frame_eff, locked_box_eff)
                    last_klt_init_frame = frame_id
                ref_hist = compute_hsv_hist(frame_eff, locked_box_eff) if USE_HSV_GATE else None
                template_gray = tm_update_template(gray_now, locked_box_eff)
                acquire_score = 0
                acquire_miss = 0
                preacq_hist.clear()
                preacq_hits = 0

            if USE_HSV_GATE and have_yolo:
                cur_hist = compute_hsv_hist(frame_eff, locked_box_eff)
                if ref_hist is None:
                    ref_hist = cur_hist
                else:
                    if (frame_id % HSV_UPDATE_EVERY == 0) or (target_track is not None and float(target_track.score) >= BT_HIGH):
                        ref_hist = blend_hist(ref_hist, cur_hist, alpha=0.80)

            if TM_ENABLE and (have_yolo or frame_id % 5 == 0):
                template_gray = tm_update_template(gray_now, locked_box_eff)

            if soft_yolo_adopted and SOFT_YOLO_REFRESH_KLT and confirmed and locked_box_eff is not None:
                klt.init(frame_eff, locked_box_eff)
                last_klt_init_frame = frame_id

            if yolo_reanchor_adopted and YOLO_REANCHOR_RESET_KLT and confirmed and locked_box_eff is not None:
                klt.reset()
                klt.init(frame_eff, locked_box_eff)
                last_klt_init_frame = frame_id
                yolo_reanchor_memory_box_eff = None
                yolo_reanchor_memory_hits = 0

            if (
                KLT_REFRESH_WITH_YOLO
                and confirmed
                and have_yolo
                and (target_track is not None)
                and (target_track.time_since_update == 0)
                and (locked_box_eff is not None)
                and (float(target_track.score) >= float(KLT_REFRESH_MIN_SCORE))
            ):
                klt_age = int(frame_id - last_klt_init_frame)
                klt_iou = iou(klt.box, locked_box_eff) if klt.box is not None else 0.0
                weak_anchor = (
                    (not klt_valid)
                    or (klt.good_count < int(KLT_REINIT_MIN_POINTS))
                    or (klt.quality < float(KLT_REFRESH_MIN_QUALITY))
                )
                stale_anchor = klt_age >= int(max(1, KLT_REFRESH_EVERY))
                drifted_anchor = (klt.box is None) or (klt_iou < float(KLT_REFRESH_MIN_IOU))
                if (klt_age > 0) and (weak_anchor or stale_anchor or drifted_anchor):
                    klt.init(frame_eff, locked_box_eff)
                    last_klt_init_frame = frame_id

        else:
            used_prediction_hold = False
            if confirmed and stale_lock_active:
                guidance_ctrl.reset_for_target_switch(
                    np.array([0.5 * float(ew), 0.5 * float(eh)], dtype=np.float32),
                    cmd_damp=float(FAST_HANDOFF_GUIDANCE_CMD_DAMP),
                )
                guidance_reset_event = "stale_lock_break"
                guidance_force_neutral = True

            if TURN_SAFE_ENABLE and have_yolo and dets_eff and (miss_streak >= TURN_SAFE_MISS_BEFORE_RESET):
                best_det, best_conf = pick_best_det_with_score(dets_eff, ew, eh)
                if best_det is not None and float(best_conf) >= float(TURN_SAFE_MIN_SCORE):
                    adopted_track_id = None
                    adopted_track = None
                    best_iou = 0.0
                    for tt in tracks_for_select:
                        if tt.time_since_update > 0:
                            continue
                        btt = clip_box(tt.tlbr, ew, eh)
                        i = iou(btt, best_det)
                        if i > best_iou:
                            best_iou = i
                            adopted_track = tt
                    if adopted_track is not None and best_iou >= 0.15:
                        adopted_track_id = adopted_track.track_id

                    if (not TURN_SAFE_REQUIRE_TRACK_ID) or (adopted_track_id is not None):
                        kf.init_from_box(best_det)
                        locked_box_eff = best_det
                        confirmed = True
                        miss_streak = 0
                        hit_streak = CONFIRM_HITS
                        target_id = adopted_track_id
                        target_absent_frames = 0
                        switch_candidate_id = None
                        switch_candidate_hits = 0
                        klt.reset()
                        if locked_box_eff is not None:
                            klt.init(frame_eff, locked_box_eff)
                            last_klt_init_frame = frame_id
                        if USE_HSV_GATE:
                            ref_hist = compute_hsv_hist(frame_eff, locked_box_eff)
                        if TM_ENABLE:
                            template_gray = tm_update_template(gray_now, locked_box_eff)
                        acquire_score = 0
                        acquire_miss = 0
                        preacq_hist.clear()
                        preacq_hits = 0

            if confirmed and KLT_VALID_HOLD_ENABLE and klt_valid and locked_box_eff is not None:
                miss_streak = min(int(miss_streak) + 1, int(KLT_VALID_HOLD_MAX_MISS))
                hit_streak = 0
                used_prediction_hold = True
            elif confirmed and pred_box_eff is not None and (not stale_lock_active) and miss_streak < KALMAN_HOLD_MAX:
                hold_box_eff = pred_box_eff
                if (
                    TRAJ_USE_PRIMARY_FOR_HOLD
                    and trajectory_primary_box_eff is not None
                    and (miss_streak >= int(TRAJ_HOLD_MISS_GE))
                ):
                    hold_box_eff = trajectory_primary_box_eff
                locked_box_eff = hold_box_eff
                miss_streak += 1
                hit_streak = 0
                used_prediction_hold = True
            elif (not confirmed) and PROVISIONAL_HOLD_ENABLE and kf.initialized and pred_box_eff is not None and miss_streak < int(PROVISIONAL_HOLD_MAX):
                locked_box_eff = pred_box_eff
                miss_streak += 1
                hit_streak = max(0, int(hit_streak) - int(max(1, PROVISIONAL_HIT_DECAY)))
                acquire_miss += 1
                acquire_score = max(0, int(acquire_score) - int(max(1, ACQUIRE_MISS_PENALTY)))
                used_prediction_hold = True
            elif not (confirmed and klt_valid):
                miss_streak += 1
                if confirmed:
                    hit_streak = 0
                else:
                    hit_streak = max(0, int(hit_streak) - int(max(1, PROVISIONAL_HIT_DECAY)))
                    acquire_miss += 1
                    acquire_score = max(0, int(acquire_score) - int(max(1, ACQUIRE_MISS_PENALTY)))
                    if acquire_miss >= int(ACQUIRE_MAX_MISS):
                        acquire_score = 0

            if (not used_prediction_hold) and confirmed and (yolo_no_det >= YOLO_NO_DET_LIMIT) and (not klt_valid):
                confirmed = False
                locked_box_eff = None
                ref_hist = None
                template_gray = None
                target_id = None
                target_absent_frames = 0
                switch_candidate_id = None
                switch_candidate_hits = 0
                klt.reset()
                miss_streak = 0
                hit_streak = 0
                yolo_no_det = 0
                acquire_score = 0
                acquire_miss = 0
                preacq_hist.clear()
                preacq_hits = 0

            elif confirmed and (target_id is None) and (miss_streak >= int(CONFIRMED_NO_ID_MAX_MISS)):
                confirmed = False
                locked_box_eff = None
                ref_hist = None
                template_gray = None
                target_id = None
                target_absent_frames = 0
                switch_candidate_id = None
                switch_candidate_hits = 0
                klt.reset()
                hit_streak = 0
                acquire_score = 0
                acquire_miss = 0
                preacq_hist.clear()
                preacq_hits = 0

            elif miss_streak >= MAX_MISSES:
                confirmed = False
                locked_box_eff = None
                ref_hist = None
                template_gray = None
                target_id = None
                target_absent_frames = 0
                switch_candidate_id = None
                switch_candidate_hits = 0
                klt.reset()
                acquire_score = 0
                acquire_miss = 0
                preacq_hist.clear()
                preacq_hits = 0

        # ─── Stationary clutter killer ──────────────────────
        # Независимый "полицейский": следит за движением центра
        # confirmed трека и motion saliency в его зоне. Форсит
        # сброс если трек прилип к статичному объекту (снег,
        # горизонт, ЛЭП).
        killer.update(
            frame_id=frame_id,
            target_id=target_id,
            locked_box_eff=locked_box_eff,
            motion_sal=motion_sal,
            confirmed=confirmed,
            frame_h=eh,
        )
        if killer.should_kill(frame_id):
            print(f"[stationary_killer] kill tid={target_id} "
                  f"reason={killer.last_kill_reason}")
            killer.notify_kill_done(frame_id)
            confirmed = False
            locked_box_eff = None
            ref_hist = None
            template_gray = None
            target_id = None
            target_absent_frames = 0
            switch_candidate_id = None
            switch_candidate_hits = 0
            klt.reset()
            miss_streak = 0
            hit_streak = 0
            acquire_score = 0
            acquire_miss = 0
            preacq_hist.clear()
            preacq_hits = 0

        locked_box_orig = None
        if locked_box_eff is not None:
            locked_box_orig = unscale_box(locked_box_eff, sx, sy)
            locked_box_orig = clip_box(locked_box_orig, frame_orig.shape[1], frame_orig.shape[0])

        if GUIDANCE_OVERRIDE_ENABLE:
            guidance_override_candidate_box_eff = pick_guidance_override_box(
                track_candidate_box=guidance_candidate_box_eff,
                raw_yolo_box=guidance_raw_yolo_box_eff,
            )
            guidance_override_candidate_box_eff, guidance_override_memory_ttl = update_guidance_override_latch(
                new_box=guidance_override_candidate_box_eff,
                prev_box=guidance_override_memory_box_eff,
                prev_ttl=guidance_override_memory_ttl,
                max_ttl=int(GUIDANCE_OVERRIDE_TTL),
            )
            guidance_override_memory_box_eff = guidance_override_candidate_box_eff
            guidance_override_active = should_override_guidance(
                confirmed=confirmed,
                target_track_missing=(target_track is None),
                have_fresh_candidate=(guidance_override_candidate_box_eff is not None),
                candidate_matches_target=(
                    guidance_candidate_track_id is not None
                    and target_id is not None
                    and int(guidance_candidate_track_id) == int(target_id)
                ),
                miss_streak=int(miss_streak),
                override_miss_ge=int(GUIDANCE_OVERRIDE_MISS_GE),
            )
            if guidance_override_active:
                guidance_override_box_eff = guidance_override_candidate_box_eff
            else:
                guidance_override_memory_box_eff = None
                guidance_override_memory_ttl = 0

        if DRAW_ALL_BOXES and dets_eff:
            for d in dets_eff:
                b_eff = d[:4]
                b_orig = unscale_box(b_eff, sx, sy)
                b_orig = clip_box(b_orig, frame_orig.shape[1], frame_orig.shape[0])
                x1, y1, x2, y2 = map(int, b_orig)
                cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (0, 255, 0), 1)
                cv2.putText(frame_orig, f"{float(d[4]):.2f}", (x1, max(0, y1 - 6)),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)

        if DRAW_BT_TRACKS and tracks:
            for t in tracks:
                if BT_DRAW_ONLY_CONFIRMED and (t.hits < BT_MIN_HITS):
                    continue
                if DRAW_BT_ONLY_FRESH and (int(t.time_since_update) > 0):
                    continue
                b_eff = clip_box(t.tlbr, ew, eh)
                b_orig = unscale_box(b_eff, sx, sy)
                b_orig = clip_box(b_orig, frame_orig.shape[1], frame_orig.shape[0])
                x1, y1, x2, y2 = map(int, b_orig)
                cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (255, 255, 0), 2)
                cv2.putText(frame_orig, f"T{t.track_id}:{t.score:.2f} a={int(t.time_since_update)}",
                            (x1, max(0, y1 - 8)),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 2)

        if DRAW_LOCK_BOX and locked_box_orig is not None:
            x1, y1, x2, y2 = map(int, locked_box_orig)
            if miss_streak <= int(DRAW_KALMAN_WHEN_MISS_LE):
                cv2.rectangle(frame_orig, (x1, y1), (x2, y2), (0, 0, 255), 2)
            cv2.putText(frame_orig, f"ID={target_id}", (x1, max(0, y1 - 10)),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)

        if DRAW_KALMAN and kf.initialized:
            pb_eff = clip_box(kf.to_box(), ew, eh)
            pb_orig = unscale_box(pb_eff, sx, sy)
            cx, cy = box_center(pb_orig)
            cv2.circle(frame_orig, (int(cx), int(cy)), 5, (255, 0, 0), -1)

        if DEBUG and DRAW_MOTION_ROI and (not confirmed) and (motion_roi_eff is not None):
            m_orig = unscale_box(motion_roi_eff, sx, sy)
            m_orig = clip_box(m_orig, frame_orig.shape[1], frame_orig.shape[0])
            mx1, my1, mx2, my2 = map(int, m_orig)
            cv2.rectangle(frame_orig, (mx1, my1), (mx2, my2), (0, 165, 255), 1)
            cv2.putText(
                frame_orig,
                f"MOTION zones={motion_active_zones}",
                (mx1, max(0, my1 - 6)),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.5,
                (0, 165, 255),
                2,
            )

        if DEBUG and DRAW_AUTOGAZE_ROI and len(autogaze_rois_eff) > 0:
            for gi, g_eff in enumerate(autogaze_rois_eff):
                g_orig = unscale_box(g_eff, sx, sy)
                g_orig = clip_box(g_orig, frame_orig.shape[1], frame_orig.shape[0])
                gx1, gy1, gx2, gy2 = map(int, g_orig)
                color = (255, 80, 20) if gi == 0 else (255, 160, 80)
                cv2.rectangle(frame_orig, (gx1, gy1), (gx2, gy2), color, 1)
                if gi == 0:
                    cv2.putText(
                        frame_orig,
                        "GAZE",
                        (gx1, max(0, gy1 - 6)),
                        cv2.FONT_HERSHEY_SIMPLEX,
                        0.5,
                        color,
                        2,
                    )

        if DRAW_TRAJ:
            if CLEAR_TRAJ_ON_RECOVER and (not confirmed):
                traj.clear()
            if (not DRAW_TRAJ_ONLY_WHEN_LOCKED) or (locked_box_eff is not None and confirmed):
                traj_frame_i += 1
                if traj_frame_i % max(1, int(TRAIL_DRAW_EVERY_N)) == 0:
                    src_eff = locked_box_eff if locked_box_eff is not None else pred_box_eff
                    if src_eff is not None:
                        c_eff = box_center(src_eff)
                        c_orig = c_eff / np.array([sx, sy], dtype=np.float32)
                        pt = (int(c_orig[0]), int(c_orig[1]))
                        if (not traj) or (abs(pt[0] - traj[-1][0]) + abs(pt[1] - traj[-1][1]) >= int(TRAIL_MIN_STEP_PX)):
                            traj.append(pt)
            if len(traj) >= 2:
                overlay = frame_orig.copy()
                for i in range(1, len(traj)):
                    p0 = traj[i - 1]
                    p1 = traj[i]
                    age = i / max(1, (len(traj) - 1))
                    thickness = 1 if age < 0.85 else 2
                    cv2.line(overlay, p0, p1, (0, 0, 255), thickness, cv2.LINE_AA)
                a = float(clamp(TRAIL_ALPHA, 0.05, 0.6))
                frame_orig[:] = cv2.addWeighted(overlay, a, frame_orig, 1.0 - a, 0)

        if DEBUG and DRAW_TRAJ_PREDICTIONS and trajectory_hypotheses:
            src_eff = locked_box_eff if locked_box_eff is not None else pred_box_eff
            if src_eff is not None:
                src_c = box_center(src_eff) / np.array([sx, sy], dtype=np.float32)
                for h in trajectory_hypotheses:
                    hb_orig = unscale_box(h["box"], sx, sy)
                    hb_orig = clip_box(hb_orig, frame_orig.shape[1], frame_orig.shape[0])
                    hx1, hy1, hx2, hy2 = map(int, hb_orig)
                    hc = box_center(h["box"]) / np.array([sx, sy], dtype=np.float32)
                    cv2.rectangle(frame_orig, (hx1, hy1), (hx2, hy2), (180, 0, 255), 1)
                    cv2.line(frame_orig, (int(src_c[0]), int(src_c[1])), (int(hc[0]), int(hc[1])), (180, 0, 255), 1, cv2.LINE_AA)
                    cv2.putText(frame_orig, str(h.get("label", "tr")), (hx1, max(0, hy1 - 4)),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.4, (180, 0, 255), 1)

        status = "CONFIRMED" if confirmed else "RECOVER"
        cv2.putText(frame_orig, status, (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 2)

        ag_ready = int(getattr(autogaze_worker, "ready", False))
        ag_stale = int(autogaze_info["stale"]) if autogaze_info is not None else 1
        ag_age = int(autogaze_info["age"]) if autogaze_info is not None else -1
        ag_cells = int(autogaze_info["active_cells"]) if autogaze_info is not None else 0
        ag_ms = float(autogaze_info["infer_ms"]) if autogaze_info is not None else 0.0
        ag_rois = len(autogaze_rois_eff)
        target_track_score = float(target_track.score) if target_track is not None else None
        vx_guid = float(kf.x[4, 0]) if kf.initialized else 0.0
        vy_guid = float(kf.x[5, 0]) if kf.initialized else 0.0
        guidance_state = guidance_ctrl.update(
            frame_id=frame_id,
            frame_w=ew,
            frame_h=eh,
            confirmed=bool(confirmed and (not guidance_force_neutral)),
            locked_box=None if guidance_force_neutral else locked_box_eff,
            pred_box=None if guidance_force_neutral else pred_box_eff,
            override_box=None if guidance_force_neutral else guidance_override_box_eff,
            target_id=target_id,
            target_track_score=target_track_score,
            klt_valid=klt_valid,
            klt_quality=float(klt.quality),
            miss_streak=miss_streak,
            vx=vx_guid,
            vy=vy_guid,
        )

        if DEBUG:
            cv2.putText(frame_orig, f"src={source_kind} p={ACTIVE_ANTI_FP_PROFILE} eff={ew}x{eh} miss={miss_streak} hit={hit_streak} acq={acquire_score} pH={preacq_hits} tAbs={target_absent_frames} swHit={switch_candidate_hits} rHit={yolo_reanchor_memory_hits} rAd={int(yolo_reanchor_adopted)} trH={len(trajectory_hypotheses)} trA={int(trajectory_reanchor_used)} trL={trajectory_det_label} stale={int(stale_lock_active)} fastH={int(fast_handoff_active)} detEvery={det_every} yNoDet={yolo_no_det} mZones={motion_active_zones} fT={flash_ttl} wRT={wavelet_roi_ttl} wRP={wavelet_roi_peak:.2f} aR={ag_ready} aS={ag_stale} aAge={ag_age} aC={ag_cells} aK={ag_rois}",
                        (20, 75), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2)
            cv2.putText(frame_orig, f"YOLO new={int(have_yolo)} dets={yolo_raw_count}->{len(dets_eff)} merge={merged_part_count} infer={infer_ms:.1f}ms mode={yolo_mode} gReset={guidance_reset_event or '-'}",
                        (20, 105), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2)
            cv2.putText(frame_orig, f"KLT valid={int(klt_valid)} q={klt.quality:.2f} pts={klt.good_count} speed={speed:.1f} dt={dt * 1000:.1f}ms dtSrc={dt_source} ch={adaptive_chase_stage} appr={int(approach_active)} fClose={int(close_force_fullscan)} swFast={int(fast_maneuver_guard)} wA={int(wavelet_active)} wCd={wavelet_cooldown} wE={best_wavelet_energy:.3f} wB={best_wavelet_bonus:.2f} wH={best_wavelet_hits} aMS={ag_ms:.1f} aCd={autogaze_cooldown}",
                        (20, 135), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2)
        guidance_ctrl.draw_overlay(frame_orig, guidance_state, sx, sy)

        track_logger.log_frame(
            frame_id=frame_id,
            timestamp_sec=frame_ts,
            dt_sec=dt,
            source_kind=source_kind,
            status=status,
            confirmed=confirmed,
            target_id=target_id,
            target_track_score=target_track_score,
            locked=(locked_box_eff is not None),
            locked_box=locked_box_eff,
            pred_box=pred_box_eff,
            hit_streak=hit_streak,
            miss_streak=miss_streak,
            acquire_score=acquire_score,
            preacq_hits=preacq_hits,
            switch_candidate_hits=switch_candidate_hits,
            best_score=best_score,
            have_yolo=have_yolo,
            yolo_mode=yolo_mode,
            yolo_raw_count=yolo_raw_count,
            det_count=len(dets_eff) if dets_eff else 0,
            merged_part_count=merged_part_count,
            infer_ms=infer_ms,
            stale_lock_active=stale_lock_active,
            stale_lock_reason=stale_lock_reason,
            fast_handoff_active=fast_handoff_active,
            guidance_reset_event=guidance_reset_event,
            used_prediction_hold=used_prediction_hold,
            klt_valid=klt_valid,
            klt_quality=klt.quality,
            klt_points=klt.good_count,
            speed=speed,
            adaptive_chase_stage=adaptive_chase_stage,
            approach_active=approach_active,
            close_force_fullscan=close_force_fullscan,
            fast_maneuver_guard=fast_maneuver_guard,
            motion_active_zones=motion_active_zones,
            wavelet_active=wavelet_active,
            wavelet_energy=best_wavelet_energy,
            wavelet_bonus=best_wavelet_bonus,
            wavelet_hits=best_wavelet_hits,
            autogaze_ready=ag_ready,
            autogaze_stale=ag_stale,
            autogaze_age=ag_age,
            autogaze_cells=ag_cells,
            autogaze_ms=ag_ms,
            guidance_active=guidance_state["active"],
            guidance_status=guidance_state["status"],
            guidance_confidence=guidance_state["confidence"],
            guidance_error_x=guidance_state["error_x"],
            guidance_error_y=guidance_state["error_y"],
            guidance_cmd_x=guidance_state["cmd_x"],
            guidance_cmd_y=guidance_state["cmd_y"],
            guidance_on_target=guidance_state["on_target"],
        )

        if TRAJ_PREDICT_ENABLE:
            if confirmed and locked_box_eff is not None:
                src_ok = bool(
                    (hit_streak > 0)
                    or klt_valid
                    or soft_yolo_adopted
                    or yolo_reanchor_adopted
                    or (miss_streak <= int(TRAJ_HISTORY_KEEP_MISS_LE))
                )
                if src_ok:
                    trajectory_obs_hist.append({
                        "frame": int(frame_id),
                        "ts": float(frame_ts),
                        "center": box_center(locked_box_eff).astype(np.float32),
                        "box": clip_box(locked_box_eff, ew, eh).copy(),
                    })
            else:
                trajectory_obs_hist.clear()

        if writer is not None:
            writer.write(frame_orig)

        if SHOW_OUTPUT:
            cv2.imshow(WINDOW_NAME, frame_orig)

            iter_ms = (time.perf_counter() - iter_start) * 1000.0
            perf_hist.append(iter_ms)

            elapsed = time.perf_counter() - iter_start
            wait_ms = 1
            if VIDEO_REALTIME and target_out_fps > 0.0:
                target_dt = 1.0 / target_out_fps
                if elapsed < target_dt:
                    wait_ms = int((target_dt - elapsed) * 1000.0)
                    wait_ms = clamp(wait_ms, 1, 50)

            key = cv2.waitKey(int(wait_ms)) & 0xFF
            if key == 27:
                break

        if frame_id % 60 == 0 and perf_hist:
            p50 = np.percentile(perf_hist, 50)
            p95 = np.percentile(perf_hist, 95)
            fps = 1000.0 / max(np.mean(perf_hist), 1e-6)
            if yolo_hist:
                yp50 = np.percentile(yolo_hist, 50)
                yp95 = np.percentile(yolo_hist, 95)
            else:
                yp50, yp95 = 0.0, 0.0
            print(f"[perf] fps~{fps:.1f} iter p50={p50:.1f} p95={p95:.1f} | yolo p50={yp50:.1f} p95={yp95:.1f}")

        frame_id += 1

    autogaze_worker.stop()
    yolo_worker.stop()
    track_summary = track_logger.close()
    if track_summary is not None:
        print(
            f"[track-log] confirmed={track_summary['confirmed_frames']}/{track_summary['frames']} "
            f"switches={track_summary['target_switches']} "
            f"csv={track_summary['csv_path']}"
        )
    if writer is not None:
        writer.release()
    cap.release()
    cv2.destroyAllWindows()


if __name__ == "__main__":
    main()
