StarPilot/openpilot/starpilot/curve_speed/runtime.py

"""Session-local curve-speed decisions; no Params, bus, or actuator access."""

from dataclasses import dataclass

from openpilot.starpilot.curve_speed.learning import LearnedCurve, finite_number
from openpilot.starpilot.curve_speed.target import CurveProfile, TargetFilter, evaluate, MAX_STEP_SECONDS

MODEL_MAX_AGE_NS = 150_000_000
INPUT_MAX_AGE_NS = 150_000_000
OVERRIDE_WATCH_NS = 6_000_000_000
TRAINING_QUIET_NS = 5_000_000_000
TRAINING_SETTLE_NS = 2_000_000_000
LEAD_CLEAR_NS = 1_000_000_000
GLOW_RELEASE_NS = 3_000_000_000
PERSIST_INTERVAL_NS = 30_000_000_000
CRUISING_SPEED = 5.0
ACTIVE_ON_DELTA = 0.5
ACTIVE_OFF_DELTA = 0.25
IN_CURVE_ACCEL = 1.3
NUDGE = 0.15


@dataclass(frozen=True)
class Frame:
  now_ns: int
  model_ns: int
  inputs_fresh: bool
  profile: CurveProfile | None
  system_longitudinal: bool
  long_active: bool
  enabled: bool
  force_decel: bool
  cruise_mps: float
  ego_mps: float
  measured_curvature: float
  tracking_lead: bool | None
  blinker: bool
  gas_pressed: bool
  brake_pressed: bool
  drive_id: int = 0
  following_lead: bool | None = None


@dataclass(frozen=True)
class DriverEvent:
  producer_session: str
  event_id: int
  observed_ns: int
  kind: str
  button: str
  car_state_ns: int


@dataclass(frozen=True)
class Result:
  candidate_mps: float | None
  ceiling_mps: float | None
  binding_distance_m: float
  reason: str
  training: bool
  progress: float
  dirty_revision: int | None
  learning_reason: str = ''


