import numpy as np
import pytest
import pickle
from multiprocessing import get_context
from queue import Empty, Queue
from types import SimpleNamespace
import xml.etree.ElementTree as ET

from iiwa_genesis.control import (
    CartesianCommand,
    JointCommand,
    RobotController,
    VelocityLimitedMotion,
    configure_robot_control,
    base_pose_to_world,
    parse_command,
    GuiCommandQueue,
)
from iiwa_genesis.control_gui import ControlGui, _observe_parent_close, command_from_fields
from iiwa_genesis.scene import SceneMotionController


IIWA_VELOCITY_LIMITS = (1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14)


class FakeRobot:
    def __init__(self):
        self.calls = []
        self.position = np.zeros(7)

    def control_dofs_position(self, position, dofs_idx_local=None):
        self.position = np.asarray(position, dtype=float)
        self.calls.append(("control", self.position.copy(), dofs_idx_local))

    def get_dofs_position(self, dofs_idx_local=None):
        self.calls.append(("position", dofs_idx_local))
        return self.position

    def get_link(self, name):
        self.calls.append(("link", name))
        return name

    def inverse_kinematics(self, **kwargs):
        self.calls.append(("ik", kwargs))
        qpos = np.arange(7, dtype=float) * .2
        return (qpos, np.zeros(6)) if kwargs.get("return_error") else qpos

    def set_dofs_kp(self, kp, dofs_idx_local=None):
        self.calls.append(("kp", np.asarray(kp), dofs_idx_local))

    def set_dofs_kv(self, kv, dofs_idx_local=None):
        self.calls.append(("kv", np.asarray(kv), dofs_idx_local))

    def set_dofs_force_range(self, lower, upper, dofs_idx_local=None):
        self.calls.append(("force", np.asarray(lower), np.asarray(upper), dofs_idx_local))


def test_parses_joint_mode_and_seven_angles():
    command = parse_command("joints 0 0.2 0 1.0 0 -0.3 0.1")
    assert isinstance(command, JointCommand)
    np.testing.assert_allclose(command.angles, [0, 0.2, 0, 1.0, 0, -0.3, 0.1])


def test_set_uses_all_seven_values_after_selecting_joints_mode():
    command = parse_command("set 0 0.2 0 1.0 0 0 0", mode="joints")
    assert isinstance(command, JointCommand)
    np.testing.assert_allclose(command.angles, [0, 0.2, 0, 1.0, 0, 0, 0])


def test_selected_joints_mode_accepts_bare_angle_values():
    command = parse_command("0 0.2 0 1.0 0 0 0", mode="joints")
    assert isinstance(command, JointCommand)
    np.testing.assert_allclose(command.angles, [0, 0.2, 0, 1.0, 0, 0, 0])


def test_parses_cartesian_tcp_pose_in_robot_base_frame():
    command = parse_command("cartesian 0.35 0 0.55 0 1.57 0")
    assert isinstance(command, CartesianCommand)
    np.testing.assert_allclose(command.position, [0.35, 0, 0.55])
    np.testing.assert_allclose(command.rpy, [0, 1.57, 0])


def test_cartesian_target_is_transformed_from_robot_base_to_world():
    pose = base_pose_to_world([1, 2, 3], [0, 0, 90], [0.5, 0, 0.2], [0, 0, 0])
    np.testing.assert_allclose(pose[:3, 3], [1, 2.5, 3.2], atol=1e-7)


@pytest.mark.parametrize("line", ["joints 0 0 0", "cartesian 0 0 0 0", "mode unknown"])
def test_rejects_invalid_commands(line):
    with pytest.raises(ValueError):
        parse_command(line)


def test_controller_sends_joint_target_to_position_controller():
    robot = FakeRobot()
    controller = RobotController(robot, [1, 2, 3, 4, 5, 6, 7],
                                 {"position": [1, 2, 3], "euler": [0, 0, 0]})
    controller.apply(JointCommand((0, 1, 2, 3, 4, 5, 6)))
    assert robot.calls[0][0] == "control"
    np.testing.assert_array_equal(robot.calls[0][1], [0, 1, 2, 3, 4, 5, 6])


