StarPilot/openpilot/starpilot/flm/live_tracking_episodes.py

"""Whole fullrate geometry joined to independent, unmodified physical endpoints.

This adapter grants no driving permission and never interpolates a pose. Its
inputs must come from the collector's original source pair, not UI telemetry.
"""

from collections import OrderedDict, deque
from typing import cast
from dataclasses import dataclass, replace
import hashlib
import math
import struct

from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.starpilot.flm.canonical_surface_adapter import NAMES, PROBE_GROUPS, probe_names, full_profile_sha256
from openpilot.starpilot.flm.fullrate_collector import ControlEvidence, AppliedEvidence
from openpilot.starpilot.flm.live_trial_contract import MAX_STEP
from openpilot.starpilot.flm.live_tracking import TrackingEpisode, HookSensitivity
from openpilot.starpilot.flm.live_tracking_response import validate_certificate, validate_response_witness, physical_calibration_generation
from openpilot.starpilot.flm.original_source_capture import OriginalSourcePose, validate_pose
from openpilot.starpilot.flm.torque_eligibility import consumed_sources_current, exact_unclipped_pair, asynchronous_hint

MAX_CONTROL_GAP_NS = 50_000_000
MAX_POSE_GAP_NS = 100_000_000
MAX_EVENT_NS = 10_000_000_000
HISTORY_NS = 12_000_000_000
PROBE_DEADLINE_NS = 30_000_000_000
MAX_COMPLETED = 16
MAX_PROBE_REQUESTS = MAX_COMPLETED * 240


def _finite(value):
  return type(value) in (int, float) and math.isfinite(value)


@dataclass(frozen=True)
class _Tick:
  control: ControlEvidence
  output: AppliedEvidence | None
  profile: str
  geometry: float
  goal: float


@dataclass(frozen=True)
class _Endpoint:
  control_ns: int
  apply_ns: int
  pose_ns: int
  speed: float
  goal: float
  error: float
  measured: float
  reference_ns: int = 0


def tracking_reference_force(control):
  """Actual selected controller target, not its incoming future FF request.

  The torque controller's request buffer already aligns its PID target with
  actuator delay. PID torque publishes its own actual kinematic target in the
  same stage field. Retain the existing original roll/force convention and
  independent actual-pose conversion; no reference time is shifted or inferred.
  """
  stage = dict(control.stage_values)
  inputs = dict(control.replay_inputs)
  setpoint, roll = stage['setpoint'], inputs['roll']
  if len(stage) != len(control.stage_values) or not _finite(setpoint) or not _finite(roll):
    raise ValueError('actual finite controller tracking target required')
  return float(setpoint) - ACCELERATION_DUE_TO_GRAVITY * math.sin(float(roll))


def tracking_reference_acceleration(control):
  setpoint = dict(control.stage_values)['setpoint']
  if not _finite(setpoint):
    raise ValueError('actual finite controller tracking target required')
  return float(setpoint)


def tracking_reversal_rate(errors, duration_s, model_rmse):
  # Count complete excursions above the SAME minimum error that discovery
  # can act on. Milligravity crossings of a sampled/quantized reference are
  # not evidence of vehicle weave. Original RMS and drift tests still see
  # every unfiltered endpoint; this changes only the oscillation counter.
  threshold = max(0.015, 3 * model_rmse)
  signs = [1 if error > threshold else -1 if error < -threshold else 0 for error in errors]
  nonzero = [sign for sign in signs if sign]
  return sum(left != right for left, right in zip(nonzero, nonzero[1:], strict=False)) / duration_s


