flywms/flywms_navigation.py

import argparse
import base64
import configparser
import json
import queue
import socket
import sys
import threading
import time
import uuid
from dataclasses import dataclass, field
from pathlib import Path
from urllib import request, error

import cv2
import numpy as np


DEFAULT_CONFIG_PATH = "flywms_navigation.ini"
CUDA_MIN_PIXELS = 640 * 360


def opencv_cuda_available() -> bool:
    try:
        return hasattr(cv2, "cuda") and cv2.cuda.getCudaEnabledDeviceCount() > 0
    except cv2.error:
        return False


OPENCV_CUDA_AVAILABLE = opencv_cuda_available()


def cuda_resize(
    image: np.ndarray,
    size: tuple[int, int],
    interpolation: int = cv2.INTER_LINEAR,
    min_pixels: int = CUDA_MIN_PIXELS,
) -> np.ndarray:
    if not OPENCV_CUDA_AVAILABLE or image.size < min_pixels:
        return cv2.resize(image, size, interpolation=interpolation)
    try:
        gpu = cv2.cuda_GpuMat()
        gpu.upload(image)
        return cv2.cuda.resize(gpu, size, interpolation=interpolation).download()
    except cv2.error:
        return cv2.resize(image, size, interpolation=interpolation)


def cuda_cvt_color(
    image: np.ndarray,
    code: int,
    min_pixels: int = CUDA_MIN_PIXELS,
) -> np.ndarray:
    if not OPENCV_CUDA_AVAILABLE or image.size < min_pixels:
        return cv2.cvtColor(image, code)
    try:
        gpu = cv2.cuda_GpuMat()
        gpu.upload(image)
        return cv2.cuda.cvtColor(gpu, code).download()
    except cv2.error:
        return cv2.cvtColor(image, code)


@dataclass(frozen=True)
class Detection:
    class_id: int
    class_name: str
    confidence: float
    bbox: tuple[int, int, int, int]


@dataclass(frozen=True)
class CandidateSnapshot:
    frame_id: int
    timestamp: float
    capture_pose: dict[str, object]
    frame: np.ndarray
    bbox: tuple[int, int, int, int]
    label_bbox: tuple[int, int, int, int]
    label_confidence: float
    score: float
    center_score: float
    size_score: float
    cut_score: float


@dataclass
class Track:
    id: int
    bbox: tuple[int, int, int, int]
    confidence: float
    first_seen_frame: int
    last_seen_frame: int
    hits: int = 1
    missed: int = 0
    state: str = "entering"
    last_candidate_reason: str = ""
    pending_remote_response: str = "none"
    already_snapshotted: bool = False
    bbox_history: list[tuple[int, int, int, int]] = field(default_factory=list)
    center_history: list[tuple[float, float]] = field(default_factory=list)
    area_history: list[float] = field(default_factory=list)
    candidates: list[CandidateSnapshot] = field(default_factory=list)

    def __post_init__(self) -> None:
        self._append_history(self.bbox)

    def update(self, bbox: tuple[int, int, int, int], confidence: float, frame_id: int) -> None:
        self.bbox = bbox
        self.confidence = confidence
        self.last_seen_frame = frame_id
        self.hits += 1
        self.missed = 0
        self._append_history(bbox)

    def mark_missed(self) -> None:
        self.missed += 1
        if self.missed > 0 and self.state != "snapshotted":
            self.state = "exiting"

    def _append_history(self, bbox: tuple[int, int, int, int]) -> None:
        self.bbox_history.append(bbox)
        self.center_history.append(bbox_center(bbox))
        self.area_history.append(float(bbox_area(bbox)))
        keep = 20
        self.bbox_history = self.bbox_history[-keep:]
        self.center_history = self.center_history[-keep:]
        self.area_history = self.area_history[-keep:]

    def area_trend(self) -> float:
        if len(self.area_history) < 4:
            return 0.0
        old = self.area_history[-4]
        new = self.area_history[-1]
        return (new - old) / max(old, 1.0)


@dataclass(frozen=True)
class NavigationSnapshot:
    snapshot_id: int
    frame_id: int
    timestamp: float
    capture_pose: dict[str, object]
    simulated_position: str
    track_id: int
    bbox: tuple[int, int, int, int]
    label_bbox: tuple[int, int, int, int]
    score: float
    debug_frame_path: str
    ocr_payload_path: str
    label_payload_path: str
    movement_vector_px: tuple[float, float]


@dataclass(frozen=True)
class CapturedFrame:
    frame_id: int
    timestamp: float
    video_time_sec: float
    frame: np.ndarray
    pose: dict[str, object]
    read_ms: float


@dataclass(frozen=True)
class WmsSnapshotJob:
    request_id: str
    snapshot: NavigationSnapshot
    metadata: dict[str, object]
    label_image_path: str
    gaylord_image_path: str | None


@dataclass(frozen=True)
class WmsResult:
    request_id: str
    snapshot_id: int
    status: str
    message: str
    response: dict[str, object] | None = None
    error: str = ""


class UltralyticsDetector:
    def __init__(self, model_path: str, device: str, half: bool):
        from ultralytics import YOLO

        self.model = YOLO(model_path)
        self.device = device
        self.half = bool(half) and str(device).strip().lower() != "cpu"
        names = self.model.names
        if isinstance(names, dict):
            self.classes = [str(names[i]) for i in sorted(names)]
        else:
            self.classes = [str(name) for name in names]

    def detect(
        self,
        frame: np.ndarray,
        min_confidence: float,
        input_size: int,
    ) -> tuple[list[Detection], float]:
        t0 = time.perf_counter()
        results = self.model.predict(
            source=frame,
            imgsz=input_size,
            conf=min_confidence,
            device=self.device,
            half=self.half,
            verbose=False,
        )
        elapsed_ms = (time.perf_counter() - t0) * 1000.0

        detections: list[Detection] = []
        if not results:
            return detections, elapsed_ms

        boxes = results[0].boxes
        if boxes is None:
            return detections, elapsed_ms

        xyxy = boxes.xyxy.cpu().numpy()
        confs = boxes.conf.cpu().numpy()
        clss = boxes.cls.cpu().numpy().astype(int)
        for box, conf, cls_id in zip(xyxy, confs, clss):
            x1, y1, x2, y2 = [int(round(v)) for v in box]
            x1, y1, x2, y2 = clip_box(x1, y1, x2, y2, frame.shape[1], frame.shape[0])
            if x2 <= x1 or y2 <= y1:
                continue
            class_name = self.classes[cls_id] if 0 <= cls_id < len(self.classes) else str(cls_id)
            detections.append(Detection(
                class_id=int(cls_id),
                class_name=class_name,
                confidence=float(conf),
                bbox=(x1, y1, x2, y2),
            ))
        return detections, elapsed_ms


class WmsAsyncClient:
    def __init__(self, args):
        self.args = args
        self.enabled = bool(args.wms_enabled)
        self.jobs: queue.Queue[WmsSnapshotJob | None] = queue.Queue(maxsize=args.wms_queue_max_size)
        self.results: queue.Queue[WmsResult] = queue.Queue()
        self._stop = threading.Event()
        self._thread: threading.Thread | None = None
        if self.enabled:
            self._thread = threading.Thread(target=self._worker, name="flywms-wms-client", daemon=True)
            self._thread.start()

    def close(self) -> None:
        if not self.enabled:
            return
        self._stop.set()
        try:
            self.jobs.put_nowait(None)
        except queue.Full:
            pass
        if self._thread is not None:
            self._thread.join(timeout=2.0)

    def submit(self, snapshot: NavigationSnapshot) -> str | None:
        if not self.enabled:
            return None
        request_id = str(uuid.uuid4())
        metadata = {
            "request_id": request_id,
            "client_id": self.args.wms_client_id,
            "snapshot_id": snapshot.snapshot_id,
            "frame_id": snapshot.frame_id,
            "timestamp": snapshot.timestamp,
            "simulated_position": snapshot.simulated_position,
            "track_id": snapshot.track_id,
            "gaylord_bbox": list(snapshot.bbox),
            "label_bbox": list(snapshot.label_bbox),
            "movement_vector_px": {
                "dx": snapshot.movement_vector_px[0],
                "dy": snapshot.movement_vector_px[1],
            },
        }
        job = WmsSnapshotJob(
            request_id=request_id,
            snapshot=snapshot,
            metadata=metadata,
            label_image_path=snapshot.label_payload_path,
            gaylord_image_path=snapshot.debug_frame_path if self.args.wms_send_gaylord_debug else None,
        )
        try:
            self.jobs.put_nowait(job)
        except queue.Full:
            self.results.put(WmsResult(
                request_id=request_id,
                snapshot_id=snapshot.snapshot_id,
                status="ERROR",
                message="WMS_QUEUE_FULL",
                error="queue full",
            ))
            return request_id
        log(f"[WMS] job accodato request_id={request_id} snapshot={snapshot.snapshot_id}")
        return request_id

    def poll_results(self) -> list[WmsResult]:
        results: list[WmsResult] = []
        while True:
            try:
                results.append(self.results.get_nowait())
            except queue.Empty:
                return results

    def wait_for_result(self, request_id: str | None, timeout_sec: float) -> WmsResult | None:
        if not request_id:
            return None
        deadline = time.perf_counter() + max(0.0, timeout_sec)
        deferred: list[WmsResult] = []
        try:
            while time.perf_counter() <= deadline:
                try:
                    result = self.results.get(timeout=0.05)
                except queue.Empty:
                    continue
                if result.request_id == request_id:
                    return result
                deferred.append(result)
            return None
        finally:
            for result in deferred:
                self.results.put(result)

    def _worker(self) -> None:
        while not self._stop.is_set():
            try:
                job = self.jobs.get(timeout=0.2)
            except queue.Empty:
                continue
            if job is None:
                break
            result = self._send_job(job)
            self.results.put(result)

    def _send_job(self, job: WmsSnapshotJob) -> WmsResult:
        try:
            body, content_type = build_wms_multipart(job)
            req = request.Request(
                self.args.wms_server_url,
                data=body,
                headers={"Content-Type": content_type},
                method="POST",
            )
            with request.urlopen(req, timeout=self.args.wms_timeout_sec) as response:
                payload = json.loads(response.read().decode("utf-8"))
            status = str(payload.get("status", "ERROR"))
            message = str(payload.get("message", ""))
            log(f"[WMS] risposta request_id={job.request_id} status={status} message={message}")
            return WmsResult(job.request_id, job.snapshot.snapshot_id, status, message, payload)
        except (OSError, error.URLError, TimeoutError, json.JSONDecodeError) as exc:
            log(f"[WMS] errore request_id={job.request_id}: {exc}")
            return WmsResult(
                request_id=job.request_id,
                snapshot_id=job.snapshot.snapshot_id,
                status="ERROR",
                message="WMS_SEND_ERROR",
                error=str(exc),
            )