def test_configures_position_controller_for_all_arm_dofs():
    robot = FakeRobot()
    configure_robot_control(robot, [1, 2, 3, 4, 5, 6, 7])
    assert [call[0] for call in robot.calls] == ["kp", "kv", "force"]


def test_controller_sends_cartesian_target_to_ik_in_world_frame():
    robot = FakeRobot()
    controller = RobotController(robot, list(range(7)),
                                 {"position": [1, 2, 3], "euler": [0, 0, 0]}, "tcp")
    controller.apply(CartesianCommand((0.5, 0, 0.2), (0, 0, 0)))
    ik = next(call[1] for call in robot.calls if call[0] == "ik")
    np.testing.assert_allclose(ik["pos"], [1.5, 2, 3.2])
    assert ik["link"] == "tcp"


def test_cartesian_ik_accepts_componentwise_genesis_tolerances():
    robot = FakeRobot()
    robot.inverse_kinematics = lambda **kwargs: (np.zeros(7), np.full(6, 0.0004))
    controller = SceneMotionController(robot, list(range(7)), motion_config())
    controller.apply(CartesianCommand((0.5, 0, 0.2), (0, 0, 0)), 50)


def test_scene_motion_controller_plans_gui_cartesian_command_at_selected_speed():
    robot = FakeRobot()
    controller = SceneMotionController(
        robot,
        list(range(7)),
        {
            "scene": {"mode": "with_gripper", "dt": 0.1},
            "robot": {
                "urdf": "iiwa_genesis_description/urdf/iiwa_with_gripper.urdf",
                "position": [1, 2, 3],
                "euler": [0, 0, 0],
            },
        },
    )

    controller.apply_gui({
        "type": "cartesian", "values": [0.5, 0, 0.2, 0, 0, 0],
        "speed_percent": 50,
    })
    controller.step()

    ik = next(call[1] for call in robot.calls if call[0] == "ik")
    np.testing.assert_allclose(ik["pos"], [1.5, 2, 3.2])
    assert ik["link"] == "tcp"
    np.testing.assert_allclose(
        robot.calls[-1][1], [0, .0855, .0875, .1135, .122, .157, .157])


class LaggingRobot(FakeRobot):
    def control_dofs_position(self, position, dofs_idx_local=None):
        self.calls.append(("control", np.asarray(position).copy(), dofs_idx_local))


def motion_config(mode="with_gripper"):
    return {
        "scene": {"mode": mode, "dt": .1},
        "robot": {
            "urdf": f"iiwa_genesis_description/urdf/iiwa_{mode}.urdf",
            "position": [1, 2, 3], "euler": [0, 0, 90],
        },
    }


@pytest.mark.parametrize("mode,link", [("with_gripper", "tcp"), ("without_gripper", "link7")])
def test_queued_cartesian_motion_advances_despite_lagging_feedback(mode, link):
    robot = LaggingRobot()
    controller = SceneMotionController(robot, list(range(7)), motion_config(mode))
    commands = Queue()
    commands.put({"type": "cartesian", "values": [.5, 0, .2, 0, 0, 0],
                  "speed_percent": 50})
    controller.apply_gui(commands.get_nowait())
    for _ in range(3):
        controller.step()
    targets = [call[1] for call in robot.calls if call[0] == "control"]
    np.testing.assert_allclose(targets[1], [0, .171, .175, .227, .244, .314, .314])
    np.testing.assert_allclose(targets[2], [0, .2, .2625, .3405, .366, .471, .471])
    ik = next(call[1] for call in robot.calls if call[0] == "ik")
    assert ik["link"] == link
    assert ik["return_error"] is True
    assert ik["pos_tol"] == .0005
    assert ik["rot_tol"] == .005
    np.testing.assert_allclose(ik["pos"], [1, 2.5, 3.2])
    np.testing.assert_allclose(ik["quat"], [2**-.5, 0, 0, 2**-.5], atol=1e-7)


