#!/usr/bin/env python3
"""Closed-loop bilateral simulation for the heterogeneous 7-DoF arms.

This program is deliberately a *simulation validation*, not a replacement for
the future prototype experiment.  Both arms are integrated with their URDF
rigid-body dynamics.  A computed-torque human proxy drives the master, a
computed-torque controller drives the slave, and a unilateral spring-damper
wall acts at a documented simulation-only TCP attached to the terminal wrist.

Three otherwise identical cases are run:

``proposed_energy``
    ``tau_m = A(q_m).T @ tau_s`` plus final applied-port energy supervision.
``direct_energy``
    The former direct baseline ``tau_m = J_m.T @ F_s`` with the same output
    shaping and energy supervision.
``proposed_no_energy``
    The proposed differential mapping with the energy projection bypassed.

The comparison separates the two implementation questions: virtual-work
consistency of the retargeting map, and the final-port energy safety layer.
"""

from __future__ import annotations

import argparse
import csv
import json
import math
import time
from collections import deque
from dataclasses import asdict, dataclass, fields, replace
from pathlib import Path
from typing import Any, Iterable

import numpy as np
import pinocchio as pin
import yaml

from core.haptic_render import HapticRenderer
from core.interaction_estimater import InteractionEstimator
from core.feedback_protocol import (
    ForwardPacket,
    MapPolicy,
    MapRegistry,
    MapSnapshot,
    MappingKind,
    PacketState,
    ReturnPacket,
    map_return_feedback,
)
from core.network_emulator import (
    DeterministicChannel,
    PacketReceiver,
    generate_network_trace,
)
from core.model_contract import (
    MASTER_FRAMES,
    MASTER_JOINT_NAMES,
    MASTER_URDF,
    SLAVE_FRAMES,
    SLAVE_JOINT_NAMES,
    SLAVE_URDF,
    TeleoperationModels,
    clip_configuration,
    load_models,
    require_frame,
)
from core.sew_mapper2 import BallJointConfig, SEWMapper
from core.time_domain_popc import TimeDomainPOPC
from core.wrench_solver import ScaledDLSSolver


DEFAULT_OUTPUT_DIR = Path(__file__).resolve().parents[1] / "output" / "simulation"
DEFAULT_CONFIG_PATH = Path(__file__).resolve().parent / "config" / "config.yaml"


@dataclass(frozen=True)
class SimulationConfig:
    """Numerical and controller parameters for the reproducible comparison."""

    dt: float = 0.002
    duration: float = 4.0
    mapping_hz: float = 50.0
    seed: int = 7
    slave_contact_frame: str = SLAVE_FRAMES["ee"]
    feedback_delay_s: float = 0.080
    forward_delay_s: float = 0.0
    forward_jitter_s: float = 0.0
    return_jitter_s: float = 0.0
    forward_packet_loss: float = 0.0
    return_packet_loss: float = 0.0
    forward_timeout_s: float = 0.20
    return_timeout_s: float = 0.20
    contact_probe_fraction: float = 0.03
    contact_probe_cycles: float = 3.0

    wall_fraction: float = 0.55
    wall_stiffness: float = 800.0
    wall_damping: float = 45.0
    wall_force_limit: float = 80.0
    wall_transition_depth: float = 0.0001

    master_kp: tuple[float, ...] = (
        196.0,
        196.0,
        144.0,
        256.0,
        100.0,
        100.0,
        81.0,
    )
    master_kd: tuple[float, ...] = (
        28.0,
        28.0,
        24.0,
        32.0,
        20.0,
        20.0,
        18.0,
    )
    slave_kp: tuple[float, ...] = (
        900.0,
        900.0,
        676.0,
        1156.0,
        400.0,
        324.0,
        324.0,
    )
    slave_kd: tuple[float, ...] = (
        54.0,
        54.0,
        46.8,
        61.2,
        36.0,
        32.4,
        32.4,
    )
    master_acceleration_limits: tuple[float, ...] = (
        100.0,
        100.0,
        120.0,
        120.0,
        160.0,
        160.0,
        160.0,
    )
    slave_acceleration_limits: tuple[float, ...] = (
        120.0,
        120.0,
        150.0,
        150.0,
        180.0,
        180.0,
        180.0,
    )
    master_tracking_effort_fraction: float = 0.65
    slave_tracking_effort_fraction: float = 0.70
    velocity_limit_fraction: float = 0.80
    soft_limit_buffer: float = 0.12

    feedback_strength: float = 0.50
    haptic_filter_alpha: float = 0.222
    haptic_torque_limits: tuple[float, ...] = (
        6.0,
        6.0,
        4.0,
        3.0,
        1.0,
        1.0,
        1.0,
    )
    haptic_rate_limits: tuple[float, ...] = (
        150.0,
        150.0,
        100.0,
        80.0,
        30.0,
        30.0,
        30.0,
    )

    energy_min: float = 0.05
    energy_max: float = 0.055
    energy_initial: float = 0.05

    sensor_noise_std: float = 0.001
    wrench_characteristic_length_m: float = 0.30
    wrench_scaled_damping: float = 1e-3
    sensor_bias: tuple[float, ...] = (
        0.080,
        -0.050,
        0.035,
        -0.025,
        0.015,
        -0.010,
        0.020,
    )
    bias_calibration_samples: int = 200

    joint_limit_margin: float = 1e-5
    differential_step: float = 1e-4

    def validate(self) -> None:
        if not np.isfinite(self.dt) or self.dt <= 0.0:
            raise ValueError("dt must be finite and positive")
        if not np.isfinite(self.duration) or self.duration <= 0.0:
            raise ValueError("duration must be finite and positive")
        if not np.isfinite(self.mapping_hz) or self.mapping_hz <= 0.0:
            raise ValueError("mapping_hz must be finite and positive")
        if self.mapping_hz > 1.0 / self.dt:
            raise ValueError("mapping_hz cannot exceed the dynamics rate")
        if not 0.0 < self.wall_fraction < 1.0:
            raise ValueError("wall_fraction must lie strictly inside (0, 1)")
        if (
            self.wall_stiffness <= 0.0
            or self.wall_damping < 0.0
            or self.wall_force_limit <= 0.0
            or self.wall_transition_depth < 0.0
        ):
            raise ValueError("wall parameters must be non-negative")
        network_times = (
            self.feedback_delay_s,
            self.forward_delay_s,
            self.forward_jitter_s,
            self.return_jitter_s,
        )
        if any(value < 0.0 or not np.isfinite(value) for value in network_times):
            raise ValueError("network delays and jitter must be finite/non-negative")
        if self.forward_timeout_s <= 0.0 or self.return_timeout_s <= 0.0:
            raise ValueError("network timeouts must be positive")
        if not 0.0 <= self.forward_packet_loss <= 1.0:
            raise ValueError("forward_packet_loss must lie in [0, 1]")
        if not 0.0 <= self.return_packet_loss <= 1.0:
            raise ValueError("return_packet_loss must lie in [0, 1]")
        if self.contact_probe_fraction < 0.0 or self.contact_probe_cycles < 0.0:
            raise ValueError("contact probe parameters cannot be negative")
        if not (
            0.0 <= self.energy_min <= self.energy_initial <= self.energy_max
        ):
            raise ValueError(
                "energy values must satisfy 0 <= min <= initial <= max"
            )
        if len(self.haptic_torque_limits) != 7:
            raise ValueError("haptic_torque_limits must contain seven entries")
        if len(self.haptic_rate_limits) != 7:
            raise ValueError("haptic_rate_limits must contain seven entries")
        if len(self.sensor_bias) != 7:
            raise ValueError("sensor_bias must contain seven entries")
        if (
            self.wrench_characteristic_length_m <= 0.0
            or self.wrench_scaled_damping <= 0.0
        ):
            raise ValueError("scaled wrench solver parameters must be positive")
        vector_parameters = (
            self.master_kp,
            self.master_kd,
            self.slave_kp,
            self.slave_kd,
            self.master_acceleration_limits,
            self.slave_acceleration_limits,
        )
        if any(len(values) != 7 for values in vector_parameters):
            raise ValueError("all gain and acceleration-limit vectors need 7 entries")
        if not 0.0 < self.velocity_limit_fraction <= 1.0:
            raise ValueError("velocity_limit_fraction must lie in (0, 1]")
        if not 0.0 < self.master_tracking_effort_fraction <= 1.0:
            raise ValueError("master_tracking_effort_fraction must lie in (0, 1]")
        if not 0.0 < self.slave_tracking_effort_fraction <= 1.0:
            raise ValueError("slave_tracking_effort_fraction must lie in (0, 1]")