class Runtime:
  """The candidate is diagnostic; only ceiling_mps may later reach a planner."""

  def __init__(self, document: object = None, *, enabled: bool = False, no_lead: bool = False):
    loaded = LearnedCurve.load(document)
    self.curve: LearnedCurve = loaded.curve
    self.document_valid = loaded.valid
    self.document_reason = loaded.reason
    self.enabled = enabled
    self.no_lead = no_lead
    self.filter = TargetFilter()
    self.last_ns = 0
    self.last_model_ns = 0
    self.last_valid_ns = 0
    self.drive_id = 0
    self.was_controlling = False
    self.was_long_active = False
    self.was_gas = False
    self.was_brake = False
    self.last_training_ns = 0
    self.quiet_until_ns = 0
    self.last_persist_ns = 0
    self.last_lead_ns = 0
    self.watch_until_ns = 0
    self.watch_curvature = 0.0
    self.watch_peak = 0.0
    self.nudged = False
    self.override = False
    self.confirmed_ns = 0
    self.pending_ns = 0
    self.pending_target: float | None = None
    self.pending_cap = False
    self.pending_hold = False
    self.pending_cruise = 0.0
    self.pending_ego = 0.0
    self.glow = False
    self.glow_release_since_ns = 0
    self.event_session = ''
    self.last_event_id = -1
    self.last_event_ns = 0
    self.event_floor_ns = 0
    self.retired_sessions: set[str] = set()
    self.event_exhausted = False

  def reset(self, *, drive_id: int = 0) -> None:
    self.event_floor_ns = max(self.event_floor_ns, self.last_ns, self.last_event_ns)
    self.filter.reset()
    self.last_ns = 0
    self.last_model_ns = 0
    self.last_valid_ns = 0
    self.drive_id = drive_id
    self.was_controlling = False
    self.was_long_active = False
    self.was_gas = False
    self.was_brake = False
    self.last_training_ns = 0
    self.last_lead_ns = 0
    self.quiet_until_ns = 0
    self.watch_until_ns = 0
    self.watch_curvature = 0.0
    self.watch_peak = 0.0
    self.nudged = False
    self.override = False
    self.confirmed_ns = 0
    self.pending_ns = 0
    self.pending_target = None
    self.pending_cap = False
    self.pending_hold = False
    self.pending_cruise = 0.0
    self.pending_ego = 0.0
    self.glow = False
    self.glow_release_since_ns = 0
    self.event_session = ''
    self.last_event_id = -1
    self.last_event_ns = 0
    self.retired_sessions.clear()
    self.event_exhausted = False

  def acknowledge_saved(self, revision: int) -> None:
    self.curve.acknowledge_saved(revision)
    self.last_persist_ns = self.last_ns

  def confirm_applied(self, frame_ns: int, *, applied: bool) -> bool:
    """Call only after planner composition identifies the selected curve owner."""
    if (type(frame_ns) is not int or type(applied) is not bool or frame_ns != self.pending_ns or
        self.confirmed_ns == frame_ns):
      self.was_controlling = False
      self.glow = False
      return False
    self.confirmed_ns = frame_ns
    if self.pending_hold:
      return True
    target = self.pending_target
    selected = applied and self.pending_cap and target is not None
    self.was_controlling = bool(selected and target < self.pending_cruise - 1.0 and
                                self.pending_ego >= target - ACTIVE_OFF_DELTA)
    if not selected and (self.pending_cap or target is None):
      self.glow = False
      self.glow_release_since_ns = 0
    elif self.was_controlling:
      self.glow = True
      self.glow_release_since_ns = 0
      self.quiet_until_ns = frame_ns + TRAINING_QUIET_NS
    elif self.glow and target is not None and target > self.pending_cruise - ACTIVE_OFF_DELTA:
      if not self.glow_release_since_ns:
        self.glow_release_since_ns = frame_ns
      elif frame_ns - self.glow_release_since_ns >= GLOW_RELEASE_NS:
        self.glow = False
        self.glow_release_since_ns = 0
    return True

  def _event(self, event: DriverEvent | None, frame: Frame) -> bool:
    if event is None or self.event_exhausted:
      return False
    if (not event.producer_session or type(event.event_id) is not int or event.event_id < 0 or
        type(event.observed_ns) is not int or not self.event_floor_ns < event.observed_ns <= frame.now_ns or
        frame.now_ns - event.observed_ns > MODEL_MAX_AGE_NS or
        type(event.car_state_ns) is not int or not 0 < event.car_state_ns <= frame.now_ns or
        not event.observed_ns <= event.car_state_ns or
        event.car_state_ns - event.observed_ns > MODEL_MAX_AGE_NS):
      return False
    if event.producer_session in self.retired_sessions:
      return False
    if event.producer_session != self.event_session:
      if event.observed_ns <= self.last_event_ns:
        return False
      if self.event_session:
        self.retired_sessions.add(self.event_session)
        if len(self.retired_sessions) > 64:
          self.event_exhausted = True
          return False
      self.event_session = event.producer_session
      self.last_event_id = -1
    if event.event_id <= self.last_event_id:
      return False
    self.last_event_id = event.event_id
    self.last_event_ns = event.observed_ns
    # SLC confirmation, command receipt and ordinary cruise changes are not
    # evidence of an unconsumed physical press. Card emits the separate kind
    # only after excluding actions owned by SLC.
    return event.kind == 'curveAccelPress' and event.button == 'accel'

  def _finish_watch(self, frame: Frame, measured_accel: float) -> None:
    if not self.watch_until_ns:
      return
    if finite_number(measured_accel) and measured_accel > self.watch_peak:
      self.watch_peak = measured_accel
      self.watch_curvature = abs(frame.measured_curvature)
    if frame.now_ns < self.watch_until_ns and (frame.gas_pressed or frame.brake_pressed or
                                               measured_accel >= IN_CURVE_ACCEL):
      return
    if self.document_valid and self.watch_curvature > 0:
      self.curve.nudge(self.watch_curvature, max(self.watch_peak, self.curve.comfort(self.watch_curvature) + NUDGE))
    self.watch_until_ns = 0

  def step(self, frame: Frame, *, event: DriverEvent | None = None) -> Result:
    now = frame.now_ns
    previous_target = self.pending_target
    if type(now) is not int or now <= 0:
      self.reset()
      return self._result(None, None, 0.0, 'invalid_clock', False, 0)
    if (self.last_ns and (now <= self.last_ns or now - self.last_ns > int(MAX_STEP_SECONDS * 1e9)) or
        self.drive_id and frame.drive_id and frame.drive_id != self.drive_id):
      self.reset(drive_id=frame.drive_id)
    elif self.last_ns and self.confirmed_ns != self.last_ns:
      self.was_controlling = False
    if frame.drive_id:
      self.drive_id = frame.drive_id
    dt = (now - self.last_ns) / 1e9 if self.last_ns else 0.05
    self.last_ns = now
    self.pending_ns = now
    self.pending_target = None
    self.pending_cap = False
    self.pending_hold = False
    valid_profile = (frame.inputs_fresh and frame.profile is not None and
                     type(frame.model_ns) is int and 0 < frame.model_ns <= now and
                     now - frame.model_ns <= MODEL_MAX_AGE_NS and
                     frame.profile.observed_ns == frame.model_ns and
                     frame.model_ns > self.last_model_ns)
    if not valid_profile:
      self.watch_until_ns = 0
      self.last_training_ns = 0
      self.pending_hold = (self.filter.target is not None and
                           now - self.last_valid_ns <= int(MAX_STEP_SECONDS * 1e9))
      if self.pending_hold:
        self.pending_target = self.filter.target
      else:
        self.filter.reset()
        self.nudged = False
        self.override = False
        self.was_controlling = False
        self.glow = False
        self.was_long_active = frame.long_active
        self.was_gas = frame.gas_pressed
        self.was_brake = frame.brake_pressed
      return self._result(None, None, 0.0, 'stale_or_invalid_evidence', False, now)
    self.last_model_ns = frame.model_ns
    self.last_valid_ns = now
    if (not finite_number(frame.cruise_mps) or frame.cruise_mps <= 0 or
        not finite_number(frame.ego_mps) or frame.ego_mps < 0 or
        frame.ego_mps > 100.0 or not finite_number(frame.measured_curvature) or
        abs(frame.measured_curvature) > 0.1):
      self.filter.reset()
      self.watch_until_ns = 0
      self.last_training_ns = 0
      self.override = False
      self.was_controlling = False
      self.glow = False
      self.was_long_active = frame.long_active
      return self._result(None, None, 0.0, 'invalid_numeric_input', False, now)
    try:
      envelope = evaluate(frame.profile, self.curve, frame.cruise_mps)
    except (ValueError, OverflowError):
      self.filter.reset()
      self.watch_until_ns = 0
      self.last_training_ns = 0
      self.override = False
      self.was_controlling = False
      self.glow = False
      self.was_long_active = frame.long_active
      return self._result(None, None, 0.0, 'invalid_profile', False, now)
    candidate = envelope.speed_mps
    measured_accel = abs(frame.measured_curvature) * frame.ego_mps**2
    in_curve = measured_accel >= IN_CURVE_ACCEL
    accel_press = self._event(event, frame)
    gas_edge = frame.gas_pressed and not self.was_gas
    gas_override = gas_edge and previous_target is not None and previous_target < frame.ego_mps - 0.5
    brake_edge = frame.brake_pressed and not self.was_brake
    long_drop = self.was_long_active and not frame.long_active
    feedback_allowed = self.enabled and self.document_valid and frame.system_longitudinal
    if not feedback_allowed:
      self.watch_until_ns = 0
      self.override = False
      self.nudged = False
    if self.was_controlling and feedback_allowed and (accel_press or gas_override):
      self.override = True
    if self.override and not in_curve and candidate >= frame.cruise_mps - ACTIVE_OFF_DELTA:
      self.override = False
    allowed = (self.enabled and self.document_valid and frame.system_longitudinal and frame.enabled and
               frame.long_active and not frame.force_decel and frame.ego_mps > CRUISING_SPEED and
               not frame.gas_pressed and not frame.brake_pressed and not self.override and
               (not self.no_lead or frame.following_lead is False) and not (frame.blinker and not in_curve))
    if not allowed:
      self.filter.reset()
      if not self.enabled or not self.document_valid or not frame.system_longitudinal:
        self.watch_until_ns = 0
    try:
      target = self.filter.step(envelope, ego_mps=frame.ego_mps, cruise_mps=frame.cruise_mps, dt=dt) if allowed else None
    except ValueError:
      target = None
    ceiling = target if target is not None and target < frame.cruise_mps - ACTIVE_ON_DELTA else None
    controlling = ceiling is not None
    self.pending_target = target
    self.pending_cap = ceiling is not None
    self.pending_cruise = frame.cruise_mps
    self.pending_ego = frame.ego_mps
    if feedback_allowed:
      self._finish_watch(frame, measured_accel)
    if self.was_controlling and not self.nudged and feedback_allowed:
      if (accel_press or gas_override) and finite_number(measured_accel):
        self.watch_until_ns = now + OVERRIDE_WATCH_NS
        self.watch_curvature = abs(frame.measured_curvature)
        self.watch_peak = measured_accel
        self.nudged = True
      elif (brake_edge or long_drop) and in_curve:
        curvature = abs(frame.measured_curvature)
        self.curve.nudge(curvature, self.curve.comfort(curvature) - NUDGE)
        self.nudged = True
    if not controlling and not self.override:
      self.nudged = False
    if frame.tracking_lead is not False:
      self.last_lead_ns = now
      self.last_training_ns = 0
    elif not self.last_lead_ns:
      self.last_lead_ns = now
    manual = not frame.long_active or frame.gas_pressed or frame.brake_pressed
    trainable = (feedback_allowed and manual and
                 frame.ego_mps > CRUISING_SPEED and frame.tracking_lead is False and not frame.blinker and
                 now - self.last_lead_ns >= LEAD_CLEAR_NS and
                 now >= self.quiet_until_ns and finite_number(measured_accel) and
                 not controlling)
    learned = False
    if trainable:
      if not self.last_training_ns:
        self.last_training_ns = now
      if now - self.last_training_ns >= TRAINING_SETTLE_NS and in_curve:
        self.curve.observe(abs(frame.measured_curvature), measured_accel)
        learned = True
    else:
      self.last_training_ns = 0
    learning_reason = ('learning_disabled' if not self.enabled else
                       'invalid_learning_document' if not self.document_valid else
                       'learning_requires_system_longitudinal' if not frame.system_longitudinal else
                       'learning_requires_driver_speed' if not manual else
                       'learning_requires_cruising_speed' if frame.ego_mps <= CRUISING_SPEED else
                       'learning_lead_unknown' if frame.tracking_lead is None else
                       'learning_following_lead' if frame.tracking_lead else
                       'learning_lead_clear_wait' if now - self.last_lead_ns < LEAD_CLEAR_NS else
                       'learning_indicator_active' if frame.blinker else
                       'learning_feedback_quiet_wait' if now < self.quiet_until_ns else
                       'learning_controller_active' if controlling else
                       'learning_settling' if now - self.last_training_ns < TRAINING_SETTLE_NS else
                       'learning' if learned else 'learning_waiting_for_curve')
    self.was_long_active = frame.long_active
    self.was_gas = frame.gas_pressed
    self.was_brake = frame.brake_pressed
    reason = ('available' if controlling else 'not_binding' if allowed else
              'invalid_learning_document' if not self.document_valid else 'not_eligible')
    return self._result(candidate, ceiling, envelope.binding_distance_m, reason, learned, now, learning_reason)

  def _result(self, candidate: float | None, ceiling: float | None, distance: float,
              reason: str, training: bool, now_ns: int, learning_reason: str | None = None) -> Result:
    revision = (self.curve.revision if self.document_valid and self.curve.dirty and
                (not training or now_ns - self.last_persist_ns >= PERSIST_INTERVAL_NS) else None)
    return Result(candidate, ceiling, distance, reason, training, self.curve.progress, revision, learning_reason or reason)