def test_queued_stop_freezes_commanded_target_and_next_move_starts_there():
    robot = LaggingRobot()
    controller = SceneMotionController(robot, list(range(7)), motion_config())
    commands = Queue()
    commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50})
    controller.apply_gui(commands.get_nowait())
    controller.step()
    controller.step()
    frozen = np.array([.171, .171, .175, .227, .244, .314, .314])
    commands.put({"type": "stop"})
    controller.apply_gui(commands.get_nowait())
    for _ in range(3):
        assert controller.step() is True
        np.testing.assert_allclose(robot.calls[-1][1], frozen)
    controller.apply(JointCommand((0,) * 7))
    controller.step()
    np.testing.assert_allclose(robot.calls[-1][1], np.zeros(7), atol=1e-15)


@pytest.mark.parametrize("target", [[4] * 7, [-4] * 7, [0] * 6, [np.nan] * 7, [np.inf] * 7])
@pytest.mark.parametrize("mode", ["with_gripper", "without_gripper"])
def test_invalid_joint_target_preserves_active_motion(target, mode):
    robot = LaggingRobot()
    controller = SceneMotionController(robot, list(range(7)), motion_config(mode))
    controller.apply(JointCommand((1,) * 7), 50)
    controller.step()
    with pytest.raises(ValueError):
        controller.apply_gui({"type": "joints", "values": target, "speed_percent": 100})
    controller.step()
    np.testing.assert_allclose(robot.calls[-1][1], [.171, .171, .175, .227, .244, .314, .314])


@pytest.mark.parametrize("index", range(7))
@pytest.mark.parametrize("boundary", [-.25, .5])
def test_selected_urdf_position_limits_apply_to_each_joint(tmp_path, index, boundary):
    cfg = motion_config()
    tree = ET.parse(cfg["robot"]["urdf"])
    limit = tree.find(f"joint[@name='joint{index + 1}']/limit")
    limit.set("lower", "-.25")
    limit.set("upper", ".5")
    cfg["robot"]["urdf"] = tmp_path / "robot.urdf"
    tree.write(cfg["robot"]["urdf"])
    robot = LaggingRobot()
    controller = SceneMotionController(robot, list(range(7)), cfg)
    target = np.zeros(7)
    target[index] = boundary
    controller.apply(JointCommand(tuple(target)))
    for _ in range(4):
        controller.step()
    np.testing.assert_allclose(robot.calls[-1][1], target)
    invalid = target.copy()
    invalid[index] += -.001 if boundary < 0 else .001
    with pytest.raises(ValueError, match="URDF"):
        controller.apply(JointCommand(tuple(invalid)))
    assert controller.step() is True
    np.testing.assert_allclose(robot.calls[-1][1], target)


@pytest.mark.parametrize("result", [
        (np.zeros(7), [.0006, 0, 0, 0, 0, 0]),
        (np.zeros(7), [0, 0, 0, .006, 0, 0]),
    (np.zeros(7), [np.nan] * 6),
    (np.zeros(7), [np.inf] * 6),
    (np.zeros(7), [0] * 5),
    (np.full(7, 4), np.zeros(6)),
    (np.full(7, np.nan), np.zeros(6)),
    Exception("Genesis solver failed"),
])
def test_failed_ik_is_recoverable_and_preserves_active_motion(result, monkeypatch):
    robot = LaggingRobot()
    controller = SceneMotionController(robot, list(range(7)), motion_config())
    controller.apply(JointCommand((1,) * 7), 50)
    controller.step()

    def inverse_kinematics(**kwargs):
        if isinstance(result, Exception):
            raise result
        return result

    monkeypatch.setattr(robot, "inverse_kinematics", inverse_kinematics)
    with pytest.raises(ValueError):
        controller.apply(CartesianCommand((100, 0, 0), (0, 0, 0)))
    assert controller.mode == "joints"
    controller.step()
    np.testing.assert_allclose(robot.calls[-1][1], [.171, .171, .175, .227, .244, .314, .314])


