from __future__ import annotations

import argparse
import hashlib
import json
import os
import re
import signal
import socket
import subprocess
import sys
import threading
import time
from contextlib import suppress
from datetime import UTC, datetime
from pathlib import Path
from types import FrameType
from typing import Any, Final

from k1link.simulation.contracts import (
    AckermannControlSetpoint,
    AuthorityProfile,
    ProviderPin,
    QualificationRun,
    ReproducibilityTier,
    RunKind,
    RunState,
)
from k1link.simulation.orchestrator import SimulationApplicationService
from k1link.simulation.process_supervisor import PosixProcessSupervisor
from k1link.simulation.px4_rover_control import (
    Px4RoverControlSnapshot,
    Ros2Px4AckermannControl,
)
from k1link.simulation.run_store import (
    ACTIVE_RECOVERY_STATES,
    QualificationRunNotFoundError,
    QualificationRunStore,
)
from k1link.simulation.s0 import ComponentPin
from k1link.simulation.stock_rover import (
    GAZEBO_STATE_PROVIDER_ID,
    PX4_COMMAND_PROVIDER_ID,
    STOCK_ROVER_PROVIDER_PROFILE,
    StockRoverTargetPaths,
    load_stock_rover_lifecycle_profile,
    stock_rover_process_environment,
    stock_rover_process_specs,
)
from k1link.simulation.worker import (
    LocalProcessWorkerAdapter,
    S0WorkerGuard,
    SimulationWorldControl,
)
from k1link.simulation.worker_gateway import (
    COMMAND_ACCEPTANCE_SCHEMA,
    MAX_MESSAGE_BYTES,
    OPERATIONS,
    REQUEST_SCHEMA,
    RESPONSE_SCHEMA,
    STATUS_SCHEMA,
    VEHICLE_STATE_SCHEMA,
)

EXPECTED_DISTRO: Final = "MissionCore-Sim"
COMMIT_PATTERN: Final = re.compile(r"^[a-f0-9]{40}$")
SAFE_ID_PATTERN: Final = re.compile(r"^[a-z0-9][a-z0-9-]{0,63}$")
PROVIDER_IDS: Final = (
    "px4-autopilot",
    "gazebo",
    "px4-gazebo-models",
    "micro-xrce-dds-agent",
)
ACTIVE_STATES: Final = frozenset(ACTIVE_RECOVERY_STATES)
POSE_TOPIC: Final = "/world/rover/dynamic_pose/info"
SIM_CLOCK_CACHE_MAX_AGE_NS: Final = 2_000_000_000


class WorkerAgentError(RuntimeError):
    """The worker agent cannot safely satisfy a gateway request."""


class _LifecycleOnlyWorldControl(SimulationWorldControl):
    def pause(self, run_id: str) -> None:
        raise WorkerAgentError(f"pause is not admitted for {run_id}")

    def resume(self, run_id: str) -> None:
        raise WorkerAgentError(f"resume is not admitted for {run_id}")

    def step(self, run_id: str, step_count: int) -> int:
        raise WorkerAgentError(f"step is not admitted for {run_id}:{step_count}")