@dataclass(frozen=True)
class Scenario:
    key: str
    mapping: str
    supervise_energy: bool
    description: str
    supervisor: str = "tank"
    map_policy: str = "current"


SCENARIOS = (
    Scenario(
        key="proposed_energy",
        mapping=MappingKind.DIFFERENTIAL_RESIDUAL.value,
        supervise_energy=True,
        description="A(q_m)^T tau_s with final applied-port supervision",
        supervisor="tank",
        map_policy=MapPolicy.CURRENT.value,
    ),
    Scenario(
        key="direct_energy",
        mapping=MappingKind.DIRECT_MASTER_JACOBIAN.value,
        supervise_energy=True,
        description="J_m^T F_s matched-wrench baseline with tank supervision",
        supervisor="tank",
        map_policy=MapPolicy.CURRENT.value,
    ),
    Scenario(
        key="proposed_no_energy",
        mapping=MappingKind.DIFFERENTIAL_RESIDUAL.value,
        supervise_energy=False,
        description="A(q_m)^T tau_s with final projection bypassed",
        supervisor="bypass",
        map_policy=MapPolicy.CURRENT.value,
    ),
    Scenario(
        key="matched_wrench_energy",
        mapping=MappingKind.MATCHED_DIFFERENTIAL_WRENCH.value,
        supervise_energy=True,
        description=(
            "A(source)^T J_s^T F_s matched to direct J_m^T F_s input"
        ),
        supervisor="tank",
        map_policy=MapPolicy.SOURCE_STAMPED.value,
    ),
    Scenario(
        key="proposed_popc",
        mapping=MappingKind.DIFFERENTIAL_RESIDUAL.value,
        supervise_energy=False,
        description="A(q_m)^T tau_s with time-domain PO/PC",
        supervisor="popc",
        map_policy=MapPolicy.CURRENT.value,
    ),
)


def load_simulation_config(path: Path) -> SimulationConfig:
    """Load the ``simulation`` block while rejecting silent key drift."""
    with path.open("r", encoding="utf-8") as stream:
        document = yaml.safe_load(stream)
    if not isinstance(document, dict) or not isinstance(
        document.get("simulation"), dict
    ):
        raise ValueError(f"{path} has no mapping-valued 'simulation' block")

    raw = dict(document["simulation"])
    if "slave_tcp_frame" in raw:
        raw["slave_contact_frame"] = raw.pop("slave_tcp_frame")
    # The offset is consumed by model_contract.py; it is retained in YAML as
    # provenance, not duplicated as a simulator constructor parameter.
    raw.pop("slave_tcp_offset", None)

    field_map = {field.name: field for field in fields(SimulationConfig)}
    unknown = sorted(set(raw) - set(field_map))
    if unknown:
        raise ValueError(f"Unknown simulation config keys: {unknown}")
    tuple_fields = {
        field.name
        for field in fields(SimulationConfig)
        if isinstance(field.default, tuple)
    }
    for name in tuple_fields & raw.keys():
        raw[name] = tuple(raw[name])
    config = SimulationConfig(**raw)
    config.validate()
    return config


@dataclass(frozen=True)
class Wall:
    point: np.ndarray
    normal: np.ndarray
    stiffness: float
    damping: float
    force_limit: float
    transition_depth: float = 0.0

    def wrench(
        self,
        position_world: np.ndarray,
        linear_velocity_world: np.ndarray,
    ) -> tuple[np.ndarray, float]:
        """Return environment-on-robot wrench and unilateral penetration."""
        penetration = max(
            0.0,
            float(np.dot(self.normal, position_world - self.point)),
        )
        if penetration <= 0.0:
            return np.zeros(6, dtype=float), 0.0

        normal_velocity = float(np.dot(self.normal, linear_velocity_world))
        if self.transition_depth > 0.0:
            damping_activation = min(1.0, penetration / self.transition_depth)
        else:
            damping_activation = 1.0
        force_magnitude = min(
            self.force_limit,
            max(
                0.0,
                self.stiffness * penetration
                + damping_activation * self.damping * normal_velocity,
            ),
        )
        force = -force_magnitude * self.normal
        return np.concatenate((force, np.zeros(3, dtype=float))), penetration


@dataclass
class ScenarioResult:
    scenario: Scenario
    metrics: dict[str, Any]
    logs: dict[str, np.ndarray]


def build_mapper(models: TeleoperationModels) -> SEWMapper:
    """Construct the bounded mapper with the axes/signs in the real slave URDF."""
    return SEWMapper(
        master_model=models.master,
        slave_model=models.slave,
        m_shoulder=MASTER_FRAMES["shoulder"],
        m_elbow=MASTER_FRAMES["elbow"],
        m_wrist=MASTER_FRAMES["wrist"],
        m_ee=MASTER_FRAMES["ee"],
        s_shoulder=SLAVE_FRAMES["shoulder"],
        s_elbow=SLAVE_FRAMES["elbow"],
        s_wrist=SLAVE_FRAMES["wrist"],
        # Retarget the physical wrist orientation; the added TCP is only used
        # for simulated contact and wrench estimation.
        s_ee=SLAVE_FRAMES["wrist"],
        master_joint_names=MASTER_JOINT_NAMES,
        slave_joint_names=SLAVE_JOINT_NAMES,
        slave_shoulder_cfg=BallJointConfig(
            axis_order="yxy",
            joint_names=SLAVE_JOINT_NAMES[:3],
            signs=(-1.0, 1.0, -1.0),
        ),
        slave_wrist_cfg=BallJointConfig(
            axis_order="yzx",
            joint_names=SLAVE_JOINT_NAMES[4:],
            signs=(-1.0, 1.0, 1.0),
        ),
        slave_elbow_axis_local=np.array([1.0, 0.0, 0.0]),
        up_dir=np.array([0.0, 0.0, 1.0]),
    )


def master_endpoint_configurations() -> tuple[np.ndarray, np.ndarray]:
    """Return an interior, smooth, exactly recoverable 4.6 cm wrist reach."""
    q_start = np.array(
        [0.25, 0.25, -0.20, 1.90, -0.10, 0.10, -0.10],
        dtype=float,
    )
    q_end = q_start.copy()
    q_end[3] = 1.80
    return q_start, q_end


def _smooth_transition(
    t: float,
    t0: float,
    t1: float,
) -> tuple[float, float, float]:
    if t <= t0:
        return 0.0, 0.0, 0.0
    if t >= t1:
        return 1.0, 0.0, 0.0
    duration = t1 - t0
    u = (t - t0) / duration
    position = 10.0 * u**3 - 15.0 * u**4 + 6.0 * u**5
    velocity = (30.0 * u**2 - 60.0 * u**3 + 30.0 * u**4) / duration
    acceleration = (
        60.0 * u - 180.0 * u**2 + 120.0 * u**3
    ) / duration**2
    return position, velocity, acceleration