class ObserverPublisher:
    def __init__(self, args):
        self.enabled = bool(getattr(args, "observer_enabled", False))
        self.host = str(getattr(args, "observer_host", "127.0.0.1"))
        self.port = int(getattr(args, "observer_port", 8765))
        self._queue: queue.Queue[dict[str, object] | None] = queue.Queue(maxsize=32)
        self._stop = threading.Event()
        self._thread: threading.Thread | None = None
        if self.enabled:
            self._thread = threading.Thread(target=self._worker, name="flywms-observer-publisher", daemon=True)
            self._thread.start()

    def close(self) -> None:
        if not self.enabled:
            return
        self._stop.set()
        self._enqueue(None)
        if self._thread is not None:
            self._thread.join(timeout=2.0)

    def publish_telemetry(self, payload: dict[str, object]) -> None:
        if not self.enabled:
            return
        self._enqueue({
            "type": "telemetry",
            **payload,
        })

    def publish_preview(
        self,
        stream: str,
        frame: np.ndarray,
        frame_id: int,
        timestamp: float,
        jpeg_quality: int,
    ) -> None:
        if not self.enabled or frame is None:
            return
        ok, encoded = cv2.imencode(
            ".jpg",
            frame,
            [int(cv2.IMWRITE_JPEG_QUALITY), int(max(20, min(100, jpeg_quality)))],
        )
        if not ok:
            return
        self._enqueue({
            "type": "preview",
            "stream": stream,
            "frame_id": int(frame_id),
            "timestamp": float(timestamp),
            "width": int(frame.shape[1]),
            "height": int(frame.shape[0]),
            "encoding": "jpeg-base64",
            "image_b64": base64.b64encode(encoded.tobytes()).decode("ascii"),
        })

    def _enqueue(self, payload: dict[str, object] | None) -> None:
        try:
            self._queue.put_nowait(payload)
            return
        except queue.Full:
            try:
                self._queue.get_nowait()
            except queue.Empty:
                pass
        try:
            self._queue.put_nowait(payload)
        except queue.Full:
            pass

    def _worker(self) -> None:
        server: socket.socket | None = None
        conn: socket.socket | None = None
        try:
            server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
            server.bind((self.host, self.port))
            server.listen(1)
            server.settimeout(0.5)
            log(f"[OBS] publisher in ascolto su {self.host}:{self.port}")
            while not self._stop.is_set():
                if conn is None:
                    try:
                        conn, addr = server.accept()
                        conn.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
                        log(f"[OBS] observer connesso da {addr[0]}:{addr[1]}")
                    except socket.timeout:
                        continue
                    except OSError:
                        break
                try:
                    payload = self._queue.get(timeout=0.2)
                except queue.Empty:
                    continue
                if payload is None:
                    break
                try:
                    raw = (json.dumps(payload, ensure_ascii=True) + "\n").encode("utf-8")
                    conn.sendall(raw)
                except OSError:
                    try:
                        conn.close()
                    except OSError:
                        pass
                    conn = None
        finally:
            if conn is not None:
                try:
                    conn.close()
                except OSError:
                    pass
            if server is not None:
                try:
                    server.close()
                except OSError:
                    pass


class LightweightTracker:
    """Greedy bbox tracker: enough to explain and test navigation decisions."""

    def __init__(
        self,
        max_missed: int,
        min_match_score: float,
        max_center_distance_ratio: float,
    ):
        self.max_missed = max_missed
        self.min_match_score = min_match_score
        self.max_center_distance_ratio = max_center_distance_ratio
        self._next_id = 1
        self.tracks: dict[int, Track] = {}

    def update(
        self,
        detections: list[Detection],
        frame_id: int,
        frame_width: int,
    ) -> list[Track]:
        unmatched_tracks = set(self.tracks.keys())
        unmatched_detections = set(range(len(detections)))
        pairs: list[tuple[float, int, int]] = []

        max_center_distance = max(1.0, frame_width * self.max_center_distance_ratio)
        for track_id, track in self.tracks.items():
            for det_idx, det in enumerate(detections):
                score = association_score(track.bbox, det.bbox, max_center_distance)
                if score >= self.min_match_score:
                    pairs.append((score, track_id, det_idx))

        pairs.sort(reverse=True, key=lambda item: item[0])
        for _, track_id, det_idx in pairs:
            if track_id not in unmatched_tracks or det_idx not in unmatched_detections:
                continue
            det = detections[det_idx]
            self.tracks[track_id].update(det.bbox, det.confidence, frame_id)
            unmatched_tracks.remove(track_id)
            unmatched_detections.remove(det_idx)

        for track_id in list(unmatched_tracks):
            self.tracks[track_id].mark_missed()
            if self.tracks[track_id].missed > self.max_missed:
                del self.tracks[track_id]

        for det_idx in unmatched_detections:
            det = detections[det_idx]
            track_id = self._next_id
            self._next_id += 1
            self.tracks[track_id] = Track(
                id=track_id,
                bbox=det.bbox,
                confidence=det.confidence,
                first_seen_frame=frame_id,
                last_seen_frame=frame_id,
            )

        return list(self.tracks.values())