class GazeboPoseCollector:
    """Read one bounded ground-truth pose sample from Gazebo Transport."""

    def __init__(self, environment: dict[str, str]) -> None:
        self.environment = dict(environment)
        self._sequence = 0
        self._lock = threading.Lock()
        self._latest_sim_time_ns: int | None = None
        self._latest_host_monotonic_ns: int | None = None

    def collect(self, run_id: str) -> dict[str, Any]:
        try:
            completed = subprocess.run(
                (
                    "/usr/bin/gz",
                    "topic",
                    "-e",
                    "--json-output",
                    "-t",
                    POSE_TOPIC,
                    "-n",
                    "1",
                ),
                check=True,
                capture_output=True,
                env=self.environment,
                timeout=3,
            )
        except (OSError, subprocess.SubprocessError) as exc:
            raise WorkerAgentError("Gazebo pose sample is unavailable") from exc
        if len(completed.stdout) > MAX_MESSAGE_BYTES:
            raise WorkerAgentError("Gazebo pose sample exceeds the size limit")
        try:
            document = json.loads(completed.stdout.decode("utf-8"))
        except (UnicodeDecodeError, json.JSONDecodeError) as exc:
            raise WorkerAgentError("Gazebo pose sample is not valid JSON") from exc
        if not isinstance(document, dict):
            raise WorkerAgentError("Gazebo pose sample must be an object")
        pose = _rover_pose(document)
        stamp = _object(_object(document.get("header"), "header").get("stamp"), "stamp")
        sim_time_ns = _integer(stamp.get("sec"), "stamp.sec") * 1_000_000_000 + _integer(
            stamp.get("nsec"),
            "stamp.nsec",
        )
        host_monotonic_ns = time.monotonic_ns()
        with self._lock:
            self._sequence += 1
            sequence = self._sequence
            self._latest_sim_time_ns = sim_time_ns
            self._latest_host_monotonic_ns = host_monotonic_ns
        return {
            "schema_version": VEHICLE_STATE_SCHEMA,
            "run_id": run_id,
            "sequence": sequence,
            "observed_at_utc": _utc_now(),
            "host_monotonic_ns": host_monotonic_ns,
            "sim_time_ns": sim_time_ns,
            "frame_id": "map_enu",
            "child_frame_id": "base_link_flu",
            "pose": {
                "position_m": {
                    "x": _number(_object(pose.get("position"), "position").get("x"), "position.x"),
                    "y": _number(_object(pose.get("position"), "position").get("y"), "position.y"),
                    "z": _number(_object(pose.get("position"), "position").get("z"), "position.z"),
                },
                "orientation_xyzw": {
                    "x": _number(
                        _object(pose.get("orientation"), "orientation").get("x"),
                        "orientation.x",
                    ),
                    "y": _number(
                        _object(pose.get("orientation"), "orientation").get("y"),
                        "orientation.y",
                    ),
                    "z": _number(
                        _object(pose.get("orientation"), "orientation").get("z"),
                        "orientation.z",
                    ),
                    "w": _number(
                        _object(pose.get("orientation"), "orientation").get("w"),
                        "orientation.w",
                    ),
                },
            },
            "source": {
                "provider": GAZEBO_STATE_PROVIDER_ID,
                "topic": POSE_TOPIC,
                "signal": "ground-truth",
                "quality": "diagnostic",
            },
            "safety": {
                "scope": "virtual-only",
                "actuator_authority": False,
                "navigation_or_safety_accepted": False,
            },
        }

    def estimate_sim_time_ns(self) -> int:
        """Return a bounded speed-factor-1 estimate anchored to a Gazebo sample."""

        now_monotonic_ns = time.monotonic_ns()
        with self._lock:
            sim_time_ns = self._latest_sim_time_ns
            sampled_at_ns = self._latest_host_monotonic_ns
        if sim_time_ns is None or sampled_at_ns is None:
            raise WorkerAgentError("Gazebo simulation clock has not been admitted")
        age_ns = now_monotonic_ns - sampled_at_ns
        if not 0 <= age_ns <= SIM_CLOCK_CACHE_MAX_AGE_NS:
            raise WorkerAgentError("Gazebo simulation clock sample is stale")
        return sim_time_ns + age_ns