def master_reference(
    t: float,
    duration: float,
    q_start: np.ndarray,
    q_end: np.ndarray,
    contact_probe_fraction: float = 0.0,
    contact_probe_cycles: float = 0.0,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Approach, probe the delayed contact channel, return, then settle."""
    approach_start = 0.10 * duration
    approach_end = 0.38 * duration
    return_start = 0.53 * duration
    return_end = 0.81 * duration

    delta = q_end - q_start
    if t < return_start:
        s, sd, sdd = _smooth_transition(t, approach_start, approach_end)
        if (
            approach_end < t < return_start
            and contact_probe_fraction > 0.0
            and contact_probe_cycles > 0.0
        ):
            probe_duration = return_start - approach_end
            u = (t - approach_end) / probe_duration
            angular_frequency_u = 2.0 * np.pi * contact_probe_cycles
            envelope = np.sin(np.pi * u) ** 2
            envelope_du = np.pi * np.sin(2.0 * np.pi * u)
            envelope_du2 = 2.0 * np.pi**2 * np.cos(2.0 * np.pi * u)
            carrier = np.sin(angular_frequency_u * u)
            carrier_du = angular_frequency_u * np.cos(
                angular_frequency_u * u
            )
            carrier_du2 = -(angular_frequency_u**2) * carrier
            probe = contact_probe_fraction * envelope * carrier
            probe_du = contact_probe_fraction * (
                envelope_du * carrier + envelope * carrier_du
            )
            probe_du2 = contact_probe_fraction * (
                envelope_du2 * carrier
                + 2.0 * envelope_du * carrier_du
                + envelope * carrier_du2
            )
            s += probe
            sd += probe_du / probe_duration
            sdd += probe_du2 / probe_duration**2
    else:
        sr, srd, srdd = _smooth_transition(t, return_start, return_end)
        s, sd, sdd = 1.0 - sr, -srd, -srdd
    return q_start + s * delta, sd * delta, sdd * delta


def _mass_matrix(model: pin.Model, data: pin.Data, q: np.ndarray) -> np.ndarray:
    upper = np.asarray(pin.crba(model, data, q), dtype=float)
    return np.triu(upper) + np.triu(upper, 1).T


def computed_torque(
    model: pin.Model,
    data: pin.Data,
    q: np.ndarray,
    qd: np.ndarray,
    q_ref: np.ndarray,
    qd_ref: np.ndarray,
    qdd_ref: np.ndarray,
    kp: float | np.ndarray,
    kd: float | np.ndarray,
    soft_limit_acceleration: np.ndarray | None = None,
) -> np.ndarray:
    """Model-based tracking command for a fixed-base revolute chain."""
    position_error = pin.difference(model, q, q_ref)
    kp_vector = np.broadcast_to(np.asarray(kp, dtype=float), (model.nv,))
    kd_vector = np.broadcast_to(np.asarray(kd, dtype=float), (model.nv,))
    acceleration_command = (
        qdd_ref
        + kp_vector * position_error
        + kd_vector * (qd_ref - qd)
    )
    if soft_limit_acceleration is not None:
        acceleration_command = (
            acceleration_command
            + np.asarray(soft_limit_acceleration, dtype=float)
        )
    nonlinear = pin.nonLinearEffects(model, data, q, qd)
    return nonlinear + _mass_matrix(model, data, q) @ acceleration_command


def _clip_actuator_torque(
    model: pin.Model,
    torque: np.ndarray,
    effort_fraction: float = 1.0,
) -> tuple[np.ndarray, bool]:
    limits = effort_fraction * np.asarray(model.effortLimit, dtype=float)
    finite_limits = np.where(np.isfinite(limits), limits, np.inf)
    clipped = np.clip(np.asarray(torque, dtype=float), -finite_limits, finite_limits)
    return clipped, bool(np.any(np.abs(clipped - torque) > 1e-12))


def soft_limit_acceleration(
    model: pin.Model,
    q: np.ndarray,
    qd: np.ndarray,
    buffer: float,
) -> np.ndarray:
    """Continuous acceleration-domain guard before the last-resort projection."""
    if buffer <= 0.0:
        return np.zeros(model.nv, dtype=float)
    lower_zone = np.asarray(model.lowerPositionLimit, dtype=float) + buffer
    upper_zone = np.asarray(model.upperPositionLimit, dtype=float) - buffer
    acceleration = np.zeros(model.nv, dtype=float)
    below = q < lower_zone
    above = q > upper_zone
    acceleration[below] += 400.0 * (lower_zone[below] - q[below])
    acceleration[below] += 36.0 * np.maximum(-qd[below], 0.0)
    acceleration[above] -= 400.0 * (q[above] - upper_zone[above])
    acceleration[above] -= 36.0 * np.maximum(qd[above], 0.0)
    return acceleration


def integrate_state(
    model: pin.Model,
    q: np.ndarray,
    qd: np.ndarray,
    qdd: np.ndarray,
    dt: float,
    joint_names: Iterable[str],
    margin: float,
    velocity_limit_fraction: float,
) -> tuple[np.ndarray, np.ndarray, bool, bool]:
    """Semi-implicit integration with URDF speed and position guards."""
    velocity_limit = (
        velocity_limit_fraction * np.asarray(model.velocityLimit, dtype=float)
    )
    qd_unlimited = qd + qdd * dt
    qd_next = np.clip(qd_unlimited, -velocity_limit, velocity_limit)
    velocity_limited = bool(
        np.any(np.abs(qd_next - qd_unlimited) > 1e-12)
    )
    q_next = pin.integrate(model, q, qd_next * dt)
    q_next, clipped = clip_configuration(
        model,
        q_next,
        tuple(joint_names),
        margin=margin,
    )

    if clipped:
        # All models in this experiment have nq == nv == 7 scalar revolute
        # joints.  At a clipped boundary, cancel only outward velocity.
        lower = np.asarray(model.lowerPositionLimit, dtype=float) + margin
        upper = np.asarray(model.upperPositionLimit, dtype=float) - margin
        at_lower = q_next <= lower + 1e-12
        at_upper = q_next >= upper - 1e-12
        qd_next = qd_next.copy()
        qd_next[at_lower & (qd_next < 0.0)] = 0.0
        qd_next[at_upper & (qd_next > 0.0)] = 0.0
    return q_next, qd_next, clipped, velocity_limited


def frame_kinematics(
    model: pin.Model,
    data: pin.Data,
    q: np.ndarray,
    qd: np.ndarray,
    frame_id: int,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    pin.forwardKinematics(model, data, q, qd)
    pin.updateFramePlacements(model, data)
    jacobian = pin.computeFrameJacobian(
        model,
        data,
        q,
        frame_id,
        pin.ReferenceFrame.LOCAL_WORLD_ALIGNED,
    )
    position = np.asarray(data.oMf[frame_id].translation, dtype=float).copy()
    linear_velocity = np.asarray(jacobian[:3] @ qd, dtype=float)
    return position, linear_velocity, np.asarray(jacobian, dtype=float)


def make_wall(
    config: SimulationConfig,
    models: TeleoperationModels,
    mapper: SEWMapper,
) -> tuple[Wall, dict[str, Any], np.ndarray]:
    q_start, q_end = master_endpoint_configurations()
    q_slave_start, start_debug = mapper.retarget(q_start)
    q_slave_end, end_debug = mapper.retarget(q_end, q_s_init=q_slave_start)
    if not start_debug["success"] or not end_debug["success"]:
        raise RuntimeError(
            "Cannot place the wall because endpoint retargeting failed: "
            f"start={start_debug['events']}, end={end_debug['events']}"
        )

    frame_id = require_frame(models.slave, config.slave_contact_frame)
    data = models.slave.createData()
    p_start, _, _ = frame_kinematics(
        models.slave,
        data,
        q_slave_start,
        np.zeros(models.slave.nv),
        frame_id,
    )
    p_end, _, _ = frame_kinematics(
        models.slave,
        data,
        q_slave_end,
        np.zeros(models.slave.nv),
        frame_id,
    )
    travel = p_end - p_start
    travel_norm = float(np.linalg.norm(travel))
    if travel_norm < 1e-6:
        raise RuntimeError("Retargeted TCP motion is too small to define a wall")
    normal = travel / travel_norm
    point = p_start + config.wall_fraction * travel
    wall = Wall(
        point=point,
        normal=normal,
        stiffness=config.wall_stiffness,
        damping=config.wall_damping,
        force_limit=config.wall_force_limit,
        transition_depth=config.wall_transition_depth,
    )
    metadata = {
        "start_tcp_world_m": p_start.tolist(),
        "end_tcp_world_m": p_end.tolist(),
        "free_space_travel_m": travel_norm,
        "point_world_m": point.tolist(),
        "normal_world": normal.tolist(),
        "fraction_of_free_space_travel": config.wall_fraction,
        "stiffness_N_per_m": config.wall_stiffness,
        "damping_Ns_per_m": config.wall_damping,
        "force_limit_N": config.wall_force_limit,
        "transition_depth_m": config.wall_transition_depth,
    }
    return wall, metadata, q_slave_start


def make_renderer(
    model: pin.Model,
    config: SimulationConfig,
) -> HapticRenderer:
    return HapticRenderer(
        model,
        chest_frame_name=MASTER_FRAMES["base"],
        ee_frame_name=MASTER_FRAMES["ee"],
        feedback_strength=config.feedback_strength,
        E_init=config.energy_min,
        E_max=config.energy_max,
        alpha_floor=0.0,
        alpha_ceil=1.0,
        E0=config.energy_initial,
        torque_limit=np.asarray(config.haptic_torque_limits, dtype=float),
        torque_rate_limit=np.asarray(config.haptic_rate_limits, dtype=float),
        tau_filter_alpha=config.haptic_filter_alpha,
    )


def _delay_line(
    size: int,
    sample_shape: tuple[int, ...],
) -> deque[np.ndarray]:
    return deque(
        [np.zeros(sample_shape, dtype=float) for _ in range(size)],
        maxlen=size,
    )


def _push_delayed(
    queue: deque[np.ndarray],
    sample: np.ndarray,
) -> np.ndarray:
    if queue.maxlen == 0:
        return np.asarray(sample, dtype=float).copy()
    delayed = queue.popleft()
    queue.append(np.asarray(sample, dtype=float).copy())
    return delayed


def _rms(values: np.ndarray) -> float:
    values = np.asarray(values, dtype=float)
    if values.size == 0:
        return 0.0
    return float(np.sqrt(np.mean(np.square(values))))


def _finite_or_none(value: float) -> float | None:
    return float(value) if np.isfinite(value) else None


def simulate_scenario(
    scenario: Scenario,
    config: SimulationConfig,
    models: TeleoperationModels,
    wall: Wall,
    q_slave_start: np.ndarray,
) -> ScenarioResult:
    """Run one closed-loop case with an independent but identically seeded state."""
    config.validate()
    rng = np.random.default_rng(config.seed)
    mapper = build_mapper(models)
    renderer = make_renderer(models.master, config)
    estimator = InteractionEstimator(
        models.slave,
        chest_frame_name=SLAVE_FRAMES["base"],
        ee_frame_name=config.slave_contact_frame,
        lambda_damp=1e-3,
        wrench_solver=ScaledDLSSolver(
            config.wrench_characteristic_length_m,
            config.wrench_scaled_damping,
        ),
    )

    sensor_bias = np.asarray(config.sensor_bias, dtype=float)
    calibration = sensor_bias + rng.normal(
        0.0,
        config.sensor_noise_std,
        size=(config.bias_calibration_samples, models.slave.nv),
    )
    estimator.calibrate_bias(calibration)

    q_start, q_end = master_endpoint_configurations()
    q_m = q_start.copy()
    qd_m = np.zeros(models.master.nv, dtype=float)
    q_s = q_slave_start.copy()
    qd_s = np.zeros(models.slave.nv, dtype=float)

    q_s_ref, A, map_debug = mapper.retarget_with_differential(
        q_m,
        q_s_init=q_s,
        fd_step=config.differential_step,
    )
    if not map_debug["success"] or not map_debug["differential_valid"]:
        raise RuntimeError(
            "Initial retargeting differential is invalid: "
            f"pose={map_debug['events']}, "
            f"A={map_debug['differential']['events']}"
        )
    differential_feedback_valid = True
    qd_s_ref_hold = np.zeros(models.slave.nv, dtype=float)
    map_registry = MapRegistry(capacity=2048)
    map_registry.add(
        MapSnapshot(
            map_id=0,
            source_index=0,
            source_time=0.0,
            differential=A,
            valid=True,
        )
    )
    active_forward_map_id = 0
    next_map_id = 1
    forward_seq = 0

    master_data_control = models.master.createData()
    master_data_dynamics = models.master.createData()
    slave_data_control = models.slave.createData()
    slave_data_dynamics = models.slave.createData()
    slave_data_contact = models.slave.createData()
    slave_tcp_id = require_frame(models.slave, config.slave_contact_frame)

    step_count = int(round(config.duration / config.dt))
    mapping_stride = max(
        1,
        int(round(1.0 / (config.mapping_hz * config.dt))),
    )
    forward_trace = generate_network_trace(
        step_count // mapping_stride + 2,
        base_delay_s=config.forward_delay_s,
        jitter_s=config.forward_jitter_s,
        loss_probability=config.forward_packet_loss,
        seed=config.seed + 1001,
    )
    return_trace = generate_network_trace(
        step_count,
        base_delay_s=config.feedback_delay_s,
        jitter_s=config.return_jitter_s,
        loss_probability=config.return_packet_loss,
        seed=config.seed + 1002,
    )
    forward_channel: DeterministicChannel[ForwardPacket] = (
        DeterministicChannel(forward_trace)
    )
    return_channel: DeterministicChannel[ReturnPacket] = (
        DeterministicChannel(return_trace)
    )
    forward_receiver: PacketReceiver[ForwardPacket] = PacketReceiver(
        config.forward_timeout_s
    )
    return_receiver: PacketReceiver[ReturnPacket] = PacketReceiver(
        config.return_timeout_s
    )
    forward_channel.send(
        ForwardPacket(
            seq=forward_seq,
            source_index=0,
            source_time=0.0,
            map_id=0,
            q_slave_ref=q_s_ref,
            qd_slave_ctrl=qd_s_ref_hold,
        ),
        now=0.0,
    )
    forward_seq += 1

    scalar_keys = (
        "time",
        "sample_index",
        "dt",
        "missed_deadline",
        "contact_force_norm",
        "penetration",
        "force_estimation_error_norm",
        "moment_estimation_error_norm",
        "raw_master_power",
        "a_defined_slave_power",
        "a_port_identity_error",
        "source_slave_power",
        "actual_power_mismatch_abs",
        "actual_slave_environment_power",
        "candidate_power",
        "applied_power",
        "rho",
        "energy_before",
        "tank_energy",
        "energy_preclip",
        "shadow_energy",
        "popc_damping_gain",
        "map_id",
        "source_map_id",
        "return_source_index",
        "return_packet_age",
        "forward_packet_state",
        "return_packet_state",
        "return_packet_active",
        "master_tracking_error",
        "slave_tracking_error",
        "feedback_torque_norm",
        "mapped_torque_norm",
    )
    vector_keys = (
        "q_master",
        "qd_master",
        "q_master_ref",
        "q_slave",
        "qd_slave",
        "q_slave_ref",
        "tau_slave_external",
        "tau_slave_estimated",
        "tau_slave_residual_source",
        "tau_slave_matched_wrench",
        "qd_slave_source",
        "tau_master_mapped",
        "tau_master_candidate",
        "tau_master_applied",
        "tau_master_accepted",
        "wrench_external",
        "wrench_estimated",
        "wrench_feedback_source",
        "map_differential",
        "tcp_position",
    )
    log_lists: dict[str, list[np.ndarray | float]] = {
        key: [] for key in scalar_keys + vector_keys
    }

    map_update_count = 0
    map_pose_success_count = 0
    differential_valid_count = 0
    differential_fallback_count = 0
    mapping_runtimes_ms: list[float] = []
    master_limit_events = 0
    slave_limit_events = 0
    master_velocity_limit_events = 0
    slave_velocity_limit_events = 0
    master_acceleration_limit_events = 0
    slave_acceleration_limit_events = 0
    master_torque_saturation_events = 0
    slave_torque_saturation_events = 0
    haptic_rate_limit_events = 0
    haptic_torque_saturation_events = 0
    energy_identity_errors: list[float] = []
    shadow_energy = config.energy_initial
    contact_steps = 0
    forward_packets_accepted = 0
    forward_packets_rejected = 0
    return_packets_accepted = 0
    return_packets_rejected = 0
    forward_timeout_steps = 0
    return_timeout_steps = 0
    popc = TimeDomainPOPC(
        initial_energy=config.energy_initial,
        minimum_energy=config.energy_min,
        maximum_energy=config.energy_max,
    )

    run_start = time.perf_counter()
    for step in range(step_count):
        t = step * config.dt
        q_m_ref, qd_m_ref, qdd_m_ref = master_reference(
            t,
            config.duration,
            q_start,
            q_end,
            config.contact_probe_fraction,
            config.contact_probe_cycles,
        )

        if step > 0 and step % mapping_stride == 0:
            tic = time.perf_counter()
            q_s_candidate, A_candidate, update_debug = (
                mapper.retarget_with_differential(
                    q_m,
                    q_s_init=q_s_ref,
                    fd_step=config.differential_step,
                )
            )
            mapping_runtimes_ms.append(
                1e3 * (time.perf_counter() - tic)
            )
            map_update_count += 1
            pose_valid = bool(update_debug["success"])
            differential_valid = bool(update_debug["differential_valid"])
            if pose_valid:
                map_pose_success_count += 1
            if update_debug["differential_valid"]:
                A = A_candidate
                differential_valid_count += 1
                qd_s_candidate = np.clip(
                    A @ qd_m,
                    -config.velocity_limit_fraction
                    * np.asarray(models.slave.velocityLimit, dtype=float),
                    config.velocity_limit_fraction
                    * np.asarray(models.slave.velocityLimit, dtype=float),
                )
            else:
                differential_fallback_count += 1
                qd_s_candidate = np.zeros(models.slave.nv, dtype=float)

            snapshot_differential = (
                A_candidate
                if np.all(np.isfinite(A_candidate))
                else np.zeros_like(A)
            )
            map_registry.add(
                MapSnapshot(
                    map_id=next_map_id,
                    source_index=step,
                    source_time=t,
                    differential=snapshot_differential,
                    valid=differential_valid,
                    reason_code=0 if differential_valid else 1,
                )
            )
            forward_channel.send(
                ForwardPacket(
                    seq=forward_seq,
                    source_index=step,
                    source_time=t,
                    map_id=next_map_id,
                    q_slave_ref=(
                        q_s_candidate if pose_valid else q_s_ref
                    ),
                    qd_slave_ctrl=qd_s_candidate,
                    valid=pose_valid,
                ),
                now=t,
            )
            next_map_id += 1
            forward_seq += 1

        for delivery in forward_channel.poll(t):
            reception = forward_receiver.accept(delivery)
            if reception.accepted:
                forward_packets_accepted += 1
            else:
                forward_packets_rejected += 1
        held_forward = forward_receiver.sample(t)
        if held_forward.packet is not None:
            q_s_ref = held_forward.packet.q_slave_ref.copy()
            qd_s_ref_hold = held_forward.packet.qd_slave_ctrl.copy()
            active_forward_map_id = held_forward.packet.map_id
        elif held_forward.state is PacketState.TIMED_OUT:
            # The position target is held while commanded velocity goes to zero.
            qd_s_ref_hold.fill(0.0)
            forward_timeout_steps += 1
        qd_s_ref = qd_s_ref_hold
        try:
            differential_feedback_valid = map_registry.get(
                active_forward_map_id
            ).valid
        except KeyError:
            differential_feedback_valid = False

        tcp_position, tcp_linear_velocity, J_slave_world = frame_kinematics(
            models.slave,
            slave_data_contact,
            q_s,
            qd_s,
            slave_tcp_id,
        )
        wrench_external, penetration = wall.wrench(
            tcp_position,
            tcp_linear_velocity,
        )
        if penetration > 0.0:
            contact_steps += 1
        tau_slave_external = J_slave_world.T @ wrench_external

        tau_slave_control = computed_torque(
            models.slave,
            slave_data_control,
            q_s,
            qd_s,
            q_s_ref,
            qd_s_ref,
            np.zeros(models.slave.nv),
            config.slave_kp,
            config.slave_kd,
            soft_limit_acceleration(
                models.slave,
                q_s,
                qd_s,
                config.soft_limit_buffer,
            ),
        )
        tau_slave_control, torque_clipped = _clip_actuator_torque(
            models.slave,
            tau_slave_control,
            config.slave_tracking_effort_fraction,
        )
        slave_torque_saturation_events += int(torque_clipped)
        qdd_s_raw = pin.aba(
            models.slave,
            slave_data_dynamics,
            q_s,
            qd_s,
            tau_slave_control + tau_slave_external,
        )
        slave_acceleration_limits = np.asarray(
            config.slave_acceleration_limits,
            dtype=float,
        )
        qdd_s = np.clip(
            qdd_s_raw,
            -slave_acceleration_limits,
            slave_acceleration_limits,
        )
        slave_acceleration_limit_events += int(
            np.any(np.abs(qdd_s_raw) > slave_acceleration_limits)
        )

        # This is an explicitly simulated load-side equivalent measurement.
        # It satisfies the estimator's declared residual convention:
        # tau_int = tau_meas - (M qdd + h) - calibrated_bias.
        tau_slave_model = estimator._tau_model(q_s, qd_s, qdd_s)
        measurement_noise = rng.normal(
            0.0,
            config.sensor_noise_std,
            models.slave.nv,
        )
        tau_slave_measured = (
            tau_slave_model
            + tau_slave_external
            + sensor_bias
            + measurement_noise
        )
        tau_slave_estimated, wrench_estimated, J_slave_chest = estimator.estimate(
            q_s,
            qd_s,
            qdd_s,
            tau_slave_measured,
        )
        tau_slave_matched_wrench = J_slave_chest.T @ wrench_estimated
        return_channel.send(
            ReturnPacket(
                seq=step,
                source_index=step,
                source_time=t,
                echoed_map_id=active_forward_map_id,
                residual=tau_slave_estimated,
                wrench=wrench_estimated,
                js_t_wrench=tau_slave_matched_wrench,
                qd_slave_actual=qd_s,
            ),
            now=t,
        )
        for delivery in return_channel.poll(t):
            reception = return_receiver.accept(delivery)
            if reception.accepted:
                return_packets_accepted += 1
            else:
                return_packets_rejected += 1
        held_return = return_receiver.sample(t)
        delayed_packet = held_return.packet
        return_packet_active = delayed_packet is not None
        if held_return.state is PacketState.TIMED_OUT:
            return_timeout_steps += 1

        master_jacobian = renderer.CJ_master.chest_jacobian(q_m, qd_m)
        if delayed_packet is None:
            tau_master_mapped = np.zeros(models.master.nv, dtype=float)
            delayed_tau_slave = np.zeros(models.slave.nv, dtype=float)
            delayed_wrench = np.zeros(6, dtype=float)
            delayed_tau_matched = np.zeros(models.slave.nv, dtype=float)
            tau_slave_source_for_mapping = np.zeros(
                models.slave.nv, dtype=float
            )
            qd_slave_source = np.zeros(models.slave.nv, dtype=float)
            selected_map_id = -1
            source_map_id = -1
            return_source_index = -1
            return_packet_age = math.nan
        else:
            feedback = map_return_feedback(
                kind=MappingKind(scenario.mapping),
                packet=delayed_packet,
                master_jacobian=master_jacobian,
                maps=map_registry,
                map_policy=MapPolicy(scenario.map_policy),
            )
            tau_master_mapped = feedback.tau_master_raw
            delayed_tau_slave = delayed_packet.residual.copy()
            delayed_wrench = delayed_packet.wrench.copy()
            delayed_tau_matched = delayed_packet.js_t_wrench.copy()
            tau_slave_source_for_mapping = feedback.tau_slave_source.copy()
            qd_slave_source = delayed_packet.qd_slave_actual.copy()
            selected_map_id = (
                -1
                if feedback.selected_map_id is None
                else feedback.selected_map_id
            )
            source_map_id = delayed_packet.echoed_map_id
            return_source_index = delayed_packet.source_index
            return_packet_age = max(0.0, t - delayed_packet.source_time)
            if not feedback.valid:
                tau_master_mapped = np.zeros(models.master.nv, dtype=float)

        popc_damping_gain = 0.0
        if scenario.supervisor == "tank":
            tau_master_applied, rho = renderer.render_mapped_reaction(
                tau_master_mapped,
                qd_m,
                config.dt,
                supervise_energy=True,
            )
        elif scenario.supervisor == "bypass":
            tau_master_applied, rho = renderer.render_mapped_reaction(
                tau_master_mapped,
                qd_m,
                config.dt,
                supervise_energy=False,
            )
        elif scenario.supervisor == "popc":
            tau_master_candidate, candidate_valid = (
                renderer.shape_mapped_reaction_candidate(
                    tau_master_mapped,
                    qd_m,
                    config.dt,
                )
            )
            if candidate_valid:
                tau_master_applied, popc_diagnostics = popc.apply(
                    tau_master_candidate,
                    qd_m,
                    config.dt,
                )
            else:
                tau_master_applied = np.zeros_like(tau_master_candidate)
                _, popc_diagnostics = popc.apply(
                    tau_master_applied,
                    np.zeros_like(qd_m),
                    config.dt,
                )
            renderer.commit_applied(tau_master_applied)
            rho = math.nan
            popc_damping_gain = popc_diagnostics.damping_gain
        else:
            raise ValueError(f"Unknown supervisor: {scenario.supervisor}")
        haptic_rate_limit_events += int(renderer.last_rate_limit_active)
        haptic_torque_saturation_events += int(
            renderer.last_torque_saturation_active
        )
        if scenario.supervisor == "tank":
            diagnostics = renderer.tank.last_diagnostics
            assert diagnostics is not None
            tau_master_candidate = diagnostics.tau_candidate.copy()
            reconstructed_energy_preclip = float(
                diagnostics.E_before - diagnostics.power * config.dt
            )
            energy_identity_errors.append(
                abs(diagnostics.E_preclip - reconstructed_energy_preclip)
            )
            tank_energy = diagnostics.E_after
            energy_before = diagnostics.E_before
            energy_preclip = diagnostics.E_preclip
            candidate_power = diagnostics.candidate_power
        elif scenario.supervisor == "popc":
            tank_energy = popc_diagnostics.observer_after
            energy_before = popc_diagnostics.observer_before
            energy_preclip = popc_diagnostics.observer_preclip
            candidate_power = popc_diagnostics.candidate_power
        else:
            tau_master_candidate = tau_master_applied.copy()
            tank_energy = math.nan
            energy_before = math.nan
            energy_preclip = math.nan
            candidate_power = float(np.dot(tau_master_candidate, qd_m))

        applied_power = float(np.dot(tau_master_applied, qd_m))
        tau_master_accepted = tau_master_applied.copy()
        # Counterfactual storage obeys the same upper capacity but deliberately
        # has no lower projection.  Falling below E_min demonstrates the exact
        # sample at which unsupervised output violates the configured budget.
        shadow_energy = min(
            config.energy_max,
            shadow_energy - candidate_power * config.dt,
        )

        tau_human = computed_torque(
            models.master,
            master_data_control,
            q_m,
            qd_m,
            q_m_ref,
            qd_m_ref,
            qdd_m_ref,
            config.master_kp,
            config.master_kd,
            soft_limit_acceleration(
                models.master,
                q_m,
                qd_m,
                config.soft_limit_buffer,
            ),
        )
        tau_human, master_torque_clipped = _clip_actuator_torque(
            models.master,
            tau_human,
            config.master_tracking_effort_fraction,
        )
        master_torque_saturation_events += int(master_torque_clipped)
        qdd_m_raw = pin.aba(
            models.master,
            master_data_dynamics,
            q_m,
            qd_m,
            tau_human + tau_master_applied,
        )
        master_acceleration_limits = np.asarray(
            config.master_acceleration_limits,
            dtype=float,
        )
        qdd_m = np.clip(
            qdd_m_raw,
            -master_acceleration_limits,
            master_acceleration_limits,
        )
        master_acceleration_limit_events += int(
            np.any(np.abs(qdd_m_raw) > master_acceleration_limits)
        )

        try:
            differential_for_power = (
                map_registry.get(selected_map_id).differential
                if selected_map_id >= 0
                else map_registry.latest.differential
            )
        except KeyError:
            differential_for_power = np.zeros(
                (models.slave.nv, models.master.nv), dtype=float
            )
        reference_slave_velocity = differential_for_power @ qd_m
        raw_master_power = float(np.dot(tau_master_mapped, qd_m))
        a_defined_slave_power = float(
            np.dot(tau_slave_source_for_mapping, reference_slave_velocity)
        )
        a_port_identity_error = abs(
            raw_master_power - a_defined_slave_power
        )
        source_slave_power = float(
            np.dot(tau_slave_source_for_mapping, qd_slave_source)
        )
        actual_power_mismatch_abs = abs(
            raw_master_power - source_slave_power
        )

        scalar_values = {
            "time": t,
            "sample_index": step,
            "dt": config.dt,
            "missed_deadline": 0,
            "contact_force_norm": float(np.linalg.norm(wrench_external[:3])),
            "penetration": penetration,
            "force_estimation_error_norm": float(
                np.linalg.norm(wrench_estimated[:3] - wrench_external[:3])
            ),
            "moment_estimation_error_norm": float(
                np.linalg.norm(wrench_estimated[3:] - wrench_external[3:])
            ),
            "raw_master_power": raw_master_power,
            "a_defined_slave_power": a_defined_slave_power,
            "a_port_identity_error": a_port_identity_error,
            "source_slave_power": source_slave_power,
            "actual_power_mismatch_abs": actual_power_mismatch_abs,
            "actual_slave_environment_power": float(
                np.dot(tau_slave_external, qd_s)
            ),
            "candidate_power": candidate_power,
            "applied_power": applied_power,
            "rho": rho,
            "energy_before": energy_before,
            "tank_energy": tank_energy,
            "energy_preclip": energy_preclip,
            "shadow_energy": shadow_energy,
            "popc_damping_gain": popc_damping_gain,
            "map_id": selected_map_id,
            "source_map_id": source_map_id,
            "return_source_index": return_source_index,
            "return_packet_age": return_packet_age,
            "forward_packet_state": int(held_forward.state),
            "return_packet_state": int(held_return.state),
            "return_packet_active": int(return_packet_active),
            "master_tracking_error": float(
                np.linalg.norm(pin.difference(models.master, q_m, q_m_ref))
            ),
            "slave_tracking_error": float(
                np.linalg.norm(pin.difference(models.slave, q_s, q_s_ref))
            ),
            "feedback_torque_norm": float(np.linalg.norm(tau_master_applied)),
            "mapped_torque_norm": float(np.linalg.norm(tau_master_mapped)),
        }
        vector_values = {
            "q_master": q_m.copy(),
            "qd_master": qd_m.copy(),
            "q_master_ref": q_m_ref.copy(),
            "q_slave": q_s.copy(),
            "qd_slave": qd_s.copy(),
            "q_slave_ref": q_s_ref.copy(),
            "tau_slave_external": tau_slave_external.copy(),
            "tau_slave_estimated": tau_slave_estimated.copy(),
            "tau_slave_residual_source": delayed_tau_slave.copy(),
            "tau_slave_matched_wrench": delayed_tau_matched.copy(),
            "qd_slave_source": qd_slave_source.copy(),
            "tau_master_mapped": tau_master_mapped.copy(),
            "tau_master_candidate": tau_master_candidate.copy(),
            "tau_master_applied": tau_master_applied.copy(),
            "tau_master_accepted": tau_master_accepted.copy(),
            "wrench_external": wrench_external.copy(),
            "wrench_estimated": wrench_estimated.copy(),
            "wrench_feedback_source": delayed_wrench.copy(),
            "map_differential": differential_for_power.reshape(-1).copy(),
            "tcp_position": tcp_position.copy(),
        }
        for key, value in scalar_values.items():
            log_lists[key].append(float(value))
        for key, value in vector_values.items():
            log_lists[key].append(value)

        q_m, qd_m, master_clipped, master_velocity_limited = integrate_state(
            models.master,
            q_m,
            qd_m,
            qdd_m,
            config.dt,
            MASTER_JOINT_NAMES,
            config.joint_limit_margin,
            config.velocity_limit_fraction,
        )
        q_s, qd_s, slave_clipped, slave_velocity_limited = integrate_state(
            models.slave,
            q_s,
            qd_s,
            qdd_s,
            config.dt,
            SLAVE_JOINT_NAMES,
            config.joint_limit_margin,
            config.velocity_limit_fraction,
        )
        master_limit_events += int(master_clipped)
        slave_limit_events += int(slave_clipped)
        master_velocity_limit_events += int(master_velocity_limited)
        slave_velocity_limit_events += int(slave_velocity_limited)

        if not (
            np.all(np.isfinite(q_m))
            and np.all(np.isfinite(qd_m))
            and np.all(np.isfinite(q_s))
            and np.all(np.isfinite(qd_s))
        ):
            raise FloatingPointError(
                f"{scenario.key}: non-finite state at t={t:.6f} s"
            )

    runtime = time.perf_counter() - run_start
    logs = {
        key: np.asarray(values, dtype=float) for key, values in log_lists.items()
    }
    contact_mask = logs["penetration"] > 0.0
    force_active_mask = logs["contact_force_norm"] > 1e-6
    power_scale = _rms(logs["a_defined_slave_power"][contact_mask])
    projection_mask = (
        logs["rho"] < (1.0 - 1e-12)
        if scenario.supervisor == "tank"
        else logs["popc_damping_gain"] > 0.0
    )
    positive_power = np.maximum(logs["applied_power"], 0.0)
    absorbed_power = np.maximum(-logs["applied_power"], 0.0)
    actual_mismatch_numerator = float(
        np.sum(logs["actual_power_mismatch_abs"]) * config.dt
    )
    actual_mismatch_denominator = float(
        0.5
        * np.sum(
            np.abs(logs["raw_master_power"])
            + np.abs(logs["source_slave_power"])
        )
        * config.dt
        + 1e-12
    )
    finite_rho = logs["rho"][np.isfinite(logs["rho"])]

    metrics = {
        "completed": True,
        "finite_state": True,
        "simulated_duration_s": config.duration,
        "wall_contact_fraction": float(np.mean(contact_mask)),
        "wall_contact_duration_s": float(np.sum(contact_mask) * config.dt),
        "wall_force_active_fraction": float(np.mean(force_active_mask)),
        "peak_contact_force_N": float(np.max(logs["contact_force_norm"])),
        "max_penetration_mm": float(1e3 * np.max(logs["penetration"])),
        "force_estimation_rmse_N": _rms(
            logs["force_estimation_error_norm"]
        ),
        "moment_estimation_rmse_Nm": _rms(
            logs["moment_estimation_error_norm"]
        ),
        "master_tracking_rmse_rad": _rms(logs["master_tracking_error"]),
        "slave_tracking_rmse_rad": _rms(logs["slave_tracking_error"]),
        "feedback_torque_rms_Nm": _rms(logs["feedback_torque_norm"]),
        "feedback_torque_peak_Nm": float(
            np.max(logs["feedback_torque_norm"])
        ),
        "a_port_identity_error_rms_W": _rms(
            logs["a_port_identity_error"]
        ),
        "a_port_identity_error_max_W": float(
            np.max(logs["a_port_identity_error"])
        ),
        "a_port_identity_relative_rms": float(
            _rms(logs["a_port_identity_error"][contact_mask])
            / max(power_scale, 1e-12)
        ),
        "actual_power_mismatch_normalized": (
            actual_mismatch_numerator / actual_mismatch_denominator
        ),
        "slave_environment_net_work_J": float(
            np.sum(logs["actual_slave_environment_power"]) * config.dt
        ),
        "positive_energy_delivered_J": float(
            np.sum(positive_power) * config.dt
        ),
        "energy_absorbed_J": float(np.sum(absorbed_power) * config.dt),
        "supervisor_intervention_fraction": float(np.mean(projection_mask)),
        "energy_projection_fraction": (
            float(np.mean(projection_mask))
            if scenario.supervisor == "tank"
            else None
        ),
        "energy_projection_contact_fraction": float(
            np.mean(projection_mask[contact_mask])
            if scenario.supervisor == "tank" and np.any(contact_mask)
            else 0.0
        ),
        "rho_min": (
            float(np.min(finite_rho)) if finite_rho.size else None
        ),
        "tank_energy_min_J": _finite_or_none(
            float(np.nanmin(logs["tank_energy"]))
            if scenario.supervisor in ("tank", "popc")
            else math.nan
        ),
        "tank_energy_final_J": _finite_or_none(
            float(logs["tank_energy"][-1])
            if scenario.supervisor in ("tank", "popc")
            else math.nan
        ),
        "shadow_energy_min_J": float(np.min(logs["shadow_energy"])),
        "shadow_energy_floor_violation_J": float(
            max(0.0, config.energy_min - np.min(logs["shadow_energy"]))
        ),
        "energy_accounting_max_error_J": (
            float(max(energy_identity_errors, default=0.0))
            if scenario.supervisor == "tank"
            else None
        ),
        "mapping_updates": map_update_count,
        "mapping_pose_success_rate": float(
            map_pose_success_count / max(map_update_count, 1)
        ),
        "differential_valid_rate": float(
            differential_valid_count / max(map_update_count, 1)
        ),
        "differential_fallback_count": differential_fallback_count,
        "mapping_runtime_median_ms": float(
            np.median(mapping_runtimes_ms) if mapping_runtimes_ms else 0.0
        ),
        "mapping_runtime_p95_ms": float(
            np.percentile(mapping_runtimes_ms, 95.0)
            if mapping_runtimes_ms
            else 0.0
        ),
        "mapping_runtime_max_ms": float(
            max(mapping_runtimes_ms, default=0.0)
        ),
        "master_joint_limit_events": master_limit_events,
        "slave_joint_limit_events": slave_limit_events,
        "master_velocity_limit_events": master_velocity_limit_events,
        "slave_velocity_limit_events": slave_velocity_limit_events,
        "master_acceleration_limit_events": master_acceleration_limit_events,
        "slave_acceleration_limit_events": slave_acceleration_limit_events,
        "master_torque_saturation_events": master_torque_saturation_events,
        "slave_torque_saturation_events": slave_torque_saturation_events,
        "haptic_rate_limit_events": haptic_rate_limit_events,
        "haptic_torque_saturation_events": haptic_torque_saturation_events,
        "forward_packets_accepted": forward_packets_accepted,
        "forward_packets_rejected": forward_packets_rejected,
        "return_packets_accepted": return_packets_accepted,
        "return_packets_rejected": return_packets_rejected,
        "forward_timeout_steps": forward_timeout_steps,
        "return_timeout_steps": return_timeout_steps,
        "wall_contact_steps": contact_steps,
        "wall_time_s": runtime,
    }
    return ScenarioResult(scenario=scenario, metrics=metrics, logs=logs)


def _write_npz(path: Path, logs: dict[str, np.ndarray]) -> None:
    np.savez_compressed(path, **logs)


def _write_csv(path: Path, logs: dict[str, np.ndarray]) -> None:
    scalar_keys = [
        key for key, value in logs.items() if value.ndim == 1
    ]
    vector_keys = [
        key for key, value in logs.items() if value.ndim == 2
    ]
    fieldnames = scalar_keys + [
        f"{key}_{index}"
        for key in vector_keys
        for index in range(logs[key].shape[1])
    ]
    with path.open("w", encoding="utf-8", newline="") as stream:
        writer = csv.DictWriter(stream, fieldnames=fieldnames)
        writer.writeheader()
        row_count = logs["time"].shape[0]
        for row_index in range(row_count):
            row: dict[str, float] = {
                key: float(logs[key][row_index]) for key in scalar_keys
            }
            for key in vector_keys:
                for column_index, value in enumerate(logs[key][row_index]):
                    row[f"{key}_{column_index}"] = float(value)
            writer.writerow(row)


def make_comparison_plot(
    results: list[ScenarioResult],
    config: SimulationConfig,
    output_path: Path,
) -> None:
    import matplotlib

    matplotlib.use("Agg")
    import matplotlib.pyplot as plt

    colors = {
        "proposed_energy": "#0072B2",
        "direct_energy": "#D55E00",
        "proposed_no_energy": "#009E73",
        "matched_wrench_energy": "#CC79A7",
        "proposed_popc": "#E69F00",
    }
    labels = {
        "proposed_energy": r"$A^\mathsf{T}$ + final energy",
        "direct_energy": r"$J_m^\mathsf{T}F$ + final energy",
        "proposed_no_energy": r"$A^\mathsf{T}$, no energy projection",
        "matched_wrench_energy": (
            r"$A_\mathrm{src}^\mathsf{T}J_s^\mathsf{T}F$ + final energy"
        ),
        "proposed_popc": r"$A^\mathsf{T}$ + time-domain PO/PC",
    }
    fig, axes = plt.subplots(3, 2, figsize=(13.0, 10.0), sharex=True)

    for result in results:
        key = result.scenario.key
        color = colors[key]
        label = labels[key]
        time_axis = result.logs["time"]
        axes[0, 0].plot(
            time_axis,
            result.logs["contact_force_norm"],
            color=color,
            label=label,
        )
        axes[0, 1].semilogy(
            time_axis,
            np.maximum(result.logs["a_port_identity_error"], 1e-14),
            color=color,
            label=label,
        )
        axes[1, 0].plot(
            time_axis,
            result.logs["feedback_torque_norm"],
            color=color,
            label=label,
        )
        supervisor_trace = (
            (result.logs["popc_damping_gain"] > 0.0).astype(float)
            if result.scenario.supervisor == "popc"
            else result.logs["rho"]
        )
        axes[1, 1].plot(
            time_axis,
            supervisor_trace,
            color=color,
            label=label,
        )
        if result.scenario.supervisor in ("tank", "popc"):
            axes[2, 0].plot(
                time_axis,
                result.logs["tank_energy"],
                color=color,
                label=label,
            )
        else:
            axes[2, 0].plot(
                time_axis,
                result.logs["shadow_energy"],
                color=color,
                linestyle="--",
                label="unsupervised counterfactual reserve",
            )
        axes[2, 1].plot(
            time_axis,
            1e3 * result.logs["penetration"],
            color=color,
            label=label,
        )

    axes[0, 0].set_ylabel("contact force [N]")
    axes[0, 0].set_title("Closed-loop wall interaction")
    axes[0, 1].set_ylabel("A-port identity error [W]")
    axes[0, 1].set_title(
        "Raw A-port power consistency (plot floor $10^{-14}$ W)"
    )
    axes[1, 0].set_ylabel(r"$\|\tau_{m,app}\|$ [N m]")
    axes[1, 0].set_title("Applied master haptic torque")
    axes[1, 1].set_ylabel(r"tank $\rho$ / PO-PC active")
    axes[1, 1].set_ylim(-0.04, 1.04)
    axes[1, 1].set_title("Final-port supervisor activity")
    axes[2, 0].axhline(
        config.energy_min,
        color="black",
        linestyle="--",
        linewidth=1.0,
        label="configured E_min",
    )
    axes[2, 0].set_ylabel("tank energy [J]")
    axes[2, 0].set_title("Accounted / counterfactual energy reserve")
    axes[2, 1].set_ylabel("wall penetration [mm]")
    axes[2, 1].set_title("Slave TCP penetration")
    for axis in axes[-1, :]:
        axis.set_xlabel("time [s]")
    for axis in axes.flat:
        axis.grid(True, alpha=0.3)
    axes[0, 0].legend(loc="best", fontsize=8)
    axes[2, 0].legend(loc="best", fontsize=8)
    fig.suptitle(
        "Simulation only — master_7dof.urdf / real_slave_7dof.urdf",
        fontsize=13,
    )
    fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.97))
    fig.savefig(output_path, dpi=180)
    plt.close(fig)


def _json_ready(value: Any) -> Any:
    if isinstance(value, np.ndarray):
        return value.tolist()
    if isinstance(value, (np.floating, np.integer)):
        return value.item()
    if isinstance(value, dict):
        return {key: _json_ready(item) for key, item in value.items()}
    if isinstance(value, (tuple, list)):
        return [_json_ready(item) for item in value]
    return value


def run_comparison(
    config: SimulationConfig,
    output_dir: Path,
    scenarios: tuple[Scenario, ...] = SCENARIOS,
) -> dict[str, Any]:
    config.validate()
    output_dir.mkdir(parents=True, exist_ok=True)
    models = load_models(add_simulated_tcp=True)
    wall_mapper = build_mapper(models)
    wall, wall_metadata, q_slave_start = make_wall(
        config,
        models,
        wall_mapper,
    )

    results: list[ScenarioResult] = []
    for scenario in scenarios:
        print(f"[simulation] running {scenario.key}: {scenario.description}")
        result = simulate_scenario(
            scenario,
            config,
            models,
            wall,
            q_slave_start,
        )
        results.append(result)
        _write_npz(output_dir / f"{scenario.key}.npz", result.logs)
        _write_csv(output_dir / f"{scenario.key}.csv", result.logs)

    make_comparison_plot(
        results,
        config,
        output_dir / "closed_loop_comparison.png",
    )
    summary = {
        "evidence_scope": (
            "Rigid-body closed-loop simulation only; no prototype or human "
            "subject result is claimed."
        ),
        "metric_definitions_and_limits": {
            "a_port_identity": (
                "Compares tau_mapped^T qd_m with delayed_tau_s^T "
                "(A qd_m). It is an algebraic implementation-consistency "
                "metric, not equality of the actual master/slave port powers."
            ),
            "contact": (
                "wall_contact_fraction/duration/steps use penetration > 0; "
                "wall_force_active_fraction separately uses force norm > 1e-6."
            ),
            "energy_stress_protocol": (
                "The tank starts at E_min with only "
                f"{config.energy_max - config.energy_min:.6g} J capacity above "
                "the floor. This deliberately tight budget exercises the "
                "projection and is not a hardware tuning recommendation."
            ),
            "shadow_energy": (
                "For no-energy, this reconstructs that scenario's actual "
                "unprojected candidate sequence. For supervised scenarios, "
                "it is a stepwise witness on the supervised closed-loop "
                "candidate sequence, not a second counterfactual simulation."
            ),
            "plotting_floor_W": 1e-14,
            "estimator_scope": (
                "Torque bias/noise and load-side measurements are synthetic, "
                "and plant/estimator share one URDF model; hardware robustness "
                "is therefore not established."
            ),
            "claims_not_supported": (
                "No claim of prototype performance, human-in-the-loop "
                "stability, global passivity, delay robustness, transparency, "
                "or workspace-wide/statistical robustness."
            ),
            "ablation_scope": (
                "proposed_no_energy bypasses only the final energy projection. "
                "It retains feedback gain/filtering, haptic rate and torque "
                "limits, tracking effort limits, acceleration/velocity/position "
                "guards, soft joint-limit guards, and the wall force cap."
            ),
            "feedback_chain_comparison": (
                "A.T receives the estimated joint residual, whereas the direct "
                "baseline receives its DLS wrench projection. This is an "
                "end-to-end feedback-chain ablation, not a same-input pure "
                "matrix comparison."
            ),
            "slave_reference_sampling": (
                "q_s_ref and qd_s_ref are both sampled and held at the 50 Hz "
                "mapping update rate; qdd_s_ref is zero."
            ),
        },
        "models": {
            "master_urdf": str(MASTER_URDF),
            "slave_urdf": str(SLAVE_URDF),
            "slave_contact_frame": config.slave_contact_frame,
            "contact_frame_status": (
                "R_EE_SIM is a simulation-only fixed frame from the repository "
                "MJCF marker; a measured TCP/FT transform is required for hardware"
                if config.slave_contact_frame == SLAVE_FRAMES["ee"]
                else "terminal frame present in real_slave_7dof.urdf"
            ),
        },
        "config": asdict(config),
        "wall": wall_metadata,
        "scenarios": {
            result.scenario.key: {
                "mapping": result.scenario.mapping,
                "energy_supervision": result.scenario.supervise_energy,
                "supervisor": result.scenario.supervisor,
                "map_policy": result.scenario.map_policy,
                "description": result.scenario.description,
                "metrics": result.metrics,
            }
            for result in results
        },
        "artifacts": {
            "plot": "closed_loop_comparison.png",
            "machine_readable_logs": [
                f"{result.scenario.key}.npz" for result in results
            ],
            "tabular_logs": [
                f"{result.scenario.key}.csv" for result in results
            ],
        },
    }
    with (output_dir / "summary.json").open("w", encoding="utf-8") as stream:
        json.dump(_json_ready(summary), stream, indent=2, ensure_ascii=False)

    print("\nSimulation-only comparison")
    print(
        "scenario                 contact[N]  A-port err[W]  "
        "rho_min  E_min[J]  map p95[ms]"
    )
    for result in results:
        metrics = result.metrics
        tank_min = metrics["tank_energy_min_J"]
        tank_text = "   n/a" if tank_min is None else f"{tank_min:8.4f}"
        rho_min = metrics["rho_min"]
        rho_text = "    n/a" if rho_min is None else f"{rho_min:9.3f}"
        print(
            f"{result.scenario.key:24s}"
            f"{metrics['peak_contact_force_N']:10.3f}"
            f"{metrics['a_port_identity_error_rms_W']:15.3e}"
            f"{rho_text}"
            f"{tank_text}"
            f"{metrics['mapping_runtime_p95_ms']:13.3f}"
        )
    print(f"\nArtifacts: {output_dir}")
    return summary


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(
        description="Run the closed-loop bilateral simulation comparison."
    )
    parser.add_argument(
        "--config",
        type=Path,
        default=DEFAULT_CONFIG_PATH,
        help=f"YAML configuration (default: {DEFAULT_CONFIG_PATH})",
    )
    parser.add_argument(
        "--output-dir",
        type=Path,
        default=DEFAULT_OUTPUT_DIR,
        help=f"artifact directory (default: {DEFAULT_OUTPUT_DIR})",
    )
    parser.add_argument(
        "--duration",
        type=float,
        default=None,
        help="override simulated duration per scenario in seconds",
    )
    parser.add_argument(
        "--dt",
        type=float,
        default=None,
        help="override dynamics integration period in seconds",
    )
    parser.add_argument(
        "--mapping-hz",
        type=float,
        default=None,
        help="override retargeting/differential update rate",
    )
    return parser.parse_args()


def main() -> None:
    args = parse_args()
    config = load_simulation_config(args.config.resolve())
    overrides = {
        key: value
        for key, value in {
            "dt": args.dt,
            "duration": args.duration,
            "mapping_hz": args.mapping_hz,
        }.items()
        if value is not None
    }
    if overrides:
        config = replace(config, **overrides)
    run_comparison(config, args.output_dir.resolve())


if __name__ == "__main__":
    main()