class NavigationController:
    def __init__(self, args):
        self.args = args
        self.output_dir = Path(args.snapshot_output_dir)
        self.output_dir.mkdir(parents=True, exist_ok=True)
        self.metadata_path = self.output_dir / "snapshots.jsonl"
        self.snapshot_counter = 0
        self.position_counter = 0
        self.last_command_text = ""
        self.last_command_lines: list[str] = []
        self.last_snapshot_frame: np.ndarray | None = None
        self.last_ocr_payload_frame: np.ndarray | None = None
        self.last_label_payload_frame: np.ndarray | None = None
        self.last_remote_result_text = ""
        self.motion_text = "MOTO: n/d"
        self.label_movement_arrow: tuple[tuple[int, int], tuple[int, int]] | None = None

    def process_track(
        self,
        track: Track,
        captured: CapturedFrame,
        labels: list[Detection] | None = None,
    ) -> NavigationSnapshot | None:
        labels = labels or []
        frame = captured.frame
        frame_id = captured.frame_id
        timestamp = captured.timestamp
        frame_h, frame_w = frame.shape[:2]
        eligible, score_parts = self._is_snapshot_candidate(track, frame_w, frame_h)
        self._update_track_state(track, eligible, frame_w)

        if track.already_snapshotted:
            return None

        if eligible:
            label = find_label_inside_bbox(track.bbox, labels)
            if label is None:
                track.last_candidate_reason = "label_missing"
                self.last_command_text = f"ETICHETTA_NON_TROVATA track={track.id}"
                self.last_command_lines = [
                    self.last_command_text,
                    "MICRO_RICERCA_ETICHETTA",
                    "ATTENDI_FRAME_SUCCESSIVO",
                ]
                log(f"[NAV] ETICHETTA_NON_TROVATA track={track.id}: attendo frame successivo")
                return None
            candidate = CandidateSnapshot(
                frame_id=frame_id,
                timestamp=timestamp,
                capture_pose=dict(captured.pose),
                frame=frame.copy(),
                bbox=track.bbox,
                label_bbox=label.bbox,
                label_confidence=label.confidence,
                score=score_parts["score"],
                center_score=score_parts["center_score"],
                size_score=score_parts["size_score"],
                cut_score=score_parts["cut_score"],
            )
            track.candidates.append(candidate)
            track.candidates = track.candidates[-self.args.snapshot_window_frames:]

            if len(track.candidates) >= self.args.snapshot_window_frames:
                return self._finalize_snapshot(track)
        elif track.candidates:
            return self._finalize_snapshot(track)

        return None

    def _is_snapshot_candidate(
        self,
        track: Track,
        frame_w: int,
        frame_h: int,
    ) -> tuple[bool, dict[str, float]]:
        x1, y1, x2, y2 = track.bbox
        cx, cy = bbox_center(track.bbox)
        center_x = frame_w * 0.5
        center_tolerance = max(1.0, frame_w * self.args.center_tolerance_ratio)
        snapshot_tolerance = max(1.0, frame_w * self.args.snapshot_line_tolerance_ratio)
        center_delta = abs(cx - center_x)
        center_score = max(0.0, 1.0 - center_delta / center_tolerance)

        area_ratio = bbox_area(track.bbox) / float(frame_w * frame_h)
        size_score = min(1.0, area_ratio / max(self.args.min_gaylord_area_ratio * 4.0, 0.001))

        if self.args.edge_margin_ratio <= 0:
            cut = False
        else:
            edge_margin_x = frame_w * self.args.edge_margin_ratio
            edge_margin_y = frame_h * self.args.edge_margin_ratio
            cut = (
                x1 <= edge_margin_x
                or y1 <= edge_margin_y
                or x2 >= frame_w - edge_margin_x
                or y2 >= frame_h - edge_margin_y
            )
        cut_score = 0.0 if cut else 1.0
        score = 0.50 * center_score + 0.30 * size_score + 0.20 * cut_score

        in_center_band = center_delta <= center_tolerance
        on_snapshot_line = center_delta <= snapshot_tolerance
        in_y_band = (
            frame_h * self.args.usable_y_min_ratio
            <= cy
            <= frame_h * self.args.usable_y_max_ratio
        )
        enough_hits = track.hits >= self.args.min_track_hits
        large_enough = area_ratio >= self.args.min_gaylord_area_ratio
        trend_ok = track.area_trend() >= self.args.min_area_trend
        eligible = (
            enough_hits
            and on_snapshot_line
            and in_y_band
            and large_enough
            and not cut
            and trend_ok
            and track.missed == 0
        )
        failed: list[str] = []
        if not enough_hits:
            failed.append(f"hits<{self.args.min_track_hits}")
        if not in_center_band:
            failed.append(f"outside_band={center_delta:.0f}>{center_tolerance:.0f}")
        elif not on_snapshot_line:
            failed.append(f"wait_line={center_delta:.0f}>{snapshot_tolerance:.0f}")
        if not in_y_band:
            failed.append("y_band")
        if not large_enough:
            failed.append(f"area={area_ratio:.3f}<{self.args.min_gaylord_area_ratio:.3f}")
        if cut:
            failed.append("edge_cut")
        if not trend_ok:
            failed.append(f"trend={track.area_trend():+.2f}<{self.args.min_area_trend:+.2f}")
        if track.missed != 0:
            failed.append(f"missed={track.missed}")
        track.last_candidate_reason = "ok" if eligible else ",".join(failed)
        return eligible, {
            "score": score,
            "center_score": center_score,
            "size_score": size_score,
            "cut_score": cut_score,
        }

    def _update_track_state(self, track: Track, eligible: bool, frame_w: int) -> None:
        if track.already_snapshotted:
            track.state = "snapshotted"
            return
        if track.missed > 0:
            track.state = "exiting"
            return
        cx, _ = bbox_center(track.bbox)
        center_delta = abs(cx - frame_w * 0.5)
        snapshot_tolerance = frame_w * self.args.snapshot_line_tolerance_ratio
        if eligible:
            track.state = "centered"
        elif track.hits < self.args.min_track_hits:
            track.state = "entering"
        elif center_delta <= snapshot_tolerance:
            track.state = "centered"
        elif center_delta <= frame_w * self.args.center_tolerance_ratio:
            track.state = "candidate"
        else:
            track.state = "entering"

    def _finalize_snapshot(self, track: Track) -> NavigationSnapshot | None:
        if not track.candidates:
            return None
        best = max(track.candidates, key=lambda item: item.score)
        track.candidates.clear()
        track.already_snapshotted = True
        track.state = "snapshotted"

        self.snapshot_counter += 1
        self.position_counter += 1
        simulated_position = f"gaylord {self.position_counter}"
        debug_name = f"snapshot_{self.snapshot_counter:04d}_track_{track.id:03d}_frame.jpg"
        payload_name = f"snapshot_{self.snapshot_counter:04d}_track_{track.id:03d}_ocr_payload.jpg"
        label_payload_name = f"snapshot_{self.snapshot_counter:04d}_track_{track.id:03d}_label_payload.jpg"
        debug_path = self.output_dir / debug_name
        payload_path = self.output_dir / payload_name
        label_payload_path = self.output_dir / label_payload_name
        cv2.imwrite(str(debug_path), best.frame)
        ocr_payload = crop_with_padding(
            best.frame,
            best.bbox,
            self.args.ocr_payload_pad_ratio,
        )
        cv2.imwrite(str(payload_path), ocr_payload)
        label_payload = crop_with_padding(
            best.frame,
            best.label_bbox,
            self.args.label_payload_pad_ratio,
        )
        cv2.imwrite(str(label_payload_path), label_payload)
        self.last_snapshot_frame = best.frame.copy()
        self.last_ocr_payload_frame = ocr_payload.copy()
        self.last_label_payload_frame = label_payload.copy()

        gaylord_center = bbox_center(best.bbox)
        label_center = bbox_center(best.label_bbox)
        movement_vector = (
            label_center[0] - gaylord_center[0],
            label_center[1] - gaylord_center[1],
        )
        self.label_movement_arrow = (
            (int(round(gaylord_center[0])), int(round(gaylord_center[1]))),
            (int(round(label_center[0])), int(round(label_center[1]))),
        )

        snapshot = NavigationSnapshot(
            snapshot_id=self.snapshot_counter,
            frame_id=best.frame_id,
            timestamp=best.timestamp,
            capture_pose=dict(best.capture_pose),
            simulated_position=simulated_position,
            track_id=track.id,
            bbox=best.bbox,
            label_bbox=best.label_bbox,
            score=best.score,
            debug_frame_path=str(debug_path),
            ocr_payload_path=str(payload_path),
            label_payload_path=str(label_payload_path),
            movement_vector_px=movement_vector,
        )
        self._write_metadata(snapshot)
        self._print_commands(snapshot)
        return snapshot

    def simulate_remote_response(self, snapshot: NavigationSnapshot) -> str:
        mode = self.args.remote_ack_mode
        if mode == "always-ack":
            result = "ACK"
        elif mode == "always-nack":
            result = "NACK"
        else:
            result = "ACK" if snapshot.snapshot_id % 2 == 1 else "NACK"

        if result == "ACK":
            self.last_remote_result_text = "ACK_RICEVUTO: codice valido su WMS"
            resume_command = f"RIPARTI_{self.args.scan_direction.upper()}"
            self.last_command_lines.extend([
                self.last_remote_result_text,
                resume_command,
            ])
            log("[REMOTE] ACK_RICEVUTO codice valido su WMS")
            log(f"[CMD] {resume_command}")
        else:
            self.last_remote_result_text = "NACK_RICEVUTO: riprovare foto"
            self.last_command_lines.extend([
                self.last_remote_result_text,
                "MICRO_MOVE_CORRETTIVO",
                "SCATTA_FOTO_RETRY",
            ])
            log("[REMOTE] NACK_RICEVUTO codice assente/non valido")
            log("[CMD] MICRO_MOVE_CORRETTIVO")
            log("[CMD] SCATTA_FOTO_RETRY")
        return result

    def apply_wms_result(self, result: WmsResult | None, snapshot: NavigationSnapshot) -> str:
        if result is None:
            self.last_remote_result_text = "WMS_TIMEOUT: nessuna risposta entro il timeout"
            self.last_command_lines.extend([
                self.last_remote_result_text,
                "MICRO_MOVE_CORRETTIVO",
                "SCATTA_FOTO_RETRY",
            ])
            log("[WMS] TIMEOUT nessuna risposta entro il timeout")
            return "TIMEOUT"

        status = result.status.upper()
        if status == "ACK":
            message = result.message or "codice valido su WMS"
            ocr_text = ""
            if result.response:
                ocr_text = str(result.response.get("ocr_text") or result.response.get("fake_ocr_text") or "")
            self.last_remote_result_text = f"WMS_ACK: {message}"
            resume_command = f"RIPARTI_{self.args.scan_direction.upper()}"
            lines = [self.last_remote_result_text]
            if ocr_text:
                lines.append(f"OCR_CODICE {ocr_text}")
            lines.append(resume_command)
            self.last_command_lines.extend(lines)
            log(f"[WMS] ACK request_id={result.request_id} snapshot={result.snapshot_id} {message}")
            log(f"[CMD] {resume_command}")
            return "ACK"

        if status == "NACK":
            message = result.message or "codice non riconosciuto"
            self.last_remote_result_text = f"WMS_NACK: {message}"
            self.last_command_lines.extend([
                self.last_remote_result_text,
                "MICRO_MOVE_CORRETTIVO",
                "SCATTA_FOTO_RETRY",
            ])
            log(f"[WMS] NACK request_id={result.request_id} snapshot={result.snapshot_id} {message}")
            return "NACK"

        message = result.error or result.message or "errore WMS"
        self.last_remote_result_text = f"WMS_ERROR: {message}"
        self.last_command_lines.extend([
            self.last_remote_result_text,
            "MICRO_MOVE_CORRETTIVO",
            "SCATTA_FOTO_RETRY",
        ])
        log(f"[WMS] ERROR request_id={result.request_id} snapshot={result.snapshot_id} {message}")
        return "ERROR"

    def set_motion_text(self, text: str) -> None:
        self.motion_text = text

    def _write_metadata(self, snapshot: NavigationSnapshot) -> None:
        record = {
            "snapshot_id": snapshot.snapshot_id,
            "frame_id": snapshot.frame_id,
            "timestamp": snapshot.timestamp,
            "simulated_position": snapshot.simulated_position,
            "drone_pose_simulated": dict(snapshot.capture_pose),
            "track_id": snapshot.track_id,
            "gaylord_bbox": list(snapshot.bbox),
            "label_bbox": list(snapshot.label_bbox),
            "score": snapshot.score,
            "movement_vector_px": {
                "dx": snapshot.movement_vector_px[0],
                "dy": snapshot.movement_vector_px[1],
            },
            "debug_frame_path": snapshot.debug_frame_path,
            "ocr_payload_path": snapshot.ocr_payload_path,
            "label_payload_path": snapshot.label_payload_path,
        }
        with self.metadata_path.open("at", encoding="utf-8") as f:
            f.write(json.dumps(record, ensure_ascii=True) + "\n")

    def _print_commands(self, snapshot: NavigationSnapshot) -> None:
        self.last_command_text = (
            f"SNAPSHOT {snapshot.snapshot_id:04d} "
            f"track={snapshot.track_id} frame={snapshot.frame_id} "
            f"pos={snapshot.simulated_position} score={snapshot.score:.2f}"
        )
        dx, dy = snapshot.movement_vector_px
        self.last_command_lines = [
            self.last_command_text,
            "STOP",
            f"ASSOCIA_POSIZIONE {snapshot.simulated_position}",
            f"ASSOCIA_ETICHETTA {snapshot.simulated_position} - etichetta {snapshot.snapshot_id}",
            f"CENTRA_ETICHETTA dx={dx:+.0f}px dy={dy:+.0f}px durata={self.args.label_move_sec:.1f}s",
            f"ATTENDI_STABILIZZAZIONE {self.args.label_stabilization_sec:.1f}s",
            f"SCATTA_FOTO_ETICHETTA {Path(snapshot.label_payload_path).name}",
            f"RITORNA_CENTRO_GAYLORD durata={self.args.label_return_sec:.1f}s",
            f"SCATTA_FOTO_GAYLORD {Path(snapshot.debug_frame_path).name}",
            f"INVIA_ROI_REMOTA {Path(snapshot.label_payload_path).name}",
            f"ATTENDI_ACK timeout={self.args.remote_ack_timeout_sec:.1f}s",
        ]
        log(f"[NAV] {self.last_command_text}")
        log("[CMD] STOP")
        log(f"[CMD] ASSOCIA_POSIZIONE {snapshot.simulated_position}")
        log(f"[CMD] ASSOCIA_ETICHETTA {snapshot.simulated_position} - etichetta {snapshot.snapshot_id}")
        log(f"[CMD] CENTRA_ETICHETTA dx={dx:+.0f}px dy={dy:+.0f}px durata={self.args.label_move_sec:.1f}s")
        log(f"[CMD] ATTENDI_STABILIZZAZIONE {self.args.label_stabilization_sec:.1f}s")
        log(f"[CMD] SCATTA_FOTO_ETICHETTA {Path(snapshot.label_payload_path).name}")
        log(f"[CMD] RITORNA_CENTRO_GAYLORD durata={self.args.label_return_sec:.1f}s")
        log(f"[CMD] SCATTA_FOTO_GAYLORD {Path(snapshot.debug_frame_path).name}")
        log(f"[CMD] INVIA_ROI_REMOTA {Path(snapshot.label_payload_path).name}")
        log(f"[CMD] ATTENDI_ACK timeout={self.args.remote_ack_timeout_sec:.1f}s")

    def set_label_sequence_phase(self, snapshot: NavigationSnapshot, phase: str) -> None:
        dx, dy = snapshot.movement_vector_px
        base = [
            self.last_command_text,
            "STOP",
            f"ASSOCIA_POSIZIONE {snapshot.simulated_position}",
            f"ASSOCIA_ETICHETTA {snapshot.simulated_position} - etichetta {snapshot.snapshot_id}",
        ]
        if phase == "move":
            detail = [
                f"CENTRA_ETICHETTA dx={dx:+.0f}px dy={dy:+.0f}px durata={self.args.label_move_sec:.1f}s",
                "FRECCIA_MOVIMENTO_GAYLORD_ETICHETTA",
            ]
        elif phase == "stabilize":
            detail = [
                "DRONE_CENTRATO_SU_ETICHETTA",
                f"ATTENDI_STABILIZZAZIONE {self.args.label_stabilization_sec:.1f}s",
            ]
        elif phase == "capture":
            detail = [
                f"SCATTA_FOTO_ETICHETTA {Path(snapshot.label_payload_path).name}",
                f"INVIA_ROI_REMOTA {Path(snapshot.label_payload_path).name}",
            ]
        elif phase == "wait_wms":
            detail = [
                f"INVIA_ROI_REMOTA {Path(snapshot.label_payload_path).name}",
                f"ATTENDI_WMS timeout={self.args.remote_ack_timeout_sec:.1f}s",
            ]
        elif phase == "return":
            rdx = -dx
            rdy = -dy
            detail = [
                f"RITORNA_CENTRO_GAYLORD dx={rdx:+.0f}px dy={rdy:+.0f}px durata={self.args.label_return_sec:.1f}s",
                "FRECCIA_RITORNO_ETICHETTA_GAYLORD",
            ]
        else:
            detail = []
        self.last_command_lines = base + detail