class SimulationWorkerAgent:
    """One loopback-only worker process owning provider lifecycle and live state."""

    def __init__(
        self,
        *,
        socket_path: Path,
        mission_core_commit: str,
        d_root: Path,
        s0_profile_path: Path,
        lifecycle_profile_path: Path,
    ) -> None:
        if not COMMIT_PATTERN.fullmatch(mission_core_commit):
            raise WorkerAgentError("Mission Core commit must be an exact 40-character revision")
        if os.geteuid() == 0:
            raise WorkerAgentError("worker agent must run as an unprivileged identity")
        if os.environ.get("WSL_DISTRO_NAME") != EXPECTED_DISTRO:
            raise WorkerAgentError(f"worker agent requires {EXPECTED_DISTRO}")
        if tuple(sorted(name for _, name in socket.if_nameindex())) != ("lo",):
            raise WorkerAgentError("worker agent requires a loopback-only network namespace")
        self.socket_path = socket_path
        self.mission_core_commit = mission_core_commit
        self.paths = StockRoverTargetPaths.from_d_root(d_root)
        self.lifecycle_profile_path = lifecycle_profile_path.expanduser().resolve()
        self.lifecycle_profile = load_stock_rover_lifecycle_profile(self.lifecycle_profile_path)
        self.guard = S0WorkerGuard(s0_profile_path)
        self._preflight()
        self.store = QualificationRunStore(self.paths.d_root / "artifacts/s1/runs")
        self.supervisor = PosixProcessSupervisor(
            self.paths.d_root / "runtime/s1/runs",
            base_environment=stock_rover_process_environment(self.paths),
        )
        self.worker = LocalProcessWorkerAdapter(
            self.guard,
            self.supervisor,
            stock_rover_process_specs(self.paths, self.lifecycle_profile),
            _LifecycleOnlyWorldControl(),
        )
        self.service = SimulationApplicationService(self.store, self.worker)
        self.collector = GazeboPoseCollector(stock_rover_process_environment(self.paths))
        self.command_controller = Ros2Px4AckermannControl()
        self._command_lock = threading.RLock()
        self._command_history: dict[str, dict[str, AckermannControlSetpoint]] = {}
        self._shutdown = threading.Event()
        self._server_socket: socket.socket | None = None
        self._connection_threads: set[threading.Thread] = set()
        self.service.reconcile(
            observed_at_utc=_utc_now(),
            host_monotonic_ns=time.monotonic_ns(),
        )
        active = tuple(run for run in self.store.list_runs() if run.state in ACTIVE_STATES)
        if len(active) > 1:
            raise WorkerAgentError("multiple active qualification runs violate worker authority")
        self._active_run_cache = active[0] if active else None

    def serve_forever(self) -> None:
        self.socket_path.parent.mkdir(mode=0o700, parents=True, exist_ok=True)
        if self.socket_path.exists():
            if not self.socket_path.is_socket():
                raise WorkerAgentError("worker socket path is occupied by a non-socket")
            self.socket_path.unlink()
        with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as server:
            self._server_socket = server
            server.bind(str(self.socket_path))
            self.socket_path.chmod(0o600)
            server.listen(8)
            server.settimeout(0.5)
            while not self._shutdown.is_set():
                try:
                    connection, _ = server.accept()
                except TimeoutError:
                    continue
                self._connection_threads = {
                    thread for thread in self._connection_threads if thread.is_alive()
                }
                thread = threading.Thread(
                    target=self._serve_connection,
                    args=(connection,),
                    daemon=True,
                )
                self._connection_threads.add(thread)
                thread.start()
        for thread in tuple(self._connection_threads):
            thread.join(timeout=1)
        self._server_socket = None
        if self.socket_path.is_socket():
            self.socket_path.unlink()

    def shutdown(self) -> None:
        self._shutdown.set()
        active = self._active_run()
        if active is not None:
            with suppress(Exception):
                self.command_controller.stop(active.run_id)
            try:
                self.service.stop(
                    active.run_id,
                    idempotency_key=f"{active.run_id}:agent-shutdown",
                    observed_at_utc=_utc_now(),
                    host_monotonic_ns=time.monotonic_ns(),
                    sim_time_ns=None,
                    terminal_state=RunState.ABORTED,
                    reason="worker-agent-shutdown",
                )
                self._active_run_cache = None
            except Exception:
                self.supervisor.stop()

    def _serve_connection(self, connection: socket.socket) -> None:
        with connection:
            self._handle_connection(connection)

    def dispatch(self, request: dict[str, Any]) -> dict[str, Any]:
        if set(request) != {"schema_version", "request_id", "operation", "payload"}:
            raise WorkerAgentError("request keys do not match v1")
        request_id = request.get("request_id")
        operation = request.get("operation")
        payload = request.get("payload")
        if (
            request.get("schema_version") != REQUEST_SCHEMA
            or not isinstance(request_id, str)
            or not 1 <= len(request_id) <= 64
            or operation not in OPERATIONS
            or not isinstance(payload, dict)
        ):
            raise WorkerAgentError("request envelope is invalid")
        if operation == "status":
            _exact_keys(payload, set(), "status payload")
            return self.status()
        if operation == "live":
            _exact_keys(payload, set(), "live payload")
            active = self._active_run()
            if active is None or active.state is not RunState.RUNNING:
                raise WorkerAgentError("no running simulation is available")
            return self.collector.collect(active.run_id)
        if operation == "start":
            _exact_keys(
                payload,
                {"run_id", "mission_core_commit", "idempotency_key"},
                "start payload",
            )
            return self.start(
                run_id=_safe_id(payload["run_id"], "run id"),
                mission_core_commit=_string(payload["mission_core_commit"], "commit", 40),
                idempotency_key=_string(payload["idempotency_key"], "idempotency key", 160),
            )
        if operation == "command":
            _exact_keys(
                payload,
                {"run_id", "speed_mps", "steering_normalized", "idempotency_key"},
                "command payload",
            )
            return self.command(
                run_id=_safe_id(payload["run_id"], "run id"),
                speed_mps=_bounded_number(payload["speed_mps"], "speed", -1.5, 1.5),
                steering_normalized=_bounded_number(
                    payload["steering_normalized"],
                    "steering",
                    -1.0,
                    1.0,
                ),
                idempotency_key=_string(payload["idempotency_key"], "idempotency key", 160),
            )
        _exact_keys(payload, {"run_id", "idempotency_key"}, "stop payload")
        return self.stop(
            run_id=_safe_id(payload["run_id"], "run id"),
            idempotency_key=_string(payload["idempotency_key"], "idempotency key", 160),
        )

    def status(self) -> dict[str, Any]:
        active = self._active_run()
        active_provider_ids: list[str] = []
        if active is not None and active.state in {RunState.RUNNING, RunState.PAUSED}:
            try:
                active_provider_ids = [
                    record.process_id for record in self.supervisor.snapshot()
                ]
            except Exception:
                active_provider_ids = []
        return {
            "schema_version": STATUS_SCHEMA,
            "worker_id": "mission-gpu-s1",
            "transport": "unix",
            "mode": "simulation",
            "available": True,
            "control_available": True,
            "active_run_id": active.run_id if active else None,
            "run_state": active.state.value if active else None,
            "active_provider_ids": active_provider_ids,
            "provider_profile": STOCK_ROVER_PROVIDER_PROFILE.to_dict(),
            "isolation": {
                "network": "loopback-only-netns",
                "process_identity": "missioncore",
                "artifact_policy": "d-only",
            },
            "authority": {
                "scope": "virtual-only",
                "actuator_authority": False,
                "direct_actuator_setpoints_allowed": False,
            },
        }

    def start(
        self,
        *,
        run_id: str,
        mission_core_commit: str,
        idempotency_key: str,
    ) -> dict[str, Any]:
        if mission_core_commit != self.mission_core_commit:
            raise WorkerAgentError("requested commit does not match the staged worker generation")
        try:
            run = self.store.load(run_id)
        except QualificationRunNotFoundError:
            run = self.store.create(self._new_run(run_id))
        if run.mission_core_commit != mission_core_commit:
            raise WorkerAgentError("existing run belongs to another Mission Core generation")
        running = self.service.start(
            run_id,
            idempotency_key=idempotency_key,
            observed_at_utc=_utc_now(),
            host_monotonic_ns=time.monotonic_ns(),
        )
        if running.state is not RunState.RUNNING:
            raise WorkerAgentError(f"provider start ended in terminal state {running.state.value}")
        self._active_run_cache = running
        try:
            self.command_controller.start(run_id)
            self.collector.collect(run_id)
        except Exception as exc:
            with suppress(Exception):
                self.command_controller.stop(run_id)
            try:
                self.service.stop(
                    run_id,
                    idempotency_key=f"{run_id}:command-controller-admission-failed",
                    observed_at_utc=_utc_now(),
                    host_monotonic_ns=time.monotonic_ns(),
                    sim_time_ns=None,
                    terminal_state=RunState.ABORTED,
                    reason="px4-command-controller-admission-failed",
                )
                self._active_run_cache = None
            finally:
                raise WorkerAgentError("PX4 command controller admission failed") from exc
        self._command_history[run_id] = {
            command.command_id: command
            for command in self.store.list_commands(run_id)
            if isinstance(command, AckermannControlSetpoint)
        }
        return self.status()

    def stop(self, *, run_id: str, idempotency_key: str) -> dict[str, Any]:
        active = self._active_run()
        if active is None or active.run_id != run_id:
            raise WorkerAgentError("requested run does not own worker authority")
        sim_time_ns: int | None
        try:
            sim_time_ns = int(self.collector.collect(run_id)["sim_time_ns"])
        except Exception:
            sim_time_ns = None
        command_stop_error: Exception | None = None
        try:
            self.command_controller.stop(run_id)
        except Exception as exc:
            command_stop_error = exc
        terminal = self.service.stop(
            run_id,
            idempotency_key=idempotency_key,
            observed_at_utc=_utc_now(),
            host_monotonic_ns=time.monotonic_ns(),
            sim_time_ns=sim_time_ns,
            terminal_state=(
                RunState.ABORTED if command_stop_error is not None else RunState.COMPLETED
            ),
            reason=(
                "px4-command-controller-stop-failed"
                if command_stop_error is not None
                else "operator-stop-clean"
            ),
        )
        if command_stop_error is not None:
            raise WorkerAgentError(
                "PX4 command controller did not stop cleanly"
            ) from command_stop_error
        if terminal.state is not RunState.COMPLETED:
            raise WorkerAgentError(f"provider stop ended in terminal state {terminal.state.value}")
        self._active_run_cache = None
        return self.status()

    def command(
        self,
        *,
        run_id: str,
        speed_mps: float,
        steering_normalized: float,
        idempotency_key: str,
    ) -> dict[str, Any]:
        with self._command_lock:
            return self._command_locked(
                run_id=run_id,
                speed_mps=speed_mps,
                steering_normalized=steering_normalized,
                idempotency_key=idempotency_key,
            )

    def _command_locked(
        self,
        *,
        run_id: str,
        speed_mps: float,
        steering_normalized: float,
        idempotency_key: str,
    ) -> dict[str, Any]:
        active = self._active_run()
        if active is None or active.run_id != run_id or active.state is not RunState.RUNNING:
            raise WorkerAgentError("requested run does not own running command authority")
        command_id = f"cmd-{hashlib.sha256(idempotency_key.encode()).hexdigest()[:24]}"
        history = self._command_history_for(run_id)
        existing = history.get(command_id)
        if existing is not None:
            if not isinstance(existing, AckermannControlSetpoint) or (
                existing.speed_mps != speed_mps
                or existing.steering_normalized != steering_normalized
            ):
                raise WorkerAgentError("command idempotency key is bound to another setpoint")
            snapshot = self.command_controller.snapshot(run_id)
            return _command_acceptance(existing, snapshot)
        issued_at_sim_ns = self.collector.estimate_sim_time_ns()
        ttl_ns = min(active.authority.command_ttl_max_ns, 250_000_000)
        command = AckermannControlSetpoint(
            run_id=run_id,
            command_id=command_id,
            sequence=len(history) + 1,
            issued_at_sim_ns=issued_at_sim_ns,
            valid_until_sim_ns=issued_at_sim_ns + ttl_ns,
            authority_generation=active.authority.generation,
            speed_mps=speed_mps,
            steering_normalized=steering_normalized,
            source="mission-core-control-station",
        )
        self.store.submit_admitted_command(command, admitted_run=active)
        history[command.command_id] = command
        try:
            snapshot = self.command_controller.submit(command)
        except Exception as exc:
            current = self.store.load(run_id)
            self.store.append_event(
                run_id,
                event_type="command.delivery-failed",
                observed_at_utc=_utc_now(),
                host_monotonic_ns=time.monotonic_ns(),
                sim_time_ns=issued_at_sim_ns,
                payload={
                    "command_id": command.command_id,
                    "provider": PX4_COMMAND_PROVIDER_ID,
                    "detail": type(exc).__name__,
                },
                expected_revision=current.revision,
            )
            raise WorkerAgentError("PX4 rejected the canonical rover setpoint") from exc
        return _command_acceptance(command, snapshot)

    def _command_history_for(
        self,
        run_id: str,
    ) -> dict[str, AckermannControlSetpoint]:
        histories = getattr(self, "_command_history", None)
        if histories is None:
            histories = {}
            self._command_history = histories
        history = histories.get(run_id)
        if history is None:
            commands = self.store.list_commands(run_id)
            history = {
                command.command_id: command
                for command in commands
                if isinstance(command, AckermannControlSetpoint)
            }
            if len(history) != len(commands):
                raise WorkerAgentError("running command history contains another command contract")
            histories[run_id] = history
        return history

    def _handle_connection(self, connection: socket.socket) -> None:
        request_id: str | None = None
        try:
            request_bytes = _read_line(connection)
            request = _json_object(request_bytes)
            raw_request_id = request.get("request_id")
            if isinstance(raw_request_id, str) and 1 <= len(raw_request_id) <= 64:
                request_id = raw_request_id
            result = self.dispatch(request)
            response = {
                "schema_version": RESPONSE_SCHEMA,
                "request_id": request_id,
                "ok": True,
                "result": result,
                "error": None,
            }
        except Exception as exc:
            response = {
                "schema_version": RESPONSE_SCHEMA,
                "request_id": request_id,
                "ok": False,
                "result": None,
                "error": _safe_error(exc),
            }
        encoded = (
            json.dumps(response, ensure_ascii=True, separators=(",", ":")).encode("utf-8") + b"\n"
        )
        if len(encoded) <= MAX_MESSAGE_BYTES:
            connection.sendall(encoded)

    def _new_run(self, run_id: str) -> QualificationRun:
        return QualificationRun(
            run_id=run_id,
            episode_id=f"episode-{run_id}",
            kind=RunKind.SIMULATION_CLOSED_LOOP,
            state=RunState.ADMITTED,
            scenario_generation="px4-v1.17.0-stock-rover-ackermann",
            scenario_sha256=_sha256(self.paths.scenario_path),
            profile_generation=self.lifecycle_profile.profile_id,
            profile_sha256=_sha256(self.lifecycle_profile_path),
            mission_core_commit=self.mission_core_commit,
            providers=_provider_pins(self.guard),
            host_profile_id=self.guard.profile.profile_id,
            host_profile_sha256=self.guard.profile_sha256,
            seed=42,
            reproducibility_tier=ReproducibilityTier.R1,
            authority=AuthorityProfile(
                generation=1,
                command_ttl_max_ns=250_000_000,
                heartbeat_timeout_monotonic_ns=500_000_000,
            ),
            clock_domain=STOCK_ROVER_PROVIDER_PROFILE.clock.domain,
            created_at_utc=_utc_now(),
        )

    def _active_run(self) -> QualificationRun | None:
        if hasattr(self, "_active_run_cache"):
            return self._active_run_cache
        active = tuple(run for run in self.store.list_runs() if run.state in ACTIVE_STATES)
        if len(active) > 1:
            raise WorkerAgentError("multiple active qualification runs violate worker authority")
        result = active[0] if active else None
        self._active_run_cache = result
        return result

    def _preflight(self) -> None:
        required = (self.paths.px4_root, self.paths.agent_binary, self.paths.scenario_path)
        if any(not path.exists() for path in required):
            raise WorkerAgentError("worker inputs are incomplete")
        if not all(
            component.qualification_state == "accepted"
            for component in self.guard.profile.components
        ):
            raise WorkerAgentError("an S0 provider generation is no longer accepted")