@pytest.fixture
def cli_runtime(monkeypatch, tmp_path):
    from iiwa_genesis import cli, control_gui, preview, runtime, scene

    robot = LaggingRobot()
    robot.get_joint = lambda name: SimpleNamespace(dofs_idx_local=[int(name[-1]) - 1])
    terminal_queue = Queue()
    gui = SimpleNamespace(commands=Queue(), diagnostics=Queue(), status="running")
    events = []
    scheduled = {}

    def poll_terminal():
        try:
            return terminal_queue.get_nowait()
        except Empty:
            return None

    terminal = SimpleNamespace(start=lambda: None, poll=poll_terminal)
    gui.start = lambda: gui
    gui.close = lambda: events.append("gui closed")
    fake_preview = SimpleNamespace(close=lambda: events.append("preview closed"),
                                   stop=SimpleNamespace(is_set=lambda: False),
                                   submit=lambda *args: None)
    steps = []

    def step():
        steps.append(robot.calls[-1][1].copy())
        if len(steps) in scheduled:
            scheduled[len(steps)]()

    fake_scene = SimpleNamespace(step=step, viewer=SimpleNamespace(is_alive=lambda: True))
    camera = SimpleNamespace(name="overhead", cfg={"fps": 10, "max_range": 6},
                             read=lambda timestamp: events.append(("frame", timestamp)))
    gs = SimpleNamespace(cpu="cpu", backend="cpu", __version__="fake",
                         init=lambda **kwargs: None, destroy=lambda: events.append("destroy"))
    cfg = motion_config()
    cfg["scene"].update(viewer=False, backend="cpu", seed=0)
    cfg["output"] = dict(save_frames=False, preview=True, directory=tmp_path, pointcloud=False)
    monkeypatch.setattr(cli, "load_config", lambda path: cfg)
    monkeypatch.setattr(runtime, "import_genesis", lambda: gs)
    monkeypatch.setattr(scene, "build_scene", lambda *args: (fake_scene, robot, [camera]))
    monkeypatch.setattr(cli, "TerminalInput", lambda: terminal)
    monkeypatch.setattr(control_gui, "ControlGui", lambda: gui)
    monkeypatch.setattr(preview, "Preview", lambda: fake_preview)
    monkeypatch.setattr(cli.time, "sleep", lambda duration: None)
    monkeypatch.setattr("sys.argv", ["run_scene.py", "--viewer", "--steps", "3",
                                     "--interactive", "--control-window"])
    return SimpleNamespace(run=cli.main, robot=robot, gui=gui, terminal=terminal_queue,
                           events=events, steps=steps, scheduled=scheduled, preview=fake_preview)


@pytest.mark.parametrize("mode", ["with_gripper", "without_gripper"])
@pytest.mark.parametrize("side", ["lower", "upper"])
@pytest.mark.parametrize("outward", [False, True])
def test_cartesian_ik_clips_float32_joint_boundaries(mode, side, outward, monkeypatch):
    robot = LaggingRobot()
    controller = SceneMotionController(robot, list(range(7)), motion_config(mode))
    boundary = getattr(controller, side).copy()
    result = boundary.astype(np.float32)
    if outward:
        result = np.nextafter(result, np.float32(-np.inf if side == "lower" else np.inf))
    monkeypatch.setattr(robot, "inverse_kinematics", lambda **kwargs: (result, np.zeros(6)))

    controller.apply(CartesianCommand((.5, 0, .2), (0, 0, 0)))
    for _ in range(40):
        controller.step()

    expected = np.array([2.97, 2.09, 2.97, 2.09, 2.97, 2.09, 3.05] if outward else [
        2.97, 2.0899999141693115, 2.97, 2.0899999141693115,
        2.97, 2.0899999141693115, 3.049999952316284])
    if side == "lower":
        expected = -expected
    np.testing.assert_array_equal(robot.calls[-1][1], expected)
    assert (robot.calls[-1][1] >= controller.lower).all()
    assert (robot.calls[-1][1] <= controller.upper).all()
    np.testing.assert_array_equal(getattr(controller, side), boundary)