class TrackingEpisodeBuilder:
  """Bounded operation-local joins, retaining true100Hz geometry and pose rate.

  observe_source_control is called for EVERY actual owner tick. Only actual
  emissions enter observe_control; no transmission is inferred. observe_pose
  receives the collector's exact lag-associated control/output pair and fresh
  response witness. Completed windows wait for a real physical watermark;
  time passing alone can never invent their missing independent endpoints.
  """

  def __init__(self, binding, response_certificate, *, controller_domain='torque'):
    binding.validate()
    certificate = validate_certificate(response_certificate)
    if certificate.binding != binding:
      raise ValueError('episode physical model binding')
    if type(controller_domain) is not str or controller_domain not in ('torque', 'torque_pid'):
      raise ValueError('actual controller domain required')
    from openpilot.starpilot.flm.pid_stages import UNSUPPORTED

    self.numeric_probe_names = tuple(name for name in NAMES if controller_domain != 'torque_pid' or name not in UNSUPPORTED)
    self.defer_unprobed = False
    self.numerical_observed_ns = 0
    from openpilot.starpilot.flm.live_tracking_response import TrackingSourceContext

    self.source_tracking = type(certificate) is TrackingSourceContext
    self.binding, self.certificate = binding, certificate
    self.replay = None
    self.ticks = OrderedDict()
    self.emissions = {}
    self.emission_count = 0
    self.endpoints = deque(maxlen=320)
    self.probes = OrderedDict()
    self.probe_received = {}
    self.profile_keys = OrderedDict()
    self.probe_requests = OrderedDict()
    self.completed = OrderedDict()
    self.closed_through_ns = 0
    self.pending = deque(maxlen=16)
    self.ready = deque(maxlen=16)
    self.last_pose_ns = self.watermark = 0
    self.reason = 'whole_tracking_geometry_required'
    self.last_input_status = 'ignored'
    self.plot = None
    self._clear_geometry()

  def _clear_geometry(self):
    self.start = self.last = self.neutral = self.prior_sign = self.first_sign = self.reversals = 0
    self.mode = self.profile = self.last_frame = None
    if not hasattr(self, 'input_window_origin'):
      self.input_window_origin = None
    self.input_window_boundary = self.input_window_wait_until = None
    self.input_window_min = self.input_window_max = self.input_window_sum = 0.0
    self.input_window_count = 0

  def interrupt(self, reason, *, preserve_completed=False):
    # Completed windows contain ORIGINAL endpoints and original probe deadlines.
    # Keeping those numbers supplies no current witness, offer or permission.
    completed = self.completed.copy() if preserve_completed else OrderedDict()
    retained_keys = set().union(*(row['requested'] for row in completed.values())) if completed else set()
    requests = OrderedDict((key, value) for key, value in self.probe_requests.items() if key in retained_keys)
    ready = tuple(self.ready) if preserve_completed else ()
    closed = self.closed_through_ns if preserve_completed else 0
    self.ticks.clear()
    self.emissions.clear()
    self.emission_count = 0
    self.endpoints.clear()
    self.probes.clear()
    self.probe_received.clear()
    self.probe_requests.clear()
    self.completed.clear()
    self.closed_through_ns = 0
    self.pending.clear()
    self.ready.clear()
    self.last_pose_ns = self.watermark = 0
    self.numerical_observed_ns = 0
    self.plot = None
    self._clear_geometry()
    self.completed.update(completed)
    self.probe_requests.update(requests)
    self.ready.extend(ready)
    self.closed_through_ns = closed
    self.reason = reason
    self.last_input_status = 'invalid'

  def _source_control(self, control, current_native):
    b = self.binding
    if (
      type(control) is not ControlEvidence
      or current_native is not True
      or (control.identity, control.source_epoch, control.baseline_revision, control.controller_revision, control.native_identity, control.native_required)
      != (b.identity, b.source_epoch, b.baseline_revision, b.controller_revision, b.native_identity, b.native_required)
      or control.domain != 'normalized_torque'
      or not control.enabled
      or not control.active
      or not control.valid
      or control.manual
      or control.override
      or control.fault
      or control.saturated
      or control.model_wobble
      or control.limited
      and not asynchronous_hint(control)
      or not 0 < control.state_ns <= control.control_ns <= control.state_ns + 30_000_000
      or not 0 < control.source_receipt_ns <= control.control_ns <= control.source_receipt_ns + 500_000_000
      or b.native_required
      and (
        not control.native_allowed
        or not 0 < control.native_observed_ns <= control.control_ns <= control.native_expires_ns <= control.native_observed_ns + 200_000_000
      )
    ):
      raise ValueError('episode original source/driver/controller/native exclusion')

  def _pair(self, control, output, current_native):
    self._source_control(control, current_native)
    b = self.binding
    from openpilot.starpilot.flm.live_tracking_response import verified_clipped_input

    observed_clip = verified_clipped_input(self.replay, control, output, b) if type(output) is AppliedEvidence and output.limited else False
    if (
      type(output) is not AppliedEvidence
      or control.limited
      and not exact_unclipped_pair(control, output)
      and not observed_clip
      or not output.active
      or not output.valid
      or output.override
      or output.fault
      or output.limited
      and not observed_clip
      or output.domain != control.domain
      or output.control_ns != control.control_ns
      or not consumed_sources_current(control, output)
      or b.native_required
      and (not output.native_allowed or output.apply_ns > control.native_expires_ns)
    ):
      raise ValueError('episode actual native pair/driver/limiter exclusion')

  def _actual_coordinates(self, control):
    # Legacy selected coordinates remain neutral TWO-space even while a full30
    # owner is active. Never reinterpret those two numbers as a full profile.
    legacy = control.applied_coordinates
    if type(legacy) is not tuple or len(legacy) != 2 or any(not _finite(value) or value != 0 for value in legacy):
      raise ValueError('old selected residual cannot impersonate full30 state')
    live = control.live_applied_coordinates
    if type(live) is not tuple:
      raise ValueError('exact numerical live consumed map')
    if live:
      if control.effect_trace is not None or len(live) != len(NAMES) or not all(_finite(value) for value in live):
        raise ValueError('one actual full30 owner required')
      return live
    if control.effect_trace is not None:
      from openpilot.starpilot.flm.effect_trace import FullEffectTrace

      trace = control.effect_trace
      if type(trace) is not FullEffectTrace:
        raise ValueError('actual old full30 trace required')
      trace.validate()
      envelope = trace.envelope
      b = self.binding
      if (
        (
          envelope.cp_sha256,
          envelope.controller_revision,
          envelope.source_epoch,
          envelope.reset_generation,
          envelope.selected_baseline_revision,
          envelope.persistent_source_epoch,
        )
        != (b.cp_sha256, b.controller_revision, b.source_epoch, b.reset_generation, b.baseline_revision, b.persistent_source_epoch)
        or trace.control_ns != control.effect_prepare_ns
        or not control.state_ns <= trace.control_ns <= control.control_ns
        or trace.qualified
        and (
          control.applied_profile_sequence <= 0
          or not 0
          < control.applied_profile_observed_ns
          <= control.control_ns
          <= control.applied_profile_expires_ns
          <= control.applied_profile_observed_ns + 500_000_000
        )
      ):
        raise ValueError('actual old full30 trace source or lease changed')
      return tuple(value for _, value in trace.applied)
    # No full owner and genuinely neutral legacy inputs mean neutral full30;
    # a pending offer/report is NEVER used to fill unseen applied coordinates.
    return (0.0,) * len(NAMES)

  def observe_source_control(self, control, *, profile_sha256, now_ns, current_native, preserve_completed=False):
    """EVERY genuine controller tick, independently of CAN transmit cadence."""
    self.last_input_status = 'ignored'
    try:
      self._source_control(control, current_native)
      if type(now_ns) is not int or not 0 <= now_ns - control.control_ns <= MAX_POSE_GAP_NS or control.domain != 'normalized_torque':
        raise ValueError('episode current actual complete profile')
      actual_coordinates = self._actual_coordinates(control)
      profile_key = struct.pack('!30d', *actual_coordinates)
      if profile_key not in self.profile_keys:
        self.profile_keys[profile_key] = full_profile_sha256(self.binding.baseline_revision, dict(zip(NAMES, actual_coordinates, strict=True)))
        if len(self.profile_keys) > 4:
          self.profile_keys.popitem(last=False)
      if self.profile_keys[profile_key] != profile_sha256:
        raise ValueError('episode current actual complete profile')
      stage = dict(control.stage_values)
      inputs = dict(control.replay_inputs)
      frame = inputs['control_frame']
      if not _finite(frame) or frame < 0 or int(frame) != frame:
        raise ValueError('actual original owner frame sequence')
      geometry = stage['setpoint']
      goal = tracking_reference_acceleration(control) if self.source_tracking else tracking_reference_force(control)
      if len(stage) != len(control.stage_values) or not _finite(geometry):
        raise ValueError('episode original stage geometry')
      stamp = control.control_ns
      if self.last and stamp <= self.last:
        old = self.ticks.get(stamp)
        if old is not None and old.control == control and old.profile == profile_sha256 and old.geometry == geometry and old.goal == goal:
          return False
        raise ValueError('episode control clock regression/duplicate mutation')
      if self.last and (stamp - self.last > MAX_CONTROL_GAP_NS or profile_sha256 != self.profile or frame != cast(float, self.last_frame) + 1):
        self.interrupt('episode control gap or actual profile changed', preserve_completed=preserve_completed)
      self.ticks[stamp] = _Tick(control, None, profile_sha256, geometry, goal)
      self.profile = profile_sha256
      self._geometry(stamp, geometry)
      self.last = stamp
      self.last_frame = frame
      if len(self.ticks) > 1400:
        raise ValueError('bounded original fullrate history')
      while self.ticks and stamp - next(iter(self.ticks)) > HISTORY_NS:
        old, _ = self.ticks.popitem(last=False)
        self.emission_count -= len(self.emissions.pop(old, {}))
        self.probes.pop(old, None)
        self.probe_received.pop(old, None)
        if not any(old in row['requested'] for row in self.completed.values()):
          self.probe_requests.pop(old, None)
      self._finish()
      self.last_input_status = 'accepted'
      return True
    except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
      self.interrupt('episode original control unqualified', preserve_completed=preserve_completed)
      return False

  def observe_control(self, control, output, *, profile_sha256, now_ns, current_native, preserve_completed=False):
    """Attach only an actual emitted native/CarOutput pair, never fill a tick."""
    self.last_input_status = 'ignored'
    try:
      self._pair(control, output, current_native)
      if type(now_ns) is not int or not 0 <= now_ns - output.apply_ns <= MAX_POSE_GAP_NS:
        raise ValueError('episode actual emission receipt stale')
      # Backwards compatibility for genuine100Hz emitting platforms. Slower
      # platforms MUST feed observe_source_control separately for EVERY tick.
      if control.control_ns > self.last:
        if not self.observe_source_control(
          control, profile_sha256=profile_sha256, now_ns=now_ns, current_native=current_native, preserve_completed=preserve_completed
        ):
          return False
      tick = self.ticks.get(control.control_ns)
      if tick is None:
        return False  # An old emission cannot recreate discarded source time.
      if tick.control != control or tick.profile != profile_sha256:
        raise ValueError('actual emission differs from original source tick')
      # One consumed CC can be emitted on multiple genuine Card ticks. Bind
      # each original application time rather than replacing or inventing one.
      prior = self.emissions.get(control.control_ns, {}).get(output.apply_ns)
      if prior is not None:
        if prior == output:
          return False
        raise ValueError('changed duplicate actual emission')
      if self.emission_count >= 2800:
        raise ValueError('bounded original emission history')
      self.emissions.setdefault(control.control_ns, {})[output.apply_ns] = output
      self.emission_count += 1
      self.ticks[control.control_ns] = replace(tick, output=output)
      self.last_input_status = 'accepted'
      return True
    except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
      self.interrupt('episode actual native join unqualified', preserve_completed=preserve_completed)
      return False

  def _complete(self, end):
    if self.start and 200_000_000 <= end - self.start <= MAX_EVENT_NS:
      self.pending.append((self.start, end, self.mode, self.first_sign, self.profile))

  def _input_window(self, stamp, goal):
    if self.input_window_origin is None:
      self.input_window_origin = stamp
      self.input_window_boundary = stamp + 2_000_000_000
      self.start = stamp
    elif not self.start:
      if self.input_window_wait_until is None:
        elapsed = stamp - self.input_window_origin
        self.input_window_wait_until = self.input_window_origin + ((elapsed + 1_999_999_999) // 2_000_000_000) * 2_000_000_000
      if stamp < self.input_window_wait_until:
        return  # Skip the interrupted partial window; never re-anchor on its outcome.
      self.start = stamp
      self.input_window_boundary = self.input_window_origin + ((stamp - self.input_window_origin) // 2_000_000_000 + 1) * 2_000_000_000
      self.input_window_wait_until = None
    if stamp >= self.input_window_boundary:
      if self.input_window_count:
        signed = self.input_window_sum / self.input_window_count
        self.first_sign = 1 if signed > 0.03 else -1 if signed < -0.03 else 0
        same_direction = self.input_window_min >= 0 or self.input_window_max <= 0
        self.mode = 'large' if self.first_sign and same_direction and max(abs(self.input_window_min), abs(self.input_window_max)) >= 0.25 else 'subtle'
        self._complete(self.last)  # Last ORIGINAL observed control, not a synthetic boundary tick.
      self.start = stamp
      self.input_window_boundary = self.input_window_origin + ((stamp - self.input_window_origin) // 2_000_000_000 + 1) * 2_000_000_000
      self.input_window_min = self.input_window_max = self.input_window_sum = 0.0
      self.input_window_count = 0
    self.input_window_min = min(self.input_window_min, goal)
    self.input_window_max = max(self.input_window_max, goal)
    self.input_window_sum += goal
    self.input_window_count += 1
    self.mode = 'subtle'

  def _geometry(self, stamp, goal):
    if self.source_tracking:
      self._input_window(stamp, goal)
      return
    sign = 1 if goal > 0.03 else -1 if goal < -0.03 else 0
    if abs(goal) >= 0.25:
      self.neutral = 0
      if self.mode != 'large':
        self.start, self.mode, self.first_sign, self.reversals = stamp, 'large', sign, 0
      elif sign != self.first_sign:
        self._complete(stamp - 1)
        self.start, self.first_sign = stamp, sign
      self.prior_sign = sign
      return
    if self.mode == 'large':
      if not sign:
        self.neutral = self.neutral or stamp
        if stamp - self.neutral >= 500_000_000:
          self._complete(stamp)
          self.start = self.neutral = self.prior_sign = self.first_sign = self.reversals = 0
          self.mode = None
      elif sign != self.first_sign:
        self._complete(stamp - 1)
        self.start = self.neutral = self.prior_sign = self.first_sign = self.reversals = 0
        self.mode = None
      else:
        self.neutral = 0
      return
    if sign and self.prior_sign and sign != self.prior_sign:
      if self.mode is None:
        self.start, self.mode, self.first_sign, self.reversals = stamp, 'subtle', sign, 0
      else:
        self.reversals += 1
        if self.reversals == 2 and sign == self.first_sign:
          self._complete(stamp - 1)
          self.start, self.first_sign, self.reversals = stamp, sign, 0
    if self.mode == 'subtle' and stamp - self.start > MAX_EVENT_NS:
      self.start = self.first_sign = self.reversals = 0
      self.mode = None
    if sign:
      self.prior_sign = sign

  def request_probe(self, control_ns, *, group, names, queued_ns):
    """Register ONLY after owned immutable prepared-job enqueue; no permission.

    The actor queue is bounded to four genuine prepared endpoint jobs. This
    reserves numerical work only, never child execution or native permission.
    queued_ns is the real parent clock, independent of archived source stamps.
    First request fixes a 30s numerical deadline; subsequent requests cannot
    extend it. Register during the window, before it has been drained.
    """
    if (
      type(control_ns) is not int
      or type(queued_ns) is not int
      or queued_ns <= 0
      or type(group) is not str
      or group not in PROBE_GROUPS
      or type(names) is not tuple
      or not 1 <= len(names) <= len(PROBE_GROUPS[group])
      or any(type(name) is not str for name in names)
      or probe_names(names) != names
      or any(name not in PROBE_GROUPS[group] for name in names)
    ):
      return False
    tick = self.ticks.get(control_ns)
    if tick is None or tick.output is None:
      return False
    old = self.probe_requests.get(control_ns)
    if old is not None:
      return old[:2] == (group, names)  # Never refresh the original deadline.
    if len(self.probe_requests) >= MAX_PROBE_REQUESTS:
      return False
    for row in self.completed.values():
      if row['episode'].start_ns <= control_ns <= row['episode'].end_ns:
        if row['group'] != group or row['names'] != names:
          return False
        row['requested'].add(control_ns)
        self.probe_requests[control_ns] = (group, names, queued_ns)
        return True  # Existing first-request deadline remains immutable.
    # A ready/drained episode cannot be amended or counted a second time.
    if control_ns <= self.closed_through_ns or any(e.start_ns <= control_ns <= e.end_ns for e in self.ready):
      return False
    self.probe_requests[control_ns] = (group, names, queued_ns)
    return True

  def observe_probe(self, result, *, now_ns=None):
    from openpilot.starpilot.flm.live_tracking_probe import validate_probe_result

    try:
      validate_probe_result(result, self.binding)
      request = self.probe_requests.get(result.control_ns)
      if request is not None and (
        type(now_ns) is not int
        or not request[2] <= now_ns <= request[2] + PROBE_DEADLINE_NS
        or result.group != request[0]
        or tuple(row.name for row in result.rows) != request[1]
      ):
        return False
      for row in self.completed.values():
        point = next((point for point in row['points'] if point.control_ns == result.control_ns), None)
        if point is None:
          continue
        if (
          request is None
          or result.control_ns not in row['requested']
          or now_ns > row['deadline']
          or result.apply_ns != point.apply_ns
          or result.profile_sha256 != row['episode'].profile_sha256
          or result.model_revision != self.certificate.model_revision
        ):
          return False
        row['probes'][result.control_ns] = result
        row.pop('_wait_signature', None)
        self._release_completed(now_ns)
        return True
      tick = self.ticks.get(result.control_ns)
      if (
        tick is None
        or result.apply_ns not in self.emissions.get(result.control_ns, {})
        or result.profile_sha256 != tick.profile
        or result.model_revision != self.certificate.model_revision
      ):
        return False
      self.probes[result.control_ns] = result
      if request is not None:
        self.probe_received[result.control_ns] = now_ns
      return True
    except (AttributeError, TypeError, ValueError):
      return False

  @staticmethod
  def _gradient_rows(points, probes):
    rows = {name: [] for name in NAMES}
    seen = set()
    for point in points:
      key = point.control_ns, point.apply_ns
      probe = probes.get(point.control_ns)
      if key in seen or probe is None or getattr(probe, 'apply_ns', None) != point.apply_ns:
        continue
      seen.add(key)
      for row in probe.rows:
        if row.name in rows and getattr(row, 'status', None) in ('separated', 'quantized'):
          rows[row.name].append((point, row))
    return rows

  @staticmethod
  def _gradient_coverage(points, probes, requested, rows):
    if not points or set(probes) != set(requested):
      return {}
    first, last = min(p.control_ns for p in points), max(p.control_ns for p in points)
    slots = {p.control_ns: min(3, (p.control_ns - first) * 4 // (last - first + 1)) for p in points}
    required = set(slots.values())
    return {name: len(values) >= 4 and {slots[point.control_ns] for point, _ in values} == required for name, values in rows.items()}

  @staticmethod
  def _native_gradient_complete(points, probes, requested, names):
    rows = TrackingEpisodeBuilder._gradient_rows(points, probes)
    coverage = TrackingEpisodeBuilder._gradient_coverage(points, probes, requested, rows)
    return bool(names) and all(coverage.get(name, False) for name in names)

  def _sensitivities(self, points, probes, *, requested=None):
    rows = self._gradient_rows(points, probes)
    coverage = self._gradient_coverage(points, probes, set(probes) if requested is None else requested, rows)
    def directed_effect(row, side):
      coordinate = getattr(row, side + '_coordinate') - row.baseline_coordinate
      native = getattr(row, side + '_delivered') - row.baseline_delivered
      native_width = row.plus_delivered - row.minus_delivered
      if coordinate == 0 or native_width == 0:
        return 0.0
      # Recover the same measured/native conversion used by the secant,
      # then measure THIS direction against the encoded baseline. A visible
      # opposite probe cannot make a quantized selected direction visible.
      gain = abs(row.physical_derivative * (row.plus_coordinate - row.minus_coordinate) / native_width)
      return abs(native) * gain * MAX_STEP / abs(coordinate)

    return tuple(
      HookSensitivity(
        name,
        sum(point.error * row.physical_derivative for point, row in values),
        sum(row.physical_derivative**2 for _, row in values),
        sum(row.native_separated for _, row in values),
        len(values),
        math.sqrt(sum(row.max_step_physical_delta**2 for _, row in values) / len(values)),
        coverage.get(name, False),
        sum(row.minus_delivered != row.baseline_delivered for _, row in values),
        sum(row.plus_delivered != row.baseline_delivered for _, row in values),
        math.sqrt(sum(directed_effect(row, 'minus')**2 for _, row in values) / len(values)),
        math.sqrt(sum(directed_effect(row, 'plus')**2 for _, row in values) / len(values)),
      )
      for name, values in rows.items()
      if values
    )

  def _release_completed(self, now_ns):
    if type(now_ns) is not int or now_ns <= 0:
      return  # Missing real clock holds; never fabricate a numerical timeout.
    from openpilot.starpilot.flm.live_tracking_probe import validate_probe_result

    for key, row in tuple(self.completed.items()):
      wait_signature = len(row['probes']), len(row['requested'])
      if now_ns < row['deadline'] and (set(row['probes']) != row['requested'] or row.get('_wait_signature') == wait_signature):
        continue  # Only cache a decision to WAIT, never a proof to publish.
      valid = {}
      for control_ns, probe in row['probes'].items():
        try:
          validate_probe_result(probe, self.binding)
          point = next(point for point in row['points'] if point.control_ns == control_ns)
          if (
            control_ns in row['requested']
            and probe.apply_ns == point.apply_ns
            and probe.profile_sha256 == row['episode'].profile_sha256
            and probe.model_revision == self.certificate.model_revision
            and probe.group == row['group']
            and tuple(item.name for item in probe.rows) == row['names']
          ):
            valid[control_ns] = probe
        except (AttributeError, TypeError, ValueError, StopIteration):
          pass
      supported = all(sum(any(item.name == name for item in probe.rows) for probe in valid.values()) >= 4 for name in row['names'])
      sensitivities = self._sensitivities(row['points'], valid, requested=row['requested'])
      # One unavailable coordinate cannot invalidate another independently
      # complete native gradient. Every proposed coordinate checks its own flag.
      gradient_complete = any(feature.native_gradient_complete for feature in sensitivities)
      if not gradient_complete and now_ns < row['deadline']:
        row['_wait_signature'] = wait_signature
        continue
      if (supported and gradient_complete) or now_ns >= row['deadline']:
        episode = replace(row['episode'], sensitivity=sensitivities, native_gradient_complete=gradient_complete).validate()
        self.ready.append(episode)
        self.closed_through_ns = max(self.closed_through_ns, episode.end_ns)
        self.plot = (episode, row['points'])
        del self.completed[key]
        for point in row['points']:
          self.probe_requests.pop(point.control_ns, None)
        self.reason = 'qualified_whole_tracking_episode' if gradient_complete else 'whole_episode_probe_deadline_partial_support'

  def observe_pose(self, pose, control, output, *, witness, now_ns, current_native, preserve_completed=False):
    self.last_input_status = 'ignored'
    try:
      self._pair(control, output, current_native)
      raw_witness = validate_response_witness(witness, self.binding, now_ns)
      if raw_witness.certificate != self.certificate or (raw_witness.last_control_ns, raw_witness.last_apply_ns, raw_witness.last_pose_ns) != (
        control.control_ns,
        output.apply_ns,
        pose.mono_ns,
      ):
        raise ValueError('episode current physical model or exact residual pair changed')
      if type(pose) is not OriginalSourcePose:
        raise ValueError('exact original pose')
      validate_pose(pose, now_ns)
      b, lag = self.binding, self.certificate.lag_ns
      if (
        (pose.identity, pose.source_epoch, pose.baseline_revision, pose.controller_revision, pose.calibration, pose.native_identity, pose.native_required)
        != (b.identity, b.source_epoch, b.baseline_revision, b.controller_revision, physical_calibration_generation(b), b.native_identity, b.native_required)
        or not 0 <= pose.source_upper_ns - lag - output.apply_ns <= 30_000_000
        or output.apply_ns > pose.source_lower_ns - lag
        or not 0 <= now_ns - pose.mono_ns <= MAX_POSE_GAP_NS
      ):
        raise ValueError('episode lag/source interval changed')
      # A genuine lagged pair from BEFORE a source/profile boundary cannot
      # count toward the new lineage. It also must not repeatedly erase its
      # new 100Hz history while the physical lag catches up.
      if self.ticks and control.control_ns < next(iter(self.ticks)):
        return False
      tick = self.ticks.get(control.control_ns)
      if (
        tick is None
        or tick.control != control
        or self.emissions.get(control.control_ns, {}).get(output.apply_ns) != output
        or dict(control.stage_values).get('setpoint') != tick.geometry
        or (tracking_reference_acceleration(control) if self.source_tracking else tracking_reference_force(control)) != tick.goal
      ):
        raise ValueError('episode pose not in exact observed owner history')
      if pose.mono_ns <= self.last_pose_ns:
        return False
      if self.endpoints and control.control_ns <= self.endpoints[-1].control_ns:
        return False
      if self.last_pose_ns and pose.mono_ns - self.last_pose_ns > MAX_POSE_GAP_NS:
        raise ValueError('episode independent pose gap')
      # Plant identification associates a command with its later response.
      # Tracking accuracy instead compares the desired and actual state at
      # the SAME observed time, as the existing controller reports do.
      # Keep the causal command/native probe below; use a distinct original
      # reference control, never an interpolated or future desired sample.
      reference = next((self.ticks[stamp] for stamp in reversed(self.ticks) if stamp <= pose.source_lower_ns), None)
      if reference is None or pose.source_upper_ns - reference.control.control_ns > 30_000_000 or reference.profile != tick.profile:
        self.last_input_status = 'pending'
        return False
      self._source_control(reference.control, current_native)
      goal = tracking_reference_acceleration(reference.control) if self.source_tracking else tracking_reference_force(reference.control)
      if goal != reference.goal:
        raise ValueError('original tracking reference changed')
      measured = pose.speed_mps * pose.yaw_rate
      if not self.source_tracking:
        measured -= ACCELERATION_DUE_TO_GRAVITY * math.sin(pose.roll_rad)
      if not _finite(measured) or pose.speed_mps <= 0:
        raise ValueError('episode finite independent response')
      self.endpoints.append(
        _Endpoint(control.control_ns, output.apply_ns, pose.mono_ns, pose.speed_mps, goal, goal - measured, measured, reference.control.control_ns)
      )
      self.last_pose_ns = pose.mono_ns
      self.watermark = pose.source_lower_ns - lag
      self.numerical_observed_ns = now_ns  # Already validated original pose/witness receipt clock.
      self._finish()
      self.last_input_status = 'accepted'
      return True
    except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
      self.interrupt('episode original physical join unqualified', preserve_completed=preserve_completed)
      return False

  def _finish(self):
    while self.pending and self.watermark > self.pending[0][1] + 30_000_000:
      start, end, kind, direction, profile = self.pending.popleft()
      ticks = [tick for stamp, tick in self.ticks.items() if start <= stamp <= end]
      points = [point for point in self.endpoints if start <= (point.reference_ns if self.source_tracking else point.control_ns) <= end]
      if not ticks or not 4 <= len(points) <= 240:
        self.reason = 'whole_episode_independent_endpoints_required'
        continue
      stamps = [tick.control.control_ns for tick in ticks]
      gaps = [right - left for left, right in zip(stamps, stamps[1:], strict=False)]
      max_gap = max(gaps, default=0)
      if (
        stamps[0] != start
        or not 0 < max_gap <= MAX_CONTROL_GAP_NS
        or any(tick.profile != profile for tick in ticks)
        or max(point.speed for point in points) > 1.15 * min(point.speed for point in points)
      ):
        continue
      from openpilot.starpilot.flm.live_tracking_probe import validate_probe_result

      qualified_probes = {}
      for point in points:
        probe = self.probes.get(point.control_ns)
        if probe is not None:
          try:
            validate_probe_result(probe, self.binding)
            if (
              probe.control_ns == point.control_ns
              and probe.apply_ns == point.apply_ns
              and probe.profile_sha256 == profile
              and probe.model_revision == self.certificate.model_revision
            ):
              qualified_probes[point.control_ns] = probe
          except (AttributeError, TypeError, ValueError):
            pass  # Numerical support disappears; never invent a derivative.
      goal_rms = math.sqrt(sum(point.goal**2 for point in points) / len(points))
      bins = [[] for _ in range(8)]
      for tick in ticks:
        index = min(7, (tick.control.control_ns - start) * 8 // (end - start + 1))
        bins[index].append(tick.goal)
      scale = max(0.1, goal_rms) if self.source_tracking else goal_rms
      if scale <= 0 or any(not values for values in bins):
        self.reason = 'whole_episode_actual_goal_shape_required'
        continue
      goal_shape = tuple(sum(values) / len(values) / scale for values in bins)
      sensitivities = self._sensitivities(points, qualified_probes)
      errors = [point.error for point in points]
      from openpilot.starpilot.flm.live_tracking_response import TrackingSourceContext

      # Prediction residuals do not define the size of observed tracking weave.
      floor = 0.0 if type(self.certificate) is TrackingSourceContext else self.certificate.heldout_rmse
      reversal_hz = tracking_reversal_rate(errors, (end - start) / 1e9, floor)
      digest = hashlib.sha256(
        repr(
          (
            ('fixed-input-kinematic-windows-v1' if self.source_tracking else 'live-whole-tracking-v4-actionable-error-excursions'),
            self.binding,
            profile,
            start,
            end,
            kind,
            tuple((point.control_ns, point.apply_ns, point.pose_ns, point.reference_ns) for point in points),
          )
        ).encode()
      ).hexdigest()
      episode = TrackingEpisode(
        self.binding,
        profile,
        digest,
        start,
        end,
        kind,
        direction,
        min(point.speed for point in points),
        max(point.speed for point in points),
        goal_rms,
        math.sqrt(sum(error**2 for error in errors) / len(errors)),
        reversal_hz,
        len(points),
        max_gap,
        sensitivities,
        goal_shape,
        normalization_accel=scale if self.source_tracking else None,
      )
      episode.validate()
      requests = {point.control_ns: self.probe_requests[point.control_ns] for point in points if point.control_ns in self.probe_requests}
      defer = (
        (kind == 'subtle' or self.source_tracking)
        and getattr(self, 'defer_unprobed', False) is True
        and type(getattr(self, 'numerical_observed_ns', 0)) is int
        and self.numerical_observed_ns > 0
      )
      if requests or defer:
        if requests:
          group, names, _ = next(iter(requests.values()))
          deadline = min(value[2] for value in requests.values()) + PROBE_DEADLINE_NS
        else:
          # A short whole maneuver fits inside the EXISTING historical input
          # age. Keep its ORIGINAL endpoints for one bounded numerical batch,
          # rather than closing it before the4Hz coordinator can request them.
          group, names = 'all30', self.numeric_probe_names
          deadline = self.numerical_observed_ns + PROBE_DEADLINE_NS
        if any(value[:2] != (group, names) for value in requests.values()) or len(self.completed) >= MAX_COMPLETED:
          self.reason = 'whole_episode_probe_group_or_backlog_unavailable'
          continue
        self.completed[episode.episode_id] = {
          'episode': episode,
          'points': tuple(points),
          'group': group,
          'names': names,
          'requested': set(requests),
          'deadline': deadline,
          'probes': {
            key: value for key, value in qualified_probes.items() if key in requests and requests[key][2] <= self.probe_received.get(key, 0) <= deadline
          },
        }
      else:
        self.ready.append(episode)
        self.closed_through_ns = max(self.closed_through_ns, episode.end_ns)
        self.plot = (episode, tuple(points))
      self.reason = 'qualified_whole_tracking_episode'

  def release_measured_only(self):
    """Release already validated measurements without inventing derivatives.

    Only the coordinator's post-selection comparison path calls this. Every
    episode already passed its original control/native/CAN/pose/clock checks.
    Optional counterfactual probes are not actual actuation permission and
    cannot delay an uncensored baseline or a later measured keep/revert score.
    """
    if len(self.ready) + len(self.completed) > cast(int, self.ready.maxlen):
      self.interrupt('measured_episode_backlog')
      return False
    for key, row in tuple(self.completed.items()):
      episode = replace(row['episode'], sensitivity=(), native_gradient_complete=False).validate()
      self.ready.append(episode)
      self.closed_through_ns = max(self.closed_through_ns, episode.end_ns)
      self.plot = (episode, row['points'])
      for point in row['points']:
        self.probe_requests.pop(point.control_ns, None)
      del self.completed[key]
    self.probe_requests.clear()
    return True

  def drain(self, *, now_ns=None):
    self._release_completed(now_ns)
    # Optional native probes may finish after a later unprobed maneuver.
    # Preserve source order before the optimizer's strict chronological check;
    # never replace source time with numerical completion time. The original
    # probe deadline and bounded ready deque still bound this wait.
    if self.source_tracking and self.ready:
      # Fixed source windows are independently usable. An earlier incomplete
      # numerical job must not hold every later complete window for 30 seconds.
      # Drop it by source order alone, without inspecting tracking error or
      # native sensitivity; late numerical results cannot resurrect it.
      through = max(episode.end_ns for episode in self.ready)
      for key, row in tuple(self.completed.items()):
        if row['episode'].end_ns <= through:
          for point in row['points']:
            self.probe_requests.pop(point.control_ns, None)
          del self.completed[key]
    barrier = min((row['episode'].start_ns for row in self.completed.values()), default=None)
    ordered = sorted(self.ready, key=lambda episode: episode.start_ns)
    result = tuple(episode for episode in ordered if barrier is None or episode.end_ns < barrier)
    self.ready.clear()
    self.ready.extend(episode for episode in ordered if barrier is not None and episode.end_ns >= barrier)
    return result

  def tracking_plot(self):
    """Last completed episode's actual paired samples; never an interpolated line."""
    if self.plot is None:
      return None
    episode, points = self.plot
    indices = tuple(range(len(points))) if len(points) <= 64 else tuple(index * (len(points) - 1) // 63 for index in range(64))
    selected = tuple(points[index] for index in indices)
    return {
      'version': 'live-tracking-paired-plot-v1',
      'episodeId': episode.episode_id,
      'profileSha256': episode.profile_sha256,
      'lagNs': self.certificate.lag_ns,
      'units': 'm/s²',
      'sampled': len(selected) != len(points),
      'totalEndpoints': len(points),
      'points': [
        {
          'controlMonoTime': point.control_ns,
          'applyMonoTime': point.apply_ns,
          'poseMonoTime': point.pose_ns,
          'desiredForceAccel': point.goal,
          'measuredForceAccel': point.measured,
          'errorForceAccel': point.error,
        }
        for point in selected
      ],
    }