def _read_line(connection: socket.socket) -> bytes:
    chunks: list[bytes] = []
    size = 0
    while True:
        chunk = connection.recv(min(4096, MAX_MESSAGE_BYTES + 1 - size))
        if not chunk:
            raise WorkerAgentError("gateway request ended before newline")
        newline = chunk.find(b"\n")
        if newline >= 0:
            chunks.append(chunk[:newline])
            size += newline
            if size > MAX_MESSAGE_BYTES:
                raise WorkerAgentError("gateway request exceeds the size limit")
            return b"".join(chunks)
        chunks.append(chunk)
        size += len(chunk)
        if size > MAX_MESSAGE_BYTES:
            raise WorkerAgentError("gateway request exceeds the size limit")


def _json_object(value: bytes) -> dict[str, Any]:
    try:
        document = json.loads(value.decode("utf-8"))
    except (UnicodeDecodeError, json.JSONDecodeError) as exc:
        raise WorkerAgentError("gateway request is not valid UTF-8 JSON") from exc
    if not isinstance(document, dict) or any(not isinstance(key, str) for key in document):
        raise WorkerAgentError("gateway request must be a JSON object")
    return document


def _rover_pose(document: dict[str, Any]) -> dict[str, Any]:
    poses = document.get("pose")
    if not isinstance(poses, list) or not poses:
        raise WorkerAgentError("Gazebo pose sample contains no poses")
    for item in poses:
        if isinstance(item, dict) and str(item.get("name", "")).startswith("rover_ackermann"):
            return item
    raise WorkerAgentError("Gazebo pose sample contains no stock rover")