def parse_args():
    pre = argparse.ArgumentParser(add_help=False)
    pre.add_argument("--config", default=DEFAULT_CONFIG_PATH, help="File configurazione INI")
    pre_args, _ = pre.parse_known_args()
    defaults = load_navigation_config(pre_args.config)

    ap = argparse.ArgumentParser(parents=[pre])
    ap.add_argument("-v", "--video", default=defaults["video"], help="Percorso video. Se omesso usa webcam 0")
    ap.add_argument(
        "--weights",
        default=defaults["weights"],
        help="Modello Ultralytics .pt",
    )
    ap.add_argument("--ultralytics-device", default=defaults["ultralytics_device"], help="Device Ultralytics: cpu oppure 0")
    ap.add_argument("--yolo-half", action=argparse.BooleanOptionalAction, default=defaults["yolo_half"],
                    help="Usa FP16/half precision per YOLO quando il device e' GPU")
    ap.add_argument("--input-size", type=int, default=defaults["input_size"], help="Dimensione input YOLO")
    ap.add_argument("--min-confidence", type=float, default=defaults["min_confidence"], help="Confidenza minima")
    ap.add_argument("--target-class", default=defaults["target_class"], help="Classe da tracciare")
    ap.add_argument("--label-class", default=defaults["label_class"], help="Classe etichetta da associare al gaylord")

    ap.add_argument("--max-track-missed", type=int, default=defaults["max_track_missed"], help="Frame persi prima di rimuovere una track")
    ap.add_argument("--min-match-score", type=float, default=defaults["min_match_score"], help="Soglia associazione detection-track")
    ap.add_argument("--max-center-distance-ratio", type=float, default=defaults["max_center_distance_ratio"], help="Distanza max centri per matching")

    ap.add_argument("--center-tolerance-ratio", type=float, default=defaults["center_tolerance_ratio"], help="Mezza ampiezza zona centrale")
    ap.add_argument("--snapshot-line-tolerance-ratio", type=float, default=defaults["snapshot_line_tolerance_ratio"],
                    help="Tolleranza stretta dalla linea centrale per scattare")
    ap.add_argument("--usable-y-min-ratio", type=float, default=defaults["usable_y_min_ratio"], help="Limite alto fascia utile Y")
    ap.add_argument("--usable-y-max-ratio", type=float, default=defaults["usable_y_max_ratio"], help="Limite basso fascia utile Y")
    ap.add_argument("--min-track-hits", type=int, default=defaults["min_track_hits"], help="Detection consecutive minime")
    ap.add_argument("--min-gaylord-area-ratio", type=float, default=defaults["min_gaylord_area_ratio"], help="Area bbox minima sul frame")
    ap.add_argument("--edge-margin-ratio", type=float, default=defaults["edge_margin_ratio"], help="Margine per considerare bbox tagliato")
    ap.add_argument("--ocr-payload-pad-ratio", type=float, default=defaults["ocr_payload_pad_ratio"],
                    help="Padding intorno al bbox centrale inviato all'OCR remoto")
    ap.add_argument("--label-payload-pad-ratio", type=float, default=defaults["label_payload_pad_ratio"],
                    help="Padding intorno al bbox etichetta inviato all'OCR remoto")
    ap.add_argument("--min-area-trend", type=float, default=defaults["min_area_trend"], help="Trend area minimo ammesso")
    ap.add_argument("--snapshot-window-frames", type=int, default=defaults["snapshot_window_frames"], help="Candidati da valutare prima dello snapshot")
    ap.add_argument("--snapshot-output-dir", default=defaults["snapshot_output_dir"], help="Directory snapshot e JSONL")
    ap.add_argument("--remote-ack-timeout-sec", type=float, default=defaults["remote_ack_timeout_sec"],
                    help="Tempo simulato di attesa OCR remoto/WMS")
    ap.add_argument("--remote-ack-mode", choices=["always-ack", "always-nack", "alternate"],
                    default=defaults["remote_ack_mode"], help="Risposta remota simulata")
    ap.add_argument("--wms-enabled", action="store_true", default=defaults["wms_enabled"],
                    help="Invia realmente snapshot al WMS demo")
    ap.add_argument("--wms-server-url", default=defaults["wms_server_url"], help="Endpoint HTTP WMS")
    ap.add_argument("--wms-client-id", default=defaults["wms_client_id"], help="Identificativo client WMS")
    ap.add_argument("--wms-timeout-sec", type=float, default=defaults["wms_timeout_sec"], help="Timeout chiamata WMS")
    ap.add_argument("--wms-queue-max-size", type=int, default=defaults["wms_queue_max_size"], help="Massimo job WMS in coda")
    ap.add_argument("--wms-send-gaylord-debug", action="store_true", default=defaults["wms_send_gaylord_debug"],
                    help="Invia anche immagine/debug gaylord al server WMS")
    ap.add_argument("--scan-direction", choices=["destra", "sinistra"], default=defaults["scan_direction"],
                    help="Direzione simulata di ripartenza dopo ACK")
    ap.add_argument("--label-move-sec", type=float, default=defaults["label_move_sec"],
                    help="Durata simulata movimento verso etichetta")
    ap.add_argument("--label-stabilization-sec", type=float, default=defaults["label_stabilization_sec"],
                    help="Attesa simulata stabilizzazione su etichetta")
    ap.add_argument("--label-return-sec", type=float, default=defaults["label_return_sec"],
                    help="Durata simulata ritorno al centro gaylord")

    ap.add_argument("--preview-width", type=int, default=defaults["preview_width"], help="Larghezza preview")
    ap.add_argument("--benchmark-mode", action=argparse.BooleanOptionalAction, default=defaults["benchmark_mode"],
                    help="Profilo benchmark: niente finestre, preview a 30 fps e log tempi dedicato")
    ap.add_argument("--benchmark-preview-fps", type=float, default=defaults["benchmark_preview_fps"],
                    help="FPS target preview/cattura usati dal profilo benchmark")
    ap.add_argument("--realtime-playback", action="store_true", default=defaults["realtime_playback"], help="Rispetta FPS video")
    ap.add_argument("--preview-fps", type=float, default=defaults["preview_fps"],
                    help="FPS massimo per lettura/preview realtime. 0 = FPS sorgente")
    ap.add_argument("--yolo-fps", type=float, default=defaults["yolo_fps"],
                    help="FPS massimo per inferenza YOLO. 0 = ogni frame di preview")
    ap.add_argument("--adaptive-yolo-enabled", action=argparse.BooleanOptionalAction, default=defaults["adaptive_yolo_enabled"],
                    help="Abilita scheduling adattivo di YOLO in base allo stato delle track")
    ap.add_argument("--idle-yolo-fps", type=float, default=defaults["idle_yolo_fps"],
                    help="FPS YOLO quando non ci sono track attive")
    ap.add_argument("--tracking-yolo-fps", type=float, default=defaults["tracking_yolo_fps"],
                    help="FPS YOLO quando ci sono track attive ma non ancora critiche")
    ap.add_argument("--critical-yolo-fps", type=float, default=defaults["critical_yolo_fps"],
                    help="FPS YOLO quando una track e' candidate/centered")
    ap.add_argument("--max-frames", type=int, default=defaults["max_frames"], help="Numero massimo frame; 0 = tutto")
    ap.add_argument("--stats-interval", type=float, default=defaults["stats_interval"], help="Intervallo log prestazioni")
    ap.add_argument("--perf-log-path", default=defaults["perf_log_path"], help="File log tempi dettagliati")
    ap.add_argument("--perf-log-flush-interval-sec", type=float, default=defaults["perf_log_flush_interval_sec"],
                    help="Intervallo flush file log tempi")
    ap.add_argument("--perf-log-flush-lines", type=int, default=defaults["perf_log_flush_lines"],
                    help="Numero righe bufferizzate prima del flush del log tempi")
    ap.add_argument("--motion-report-interval", type=int, default=defaults["motion_report_interval"],
                    help="Ogni quanti frame aggiornare la direzione moto stimata")
    ap.add_argument("--motion-min-pixels", type=float, default=defaults["motion_min_pixels"],
                    help="Spostamento medio minimo per dichiarare una direzione")
    ap.add_argument("--debug-tracks", action="store_true", default=defaults["debug_tracks"], help="Logga stato e criteri delle track")
    ap.add_argument("--flash-alpha", type=float, default=defaults["flash_alpha"], help="Intensita' flash 0..1 al momento dello scatto")
    ap.add_argument("--no-display", action="store_true", default=defaults["no_display"], help="Disabilita finestra video")
    ap.add_argument("--observer-enabled", action=argparse.BooleanOptionalAction, default=defaults["observer_enabled"],
                    help="Abilita observer esterno su socket locale; la UI locale viene disattivata")
    ap.add_argument("--observer-host", default=defaults["observer_host"], help="Host observer, tipicamente 127.0.0.1")
    ap.add_argument("--observer-port", type=int, default=defaults["observer_port"], help="Porta TCP observer")
    ap.add_argument("--observer-preview-fps", type=float, default=defaults["observer_preview_fps"],
                    help="FPS massimi delle preview inviate all'observer")
    ap.add_argument("--observer-preview-width", type=int, default=defaults["observer_preview_width"],
                    help="Larghezza massima delle preview inviate all'observer")
    ap.add_argument("--observer-jpeg-quality", type=int, default=defaults["observer_jpeg_quality"],
                    help="Qualita' JPEG 20..100 per le preview inviate all'observer")
    ap.add_argument("--observer-telemetry-fps", type=float, default=defaults["observer_telemetry_fps"],
                    help="Frequenza telemetria observer in Hz")
    ap.add_argument("--window-layout-enabled", action="store_true", default=defaults["window_layout_enabled"],
                    help="Posiziona e ridimensiona le finestre OpenCV")
    ap.add_argument("--navigate-window", default=defaults["navigate_window"], help="Layout finestra navigate: x,y,w,h")
    ap.add_argument("--commands-window", default=defaults["commands_window"], help="Layout finestra comandi: x,y,w,h")
    ap.add_argument("--snapshot-window", default=defaults["snapshot_window"], help="Layout finestra snapshot: x,y,w,h")
    ap.add_argument("--label-window", default=defaults["label_window"], help="Layout finestra etichetta: x,y,w,h")
    return ap.parse_args()


def load_navigation_config(path_str: str) -> dict[str, object]:
    defaults: dict[str, object] = {
        "video": "testhd.mp4",
        "weights": r"C:\devel\flywms\runs\flywms_yolo11n_quick20\weights\best.pt",
        "ultralytics_device": "cpu",
        "yolo_half": True,
        "input_size": 640,
        "min_confidence": 0.25,
        "target_class": "gaylord",
        "label_class": "etichetta",
        "max_track_missed": 8,
        "min_match_score": 0.25,
        "max_center_distance_ratio": 0.18,
        "center_tolerance_ratio": 0.18,
        "snapshot_line_tolerance_ratio": 0.035,
        "usable_y_min_ratio": 0.15,
        "usable_y_max_ratio": 0.85,
        "min_track_hits": 3,
        "min_gaylord_area_ratio": 0.02,
        "edge_margin_ratio": 0.0,
        "ocr_payload_pad_ratio": 0.03,
        "label_payload_pad_ratio": 0.20,
        "min_area_trend": -0.35,
        "snapshot_window_frames": 1,
        "snapshot_output_dir": "navigate_snapshots",
        "remote_ack_timeout_sec": 3.0,
        "remote_ack_mode": "always-ack",
        "wms_enabled": False,
        "wms_server_url": "http://127.0.0.1:8088/api/v1/navigation-snapshot",
        "wms_client_id": "flywms-demo-01",
        "wms_timeout_sec": 2.0,
        "wms_queue_max_size": 8,
        "wms_send_gaylord_debug": True,
        "scan_direction": "destra",
        "label_move_sec": 3.0,
        "label_stabilization_sec": 2.0,
        "label_return_sec": 3.0,
        "preview_width": 1280,
        "benchmark_mode": False,
        "benchmark_preview_fps": 30.0,
        "realtime_playback": True,
        "preview_fps": 24.0,
        "yolo_fps": 15.0,
        "adaptive_yolo_enabled": False,
        "idle_yolo_fps": 8.0,
        "tracking_yolo_fps": 12.0,
        "critical_yolo_fps": 15.0,
        "max_frames": 0,
        "stats_interval": 2.0,
        "perf_log_path": "tempistiche.txt",
        "perf_log_flush_interval_sec": 2.0,
        "perf_log_flush_lines": 120,
        "motion_report_interval": 5,
        "motion_min_pixels": 1.5,
        "debug_tracks": True,
        "flash_alpha": 0.70,
        "no_display": False,
        "observer_enabled": False,
        "observer_host": "127.0.0.1",
        "observer_port": 8765,
        "observer_preview_fps": 4.0,
        "observer_preview_width": 960,
        "observer_jpeg_quality": 75,
        "observer_telemetry_fps": 8.0,
        "window_layout_enabled": True,
        "navigate_window": "20,40,1100,620",
        "commands_window": "1140,40,760,520",
        "snapshot_window": "1140,590,520,360",
        "label_window": "1140,980,520,260",
    }

    path = Path(path_str)
    if not path.exists():
        return defaults

    parser = configparser.ConfigParser()
    parser.read(path, encoding="utf-8")
    section = parser["navigation"] if parser.has_section("navigation") else {}

    types = {key: type(value) for key, value in defaults.items()}
    for key, default_value in defaults.items():
        if key not in section:
            continue
        if types[key] is bool:
            defaults[key] = parser.getboolean("navigation", key, fallback=bool(default_value))
        elif types[key] is int:
            defaults[key] = parser.getint("navigation", key, fallback=int(default_value))
        elif types[key] is float:
            defaults[key] = parser.getfloat("navigation", key, fallback=float(default_value))
        else:
            value = section.get(key, str(default_value)).strip()
            defaults[key] = None if value.lower() in ("", "none", "null") else value
    return defaults