@pytest.mark.parametrize("index", range(7))
@pytest.mark.parametrize("side,offset", [("lower", -1e-5), ("upper", 1e-5)])
def test_cartesian_ik_rejects_real_boundary_violation(index, side, offset, monkeypatch):
    robot = LaggingRobot()
    controller = SceneMotionController(robot, list(range(7)), motion_config())
    controller.apply(JointCommand((1,) * 7), 50)
    controller.step()
    result = np.zeros(7)
    result[index] = getattr(controller, side)[index] + offset
    monkeypatch.setattr(robot, "inverse_kinematics", lambda **kwargs: (result, np.zeros(6)))

    with pytest.raises(ValueError, match="URDF"):
        controller.apply(CartesianCommand((.5, 0, .2), (0, 0, 0)))
    assert controller.mode == "joints"
    controller.step()
    np.testing.assert_allclose(robot.calls[-1][1], [.171, .171, .175, .227, .244, .314, .314])


class SustainedQueue:
    def __init__(self, app, value):
        self.app = app
        self.value = value
        self.reads = {}

    def get_nowait(self):
        tick = len(self.app.steps)
        self.reads[tick] = self.reads.get(tick, 0) + 1
        assert self.reads[tick] < 1000, "input processing starved physics"
        return self.value


@pytest.mark.parametrize("channel", ["terminal", "commands", "diagnostics"])
def test_cli_sustained_input_yields_to_physics_and_frames(cli_runtime, channel, monkeypatch):
    from iiwa_genesis import cli

    app = cli_runtime
    value = "joints 1 1 1 1 1 1 1" if channel == "terminal" else {
        "type": "joints", "values": [1] * 7, "speed_percent": 100}
    queue = SustainedQueue(app, value)
    if channel == "terminal":
        monkeypatch.setattr(cli, "TerminalInput", lambda: SimpleNamespace(
            start=lambda: None, poll=queue.get_nowait))
    else:
        setattr(app.gui, channel, queue)
        app.terminal.put("joints 1 1 1 1 1 1 1")

    app.run()

    assert len(app.steps) == 3
    assert set(queue.reads) == {0, 1, 2}
    assert len([event for event in app.events if isinstance(event, tuple)]) == 3
    assert (app.steps[-1] > app.steps[0]).all()


@pytest.mark.parametrize("quit_line", ["quit", " EXIT "])
def test_cli_quit_exits_before_pending_movement_gui_or_physics(cli_runtime, quit_line):
    app = cli_runtime
    app.terminal.put("joints 1 1 1 1 1 1 1")

    def request_quit():
        app.terminal.put("cartesian .5 0 .2 0 0 0")
        app.terminal.put(quit_line)
        app.gui.commands.put({"type": "joints", "values": [-1] * 7, "speed_percent": 100})
        app.gui.diagnostics.put({"message": "pending diagnostic"})

    app.scheduled[1] = request_quit
    app.run()

    assert len(app.steps) == 1
    assert not any(call[0] == "ik" for call in app.robot.calls)
    assert app.gui.commands.qsize() == 1
    assert app.gui.diagnostics.qsize() == 1
    assert len([event for event in app.events if isinstance(event, tuple)]) == 1
    assert app.events[-3:] == ["gui closed", "preview closed", "destroy"]


def test_cli_stop_preempts_pending_ik_from_both_inputs_and_allows_later_move(cli_runtime):
    app = cli_runtime
    app.terminal.put("cartesian .5 0 .2 0 0 0")
    app.gui.commands.put({"type": "cartesian", "values": [.5, 0, .2, 0, 0, 0],
                          "speed_percent": 100})
    app.gui.commands.put({"type": "stop"})
    app.scheduled[1] = lambda: app.gui.commands.put({
        "type": "joints", "values": [1] * 7, "speed_percent": 50})
    app.run()

    assert not any(call[0] == "ik" for call in app.robot.calls)
    np.testing.assert_array_equal(app.steps[0], np.zeros(7))
    np.testing.assert_allclose(app.steps[1], [.0855, .0855, .0875, .1135, .122, .157, .157])