def _provider_pins(guard: S0WorkerGuard) -> tuple[ProviderPin, ...]:
    components = {component.identifier: component for component in guard.profile.components}
    if any(identifier not in components for identifier in PROVIDER_IDS):
        raise WorkerAgentError("S0 profile is missing a required provider")
    return tuple(_provider_pin(components[identifier]) for identifier in PROVIDER_IDS)


def _provider_pin(component: ComponentPin) -> ProviderPin:
    version = component.resolved_version or component.requested_ref
    return ProviderPin(
        identifier=component.identifier,
        version=version,
        revision=component.resolved_commit or version,
    )


def _exact_keys(value: dict[str, Any], expected: set[str], label: str) -> None:
    if set(value) != expected:
        raise WorkerAgentError(f"{label} keys do not match v1")


def _safe_id(value: object, label: str) -> str:
    result = _string(value, label, 64)
    if not SAFE_ID_PATTERN.fullmatch(result):
        raise WorkerAgentError(f"{label} is not a safe identifier")
    return result


def _string(value: object, label: str, maximum: int) -> str:
    if not isinstance(value, str):
        raise WorkerAgentError(f"{label} must be a string")
    result = value.strip()
    if not 1 <= len(result) <= maximum:
        raise WorkerAgentError(f"{label} has an invalid length")
    return result


