from __future__ import annotations

import threading
from math import isnan
from pathlib import Path

import pytest

from k1link.simulation.contracts import (
    AckermannControlSetpoint,
    AuthorityProfile,
    ProviderPin,
    QualificationRun,
    ReproducibilityTier,
    RunKind,
    RunState,
)
from k1link.simulation.px4_rover_control import (
    Px4RoverControlSnapshot,
    Ros2Px4AckermannControl,
)
from k1link.simulation.run_store import QualificationRunStore
from k1link.simulation.worker_agent import SimulationWorkerAgent, WorkerAgentError


class _Collector:
    def collect(self, run_id: str) -> dict[str, object]:
        return {"run_id": run_id, "sim_time_ns": 1_000_000_000}

    def estimate_sim_time_ns(self) -> int:
        return 1_000_000_000


class _Controller:
    def __init__(self) -> None:
        self.commands: list[object] = []

    def submit(self, command: AckermannControlSetpoint) -> Px4RoverControlSnapshot:
        self.commands.append(command)
        return Px4RoverControlSnapshot(
            run_id=command.run_id,
            armed=True,
            offboard=True,
            ttl_expired_count=0,
        )

    def snapshot(self, run_id: str) -> Px4RoverControlSnapshot:
        return Px4RoverControlSnapshot(
            run_id=run_id,
            armed=True,
            offboard=True,
            ttl_expired_count=0,
        )


class _Message:
    pass


class _Messages:
    OffboardControlMode = _Message
    RoverThrottleSetpoint = _Message
    RoverSteeringSetpoint = _Message


class _Publisher:
    def __init__(self) -> None:
        self.messages: list[_Message] = []

    def publish(self, message: _Message) -> None:
        self.messages.append(message)


def _running_store(root: Path) -> QualificationRunStore:
    store = QualificationRunStore(root)
    admitted = store.create(
        QualificationRun(
            run_id="s1c-command-test",
            episode_id="episode-s1c-command-test",
            kind=RunKind.SIMULATION_CLOSED_LOOP,
            state=RunState.ADMITTED,
            scenario_generation="stock-rover",
            scenario_sha256="a" * 64,
            profile_generation="stock-rover-lifecycle-v1",
            profile_sha256="b" * 64,
            mission_core_commit="c" * 40,
            providers=(
                ProviderPin("px4-autopilot", "v1.17.0", "v1.17.0"),
                ProviderPin("gazebo", "harmonic", "8.14.0"),
            ),
            host_profile_id="mission-gpu-s0",
            host_profile_sha256="d" * 64,
            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="gazebo:/clock",
            created_at_utc="2026-07-24T18:00:00Z",
        )
    )
    starting = store.transition(
        admitted.run_id,
        RunState.STARTING,
        expected_revision=0,
        observed_at_utc="2026-07-24T18:00:01Z",
        host_monotonic_ns=1,
    )
    store.transition(
        admitted.run_id,
        RunState.RUNNING,
        expected_revision=starting.revision,
        observed_at_utc="2026-07-24T18:00:02Z",
        host_monotonic_ns=2,
        sim_time_ns=0,
    )
    return store


def test_worker_agent_journals_and_idempotently_delivers_virtual_command(
    tmp_path: Path,
) -> None:
    agent = object.__new__(SimulationWorkerAgent)
    agent.store = _running_store(tmp_path)
    agent.collector = _Collector()
    controller = _Controller()
    agent.command_controller = controller
    agent._command_lock = threading.RLock()

    accepted = agent.command(
        run_id="s1c-command-test",
        speed_mps=1.0,
        steering_normalized=-0.25,
        idempotency_key="browser-intent-001",
    )
    repeated = agent.command(
        run_id="s1c-command-test",
        speed_mps=1.0,
        steering_normalized=-0.25,
        idempotency_key="browser-intent-001",
    )

    assert accepted == repeated
    assert accepted["authority_scope"] == "virtual-only"
    assert accepted["delivery"]["provider_id"] == "px4-ros2-offboard"
    assert accepted["delivery"]["controller_ready"] is True
    assert accepted["delivery"]["diagnostics"]["armed"] is True
    commands = agent.store.list_commands("s1c-command-test")
    assert len(commands) == 1
    assert commands[0].valid_until_sim_ns - commands[0].issued_at_sim_ns == 250_000_000
    assert len(controller.commands) == 1


def test_worker_agent_command_dispatch_fails_closed_on_unsafe_envelope() -> None:
    agent = object.__new__(SimulationWorkerAgent)

    with pytest.raises(WorkerAgentError, match="outside"):
        agent.dispatch(
            {
                "schema_version": "missioncore.simulation-worker-request/v1",
                "request_id": "request-001",
                "operation": "command",
                "payload": {
                    "run_id": "s1c-command-test",
                    "speed_mps": 1.51,
                    "steering_normalized": 0.0,
                    "idempotency_key": "browser-intent-002",
                },
            }
        )


def test_worker_agent_caches_the_absence_of_an_active_run() -> None:
    class EmptyStore:
        def __init__(self) -> None:
            self.list_calls = 0

        def list_runs(self) -> tuple[QualificationRun, ...]:
            self.list_calls += 1
            return ()

    agent = object.__new__(SimulationWorkerAgent)
    store = EmptyStore()
    agent.__dict__["store"] = store

    first = agent.status()
    second = agent.status()

    assert first["available"] is True
    assert first["active_run_id"] is None
    assert second["active_run_id"] is None
    assert store.list_calls == 1


def test_px4_adapter_maps_speed_to_stock_rover_throttle_without_direct_actuators() -> None:
    publishers = [_Publisher() for _ in range(3)]

    Ros2Px4AckermannControl._publish_setpoint(
        _Messages,
        *publishers,
        123,
        1.0,
        -0.55,
    )

    offboard = publishers[0].messages[0]
    throttle = publishers[1].messages[0]
    steering = publishers[2].messages[0]
    assert offboard.velocity is False
    assert offboard.thrust_and_torque is True
    assert offboard.direct_actuator is False
    assert throttle.throttle_body_x == pytest.approx(1.0 / 3.1)
    assert isnan(throttle.throttle_body_y)
    assert steering.normalized_steering_setpoint == -0.55