def test_cli_stop_defers_already_queued_later_move_until_next_tick(cli_runtime):
    app = cli_runtime
    app.gui.commands.put({"type": "stop"})
    app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50})
    app.run()

    np.testing.assert_array_equal(app.steps[0], np.zeros(7))
    np.testing.assert_allclose(app.steps[1], [.0855, .0855, .0875, .1135, .122, .157, .157])
    np.testing.assert_allclose(app.steps[2], [.171, .171, .175, .227, .244, .314, .314])


@pytest.mark.parametrize("source", ["terminal", "gui"])
def test_cli_bounded_batches_preserve_backlogged_commands(cli_runtime, source, monkeypatch):
    app = cli_runtime
    for _ in range(100):
        if source == "terminal":
            app.terminal.put("help")
        else:
            app.gui.commands.put({"type": "close"})
    if source == "terminal":
        app.terminal.put("joints 1 1 1 1 1 1 1")
    else:
        app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 100})

    # Run enough ticks to reach the final command while checking frames continue.
    monkeypatch.setattr("sys.argv", ["run_scene.py", "--viewer", "--steps", "12",
                                     "--interactive", "--control-window"])
    app.run()

    np.testing.assert_array_equal(app.steps[0], np.zeros(7))
    assert (app.steps[-1] > 0).all()
    assert len(app.steps) == 12


def test_cli_batch_parses_set_after_mode_change_and_invalid_quit(cli_runtime, capsys):
    app = cli_runtime
    app.terminal.put("quit extra")
    app.terminal.put("mode cartesian")
    app.terminal.put("set .5 0 .2 0 0 0")
    app.run()

    assert len(app.steps) == 3
    assert len([call for call in app.robot.calls if call[0] == "ik"]) == 1
    assert "quit takes no values" in capsys.readouterr().out


@pytest.mark.parametrize("side,offset", [("lower", -1e-7), ("upper", 1e-7)])
def test_joint_commands_keep_strict_limits_for_tiny_excursions(side, offset):
    robot = LaggingRobot()
    controller = SceneMotionController(robot, list(range(7)), motion_config())
    target = getattr(controller, side) + offset
    with pytest.raises(ValueError, match="URDF"):
        controller.apply(JointCommand(tuple(target)))


class BrokenQueue:
    def __init__(self, error):
        self.error = error
        self.reads = 0

    def get_nowait(self):
        self.reads += 1
        raise self.error("queue disconnected")


@pytest.mark.parametrize("channel", ["commands", "diagnostics"])
@pytest.mark.parametrize("error", [EOFError, OSError, ValueError])
def test_cli_disables_only_broken_gui_queue_and_keeps_terminal_and_frames(cli_runtime, channel, error, capsys):
    app = cli_runtime
    broken = BrokenQueue(error)
    setattr(app.gui, channel, broken)
    app.terminal.put("joints 1 1 1 1 1 1 1")
    app.scheduled[1] = lambda: app.terminal.put("joints -1 -1 -1 -1 -1 -1 -1")
    if channel == "diagnostics":
        app.scheduled[2] = lambda: app.gui.commands.put({"type": "stop"})
    else:
        app.gui.diagnostics.put({"message": "child startup failed"})
    app.run()
    assert broken.reads == 1
    assert len(app.steps) == 3
    np.testing.assert_allclose(app.steps[1], np.zeros(7), atol=1e-15)
    if channel == "diagnostics":
        np.testing.assert_allclose(app.steps[2], app.steps[1])
    else:
        assert (app.steps[2] < 0).all()
    assert len([event for event in app.events if isinstance(event, tuple)]) == 3
    assert app.events[-3:] == ["gui closed", "preview closed", "destroy"]
    assert "queue disconnected" in capsys.readouterr().out


def test_cli_rejects_gui_command_but_processes_following_valid_command(cli_runtime, capsys):
    app = cli_runtime
    app.gui.commands.put({"type": "joints", "values": [4] * 7, "speed_percent": 100})
    app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50})
    app.run()
    np.testing.assert_allclose(app.steps[0], [.0855, .0855, .0875, .1135, .122, .157, .157])
    np.testing.assert_allclose(app.steps[2], [.2565, .2565, .2625, .3405, .366, .471, .471])
    assert "Command error" in capsys.readouterr().out