def _object(value: object, label: str) -> dict[str, Any]:
    if not isinstance(value, dict) or any(not isinstance(key, str) for key in value):
        raise WorkerAgentError(f"{label} must be an object")
    return value


def _number(value: object, label: str) -> float:
    if isinstance(value, bool) or not isinstance(value, int | float | str):
        raise WorkerAgentError(f"{label} must be numeric")
    try:
        result = float(value)
    except ValueError as exc:
        raise WorkerAgentError(f"{label} must be numeric") from exc
    if not -1e9 < result < 1e9:
        raise WorkerAgentError(f"{label} is outside the accepted range")
    return result


def _bounded_number(
    value: object,
    label: str,
    minimum: float,
    maximum: float,
) -> float:
    result = _number(value, label)
    if not minimum <= result <= maximum:
        raise WorkerAgentError(f"{label} is outside the admitted range")
    return result


def _integer(value: object, label: str) -> int:
    if isinstance(value, bool) or not isinstance(value, int | str):
        raise WorkerAgentError(f"{label} must be an integer")
    try:
        result = int(value)
    except ValueError as exc:
        raise WorkerAgentError(f"{label} must be an integer") from exc
    if result < 0:
        raise WorkerAgentError(f"{label} must not be negative")
    return result


def _sha256(path: Path) -> str:
    digest = hashlib.sha256()
    with path.open("rb") as stream:
        while chunk := stream.read(1024 * 1024):
            digest.update(chunk)
    return digest.hexdigest()


