"""Predictive TRACK zoom policy. All time inputs are seconds on the same clock.""" from collections import deque from dataclasses import dataclass, replace import json import math from pathlib import Path from queue import Full, Queue from threading import Lock, Thread @dataclass(frozen=True) class ZoomConfig: # Fit per camera and native command. Gains multiply ln(252/6)/full_travel_s. nominal_gain: float upper_gain: float wide_fov: float = 59.0 tele_fov: float = 2.0 full_travel_s: float = 5.5 native: int = 10 entry_fov: float = 35.0 entry_center: float = 0.15 # fraction of full frame, 30% of half-width repeat_center: float = 0.09 edge_margin: float = 0.16 tracker_score: float = 0.55 tracker_frames: int = 3 frame_age_s: float = 0.25 detector_age_s: float = 0.65 status_age_s: float = 0.65 evidence_gap_s: float = 0.20 max_outward_deg_s: float = 0.15 trajectory_slack_deg: float = 0.6 edge_accel: float = 0.55 # full frame / sВІ, outward edge_watch_margin: float = 0.24 steering_delay_s: float = 0.18 max_residual_deg_s: float = 12.0 wide_pulse_fov: float = 25.0 tele_pulse_fov: float = 15.0 wide_pulse_s: float = 0.34 tele_pulse_s: float = 0.12 stop_slack_s: float = 0.06 stop_ack_slack_s: float = 0.08 target_px: float = 48.0 status_gain: float = 0.7 status_error_log: float = 0.08 ff_gain: float = 0.7 ff_max_deg_s: float = 3.0 def __post_init__(self): for name, value in vars(self).items(): if not math.isfinite(value) or value <= 0: raise ValueError(f"invalid zoom setting {name}") if not ( self.native == 10 and 0 < self.nominal_gain <= self.upper_gain and 0 < self.tele_fov < self.tele_pulse_fov < self.wide_pulse_fov < self.entry_fov < self.wide_fov < 90 and 0 < self.repeat_center <= self.entry_center <= 0.15 and 0 < self.edge_margin <= self.edge_watch_margin < 0.5 and isinstance(self.tracker_frames, int) and 2 <= self.tracker_frames <= 12 and self.tracker_score <= 1 and self.status_gain <= 1 and self.frame_age_s <= 0.25 and self.detector_age_s <= 0.65 and self.status_age_s <= 0.65 and 0.10 <= self.tele_pulse_s <= self.wide_pulse_s <= 0.34 and self.wide_pulse_s + self.stop_slack_s <= 0.40 + 1e-9 and self.stop_ack_slack_s <= 0.12 ): raise ValueError("zoom settings exceed the native/geometry/watchdog limits") @property def log_rate(self): return self.nominal_gain * math.log(252 / 6) / self.full_travel_s @property def upper_log_rate(self): return self.upper_gain * math.log(252 / 6) / self.full_travel_s def window(self, fov): scale = min(1.0, max(0.0, (fov - self.tele_pulse_fov) / (self.wide_pulse_fov - self.tele_pulse_fov))) nominal = self.tele_pulse_s + scale * (self.wide_pulse_s - self.tele_pulse_s) return nominal, min(0.40, nominal + self.stop_slack_s) def _log_focal(fov): return -math.log(math.tan(math.radians(fov / 2))) def _fov(log_focal): return math.degrees(2 * math.atan(math.exp(-log_focal))) class OpticalStateEstimator: """Integrate accepted native-10 windows, then correct with timestamped status.""" def __init__(self, config): self.cfg = config self.actual = None self.status_at = 0.0 self.nominal = self.upper = 0.0 self.spans = deque() self.samples = deque() self.active = None self.valid = False def _seconds_since_status(self, at, status_at=None): status_at = self.status_at if status_at is None else status_at spans = self.spans if self.active: start, hard_end = self.active spans = (*spans, (start, hard_end)) return sum(max(0.0, min(end, at) - max(start, status_at)) for start, end in spans) def estimate(self, at): for status_at, nominal, upper, _ in reversed(self.samples): if status_at <= at: elapsed = self._seconds_since_status(at, status_at) tele = _log_focal(self.cfg.tele_fov) return (_fov(min(tele, nominal + self.cfg.log_rate * elapsed)), _fov(min(tele, upper + self.cfg.upper_log_rate * elapsed))) return None, None def observe(self, at, hfov, now): if not (self.status_at < at <= now and math.isfinite(hfov) and self.cfg.tele_fov - 0.1 <= hfov <= self.cfg.wide_fov + 1.0): return measured = _log_focal(hfov) estimated, upper = self.estimate(at) had_motion = self._seconds_since_status(at) > 0.0 discrepancy = (had_motion and estimated is not None and (measured > _log_focal(upper) + self.cfg.status_error_log or measured < _log_focal(self.actual) - self.cfg.status_error_log)) if discrepancy: self.valid = False elif not self.active: self.valid = True if not had_motion or estimated is None or discrepancy: self.nominal = measured else: prediction = _log_focal(estimated) self.nominal = prediction + self.cfg.status_gain * (measured - prediction) # The filtered estimate never reduces the conservative edge envelope. self.upper = max(self.nominal, measured + self.cfg.status_error_log) self.actual, self.status_at = hfov, at self.samples.append((at, self.nominal, self.upper, measured)) cutoff = at - self.cfg.status_age_s - self.cfg.frame_age_s while len(self.samples) > 1 and self.samples[1][0] <= cutoff: self.samples.popleft() while self.spans and self.spans[0][1] <= self.samples[0][0]: self.spans.popleft() def start(self, put_at, ack_at, hard_end): if self.active or not self.fresh(ack_at) or not put_at <= ack_at < hard_end: self.valid = False self.active = (put_at, hard_end) def stop(self, at, *, clean): if self.active: start, hard_end = self.active if at > hard_end + self.cfg.stop_ack_slack_s or at < start or not clean: self.valid = False if at > start: self.spans.append((start, min(at, hard_end + self.cfg.stop_ack_slack_s))) self.active = None def fresh(self, now): return self.valid and 0 <= now - self.status_at <= self.cfg.status_age_s @dataclass(frozen=True) class ZoomFrame: at: float seq: int offset: tuple[float, float] # positive right/down; fractions of full frame bbox_frac: tuple[float, float] bbox_px: tuple[float, float] angles: tuple[float, float] # camera-signed LOS error in degrees rates: tuple[float, float] # camera-signed LOS residual, deg/s aspect: float # image height / width tracker_score: float | None detector_at: float detector_conf: float | None geometry_ok: bool los_valid: bool @dataclass(frozen=True) class ZoomDecision: reason: str window: tuple[float, float] | None = None stop: bool = False ff_deg_s: tuple[float, float] = (0.0, 0.0) margin: float | None = None class ZoomController: """Per TRACK epoch; the existing worker retains ownership and all STOPs.""" def __init__(self, config, owner): self.cfg = config self.owner = owner self.optics = OpticalStateEstimator(config) self.history = deque(maxlen=config.tracker_frames) self.request = None self.first_ack_at = None self.confirmed_stop = None self.completed_request = None self.stop_cooldowns = None def accepted(self, request, put_at, ack_at, hard_end): if self.request == request: return self.request = request self.confirmed_stop = None self.completed_request = None self.stop_cooldowns = None if self.first_ack_at is None: self.first_ack_at = ack_at self.optics.start(put_at, ack_at, hard_end) def stopped(self, request, at, *, confirmed, clean): if self.request != request: return self.optics.stop(at, clean=clean) self.request = None if clean and self.optics.fresh(at): self.completed_request = request if confirmed: self.confirmed_stop = (at, self.optics.status_at) def optical_confirmation(self, request, at): if self.completed_request == request and self.request is None and self.optics.fresh(at): self.confirmed_stop = (at, self.optics.status_at) def can_reuse_feedback(self, now, feedback_after): return (self.confirmed_stop is not None and self.request is None and self.confirmed_stop[0] == feedback_after and self.optics.fresh(now) and self.optics.status_at >= self.confirmed_stop[1]) def owns(self, st): return (st.get("predictive_zoom_controller") is self and _owner(st) == self.owner and st.get("mode") == "TRACK" and st.get("tracking_active")) def repeat_after_stop(self, st, now): return bool( self.owns(st) and self.can_reuse_feedback( now, float(st.get("zoom_feedback_required_after", 0.0) or 0.0) ) and st.get("zoom_step_last_delivery_result") == "confirmed" and not any(st.get(key) for key in ( "zoom_step_feedback_pending", "zoom_step_pending", "zoom_stop_obligation_active", "zoom_no_effect_blocked", "zoom_step_cancel_requested", "zoom_rearm_required", "lens_move_stop_required", "lens_move_put_inflight", )) and self.stop_cooldowns == _cooldowns(st) and tuple(st.get("last_ptz_physical_cmd", (None, None, None)))[2] == 0 ) def _record(self, frame, fresh): history = self.history if (not fresh or frame.tracker_score is None or frame.tracker_score < self.cfg.tracker_score): history.clear() return if history: prev = history[-1] if frame.seq <= prev.seq or frame.at <= prev.at: history.clear() return dt = frame.at - prev.at if (dt > self.cfg.evidence_gap_s or any(abs(b - a - rate * dt) > self.cfg.trajectory_slack_deg for a, b, rate in zip(prev.angles, frame.angles, prev.rates))): history.clear() history.append(frame) def decide(self, frame, now, *, first, cooldown, transport_clear): cfg = self.cfg values = (*frame.offset, *frame.bbox_frac, *frame.bbox_px, *frame.angles, *frame.rates, frame.at, frame.aspect) fresh = (frame.geometry_ok and frame.los_valid and frame.seq > 0 and all(math.isfinite(x) for x in values) and 0 < frame.aspect <= 4 and min(frame.bbox_frac + frame.bbox_px) > 0 and 0 <= now - frame.at <= cfg.frame_age_s) prev = self.history[-1] if self.history else None moving = self.request is not None if frame.tracker_score is not None and not ( math.isfinite(frame.tracker_score) and frame.tracker_score >= cfg.tracker_score ): self.history.clear() return ZoomDecision("low_tracker_score", stop=moving) if not fresh or (prev is not None and (frame.at <= prev.at or frame.seq <= prev.seq)): self.history.clear() return ZoomDecision("stale_or_geometry", stop=moving) if not self.optics.fresh(now): return ZoomDecision("fov_cooldown", stop=moving) fov, min_fov = self.optics.estimate(now) captured_fov, captured_min = self.optics.estimate(frame.at) if captured_fov is None: return ZoomDecision("fov_cooldown", stop=moving) captured_base = next(measured for at, _, _, measured in reversed(self.optics.samples) if at <= frame.at) def projection(hfov): horizontal = math.tan(math.radians(hfov / 2)) return horizontal, horizontal * frame.aspect axes_at_capture = projection(captured_fov) angles = tuple(math.degrees(math.atan(2 * offset * axis)) for offset, axis in zip(frame.offset, axes_at_capture)) if prev and 0 < frame.at - prev.at <= cfg.evidence_gap_s: dt = frame.at - prev.at rates = tuple((value - old) / dt for value, old in zip(angles, prev.angles)) frame = replace(frame, rates=rates) frame = replace(frame, angles=angles) self._record(frame, fresh) if max(map(abs, frame.rates)) > cfg.max_residual_deg_s: return ZoomDecision("edge_margin_low", stop=moving) window = cfg.window(min_fov) if moving: remaining = max(0.0, self.optics.active[1] - now) else: remaining = window[1] remaining += cfg.stop_ack_slack_s horizon = cfg.steering_delay_s + (now - frame.at) + remaining # Residual is signed in camera coordinates. Use its magnitude for the # edge projection, so a sign flip or overshoot cannot shrink the bound. narrowed = min(_log_focal(cfg.tele_fov), _log_focal(min_fov) + cfg.upper_log_rate * remaining) # Use the widest supported captured view as the reference so a # narrower prediction never understates the pixel magnification. growth = math.exp(max(0.0, narrowed - captured_base + cfg.status_error_log)) half_tan = projection(captured_min) motion = [math.tan(math.radians(min(80, abs(rate) * horizon))) / (2 * axis) for rate, axis in zip(frame.rates, half_tan)] margin = min(0.5 - (abs(off) + size / 2 + move) * growth for off, size, move in zip(frame.offset, frame.bbox_frac, motion)) if margin < cfg.edge_margin: return ZoomDecision("edge_margin_low", stop=moving, margin=margin) if moving and prev and 0 < frame.at - prev.at <= cfg.evidence_gap_s: dt = frame.at - prev.at for off, rate, old_rate, axis in zip(frame.angles, frame.rates, prev.rates, half_tan): outward_accel = math.copysign(1, off) * (rate - old_rate) / (dt * 2 * math.degrees(axis)) outward_rate = math.copysign(1, off) * rate if margin < cfg.edge_watch_margin and outward_rate > 0 and outward_accel > cfg.edge_accel: return ZoomDecision("edge_acceleration", stop=True, margin=margin) norm = math.hypot(*frame.angles) outward = sum(a * r for a, r in zip(frame.angles, frame.rates)) / norm if norm else 0.0 wide_fraction = max(0.0, (fov - cfg.entry_fov) / (cfg.wide_fov - cfg.entry_fov)) center_limit = (cfg.repeat_center + min(1.0, wide_fraction) * (cfg.entry_center - cfg.repeat_center)) if first else cfg.repeat_center detector_fresh = (0 <= frame.at - frame.detector_at <= cfg.detector_age_s and 0 <= now - frame.detector_at <= cfg.detector_age_s) if moving: if not detector_fresh: return ZoomDecision("waiting_yolo", stop=True, margin=margin) if min(frame.bbox_px) >= cfg.target_px: return ZoomDecision("size_reached", stop=True, margin=margin) if now >= self.optics.active[1]: return ZoomDecision("fov_cooldown", stop=True, margin=margin) # d(theta)/dt needed to cancel (df/f)*tan(theta) on each axis. ff = tuple(max(-cfg.ff_max_deg_s, min(cfg.ff_max_deg_s, math.degrees(cfg.ff_gain * cfg.log_rate * math.sin(math.radians(a)) * math.cos(math.radians(a))))) for a in frame.angles) return ZoomDecision("pulse_active", ff_deg_s=ff, margin=margin) if min(frame.bbox_px) >= cfg.target_px: return ZoomDecision("size_reached", margin=margin) if not transport_clear or cooldown or min_fov <= cfg.tele_fov + 0.05: return ZoomDecision("fov_cooldown", margin=margin) if not detector_fresh: return ZoomDecision("waiting_yolo", margin=margin) if frame.tracker_score is None or len(self.history) < cfg.tracker_frames: return ZoomDecision("low_tracker_score", margin=margin) if max(map(abs, frame.offset)) > center_limit or outward > cfg.max_outward_deg_s: return ZoomDecision("not_centered", margin=margin) return ZoomDecision("fast_start" if first else "burst", window=window, margin=margin) class EpisodeTelemetryLogger: """Write JSONL off the control/STOP threads, with a bounded queue.""" def __init__(self, directory, capacity=4096): self.directory = Path(directory) self.queue = Queue(maxsize=capacity) self.dropped = 0 Thread(target=self._write, name="ptz-zoom-telemetry", daemon=True).start() def submit(self, camera, owner, event, at, **fields): generation, started = owner try: self.queue.put_nowait((camera, generation, started, { "cam": camera, "generation": generation, "track_start_utc": started, "utc": at, "elapsed_ms": round((at - started) * 1000, 1), "event": event, **fields, })) except Full: self.dropped += 1 def _write(self): import logging log = logging.getLogger("PTZTracker") while True: camera, generation, started, row = self.queue.get() try: self.directory.mkdir(parents=True, exist_ok=True) path = self.directory / f"ptz-{camera}-{generation}-{started:.6f}.jsonl" with path.open("a", encoding="utf-8") as out: out.write(json.dumps(row, allow_nan=False) + "\n") except (OSError, ValueError): log.exception("Unable to write PTZ zoom telemetry") finally: self.queue.task_done() def _owner(st): return int(st.get("ptz_generation", 0) or 0), float(st.get("track_started_at", 0.0) or 0.0) def controller_for_track(st, camera_config): """Called only from TRACK, under the same lock used by status and the worker.""" if not st.get("tracking_active") or st.get("mode") != "TRACK" or _owner(st)[1] <= 0: return None current = st.get("predictive_zoom_controller") if current is None or not current.owns(st): try: settings = ZoomConfig(**camera_config["predictive_zoom"]) except (TypeError, ValueError): # A malformed per-camera calibration must leave legacy control available. return None current = ZoomController(settings, _owner(st)) st["predictive_zoom_controller"] = current sample = st.get("predictive_zoom_hfov") if sample is not None: current.optics.observe(*sample, now=sample[0]) return current def transport_accepted(config, camera, st, request, native, put_at, ack_at, hard_end): controller = st.get("predictive_zoom_controller") if controller is None or not controller.owns(st): return if controller.first_ack_at is None: controller.first_ack_at = ack_at _telemetry(config, camera, controller, "first_zoom", ack_at, t_to_first_zoom_ms=round((ack_at - controller.owner[1]) * 1000, 1), t_to_first_zoom_put_ms=round((put_at - controller.owner[1]) * 1000, 1), request=request, native=native) if (st.get("predictive_zoom_request") == request and native == controller.cfg.native): controller.accepted(request, put_at, ack_at, hard_end) def _cooldowns(st): return (float(st.get("zoom_step_cooldown_until", 0.0) or 0.0), float(st.get("zoom_recenter_until", 0.0) or 0.0)) def _reusable_optics(controller, st, at): before = st.get("zoom_step_start_hfov_deg") actual = controller.optics.actual return bool(before and actual and actual < before and controller.optics.status_at == st.get("zoom_status_at") and float(st.get("zoom_step_started_at", 0.0)) < controller.optics.status_at <= at) def _normal_stop_cooldowns(config, st, at): expected = (getattr(config, "ZOOM_STEP_COOLDOWN_SEC", 0.14), getattr(config, "ZOOM_STEP_RECENTER_SEC", 0.14)) return all(observed <= at + delay for observed, delay in zip(_cooldowns(st), expected)) def transport_confirmed(config, st, request, at): controller = st.get("predictive_zoom_controller") if (controller is not None and controller.owns(st) and _reusable_optics(controller, st, at) and _normal_stop_cooldowns(config, st, at)): controller.optical_confirmation(request, at) controller.stop_cooldowns = _cooldowns(st) def transport_stopped(config, camera, st, request, at, *, reason, confirmed, clean): controller = st.get("predictive_zoom_controller") if controller is None or not controller.owns(st) or controller.request != request: return clean = clean and st.get("predictive_zoom_early_stop") != request reusable = confirmed and _reusable_optics(controller, st, at) controller.stopped(request, at, clean=clean, confirmed=reusable and _normal_stop_cooldowns(config, st, at)) controller.stop_cooldowns = _cooldowns(st) _telemetry(config, camera, controller, "stop", at, request=request, reason=reason, confirmed=confirmed, clean=clean) _telemetry_loggers = {} _telemetry_lock = Lock() def _telemetry(config, camera, controller, event, at, **fields): directory = getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", None) if directory is None: return with _telemetry_lock: log = _telemetry_loggers.get(directory) if log is None: log = EpisodeTelemetryLogger(directory) _telemetry_loggers[directory] = log log.submit(camera, controller.owner, event, at, **fields, dropped_before=log.dropped) def telemetry_tick(config, camera, controller, frame, decision, zoom, legacy_reason, at): estimated, narrowest = controller.optics.estimate(at) reason = decision.reason if zoom <= 0.0 and decision.window is not None: reason = legacy_reason _telemetry(config, camera, controller, "tick", at, seq=frame.seq, frame_utc=frame.at, zoom_cmd=zoom, reason=reason, stop_requested=decision.stop, FOV_actual=controller.optics.actual, FOV_status_utc=controller.optics.status_at or None, FOV_estimated=estimated, FOV_min_estimated=narrowest, bbox_px=frame.bbox_px, tracker_score=frame.tracker_score, detector_conf=frame.detector_conf, detector_utc=frame.detector_at or None, margin=decision.margin, ff_deg_s=decision.ff_deg_s)