@pytest.mark.parametrize("source", ["terminal", "gui"])
def test_cli_recovers_from_genesis_ik_exception_without_losing_motion(cli_runtime, source, monkeypatch, capsys):
    app = cli_runtime

    def fail(**kwargs):
        raise Exception("Genesis IK failure")

    monkeypatch.setattr(app.robot, "inverse_kinematics", fail)
    app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50})
    if source == "terminal":
        app.scheduled[1] = lambda: app.terminal.put("cartesian 100 0 0 0 0 0")
    else:
        app.scheduled[1] = lambda: app.gui.commands.put({
            "type": "cartesian", "values": [100, 0, 0, 0, 0, 0], "speed_percent": 100})
    app.scheduled[2] = lambda: app.gui.commands.put({"type": "stop"})
    app.run()
    np.testing.assert_allclose(app.steps[1], [.171, .171, .175, .227, .244, .314, .314])
    np.testing.assert_allclose(app.steps[2], app.steps[1])
    assert app.events[-1] == "destroy"
    output = capsys.readouterr().out
    assert "Command error" in output
    assert "Genesis IK failure" in output


@pytest.mark.parametrize("failure", ["startup", "close", "preview_close"])
def test_cli_gui_failures_always_destroy_genesis(cli_runtime, failure, capsys):
    app = cli_runtime

    def fail():
        raise RuntimeError(failure)

    if failure == "startup":
        app.gui.start = fail
    elif failure == "close":
        app.gui.close = fail
    else:
        app.preview.close = fail
    app.terminal.put("joints 1 1 1 1 1 1 1")
    app.run()
    assert len(app.steps) == 3
    assert app.events[-1] == "destroy"
    if failure != "preview_close":
        assert "preview closed" in app.events
    assert failure in capsys.readouterr().out


def test_velocity_limited_motion_scales_each_urdf_velocity_limit():
    motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)

    motion.set_target(np.full(7, 10.0), 50)
    position, complete = motion.step(np.zeros(7), 1.0)

    np.testing.assert_allclose(position, np.asarray(IIWA_VELOCITY_LIMITS) * 0.5)
    assert complete is False


@pytest.mark.parametrize(
    "speed_percent", [np.nan, np.inf, -1, 0, 101, "50", "fast", True,
                       np.bool_(False), 1 + 0j, None])
def test_velocity_limited_motion_rejects_invalid_speed(speed_percent):
    motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)

    with pytest.raises(ValueError):
        motion.set_target(np.ones(7), speed_percent)


def test_velocity_limited_motion_rejects_numeric_string_speed_percent():
    motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)

    with pytest.raises(ValueError):
        motion.set_target(np.full(7, 10.0), "50")


@pytest.mark.parametrize(
    "dt", ["1", "fast", True, np.bool_(False), 1 + 0j, None, np.nan,
            np.inf, -1])
def test_velocity_limited_motion_rejects_invalid_dt(dt):
    motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)
    motion.set_target(np.ones(7), 50)

    with pytest.raises(ValueError):
        motion.step(np.zeros(7), dt)


def test_velocity_limited_motion_applies_mixed_joint_caps_independently():
    limits = np.asarray([0.5, 2.0, 1.0, 3.0, 0.25, 4.0, 1.5])
    motion = VelocityLimitedMotion(limits)
    motion.set_target(np.full(7, 10.0), 50)

    position, complete = motion.step(np.zeros(7), 1.0)

    np.testing.assert_allclose(position, limits * 0.5)
    assert complete is False


def test_velocity_limited_motion_clamps_each_joint_and_reports_completion():
    motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)
    target = np.asarray([1, -1, 0.1, -0.1, 0.01, -0.01, 0.001])
    motion.set_target(target, 100)

    position, complete = motion.step(np.zeros(7), 10.0)

    np.testing.assert_allclose(position, target)
    assert complete is True
    assert motion.complete is True


def test_velocity_limited_motion_stop_holds_current_position():
    motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)
    current = np.arange(7, dtype=float)
    motion.set_target(np.ones(7), 50)

    motion.stop(current)
    position, complete = motion.step(current + 10, 1.0)

    np.testing.assert_allclose(position, current)
    assert complete is True