def _utc_now() -> str:
    return datetime.now(UTC).isoformat(timespec="milliseconds").replace("+00:00", "Z")


def _safe_error(exc: Exception) -> str:
    if isinstance(exc, WorkerAgentError):
        normalized = " ".join(str(exc).split())
        return normalized[:512] or "worker request rejected"
    return f"{type(exc).__name__}: worker operation failed"


def _command_acceptance(
    command: AckermannControlSetpoint,
    snapshot: Px4RoverControlSnapshot,
) -> dict[str, Any]:
    return {
        "schema_version": COMMAND_ACCEPTANCE_SCHEMA,
        "run_id": command.run_id,
        "command_id": command.command_id,
        "sequence": command.sequence,
        "issued_at_sim_ns": command.issued_at_sim_ns,
        "valid_until_sim_ns": command.valid_until_sim_ns,
        "speed_mps": command.speed_mps,
        "steering_normalized": command.steering_normalized,
        "authority_scope": command.authority_scope.value,
        "delivery": {
            "provider_id": PX4_COMMAND_PROVIDER_ID,
            "control_profile": command.profile.value,
            "accepted": True,
            "controller_ready": snapshot.armed and snapshot.offboard,
            "ttl_expired_count": snapshot.ttl_expired_count,
            "diagnostics": {
                "armed": snapshot.armed,
                "offboard": snapshot.offboard,
            },
        },
    }


def main() -> int:
    parser = argparse.ArgumentParser(description="Serve the Mission Core Simulation Worker agent.")
    parser.add_argument("--socket", type=Path, required=True)
    parser.add_argument("--mission-core-commit", required=True)
    parser.add_argument(
        "--d-root",
        type=Path,
        default=Path("/mnt/d/NDC_MISSIONCORE/simulation"),
    )
    parser.add_argument(
        "--s0-profile",
        type=Path,
        default=Path("/mnt/d/NDC_MISSIONCORE/simulation/source/qualification-profile.yaml"),
    )
    parser.add_argument("--lifecycle-profile", type=Path, required=True)
    args = parser.parse_args()
    agent = SimulationWorkerAgent(
        socket_path=args.socket,
        mission_core_commit=args.mission_core_commit,
        d_root=args.d_root,
        s0_profile_path=args.s0_profile,
        lifecycle_profile_path=args.lifecycle_profile,
    )

    def stop(_signum: int, _frame: FrameType | None) -> None:
        agent.shutdown()

    signal.signal(signal.SIGTERM, stop)
    signal.signal(signal.SIGINT, stop)
    try:
        agent.serve_forever()
    finally:
        agent.shutdown()
    return 0


if __name__ == "__main__":
    sys.exit(main())