def log(msg: str) -> None:
    print(f"[{time.strftime('%H:%M:%S')}] {msg}", flush=True)


def format_fps_value(value: float | int | None) -> str:
    if value is None:
        return "n/d"
    try:
        numeric = float(value)
    except (TypeError, ValueError):
        return "n/d"
    if numeric <= 0:
        return "n/d"
    return f"{numeric:.1f}"


def format_seconds(value: float | None) -> str:
    if value is None:
        return "n/d"
    try:
        numeric = float(value)
    except (TypeError, ValueError):
        return "n/d"
    if numeric < 0:
        numeric = 0.0
    minutes = int(numeric // 60)
    seconds = numeric - (minutes * 60)
    return f"{minutes:02d}:{seconds:05.2f}"


def choose_adaptive_yolo_fps(args, tracks: list[Track]) -> tuple[float, str]:
    if not getattr(args, "adaptive_yolo_enabled", False):
        return float(args.yolo_fps or 0.0), "fixed"

    active_tracks = [t for t in tracks if t.missed == 0 and not t.already_snapshotted]
    if not active_tracks:
        return float(args.idle_yolo_fps or 0.0), "idle"

    critical_states = {"candidate", "centered"}
    if any(t.state in critical_states for t in active_tracks):
        return float(args.critical_yolo_fps or 0.0), "critical"

    return float(args.tracking_yolo_fps or 0.0), "tracking"


class PerfLogWriter:
    def __init__(self, path_str: str, flush_interval_sec: float, flush_lines: int):
        self.path = Path(path_str)
        self.path.parent.mkdir(parents=True, exist_ok=True)
        self.file = self.path.open("a", encoding="utf-8")
        self.flush_interval_sec = max(0.2, float(flush_interval_sec))
        self.flush_lines = max(1, int(flush_lines))
        self.buffered_lines = 0
        self.last_flush = time.perf_counter()

    def write(self, line: str) -> None:
        self.file.write(line.rstrip("\n") + "\n")
        self.buffered_lines += 1
        self.maybe_flush()

    def maybe_flush(self) -> None:
        now = time.perf_counter()
        if self.buffered_lines >= self.flush_lines or (now - self.last_flush) >= self.flush_interval_sec:
            self.file.flush()
            self.buffered_lines = 0
            self.last_flush = now

    def close(self) -> None:
        try:
            self.file.flush()
        finally:
            self.file.close()


def safe_progress_ratio(frame_id: int, total_frames: int) -> float:
    if total_frames <= 0:
        return 0.0
    return min(max((frame_id - 1) / max(total_frames - 1, 1), 0.0), 1.0)


def sample_demo_pose(
    args,
    frame_id: int,
    video_time_sec: float,
    total_frames: int,
) -> dict[str, object]:
    return {
        "mode": "video_demo_capture",
        "frame_id": int(frame_id),
        "video_time_sec": float(video_time_sec),
        "scan_direction": str(args.scan_direction),
        "route_progress_ratio": float(safe_progress_ratio(frame_id, total_frames)),
    }


class CaptureWorker:
    def __init__(
        self,
        cap: cv2.VideoCapture,
        args,
        video_fps: float,
        frame_delay: float,
        total_frames: int,
    ) -> None:
        self.cap = cap
        self.args = args
        self.video_fps = float(video_fps or 0.0)
        self.frame_delay = float(frame_delay or 0.0)
        self.total_frames = int(total_frames)
        self.queue: queue.Queue[CapturedFrame | None] = queue.Queue(maxsize=1)
        self.stop_event = threading.Event()
        self.thread = threading.Thread(target=self._run, name="capture-worker", daemon=True)
        self.last_emit = time.perf_counter()

    def start(self) -> None:
        self.thread.start()

    def close(self) -> None:
        self.stop_event.set()
        self.thread.join(timeout=2.0)

    def get(self, timeout: float = 1.0) -> CapturedFrame | None:
        deadline = time.perf_counter() + max(timeout, 0.1)
        while True:
            try:
                return self.queue.get(timeout=0.1)
            except queue.Empty:
                if self.stop_event.is_set() and self.queue.empty():
                    return None
                if time.perf_counter() >= deadline:
                    raise

    def _offer(self, packet: CapturedFrame | None) -> None:
        while not self.stop_event.is_set():
            try:
                self.queue.put_nowait(packet)
                return
            except queue.Full:
                try:
                    self.queue.get_nowait()
                except queue.Empty:
                    return

    def _sleep_for_realtime(self) -> None:
        if self.frame_delay <= 0:
            return
        now = time.perf_counter()
        sleep_for = self.frame_delay - (now - self.last_emit)
        if sleep_for > 0:
            time.sleep(sleep_for)
        self.last_emit = time.perf_counter()

    def _run(self) -> None:
        frame_id = 0
        try:
            while not self.stop_event.is_set():
                self._sleep_for_realtime()
                read_t0 = time.perf_counter()
                ok, frame = self.cap.read()
                read_ms = (time.perf_counter() - read_t0) * 1000.0
                if not ok:
                    self._offer(None)
                    return
                frame_id += 1
                timestamp = time.perf_counter()
                pos_msec = float(self.cap.get(cv2.CAP_PROP_POS_MSEC) or 0.0)
                if pos_msec > 0:
                    video_time_sec = pos_msec / 1000.0
                elif self.video_fps > 0:
                    video_time_sec = max(frame_id - 1, 0) / self.video_fps
                else:
                    video_time_sec = 0.0
                pose = sample_demo_pose(self.args, frame_id, video_time_sec, self.total_frames)
                self._offer(CapturedFrame(
                    frame_id=frame_id,
                    timestamp=timestamp,
                    video_time_sec=video_time_sec,
                    frame=frame,
                    pose=pose,
                    read_ms=read_ms,
                ))
        finally:
            self.stop_event.set()


def require_file(path_str: str, description: str) -> Path:
    path = Path(path_str)
    if not path.exists():
        log(f"ERRORE: {description} non trovato: {path}")
        sys.exit(1)
    return path


def open_capture(video_arg: str | None):
    if video_arg is None:
        cap = cv2.VideoCapture(0, cv2.CAP_DSHOW)
        if not cap.isOpened():
            cap = cv2.VideoCapture(0)
        return cap, "camera:0"

    if str(video_arg).isdigit():
        idx = int(video_arg)
        cap = cv2.VideoCapture(idx, cv2.CAP_DSHOW)
        if not cap.isOpened():
            cap = cv2.VideoCapture(idx)
        return cap, f"camera:{idx}"

    return cv2.VideoCapture(video_arg), str(video_arg)


def build_wms_multipart(job: WmsSnapshotJob) -> tuple[bytes, str]:
    boundary = f"flywms-{uuid.uuid4().hex}"
    chunks: list[bytes] = []

    def add_field(name: str, value: str, content_type: str = "text/plain") -> None:
        chunks.extend([
            f"--{boundary}\r\n".encode("ascii"),
            f'Content-Disposition: form-data; name="{name}"\r\n'.encode("ascii"),
            f"Content-Type: {content_type}\r\n\r\n".encode("ascii"),
            value.encode("utf-8"),
            b"\r\n",
        ])

    def add_file(name: str, path_str: str) -> None:
        path = Path(path_str)
        chunks.extend([
            f"--{boundary}\r\n".encode("ascii"),
            (
                f'Content-Disposition: form-data; name="{name}"; '
                f'filename="{path.name}"\r\n'
            ).encode("utf-8"),
            b"Content-Type: image/jpeg\r\n\r\n",
            path.read_bytes(),
            b"\r\n",
        ])

    add_field("metadata", json.dumps(job.metadata, ensure_ascii=True), "application/json")
    add_file("label_image", job.label_image_path)
    if job.gaylord_image_path is not None:
        add_file("gaylord_image", job.gaylord_image_path)
    chunks.append(f"--{boundary}--\r\n".encode("ascii"))
    return b"".join(chunks), f"multipart/form-data; boundary={boundary}"


def parse_window_rect(value: str | None) -> tuple[int, int, int, int] | None:
    if value is None:
        return None
    parts = [part.strip() for part in str(value).split(",")]
    if len(parts) != 4:
        return None
    try:
        x, y, w, h = [int(part) for part in parts]
    except ValueError:
        return None
    if w <= 0 or h <= 0:
        return None
    return x, y, w, h


def apply_window_layout(args) -> None:
    if not args.window_layout_enabled:
        return
    layout = {
        "flywms navigate": args.navigate_window,
        "flywms comandi": args.commands_window,
        "flywms snapshot": args.snapshot_window,
        "flywms etichetta": args.label_window,
    }
    for name, value in layout.items():
        rect = parse_window_rect(value)
        if rect is None:
            log(f"Layout finestra ignorato per {name}: {value}")
            continue
        x, y, w, h = rect
        cv2.resizeWindow(name, w, h)
        cv2.moveWindow(name, x, y)


def clip_box(x1: int, y1: int, x2: int, y2: int, w: int, h: int) -> tuple[int, int, int, int]:
    x1 = max(0, min(x1, w - 1))
    y1 = max(0, min(y1, h - 1))
    x2 = max(0, min(x2, w - 1))
    y2 = max(0, min(y2, h - 1))
    return x1, y1, x2, y2


def crop_with_padding(
    frame: np.ndarray,
    bbox: tuple[int, int, int, int],
    pad_ratio: float,
) -> np.ndarray:
    x1, y1, x2, y2 = bbox
    bw = x2 - x1
    bh = y2 - y1
    pad_x = int(max(0.0, pad_ratio) * bw)
    pad_y = int(max(0.0, pad_ratio) * bh)
    cx1, cy1, cx2, cy2 = clip_box(
        x1 - pad_x,
        y1 - pad_y,
        x2 + pad_x,
        y2 + pad_y,
        frame.shape[1],
        frame.shape[0],
    )
    return frame[cy1:cy2, cx1:cx2].copy()


def bbox_area(bbox: tuple[int, int, int, int]) -> int:
    x1, y1, x2, y2 = bbox
    return max(0, x2 - x1) * max(0, y2 - y1)


def bbox_center(bbox: tuple[int, int, int, int]) -> tuple[float, float]:
    x1, y1, x2, y2 = bbox
    return (x1 + x2) * 0.5, (y1 + y2) * 0.5


def bbox_contains(
    outer: tuple[int, int, int, int],
    inner: tuple[int, int, int, int],
) -> bool:
    ox1, oy1, ox2, oy2 = outer
    ix1, iy1, ix2, iy2 = inner
    return ox1 <= ix1 and oy1 <= iy1 and ix2 <= ox2 and iy2 <= oy2


def find_label_inside_bbox(
    gaylord_bbox: tuple[int, int, int, int],
    labels: list[Detection],
) -> Detection | None:
    contained = [label for label in labels if bbox_contains(gaylord_bbox, label.bbox)]
    if not contained:
        return None
    if len(contained) == 1:
        return contained[0]
    # Multiple contained labels should not happen; choose a deterministic fallback.
    gx, gy = bbox_center(gaylord_bbox)
    return min(
        contained,
        key=lambda label: (
            np.hypot(bbox_center(label.bbox)[0] - gx, bbox_center(label.bbox)[1] - gy),
            -label.confidence,
        ),
    )


def bbox_iou(a: tuple[int, int, int, int], b: tuple[int, int, int, int]) -> float:
    ax1, ay1, ax2, ay2 = a
    bx1, by1, bx2, by2 = b
    ix1 = max(ax1, bx1)
    iy1 = max(ay1, by1)
    ix2 = min(ax2, bx2)
    iy2 = min(ay2, by2)
    inter = bbox_area((ix1, iy1, ix2, iy2))
    union = bbox_area(a) + bbox_area(b) - inter
    if union <= 0:
        return 0.0
    return inter / float(union)


def association_score(
    track_bbox: tuple[int, int, int, int],
    det_bbox: tuple[int, int, int, int],
    max_center_distance: float,
) -> float:
    iou = bbox_iou(track_bbox, det_bbox)
    tx, ty = bbox_center(track_bbox)
    dx, dy = bbox_center(det_bbox)
    center_dist = float(np.hypot(tx - dx, ty - dy))
    center_similarity = max(0.0, 1.0 - center_dist / max_center_distance)
    return 0.70 * iou + 0.30 * center_similarity


def resize_preview(frame: np.ndarray, max_width: int) -> np.ndarray:
    h, w = frame.shape[:2]
    if max_width <= 0 or w <= max_width:
        return frame
    scale = max_width / float(w)
    return cuda_resize(frame, (int(w * scale), int(h * scale)), interpolation=cv2.INTER_LINEAR)


def draw_navigation_debug(
    frame: np.ndarray,
    tracks: list[Track],
    args,
    labels: list[Detection] | None = None,
    movement_arrow: tuple[tuple[int, int], tuple[int, int]] | None = None,
) -> np.ndarray:
    labels = labels or []
    display = frame.copy()
    h, w = display.shape[:2]
    center_x = int(w * 0.5)
    tol = int(w * args.center_tolerance_ratio)
    y_min = int(h * args.usable_y_min_ratio)
    y_max = int(h * args.usable_y_max_ratio)

    cv2.rectangle(display, (center_x - tol, y_min), (center_x + tol, y_max), (255, 255, 0), 4)
    cv2.line(display, (center_x, 0), (center_x, h), (255, 255, 0), 3)
    cv2.line(display, (0, y_min), (w, y_min), (100, 100, 100), 2)
    cv2.line(display, (0, y_max), (w, y_max), (100, 100, 100), 2)

    for track in tracks:
        x1, y1, x2, y2 = track.bbox
        color = state_color(track.state)
        thickness = 8 if track.state == "centered" else 5
        cv2.rectangle(display, (x1, y1), (x2, y2), color, thickness)
        cx, cy = bbox_center(track.bbox)
        cv2.circle(display, (int(cx), int(cy)), 12, color, -1)
        cv2.circle(display, (int(cx), int(cy)), 18, (0, 0, 0), 3)

    for label in labels:
        x1, y1, x2, y2 = label.bbox
        cv2.rectangle(display, (x1, y1), (x2, y2), (0, 170, 255), 4)
        lx, ly = bbox_center(label.bbox)
        cv2.circle(display, (int(lx), int(ly)), 8, (0, 170, 255), -1)

    if movement_arrow is not None:
        start, end = movement_arrow
        cv2.arrowedLine(display, start, end, (0, 0, 0), 12, cv2.LINE_AA, tipLength=0.12)
        cv2.arrowedLine(display, end, start, (0, 0, 0), 12, cv2.LINE_AA, tipLength=0.12)

    return resize_preview(display, args.preview_width)


def draw_commands_window(command_lines: list[str], motion_text: str) -> np.ndarray:
    lines = command_lines if command_lines else ["Nessun comando generato"]
    visible_lines = lines[:14]
    arrow_mode = command_arrow_mode(lines)
    arrow_h = 210 if arrow_mode else 0
    canvas_h = max(340, 84 + len(visible_lines) * 34 + arrow_h)
    canvas = np.full((canvas_h, 980, 3), 245, dtype=np.uint8)
    cv2.putText(
        canvas,
        "COMANDI NAVIGAZIONE",
        (24, 42),
        cv2.FONT_HERSHEY_SIMPLEX,
        1.0,
        (0, 0, 0),
        2,
        cv2.LINE_AA,
    )
    cv2.putText(
        canvas,
        motion_text,
        (24, 76),
        cv2.FONT_HERSHEY_SIMPLEX,
        0.80,
        (120, 0, 120),
        2,
        cv2.LINE_AA,
    )
    y = 122
    for idx, line in enumerate(visible_lines):
        color = (0, 0, 180) if idx == 0 else (0, 90, 0)
        cv2.putText(
            canvas,
            line,
            (24, y),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.82,
            color,
            2,
            cv2.LINE_AA,
        )
        y += 36
    if arrow_mode:
        draw_command_arrow(canvas, arrow_mode, y + 16)
    return canvas


def build_observer_telemetry(
    frame_id: int,
    timestamp: float,
    capture_pose: dict[str, object] | None,
    processed_frames: int,
    navigator: NavigationController,
    tracks: list[Track],
    gaylords: list[Detection],
    labels: list[Detection],
    run_yolo: bool,
    last_yolo_ms: float,
    yolo_cycles: int,
    start_time: float,
    video_fps: float,
    preview_fps: float,
    yolo_target_fps: float,
    yolo_mode: str,
) -> dict[str, object]:
    elapsed = max(time.perf_counter() - start_time, 0.001)
    active = sum(1 for t in tracks if t.missed == 0)
    return {
        "frame_id": int(frame_id),
        "processed_frames": int(processed_frames),
        "timestamp": float(timestamp),
        "capture_pose": dict(capture_pose or {}),
        "command_text": navigator.last_command_text,
        "command_lines": list(navigator.last_command_lines),
        "motion_text": navigator.motion_text,
        "snapshot_counter": int(navigator.snapshot_counter),
        "det_count": int(len(gaylords)),
        "label_count": int(len(labels)),
        "track_count": int(len(tracks)),
        "active_track_count": int(active),
        "run_yolo": bool(run_yolo),
        "yolo_mode": str(yolo_mode),
        "last_yolo_ms": float(last_yolo_ms),
        "loop_fps": float(processed_frames / elapsed),
        "yolo_fps": float(yolo_cycles / elapsed),
        "video_fps": float(video_fps),
        "preview_fps": float(preview_fps),
        "yolo_target_fps": float(yolo_target_fps),
    }


def command_arrow_mode(lines: list[str]) -> tuple[str, float, float] | None:
    for line in lines:
        if line.startswith("CENTRA_ETICHETTA"):
            return "to_label", *parse_command_delta(line)
        if line.startswith("RITORNA_CENTRO_GAYLORD"):
            dx, dy = parse_command_delta(line)
            return "to_gaylord", -dx, -dy
    return None


def parse_command_delta(line: str) -> tuple[float, float]:
    dx = 0.0
    dy = 0.0
    for part in line.split():
        if part.startswith("dx="):
            dx = float(part[3:].replace("px", ""))
        elif part.startswith("dy="):
            dy = float(part[3:].replace("px", ""))
    return dx, dy


def draw_command_arrow(canvas: np.ndarray, arrow_info: tuple[str, float, float], y0: int) -> None:
    mode, dx, dy = arrow_info
    h, w = canvas.shape[:2]
    y0 = min(max(y0, 110), h - 160)
    center = (w // 2 - 120, y0 + 130)
    northeast = (w // 2 + 150, y0 + 34)
    if mode == "to_label":
        start, end = center, northeast
        title = "MOVIMENTO VERSO ETICHETTA"
        start_label = "CENTRO GAYLORD"
        end_label = "ETICHETTA"
    else:
        start, end = northeast, center
        title = "RITORNO AL CENTRO GAYLORD"
        start_label = "ETICHETTA"
        end_label = "CENTRO GAYLORD"

    cv2.rectangle(canvas, (24, y0), (w - 24, min(h - 18, y0 + 170)), (232, 232, 232), -1)
    cv2.rectangle(canvas, (24, y0), (w - 24, min(h - 18, y0 + 170)), (120, 120, 120), 2)
    cv2.putText(canvas, title, (44, y0 + 34), cv2.FONT_HERSHEY_SIMPLEX, 0.86, (0, 0, 0), 2, cv2.LINE_AA)
    cv2.putText(canvas, f"DX ORIZZONTALE: {dx:+.0f} px", (44, y0 + 72), cv2.FONT_HERSHEY_SIMPLEX, 0.86, (0, 0, 180), 2, cv2.LINE_AA)
    cv2.putText(canvas, f"DY VERTICALE:   {dy:+.0f} px", (44, y0 + 108), cv2.FONT_HERSHEY_SIMPLEX, 0.86, (0, 0, 180), 2, cv2.LINE_AA)
    cv2.circle(canvas, center, 16, (70, 70, 70), -1)
    cv2.circle(canvas, northeast, 16, (0, 120, 255), -1)
    cv2.arrowedLine(canvas, start, end, (0, 0, 0), 12, cv2.LINE_AA, tipLength=0.22)
    cv2.putText(canvas, start_label, (start[0] - 115, start[1] + 42), cv2.FONT_HERSHEY_SIMPLEX, 0.62, (0, 0, 0), 2, cv2.LINE_AA)
    cv2.putText(canvas, end_label, (end[0] - 78, max(24, end[1] - 24)), cv2.FONT_HERSHEY_SIMPLEX, 0.62, (0, 0, 0), 2, cv2.LINE_AA)


def apply_flash(frame: np.ndarray, alpha: float) -> np.ndarray:
    flash = np.full_like(frame, 255)
    alpha = min(max(alpha, 0.0), 1.0)
    return cv2.addWeighted(frame, 1.0 - alpha, flash, alpha, 0.0)


def estimate_motion_from_tracks(tracks: list[Track], min_pixels: float) -> str:
    deltas: list[tuple[float, float]] = []
    for track in tracks:
        if track.missed != 0 or len(track.center_history) < 2:
            continue
        x0, y0 = track.center_history[-2]
        x1, y1 = track.center_history[-1]
        deltas.append((x1 - x0, y1 - y0))

    if not deltas:
        return "MOTO: n/d"

    dx = sum(delta[0] for delta in deltas) / len(deltas)
    dy = sum(delta[1] for delta in deltas) / len(deltas)
    abs_dx = abs(dx)
    abs_dy = abs(dy)

    if abs_dx < min_pixels and abs_dy < min_pixels:
        direction = "stabile"
    elif abs_dx >= abs_dy:
        direction = "destra" if dx > 0 else "sinistra"
    else:
        direction = "giu" if dy > 0 else "su"

    return f"MOTO: {direction} dx={dx:+.1f}px dy={dy:+.1f}px tracks={len(deltas)}"


def state_color(state: str) -> tuple[int, int, int]:
    if state == "centered":
        return (0, 255, 255)
    if state == "snapshotted":
        return (255, 0, 255)
    if state == "candidate":
        return (0, 255, 0)
    if state == "exiting":
        return (0, 140, 255)
    return (255, 255, 255)


def main() -> int:
    args = parse_args()
    if args.benchmark_mode:
        args.no_display = True
        args.window_layout_enabled = False
        args.realtime_playback = True
        args.preview_fps = args.benchmark_preview_fps
        if args.perf_log_path == "tempistiche.txt":
            args.perf_log_path = "tempistiche-benchmark.txt"
        log(
            f"Profilo benchmark attivo: no_display=true "
            f"preview_fps={format_fps_value(args.preview_fps)} "
            f"log={args.perf_log_path}"
        )
    if args.observer_enabled:
        if not args.no_display:
            log("Observer esterno attivo: disabilito la UI locale integrata")
        args.no_display = True
        args.window_layout_enabled = False
    require_file(args.weights, "modello Ultralytics")

    detector = UltralyticsDetector(args.weights, args.ultralytics_device, args.yolo_half)
    log(f"Classi modello: {detector.classes}")
    log(f"OpenCV CUDA: {'attivo' if OPENCV_CUDA_AVAILABLE else 'non disponibile'}")
    log(f"YOLO half precision: {'attiva' if detector.half else 'disattiva'}")
    log("Nota tracker: questa versione usa tracking geometrico interno; ByteTrack/BoT-SORT restano candidati per confronto successivo.")

    cap, source_name = open_capture(args.video)
    if not cap.isOpened():
        log(f"ERRORE: impossibile aprire sorgente video: {source_name}")
        return 1

    video_fps = cap.get(cv2.CAP_PROP_FPS)
    total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT) or 0)
    video_duration_sec = (total_frames / float(video_fps)) if video_fps and video_fps > 0 and total_frames > 0 else 0.0
    preview_fps = args.preview_fps if args.preview_fps and args.preview_fps > 0 else video_fps
    if args.preview_fps and args.preview_fps > 0 and (args.video is None or str(args.video).isdigit()):
        cap.set(cv2.CAP_PROP_FPS, float(args.preview_fps))
    frame_delay = 1.0 / preview_fps if args.realtime_playback and preview_fps and preview_fps > 1 else 0.0
    log(
        f"FPS sorgente={format_fps_value(video_fps)} "
        f"preview_target={format_fps_value(preview_fps)} "
        f"yolo_target={format_fps_value(args.yolo_fps)}"
    )
    if args.adaptive_yolo_enabled:
        log(
            "YOLO adattivo:"
            f" idle={format_fps_value(args.idle_yolo_fps)}"
            f" tracking={format_fps_value(args.tracking_yolo_fps)}"
            f" critical={format_fps_value(args.critical_yolo_fps)}"
        )
    if video_duration_sec > 0:
        log(
            f"Durata nominale video={video_duration_sec:.2f}s "
            f"({format_seconds(video_duration_sec)})"
        )
    perf_writer = PerfLogWriter(
        args.perf_log_path,
        args.perf_log_flush_interval_sec,
        args.perf_log_flush_lines,
    )
    log(f"Log tempistiche: {perf_writer.path.resolve()}")
    perf_writer.write(
        "ts\tframe_id\trun_yolo\tread_ms\tyolo_ms\ttrack_ms\tdraw_ms\tui_ms\t"
        "snapshot_pause_ms\twms_wait_ms\tloop_ms\tloop_fps\tyolo_real_fps\t"
        "src_fps\tpreview_target\tyolo_target\tdet\tlabels\ttracks\tactive\t"
        "snapshots\tcommand"
    )
    tracker = LightweightTracker(
        max_missed=args.max_track_missed,
        min_match_score=args.min_match_score,
        max_center_distance_ratio=args.max_center_distance_ratio,
    )
    navigator = NavigationController(args)
    wms_client = WmsAsyncClient(args)
    observer = ObserverPublisher(args)
    capture_worker = CaptureWorker(cap, args, float(video_fps or 0.0), frame_delay, total_frames)
    capture_worker.start()

    if not args.no_display:
        cv2.namedWindow("flywms navigate", cv2.WINDOW_NORMAL)
        cv2.namedWindow("flywms snapshot", cv2.WINDOW_NORMAL)
        cv2.namedWindow("flywms etichetta", cv2.WINDOW_NORMAL)
        cv2.namedWindow("flywms comandi", cv2.WINDOW_NORMAL)
        apply_window_layout(args)

    frame_id = 0
    processed_frames = 0
    start_time = time.perf_counter()
    last_stats = start_time
    yolo_total_ms = 0.0
    yolo_cycles = 0
    total_wms_wait_ms = 0.0
    next_yolo_time = start_time
    last_yolo_ms = 0.0
    current_yolo_mode = "fixed"
    observer_preview_interval = 1.0 / args.observer_preview_fps if args.observer_preview_fps and args.observer_preview_fps > 0 else 0.0
    observer_telemetry_interval = 1.0 / args.observer_telemetry_fps if args.observer_telemetry_fps and args.observer_telemetry_fps > 0 else 0.0
    last_observer_preview = 0.0
    last_observer_telemetry = 0.0
    gaylords: list[Detection] = []
    labels: list[Detection] = []
    tracks: list[Track] = []

    def publish_observer_state(captured: CapturedFrame, run_yolo_flag: bool, force: bool = False) -> None:
        nonlocal last_observer_preview, last_observer_telemetry
        if not args.observer_enabled:
            return
        now = time.perf_counter()
        if force or observer_telemetry_interval <= 0 or now - last_observer_telemetry >= observer_telemetry_interval:
            observer.publish_telemetry(
                build_observer_telemetry(
                    frame_id=captured.frame_id,
                    timestamp=captured.timestamp,
                    capture_pose=captured.pose,
                    processed_frames=processed_frames,
                    navigator=navigator,
                    tracks=tracks,
                    gaylords=gaylords,
                    labels=labels,
                    run_yolo=run_yolo_flag,
                    last_yolo_ms=last_yolo_ms,
                    yolo_cycles=yolo_cycles,
                    start_time=start_time,
                    video_fps=float(video_fps or 0.0),
                    preview_fps=float(preview_fps or 0.0),
                    yolo_target_fps=float(args.yolo_fps or 0.0),
                    yolo_mode=current_yolo_mode,
                )
            )
            last_observer_telemetry = now
        if force or observer_preview_interval <= 0 or now - last_observer_preview >= observer_preview_interval:
            preview_frame = draw_navigation_debug(
                captured.frame,
                tracks,
                args,
                labels,
                navigator.label_movement_arrow,
            )
            preview_frame = resize_preview(preview_frame, args.observer_preview_width)
            observer.publish_preview("navigate", preview_frame, captured.frame_id, captured.timestamp, args.observer_jpeg_quality)
            if navigator.last_ocr_payload_frame is not None:
                observer.publish_preview(
                    "snapshot",
                    resize_preview(navigator.last_ocr_payload_frame, args.observer_preview_width),
                    captured.frame_id,
                    captured.timestamp,
                    args.observer_jpeg_quality,
                )
            if navigator.last_label_payload_frame is not None:
                observer.publish_preview(
                    "label",
                    resize_preview(navigator.last_label_payload_frame, args.observer_preview_width),
                    captured.frame_id,
                    captured.timestamp,
                    args.observer_jpeg_quality,
                )
            last_observer_preview = now

    try:
        while True:
            frame_loop_start = time.perf_counter()
            read_ms = 0.0
            track_ms = 0.0
            draw_ms = 0.0
            ui_ms = 0.0
            snapshot_pause_ms = 0.0
            wms_wait_ms = 0.0
            try:
                captured = capture_worker.get(timeout=2.0)
            except queue.Empty:
                continue
            if captured is None:
                log("Fine stream")
                break
            frame = captured.frame
            frame_id = captured.frame_id
            timestamp = captured.timestamp
            read_ms = captured.read_ms
            processed_frames += 1
            if args.max_frames > 0 and frame_id > args.max_frames:
                log(f"Raggiunto --max-frames={args.max_frames}")
                break

            new_snapshots: list[NavigationSnapshot] = []
            current_yolo_fps, current_yolo_mode = choose_adaptive_yolo_fps(args, tracks)
            yolo_interval = 1.0 / current_yolo_fps if current_yolo_fps and current_yolo_fps > 0 else 0.0
            run_yolo = yolo_interval <= 0 or timestamp >= next_yolo_time
            if run_yolo:
                next_yolo_time = timestamp + yolo_interval
                detections, last_yolo_ms = detector.detect(frame, args.min_confidence, args.input_size)
                yolo_total_ms += last_yolo_ms
                yolo_cycles += 1
                gaylords = [
                    det for det in detections
                    if det.class_name.strip().lower() == args.target_class.strip().lower()
                ]
                labels = [
                    det for det in detections
                    if det.class_name.strip().lower() == args.label_class.strip().lower()
                ]

                track_t0 = time.perf_counter()
                tracks = tracker.update(gaylords, frame_id, frame.shape[1])
                if args.motion_report_interval > 0 and yolo_cycles % args.motion_report_interval == 0:
                    navigator.set_motion_text(
                        estimate_motion_from_tracks(tracks, args.motion_min_pixels)
                    )
                for track in tracks:
                    if track.missed == 0:
                        snapshot = navigator.process_track(track, captured, labels)
                        if snapshot is not None:
                            new_snapshots.append(snapshot)
                track_ms = (time.perf_counter() - track_t0) * 1000.0
            if args.no_display and new_snapshots:
                if args.observer_enabled:
                    for snapshot in new_snapshots:
                        navigator.set_label_sequence_phase(snapshot, "move")
                        publish_observer_state(captured, run_yolo, force=True)
                        if args.label_move_sec > 0:
                            pause_t0 = time.perf_counter()
                            time.sleep(args.label_move_sec)
                            snapshot_pause_ms += (time.perf_counter() - pause_t0) * 1000.0

                        navigator.set_label_sequence_phase(snapshot, "stabilize")
                        publish_observer_state(captured, run_yolo, force=True)
                        if args.label_stabilization_sec > 0:
                            pause_t0 = time.perf_counter()
                            time.sleep(args.label_stabilization_sec)
                            snapshot_pause_ms += (time.perf_counter() - pause_t0) * 1000.0

                        navigator.set_label_sequence_phase(snapshot, "capture")
                        publish_observer_state(captured, run_yolo, force=True)
                        pause_t0 = time.perf_counter()
                        time.sleep(0.5)
                        snapshot_pause_ms += (time.perf_counter() - pause_t0) * 1000.0

                        request_id = wms_client.submit(snapshot) if args.wms_enabled else None

                        navigator.set_label_sequence_phase(snapshot, "return")
                        publish_observer_state(captured, run_yolo, force=True)
                        if args.label_return_sec > 0:
                            pause_t0 = time.perf_counter()
                            time.sleep(args.label_return_sec)
                            snapshot_pause_ms += (time.perf_counter() - pause_t0) * 1000.0

                        navigator.set_label_sequence_phase(snapshot, "wait_wms")
                        publish_observer_state(captured, run_yolo, force=True)
                        if args.wms_enabled:
                            wait_t0 = time.perf_counter()
                            result = wms_client.wait_for_result(request_id, args.remote_ack_timeout_sec)
                            wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0
                            navigator.apply_wms_result(result, snapshot)
                        else:
                            if args.remote_ack_timeout_sec > 0:
                                wait_t0 = time.perf_counter()
                                time.sleep(args.remote_ack_timeout_sec)
                                wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0
                            navigator.simulate_remote_response(snapshot)
                        publish_observer_state(captured, run_yolo, force=True)
                    navigator.label_movement_arrow = None
                else:
                    sequence_sec = (
                        args.label_move_sec
                        + args.label_stabilization_sec
                        + args.label_return_sec
                    )
                    if sequence_sec > 0:
                        pause_t0 = time.perf_counter()
                        time.sleep(sequence_sec)
                        snapshot_pause_ms += (time.perf_counter() - pause_t0) * 1000.0
                    for snapshot in new_snapshots:
                        if args.wms_enabled:
                            request_id = wms_client.submit(snapshot)
                            wait_t0 = time.perf_counter()
                            result = wms_client.wait_for_result(request_id, args.remote_ack_timeout_sec)
                            wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0
                            navigator.apply_wms_result(result, snapshot)
                        else:
                            if args.remote_ack_timeout_sec > 0:
                                wait_t0 = time.perf_counter()
                                time.sleep(args.remote_ack_timeout_sec)
                                wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0
                            navigator.simulate_remote_response(snapshot)

            now = time.perf_counter()
            if now - last_stats >= args.stats_interval:
                elapsed = max(now - start_time, 0.001)
                avg_yolo = yolo_total_ms / max(yolo_cycles, 1)
                active = sum(1 for t in tracks if t.missed == 0)
                log(
                    f"fps={processed_frames / elapsed:.1f} yolo_fps={yolo_cycles / elapsed:.1f} "
                    f"yolo_mode={current_yolo_mode} "
                    f"avg_yolo={avg_yolo:.1f}ms det={len(gaylords)} labels={len(labels)} "
                    f"tracks={len(tracks)} active={active} "
                    f"snapshots={navigator.snapshot_counter} {navigator.motion_text}"
                )
                if args.debug_tracks:
                    for track in tracks:
                        cx, cy = bbox_center(track.bbox)
                        area_ratio = bbox_area(track.bbox) / float(frame.shape[0] * frame.shape[1])
                        log(
                            f"  track={track.id} state={track.state} hits={track.hits} "
                            f"missed={track.missed} center=({cx:.0f},{cy:.0f}) "
                            f"area={area_ratio:.3f} trend={track.area_trend():+.2f} "
                            f"reason={track.last_candidate_reason}"
                        )
                last_stats = now

            if args.observer_enabled:
                publish_observer_state(captured, run_yolo, force=False)

            if not args.no_display:
                draw_t0 = time.perf_counter()
                display = draw_navigation_debug(
                    frame,
                    tracks,
                    args,
                    labels,
                    navigator.label_movement_arrow,
                )
                draw_ms += (time.perf_counter() - draw_t0) * 1000.0
                ui_t0 = time.perf_counter()
                cv2.imshow("flywms navigate", display)
                if navigator.last_ocr_payload_frame is not None:
                    snapshot_display = resize_preview(navigator.last_ocr_payload_frame, args.preview_width)
                    cv2.imshow("flywms snapshot", snapshot_display)
                if navigator.last_label_payload_frame is not None and not new_snapshots:
                    label_display = resize_preview(navigator.last_label_payload_frame, args.preview_width)
                    cv2.imshow("flywms etichetta", label_display)
                cv2.imshow(
                    "flywms comandi",
                    draw_commands_window(navigator.last_command_lines, navigator.motion_text),
                )
                ui_ms += (time.perf_counter() - ui_t0) * 1000.0

                if new_snapshots:
                    stop_requested = False
                    for snapshot in new_snapshots:
                        navigator.set_label_sequence_phase(snapshot, "move")
                        ui_t0 = time.perf_counter()
                        cv2.imshow("flywms navigate", display)
                        cv2.imshow(
                            "flywms comandi",
                            draw_commands_window(navigator.last_command_lines, navigator.motion_text),
                        )
                        ui_ms += (time.perf_counter() - ui_t0) * 1000.0
                        wait_t0 = time.perf_counter()
                        key = cv2.waitKey(max(1, int(args.label_move_sec * 1000))) & 0xFF
                        snapshot_pause_ms += (time.perf_counter() - wait_t0) * 1000.0
                        if key in (27, ord("q")):
                            log("Interrotto da tastiera")
                            stop_requested = True
                            break

                        navigator.set_label_sequence_phase(snapshot, "stabilize")
                        ui_t0 = time.perf_counter()
                        cv2.imshow("flywms navigate", display)
                        cv2.imshow(
                            "flywms comandi",
                            draw_commands_window(navigator.last_command_lines, navigator.motion_text),
                        )
                        ui_ms += (time.perf_counter() - ui_t0) * 1000.0
                        wait_t0 = time.perf_counter()
                        key = cv2.waitKey(max(1, int(args.label_stabilization_sec * 1000))) & 0xFF
                        snapshot_pause_ms += (time.perf_counter() - wait_t0) * 1000.0
                        if key in (27, ord("q")):
                            log("Interrotto da tastiera")
                            stop_requested = True
                            break

                        navigator.set_label_sequence_phase(snapshot, "capture")
                        flash_display = apply_flash(display, args.flash_alpha)
                        ui_t0 = time.perf_counter()
                        cv2.imshow("flywms navigate", flash_display)
                        if navigator.last_label_payload_frame is not None:
                            flash_label = apply_flash(
                                resize_preview(navigator.last_label_payload_frame, args.preview_width),
                                args.flash_alpha,
                            )
                            cv2.imshow("flywms etichetta", flash_label)
                        cv2.imshow(
                            "flywms comandi",
                            draw_commands_window(navigator.last_command_lines, navigator.motion_text),
                        )
                        ui_ms += (time.perf_counter() - ui_t0) * 1000.0
                        wait_t0 = time.perf_counter()
                        key = cv2.waitKey(500) & 0xFF
                        snapshot_pause_ms += (time.perf_counter() - wait_t0) * 1000.0
                        if key in (27, ord("q")):
                            log("Interrotto da tastiera")
                            stop_requested = True
                            break

                        request_id = wms_client.submit(snapshot) if args.wms_enabled else None

                        navigator.set_label_sequence_phase(snapshot, "return")
                        ui_t0 = time.perf_counter()
                        cv2.imshow("flywms navigate", display)
                        if navigator.last_label_payload_frame is not None:
                            cv2.imshow(
                                "flywms etichetta",
                                resize_preview(navigator.last_label_payload_frame, args.preview_width),
                            )
                        cv2.imshow(
                            "flywms comandi",
                            draw_commands_window(navigator.last_command_lines, navigator.motion_text),
                        )
                        ui_ms += (time.perf_counter() - ui_t0) * 1000.0
                        wait_t0 = time.perf_counter()
                        key = cv2.waitKey(max(1, int(args.label_return_sec * 1000))) & 0xFF
                        snapshot_pause_ms += (time.perf_counter() - wait_t0) * 1000.0
                        if key in (27, ord("q")):
                            log("Interrotto da tastiera")
                            stop_requested = True
                            break

                        navigator.set_label_sequence_phase(snapshot, "wait_wms")
                        ui_t0 = time.perf_counter()
                        cv2.imshow("flywms navigate", display)
                        if navigator.last_label_payload_frame is not None:
                            cv2.imshow(
                                "flywms etichetta",
                                resize_preview(navigator.last_label_payload_frame, args.preview_width),
                            )
                        cv2.imshow(
                            "flywms comandi",
                            draw_commands_window(navigator.last_command_lines, navigator.motion_text),
                        )
                        ui_ms += (time.perf_counter() - ui_t0) * 1000.0
                        wait_deadline = time.perf_counter() + max(0.0, args.remote_ack_timeout_sec)
                        wms_result: WmsResult | None = None
                        if args.wms_enabled:
                            wait_t0 = time.perf_counter()
                            while time.perf_counter() <= wait_deadline:
                                wms_result = wms_client.wait_for_result(request_id, 0.05)
                                key = cv2.waitKey(1) & 0xFF
                                if key in (27, ord("q")):
                                    log("Interrotto da tastiera")
                                    stop_requested = True
                                    break
                                if wms_result is not None:
                                    break
                            wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0
                        else:
                            wait_t0 = time.perf_counter()
                            key = cv2.waitKey(max(1, int(args.remote_ack_timeout_sec * 1000))) & 0xFF
                            wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0
                            if key in (27, ord("q")):
                                log("Interrotto da tastiera")
                                stop_requested = True
                        if stop_requested:
                            break
                        if args.wms_enabled:
                            navigator.apply_wms_result(wms_result, snapshot)
                        else:
                            navigator.simulate_remote_response(snapshot)
                        cv2.imshow(
                            "flywms comandi",
                            draw_commands_window(navigator.last_command_lines, navigator.motion_text),
                        )

                    navigator.label_movement_arrow = None
                    if stop_requested:
                        break

                ui_t0 = time.perf_counter()
                key = cv2.waitKey(1) & 0xFF
                ui_ms += (time.perf_counter() - ui_t0) * 1000.0
                if key in (27, ord("q")):
                    log("Interrotto da tastiera")
                    break

            loop_end = time.perf_counter()
            elapsed = max(loop_end - start_time, 0.001)
            loop_ms = (loop_end - frame_loop_start) * 1000.0
            total_wms_wait_ms += wms_wait_ms
            active = sum(1 for t in tracks if t.missed == 0)
            perf_writer.write(
                f"{time.strftime('%Y-%m-%d %H:%M:%S')}\t{frame_id}\t{int(run_yolo)}\t"
                f"{read_ms:.3f}\t{last_yolo_ms if run_yolo else 0.0:.3f}\t{track_ms:.3f}\t"
                f"{draw_ms:.3f}\t{ui_ms:.3f}\t{snapshot_pause_ms:.3f}\t{wms_wait_ms:.3f}\t"
                f"{loop_ms:.3f}\t{processed_frames / elapsed:.3f}\t{yolo_cycles / elapsed:.3f}\t"
                f"{format_fps_value(video_fps)}\t{format_fps_value(preview_fps)}\t{format_fps_value(args.yolo_fps)}\t"
                f"{len(gaylords)}\t{len(labels)}\t{len(tracks)}\t{active}\t{navigator.snapshot_counter}\t"
                f"{json.dumps(navigator.last_command_text, ensure_ascii=True)}"
            )
    finally:
        capture_worker.close()
        cap.release()
        wms_client.close()
        observer.close()
        perf_writer.close()
        if not args.no_display:
            cv2.destroyAllWindows()

    total_demo_sec = max(time.perf_counter() - start_time, 0.0)
    net_demo_sec = max(total_demo_sec - (total_wms_wait_ms / 1000.0), 0.0)
    if video_duration_sec > 0:
        delta_total_sec = total_demo_sec - video_duration_sec
        delta_net_sec = net_demo_sec - video_duration_sec
        ratio_total_pct = ((total_demo_sec / video_duration_sec) - 1.0) * 100.0
        ratio_net_pct = ((net_demo_sec / video_duration_sec) - 1.0) * 100.0
        log(
            "Durata demo:"
            f" totale={total_demo_sec:.2f}s ({format_seconds(total_demo_sec)})"
            f" netto_senza_wms={net_demo_sec:.2f}s ({format_seconds(net_demo_sec)})"
        )
        log(
            "Confronto video/demo:"
            f" video={video_duration_sec:.2f}s ({format_seconds(video_duration_sec)})"
            f" delta_totale={delta_total_sec:+.2f}s ({ratio_total_pct:+.1f}%)"
            f" delta_netto_senza_wms={delta_net_sec:+.2f}s ({ratio_net_pct:+.1f}%)"
        )
    log(
        f"Attesa WMS totale={total_wms_wait_ms / 1000.0:.2f}s "
        f"({format_seconds(total_wms_wait_ms / 1000.0)})"
    )
    log(f"Snapshot salvati in: {Path(args.snapshot_output_dir).resolve()}")
    return 0


if __name__ == "__main__":
    raise SystemExit(main())