def test_gui_queue_sends_picklable_plain_joint_command_data():
    queue = GuiCommandQueue(get_context("spawn").Queue())

    queue.send_joint([0, 1, 2, 3, 4, 5, 6], 40)
    command = queue.poll()

    assert isinstance(command, dict)
    assert command == {"type": "joints", "values": [0., 1., 2., 3., 4., 5., 6.],
                       "speed_percent": 40.}
    assert pickle.loads(pickle.dumps(command)) == command
    queue.close()


def test_gui_queue_sends_stop_and_close_commands():
    queue = GuiCommandQueue(get_context("spawn").Queue())

    queue.send_stop()
    queue.send_close()

    assert queue.poll() == {"type": "stop"}
    assert queue.poll() == {"type": "close"}
    assert queue.poll() is None
    queue.close()


def test_gui_command_from_fields_validates_mode_specific_field_counts():
    command = command_from_fields("cartesian", [".1", ".2", ".3", "0", "1", "2"], "75")

    assert command == {"type": "cartesian", "values": [.1, .2, .3, 0., 1., 2.],
                       "speed_percent": 75.}
    with pytest.raises(ValueError):
        command_from_fields("joints", ["0"] * 6, "50")
    with pytest.raises(ValueError):
        command_from_fields("other", ["0"] * 7, "50")


def test_gui_zero_speed_send_becomes_stop_command():
    assert command_from_fields("joints", ["0"] * 7, "0") == {"type": "stop"}


@pytest.mark.parametrize("speed", ["nan", "inf", "-inf", "101", "-1"])
def test_gui_command_rejects_nonfinite_or_out_of_range_speed(speed):
    with pytest.raises(ValueError):
        command_from_fields("joints", ["0"] * 7, speed)


def test_gui_queue_sends_cartesian_command_data():
    queue = GuiCommandQueue(get_context("spawn").Queue())

    queue.send_cartesian([.1, .2, .3, 0, 1, 2], 25)

    assert queue.poll() == {"type": "cartesian", "values": [.1, .2, .3, 0., 1., 2.],
                            "speed_percent": 25.}
    queue.close()


def test_gui_queue_rejects_nonfinite_speed_and_maps_zero_to_stop():
    queue = GuiCommandQueue(get_context("spawn").Queue())

    with pytest.raises(ValueError):
        queue.send_joint([0] * 7, float("nan"))
    queue.send_cartesian([0] * 6, 0)
    assert queue.poll() == {"type": "stop"}
    queue.close()


def test_control_gui_can_close_before_start_and_reports_closed_status():
    gui = ControlGui()

    gui.close()

    assert gui.status == "closed"


class FakeProcess:
    def __init__(self, alive):
        self.alive = iter(alive)
        self.joins = []
        self.terminates = 0

    def is_alive(self):
        return next(self.alive)

    def join(self, timeout):
        self.joins.append(timeout)

    def terminate(self):
        self.terminates += 1


def test_started_child_observer_destroys_root_after_parent_close():
    class Root:
        def __init__(self):
            self.destroyed = False
            self.scheduled = []

        def destroy(self):
            self.destroyed = True

        def after(self, delay, callback):
            self.scheduled.append((delay, callback))

    root = Root()
    closed = get_context("spawn").Event()

    _observe_parent_close(root, closed)
    assert root.destroyed is False
    assert len(root.scheduled) == 1

    closed.set()
    root.scheduled[0][1]()

    assert root.destroyed is True


def test_gui_close_joins_started_child_without_terminating_when_it_exits():
    gui = ControlGui()
    gui.process = FakeProcess([True, False])
    gui._started = True

    gui.close()

    assert gui.process.joins == [2]
    assert gui.process.terminates == 0


def test_gui_close_terminates_started_child_only_as_fallback():
    gui = ControlGui()
    gui.process = FakeProcess([True, True, False])
    gui._started = True

    gui.close()

    assert gui.process.joins == [2, 2]
    assert gui.process.terminates == 1