orisys-app/apps/autocal/tests/test_kinematics.py

"""The simulated 5-axis solve: residuals, the stage envelope, and the frames.

The shipped solver is **closed form** (see ``test_ik_analytical.py``, which checks
it against both the sweep and an exhaustive brute force). What this module covers
is everything around it: the mount mapping, the envelope, the frames, the reported
contact point, and the selectable sweep path.

The solver is the part of this feature that is easiest to get wrong and hardest
to notice — a wrong derivation still returns two plausible angles — so its results
are still checked against a brute-force search, just not with a fine grid over
seven normals when the closed form now guarantees exactness on its own.
"""

from __future__ import annotations

import dataclasses

import numpy as np
import pytest
from orisys_autocal.motion.kinematics import (
    ALIGNED_TOL_DEG,
    Branch,
    PlatformStack,
    Pose,
    residual_for,
    rotation_about,
)
from orisys_autocal.sensor.frames import (
    describe,
    mount_rotation,
    to_platform,
    to_sensor,
)

#: Normals to exercise, including the awkward ones.
NORMALS = (
    np.array([0.0, 1.0, 0.0]),  # the patch's nominal normal
    np.array([0.0, -1.0, 0.0]),  # straight into the tool
    np.array([1.0, 0.0, 0.0]),
    np.array([0.0, 0.0, 1.0]),
    np.array([0.3, 0.5, -0.81]),
    np.array([-0.6, 0.2, 0.77]),
    np.array([0.0, 0.3, 0.954]),
)


def _brute_force_best(
    stack: PlatformStack, unit_platform: np.ndarray, *, step: float = 0.25
) -> float:
    """The lowest residual any pose in the envelope achieves — by search.

    Kept, but no longer run over the whole grid for every normal: the shipped
    solver is closed form and ``test_ik_analytical.py`` checks it against an
    exhaustive sweep. This survives as the reference for the *sweep* path, which
    is still selectable and still needs a floor to be measured against.
    """
    tool = stack.tool_direction
    yaw_limit = min(180.0, stack.yaw_limit_deg)
    pitch_limit = min(360.0, stack.pitch_limit_deg)
    best = 1e9
    for yaw in np.arange(-yaw_limit, yaw_limit + step, step):
        turned = stack.yaw_rotation(float(yaw)) @ unit_platform
        for pitch in np.arange(-pitch_limit, pitch_limit + step, step):
            rotated = stack.pitch_rotation(float(pitch)) @ turned
            residual = float(np.degrees(np.arccos(np.clip(rotated @ tool, -1.0, 1.0))))
            best = min(best, residual)
    return best


# ------------------------------------------------------------------- the mount


def test_the_axis_map_is_a_rotation():
    """A mapping that mirrors the part is worse than none: it looks fine."""
    matrix = mount_rotation()
    assert float(np.linalg.det(matrix)) == pytest.approx(1.0)
    assert np.allclose(matrix @ matrix.T, np.eye(3))


def test_a_crown_point_moves_along_y_and_does_not_yaw():
    """The top face used to be a Y move. A 1.6° sideways lean must not yaw 90°."""
    stack = PlatformStack(sensor_offset_mm=(0.0, 0.0, 8.0), pitch_limit_deg=90.0)
    point = np.array([0.150, 8.355, 5.483])
    normal = np.array([0.028, 1.0, 0.0])
    normal = normal / np.linalg.norm(normal)
    pose = stack.solve(point, normal)
    assert pose.yaw_deg == pytest.approx(0.0, abs=1e-6)
    assert pose.pitch_deg == pytest.approx(0.0, abs=1e-6)
    assert pose.table_xy_mm[1] == pytest.approx(-5.483, abs=0.02)
    assert abs(pose.table_xy_mm[0]) < 0.5
    assert pose.branch_reason == "crown"
    assert pose.reachable
    assert pose.reason == ""


def test_a_sideways_normal_still_yaws():
    """A lean the pitch stage cannot nod away still takes the exact yaw."""
    stack = PlatformStack()
    normal = np.array([0.7, 0.7, 0.1])
    normal = normal / np.linalg.norm(normal)
    pose = stack.solve(np.zeros(3), normal)
    assert pose.residual_deg < 1e-4
    assert abs(pose.yaw_deg) > 20.0
    assert pose.branch_reason != "crown"


def test_positive_stage_commands_match_the_drives():
    """Bench 2026-10-15, one axis at a time from the commissioned zero.

    Yaw +10° turned the tip clockwise viewed from above. Positive pitch nods the
    dome toward the operator (platform −Y). The preview uses this same rotation,
    so a commanded angle draws the motion the drive will make.
    """
    stack = PlatformStack()
    assert stack.yaw_axis == "z" and stack.pitch_axis == "x"
    assert np.allclose(stack.platform_rotation(0.0, 0.0), np.eye(3))

    tip = stack.platform_rotation(10.0, 0.0) @ np.array([0.0, 1.0, 0.0])
    # Clockwise from above: the tip, pointing at +Y, swings toward +X.
    assert tip[0] > 0.0
    assert tip[1] > 0.0
    assert abs(tip[2]) < 1e-12

    dome = stack.platform_rotation(0.0, 10.0) @ np.array([0.0, 0.0, 1.0])
    # A nod, not a roll: the face leans toward −Y and does not pick up +X.
    assert dome[1] < 0.0
    assert abs(dome[0]) < 1e-12
    assert dome[2] > 0.0


def test_the_sensor_y_maps_to_the_platform_z():
    """The bench's frame relation, stated once so it can be corrected once."""
    assert np.allclose(to_platform(np.array([0.0, 1.0, 0.0])), [0.0, 0.0, 1.0])


def test_the_sensor_x_maps_to_the_platform_minus_x():
    """Both table axes were reversed on the bench. −X keeps the map a rotation."""
    assert np.allclose(to_platform(np.array([1.0, 0.0, 0.0])), [-1.0, 0.0, 0.0])


def test_the_sensor_z_maps_to_the_platform_y():
    """Shell length and machine Y point the same way, so the tip is +Y."""
    assert np.allclose(to_platform(np.array([0.0, 0.0, 1.0])), [0.0, 1.0, 0.0])


def test_a_tip_side_point_commands_negative_table_y():
    """Bringing a point toward the tip under the tool moves the table to −Y."""
    pose = PlatformStack().solve(np.array([0.0, 8.0, 6.0]), np.array([0.0, 1.0, 0.0]))
    assert pose.table_xy_mm[1] == pytest.approx(-6.0)
    side = PlatformStack().solve(np.array([2.0, 8.0, 0.0]), np.array([0.0, 1.0, 0.0]))
    assert side.table_xy_mm[0] == pytest.approx(2.0)


def test_the_mount_round_trips():
    vector = np.array([0.3, -0.5, 0.81])
    assert np.allclose(to_sensor(to_platform(vector)), vector)


def test_a_mirroring_map_is_refused():
    """A map that reflects the part is worse than none: the numbers look sane."""
    with pytest.raises(ValueError) as excinfo:
        mount_rotation({"x": "x", "y": "y", "z": "-z"})  # det = -1
    assert "not a rotation" in str(excinfo.value)


def test_an_incomplete_map_is_refused():
    with pytest.raises(ValueError) as excinfo:
        mount_rotation({"x": "x", "y": "z"})
    assert "no entry" in str(excinfo.value)


def test_the_mapping_is_described_for_the_ui():
    assert describe() == "sensor x->-x, y->z, z->y"


def test_swapping_the_mapping_changes_the_answer():
    """The one-line edit the bench may need, proven to actually do something."""
    # Asymmetric on purpose: a point on a coordinate axis maps to the same place
    # under both rotations, which would make this test pass without the mapping
    # doing anything at all.
    point = np.array([1.0, 5.0, 2.0])
    default = PlatformStack()
    # A different, still right-handed mapping: sensor x->y, y->z, z->x.
    swapped = PlatformStack(axis_map={"x": "y", "y": "z", "z": "x"})
    assert float(np.linalg.det(swapped.mount)) == pytest.approx(1.0)
    assert not np.allclose(
        default.point_to_platform(point), swapped.point_to_platform(point)
    )


# --------------------------------------------------------------- the solving


def test_the_bench_envelope_is_the_default():
    stack = PlatformStack()
    # Confirmed at the bench and against the trunnion drawing: the vertical stage
    # turns a full half-turn either way, the horizontal one turns continuously.
    assert stack.yaw_limit_deg == 180.0
    assert stack.pitch_limit_deg == 360.0


def test_the_nominal_normal_needs_no_rotation_and_lands_on_plus_z():
    """The polarity check, and the anchor for the whole solve.

    The sensor sits at the bottom of its shell with the sensing face up, so the
    face points along ``+tool_axis`` and the nominal pose is *no rotation at all*.

    This is the test that was missing. ``approach_sign`` was ``-1``, aiming every
    normal at ``-Z``; the solver dutifully used pitch 180 and turned the sensor
    over, and nothing noticed because the residual is sign-blind — a face turned
    fully over still scored 0.0000 degrees. Assert the direction landed in, not
    just the angle between the lines.
    """
    stack = PlatformStack()
    assert stack.approach_sign == 1
    assert np.allclose(stack.tool_direction, [0.0, 0.0, 1.0])

    nominal = np.array([0.0, 1.0, 0.0])  # sensor +Y, the direction the face looks
    unit = stack.normal_to_platform(nominal)
    unit = unit / np.linalg.norm(unit)
    pose = stack.solve(np.array([0.0, 5.0, 0.0]), nominal)

    landed = stack.platform_rotation(pose.yaw_deg, pose.pitch_deg) @ unit
    landed = landed / np.linalg.norm(landed)
    assert landed[2] > 0.0, f"the face was aimed at {np.round(landed, 3)}, not +Z"
    # And it gets there without turning: the commissioned pose is the identity.
    assert abs(pose.pitch_deg) < 1e-6
    assert abs(pose.yaw_deg) < 1e-6
    assert pose.upright


def test_the_sweep_is_never_worse_than_a_brute_force_search():
    """The sweep path still needs a floor, but one normal is enough to pin it.

    The shipped solver is closed form and ``test_ik_analytical.py`` checks it
    against an exhaustive sweep over many normals. This keeps the *search* path
    honest with one coarse brute force, rather than seven fine ones — the fine
    version cost about three seconds per normal to confirm something the closed
    form now guarantees outright.
    """
    stack = PlatformStack()
    normal = NORMALS[4]
    unit = stack.normal_to_platform(normal)
    unit = unit / np.linalg.norm(unit)
    pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal, method="search")
    best = _brute_force_best(stack, unit, step=2.0)
    assert pose.residual_deg <= best + 0.1, (
        f"the sweep returned {pose.residual_deg:.3f} deg where "
        f"{best:.3f} deg is achievable"
    )


# --------------------------------------------- the fast sweep vs the full grid


def _random_units(count: int, seed: int) -> list[np.ndarray]:
    generator = np.random.default_rng(seed)
    vectors = generator.normal(size=(count, 3))
    return [vector / np.linalg.norm(vector) for vector in vectors]


def test_the_fast_sweep_agrees_with_the_full_grid_sweep():
    """The closed-form sweep must find what the exhaustive grid finds.

    The shipped search solves the best pitch at each yaw in closed form, which is
    what made a full patch 14x faster. The exhaustive grid is kept as the slow
    reference precisely so this comparison exists: an optimisation in this file has
    silently cost 68 degrees before, while looking entirely reasonable.
    """
    stack = PlatformStack()
    for unit in _random_units(12, seed=20240517)[:6]:
        fast_yaw, fast_pitch, fast_residual = stack._search_alignment(  # noqa: SLF001
            unit, stack.tool_direction
        )
        ref_yaw, ref_pitch, ref_residual = stack.search_alignment_reference(unit, step=5.0)
        # The reference is quantised to the sweep step and is not polished, so it
        # may be slightly worse; it must never be better than the shipped path.
        assert fast_residual <= ref_residual + 1e-9, (
            f"fast sweep left {fast_residual:.6f} deg where the full grid found "
            f"{ref_residual:.6f} deg (fast {fast_yaw:.3f}/{fast_pitch:.3f}, "
            f"reference {ref_yaw:.3f}/{ref_pitch:.3f})"
        )
        # And the polished answer should be far better than the grid's own step.
        assert fast_residual <= ref_residual + 0.05


def test_the_closed_form_pitch_beats_a_direct_scan():
    """Guard the sign convention the closed form depends on.

    There are two cancellation-free coefficients and picking the wrong one returns
    the antipode: a pitch 180 degrees away that still looks like a number. This is
    the check that caught exactly that, so it is stated directly against a numeric
    scan of the pitch rather than against the rest of the solver.
    """
    stack = PlatformStack()
    for unit in _random_units(6, seed=99):
        yaws = np.array([-80.0, -35.0, -10.0, 0.0, 15.0, 47.0, 90.0])
        closed_form = stack._best_pitch_per_yaw(unit, yaws)  # noqa: SLF001
        for yaw, pitch in zip(yaws, closed_form):
            scan = np.arange(-360.0, 360.0, 0.1)
            yawed = stack._turn(stack.yaw_axis, np.array([float(yaw)]), unit)[0]  # noqa: SLF001
            # The residual is monotone in the dot product, so comparing the dot is
            # enough and avoids a million arccos calls.
            tool = stack.tool_direction
            cosine, sine = np.cos(np.radians(scan)), np.sin(np.radians(scan))
            stationary, first, second, sign_first, sign_second = (
                stack._signed_coefficients(stack.pitch_axis)  # noqa: SLF001
            )
            dot = (
                yawed[stationary] * tool[stationary]
                + (yawed[first] * cosine + sign_first * yawed[second] * sine) * tool[first]
                + (sign_second * yawed[first] * sine + yawed[second] * cosine) * tool[second]
            )
            achieved = stack._cell(unit, float(yaw), float(pitch))  # noqa: SLF001
            achieved_dot = float(np.dot(achieved, tool))
            assert achieved_dot >= float(dot.max()) - 1e-9, (
                f"closed-form pitch {pitch:.3f} deg at yaw {yaw:.1f} reaches dot "
                f"{achieved_dot:.6f}, but a direct scan reaches {dot.max():.6f}"
            )


def _independent_residual(
    stack: PlatformStack, normal_sensor: np.ndarray, yaw_deg: float, pitch_deg: float
) -> float:
    """The residual worked out from raw matrices built here, not by the solver.

    ``residual_for`` shares ``rotation_about`` with the solver, so it cannot catch
    an error in that shared helper. The matrices live in ``_independent_rotation``.
    """
    normal = stack.normal_to_platform(np.asarray(normal_sensor, dtype=np.float64))
    unit = normal / np.linalg.norm(normal)
    turned = _independent_rotation(yaw_deg, pitch_deg) @ unit
    return float(
        np.degrees(np.arccos(np.clip(float(turned @ stack.tool_direction), -1.0, 1.0)))
    )


@pytest.mark.parametrize("normal", NORMALS)
def test_a_returned_pose_actually_achieves_its_residual(normal):
    """Independent of how it was found: apply the angles and measure."""
    stack = PlatformStack()
    pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
    measured = residual_for(stack, normal, pose.yaw_deg, pose.pitch_deg)
    assert measured == pytest.approx(pose.residual_deg, abs=1e-6)


@pytest.mark.parametrize("normal", NORMALS)
def test_the_residual_survives_an_independently_built_rotation(normal):
    """The shared rotation helper cannot be the only thing agreeing with itself."""
    stack = PlatformStack()
    pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
    measured = _independent_residual(stack, normal, pose.yaw_deg, pose.pitch_deg)
    assert measured == pytest.approx(pose.residual_deg, abs=1e-6)


def test_every_sampled_patch_normal_is_reachable_within_the_yaw_limit():
    """The whole patch aligns, and the yaw limit is not what stops it.

    An earlier revision of this solver claimed a quarter of the patch was
    unreachable at 65 deg. That was the solver's bug. The real answer is that the
    360 deg pitch stage does the work and the +-90 deg yaw limit costs at most a
    quarter of a degree anywhere on the patch, so no re-mount is needed.
    """
    stack = PlatformStack()
    worst = 0.0
    for normal in NORMALS:
        pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
        assert pose.ok
        worst = max(worst, pose.residual_deg)
    assert worst < 0.5, f"worst residual across the sampled patch is {worst:.3f} deg"


def test_a_generic_normal_aligns_almost_exactly():
    """Away from the yaw clamp the solver should reach the polish floor."""
    stack = PlatformStack()
    for normal in NORMALS[:6]:
        pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
        assert pose.residual_deg < 0.01, (
            f"normal {normal} left {pose.residual_deg:.4f} deg behind"
        )


def test_the_nominal_normal_needs_no_rotation():
    """Sensor +Y is the platform +Z, so the patch's centre is already aligned."""
    stack = PlatformStack()
    pose = stack.solve(np.array([0.0, 5.0, 0.0]), np.array([0.0, 1.0, 0.0]))
    assert pose.ok
    assert pose.residual_deg == pytest.approx(0.0, abs=1e-6)


def test_a_normal_straight_into_the_tool_is_flagged_indeterminate():
    stack = PlatformStack()
    pose = stack.solve(np.array([0.0, 5.0, 0.0]), np.array([0.0, -1.0, 0.0]))
    assert pose.ok
    assert not pose.yaw_determined
    assert "indeterminate" in pose.reason


def test_a_degenerate_normal_is_refused():
    stack = PlatformStack()
    pose = stack.solve(np.array([0.0, 5.0, 0.0]), np.zeros(3))
    assert not pose.ok
    assert "degenerate" in pose.reason


def test_the_yaw_limit_is_respected():
    stack = PlatformStack(yaw_limit_deg=90.0)
    for normal in NORMALS:
        pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
        assert abs(pose.yaw_deg) <= 90.0 + 1e-6


def test_the_full_yaw_travel_keeps_the_second_solution_of_a_measured_point():
    """±180 is what keeps "cannot reach" from being reported for an aligned pose.

    ``docs/motion-correction-plan.zh.md`` measures a point at (-2.698, 5.186, 18.228)
    with normal (-0.343, 0.699, 0.627). Under the drive's rotation it has two exact
    solutions, -28.681° and +151.319° of yaw, and the second sits outside the ±90
    note this bench was once configured with. At ±180 both branches are reachable,
    so a change in which branch is preferred cannot strand the point on "cannot reach".
    """
    point = np.array([-2.698, 5.186, 18.228])
    normal = np.array([-0.343, 0.699, 0.627])
    wide = PlatformStack(yaw_limit_deg=180.0, pitch_limit_deg=90.0)
    narrow = PlatformStack(yaw_limit_deg=90.0, pitch_limit_deg=90.0)

    unit = wide.setup_normal(normal)
    unit = unit / float(np.linalg.norm(unit))
    wide_branches = wide.alignment_branches(unit, wide.tool_direction)
    narrow_branches = narrow.alignment_branches(unit, narrow.tool_direction)

    assert {round(branch.yaw_deg, 3) for branch in wide_branches} == {-28.681, 151.319}
    assert all(branch.in_range for branch in wide_branches)
    assert sum(branch.in_range for branch in narrow_branches) == 1, (
        "±90 was expected to drop one of the two exact solutions"
    )
    assert wide.solve(point, normal).in_range


def test_a_yaw_limit_past_the_trunnion_travel_is_capped():
    """The stage turns ±180. A stored 360 must not widen the envelope."""
    stack = PlatformStack(yaw_limit_deg=360.0, pitch_limit_deg=90.0)
    assert stack._yaw_limit_effective() == 180.0  # the cap lives here, not in the caller
    for normal in NORMALS:
        pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
        assert abs(pose.yaw_deg) <= 180.0 + 1e-6


def test_the_pitch_limit_is_respected():
    stack = PlatformStack(pitch_limit_deg=45.0)
    for normal in NORMALS:
        pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
        assert abs(pose.pitch_deg) <= 45.0 + 1e-6


def test_tightening_the_envelope_can_only_leave_more_residual():
    """A narrower stage cannot align better than a wider one."""
    normal = np.array([-0.6, 0.2, 0.77])
    point = np.array([0.0, 5.0, 0.0])
    wide = PlatformStack(yaw_limit_deg=180.0).solve(point, normal).residual_deg
    narrow = PlatformStack(yaw_limit_deg=30.0).solve(point, normal).residual_deg
    assert narrow >= wide - 1e-6


def test_an_unbounded_yaw_can_align_arbitrary_normals():
    """The envelope, not the solver, is what leaves residual."""
    stack = PlatformStack(yaw_limit_deg=180.0, pitch_limit_deg=180.0)
    worst = 0.0
    for index in range(40):
        normal = np.random.default_rng(index).normal(size=3)
        normal = normal / np.linalg.norm(normal)
        worst = max(worst, stack.solve(np.array([0.0, 5.0, 0.0]), normal).residual_deg)
    assert worst < 0.5, f"worst residual {worst:.3f} deg with a free yaw"


def test_the_reported_xy_is_the_contact_point_not_the_origin():
    stack = PlatformStack()
    point = np.array([2.0, 5.0, 3.0])
    pose = stack.solve(point, np.array([0.0, 1.0, 0.0]))
    platform = stack.point_to_platform(point)
    assert pose.target_xy_mm[0] == pytest.approx(platform[0])
    assert pose.target_xy_mm[1] == pytest.approx(platform[1])


def test_the_plate_lifts_the_point_along_the_tool_axis():
    stack = PlatformStack()
    point = np.array([1.0, 5.0, 2.0])
    plain = stack.point_to_platform(point)
    lifted = stack.point_to_platform(point, plate_thickness_mm=2.5)
    assert np.allclose(lifted - plain, [0.0, 0.0, 2.5])


def test_the_tool_position_is_the_same_number_the_move_uses():
    """One field for "where the tool is", not two that can drift apart.

    There used to be a separate ``tool_offset_mm`` added to the *reported* contact
    while the drawing left the geometry alone: the panel then reported one landing
    point and drew another, 10 mm away. The reported move, the drawn move and the
    landing check all read ``tool_xy_mm`` now, and this pins that.
    """
    stack = PlatformStack(tool_xy_mm=(10.0, -4.0))
    point = np.array([2.0, 9.0, 1.0])
    normal = np.array([0.3, 0.9, 0.2])
    normal = normal / np.linalg.norm(normal)
    pose = stack.solve(point, normal)
    landed = stack.platform_rotation(pose.yaw_deg, pose.pitch_deg) @ stack.setup_point(point)
    final = landed + np.array([pose.table_xy_mm[0], pose.table_xy_mm[1], 0.0])
    assert final[0] == pytest.approx(10.0, abs=1e-9)
    assert final[1] == pytest.approx(-4.0, abs=1e-9)
    # And there is no second place a tool position could hide.
    assert not hasattr(stack, "tool_offset_mm")


def test_the_reported_xy_is_where_the_point_is_not_where_the_table_goes():
    """Two different numbers, kept apart on purpose."""
    stack = PlatformStack(tool_xy_mm=(3.0, 3.0))
    point = np.array([2.0, 5.0, 3.0])
    stored = stack.setup_point(point)
    pose = stack.solve(point, np.array([0.0, 1.0, 0.0]))
    assert pose.target_xy_mm[0] == pytest.approx(stored[0])
    assert pose.target_xy_mm[1] == pytest.approx(stored[1])
    # The move is what carries that point to the tool, so it is the rest of the way.
    assert pose.table_xy_mm[0] == pytest.approx(3.0 - stored[0])
    assert pose.table_xy_mm[1] == pytest.approx(3.0 - stored[1])


# ------------------------------------------------- the setup and the tool position


def test_the_default_tool_is_above_the_table_origin():
    """The default has to be the origin, or every reported move is off by a patch."""
    stack = PlatformStack()
    assert stack.tool_xy_mm == (0.0, 0.0)
    point = np.array([3.0, 8.0, 1.0])
    pose = stack.solve(point, np.array([0.0, 1.0, 0.0]))
    # With the tool at the origin, the move is the contact's position negated: the
    # table has to bring a point that is over there back to the middle.
    assert pose.table_xy_mm[0] == pytest.approx(-pose.target_xy_mm[0])
    assert pose.table_xy_mm[1] == pytest.approx(-pose.target_xy_mm[1])


def test_the_drive_x_is_opposite_the_model_slide_and_a_little_shorter():
    """The X motor is opposite the model, and the slide is 10/11 of it.

    The preview keeps the full model slide, so the drawn contact stays on the tool.
    """
    from orisys_autocal.sensor.frames import TABLE_X_GAIN, machine_table_xy

    pose = PlatformStack().solve(np.array([3.0, 8.0, 1.0]), np.array([0.0, 1.0, 0.0]))
    drive_x, drive_y = machine_table_xy(pose.table_xy_mm)
    assert drive_x == pytest.approx(-TABLE_X_GAIN * pose.table_xy_mm[0])
    assert drive_y == pytest.approx(pose.table_xy_mm[1])
    assert f"{drive_x:+.3f}" in pose.describe()


def test_the_table_move_lands_the_contact_on_the_tool():
    """The one invariant the whole machine exists to satisfy."""
    stack = PlatformStack(
        tool_xy_mm=(4.0, -7.0),
        sensor_offset_mm=(1.0, 2.0, 3.0),
        sensor_yaw_deg=25.0,
    )
    for point, normal in (
        (np.array([2.0, 9.0, 1.0]), np.array([0.3, 0.9, 0.2])),
        (np.array([-4.0, 6.0, 3.0]), np.array([-0.6, 0.5, 0.4])),
        (np.array([0.0, 11.0, 0.0]), np.array([0.0, 1.0, 0.0])),
    ):
        pose = stack.solve(point, normal / np.linalg.norm(normal))
        assert pose.ok
        landed = stack.platform_rotation(pose.yaw_deg, pose.pitch_deg) @ stack.setup_point(
            point
        )
        final = landed + np.array([pose.table_xy_mm[0], pose.table_xy_mm[1], 0.0])
        assert final[0] == pytest.approx(stack.tool_xy_mm[0], abs=1e-9)
        assert final[1] == pytest.approx(stack.tool_xy_mm[1], abs=1e-9)
        # The reported contact is that same landing point, height included — the
        # whole point of the move is to arrive there, not merely at its xy.
        assert pose.contact_point_mm == pytest.approx(tuple(final), abs=1e-9)
        assert pose.contact_z_mm == pytest.approx(float(final[2]), abs=1e-9)


def test_the_mounting_offset_moves_the_point_and_the_mounting_yaw_the_whole_frame():
    """x/y translate the contact; the yaw turns it about the table origin."""
    stack = PlatformStack()
    point = np.array([2.0, 6.0, 1.0])
    normal = np.array([0.2, 0.95, 0.1])
    normal = normal / np.linalg.norm(normal)
    base = stack.solve(point, normal)

    shifted = dataclasses.replace(stack, sensor_offset_mm=(1.5, -2.5, 0.0))
    moved = shifted.solve(point, normal)
    assert moved.target_xy_mm[0] - base.target_xy_mm[0] == pytest.approx(1.5)
    assert moved.target_xy_mm[1] - base.target_xy_mm[1] == pytest.approx(-2.5)
    # An in-plane offset leaves the mounting height alone. The stages then present
    # a different height, because the same angles tilt a contact that sits somewhere
    # else relative to the pitch axis.
    assert moved.mounted_point_mm[2] == pytest.approx(base.mounted_point_mm[2])
    assert abs(moved.contact_z_mm - base.contact_z_mm) > 0.1

    turned = dataclasses.replace(stack, sensor_yaw_deg=35.0)
    yawed = turned.solve(point, normal)
    # A yaw about the table's z keeps the radius from the origin and the height.
    assert float(np.hypot(*yawed.target_xy_mm)) == pytest.approx(
        float(np.hypot(*base.target_xy_mm)), rel=1e-9
    )
    assert yawed.contact_z_mm == pytest.approx(base.contact_z_mm)
    # The mounting yaw is absorbed by the stage: it has to turn that much less.
    # Mounting yaw is right-handed about Z. Positive stage yaw is the opposite
    # sense, so the same-pitch branch's command rises by the mounting angle.
    # The shorter branch may be the other one, a half-turn away.
    unit = turned.setup_normal(normal)
    unit = unit / float(np.linalg.norm(unit))
    same_pitch = [
        branch
        for branch in turned.alignment_branches(unit, turned.tool_direction)
        if branch.pitch_deg == pytest.approx(base.pitch_deg)
    ]
    assert len(same_pitch) == 1
    shift = (same_pitch[0].yaw_deg - base.yaw_deg + 180.0) % 360.0 - 180.0
    assert shift == pytest.approx(35.0, abs=1e-9)
    assert yawed.residual_deg == pytest.approx(base.residual_deg, abs=1e-9)


def test_the_setup_z_is_the_mounting_plate():
    """The plate is not a separate input: it is the z of the mounting pose."""
    stack = PlatformStack()
    point = np.array([1.0, 7.0, 2.0])
    normal = np.array([0.1, 0.99, 0.0])
    normal = normal / np.linalg.norm(normal)
    plain = stack.solve(point, normal)

    lifted = dataclasses.replace(stack, sensor_offset_mm=(0.0, 0.0, 4.25))
    raised = lifted.solve(point, normal)
    # The plate lifts the mounting pose by exactly its thickness. The height the
    # stages present rises by less, because the lift is applied before the stage
    # rotation and the rotation tilts it — 4.2285 mm of the 4.25 mm here.
    assert raised.mounted_point_mm[2] - plain.mounted_point_mm[2] == pytest.approx(4.25)
    assert 0.0 < raised.contact_z_mm - plain.contact_z_mm < 4.25
    assert raised.target_xy_mm == pytest.approx(plain.target_xy_mm)
    # And it is the same number as the plate parameter it replaced.
    assert lifted.setup_point(point)[2] == pytest.approx(
        stack.point_to_platform(point, plate_thickness_mm=4.25)[2]
    )


def test_the_bench_mount_points_the_tip_forward():
    """Yaw 180 put the tip on the near side. Zero yaw points it away."""
    stack = PlatformStack(sensor_yaw_deg=0.0)
    tip = stack.setup_point(np.array([0.0, 0.0, 1.0]))
    cable = stack.setup_point(np.array([0.0, 0.0, -1.0]))
    assert tip[:2] == pytest.approx((0.0, 1.0))
    assert cable[:2] == pytest.approx((0.0, -1.0))
    assert stack.setup_point(np.array([0.0, 1.0, 0.0]))[2] == pytest.approx(1.0)


def test_the_setup_normal_is_the_mounting_yaw_applied_to_the_mount():
    stack = PlatformStack(sensor_yaw_deg=90.0)
    normal = np.array([1.0, 0.0, 0.0])
    # Sensor +x is platform −x; a +90° mounting yaw about Z sends it to platform −y.
    assert np.allclose(stack.setup_normal(normal), [0.0, -1.0, 0.0], atol=1e-12)
    assert stack.normal_to_platform(normal) == pytest.approx([-1.0, 0.0, 0.0])


def test_a_non_finite_sensor_yaw_is_refused():
    with pytest.raises(ValueError):
        PlatformStack(sensor_yaw_deg=float("nan")).setup_rotation()


def test_the_approach_sign_is_validated():
    with pytest.raises(ValueError):
        PlatformStack(approach_sign=0)


def test_swapping_the_approach_sign_changes_the_target():
    """The sign is the difference between pressing in and pulling away."""
    normal = np.array([0.0, 1.0, 0.0])
    point = np.array([0.0, 5.0, 0.0])
    inward = PlatformStack(approach_sign=-1).solve(point, normal)
    outward = PlatformStack(approach_sign=1).solve(point, normal)
    assert inward.residual_deg == pytest.approx(0.0, abs=1e-6)
    assert outward.pitch_deg != pytest.approx(inward.pitch_deg)


def test_a_pose_describes_itself():
    pose = Pose(ok=True, target_xy_mm=(1.0, 2.0), yaw_deg=3.0, pitch_deg=4.0, residual_deg=0.01)
    text = pose.describe()
    assert "yaw" in text and "pitch" in text and "residual" in text
    assert Pose(ok=False, reason="nope").describe() == "no pose: nope"


def test_an_aligned_pose_reports_that_it_reached_tolerance():
    aligned = Pose(ok=True, residual_deg=0.0)
    assert aligned.reached_tolerance and aligned.reachable
    # ``reachable`` is the coarser statement: within half a degree counts as
    # "the machine can put the tool on this normal".
    loose = Pose(ok=True, residual_deg=ALIGNED_TOL_DEG * 10)
    assert not loose.reached_tolerance
    assert loose.reachable
    far = Pose(ok=True, residual_deg=30.0)
    assert not far.reached_tolerance and not far.reachable


# --------------------------------------- the branch, the height and the up axis


def _independent_rotation(yaw_deg: float, pitch_deg: float) -> np.ndarray:
    """``R_pitch @ R_yaw`` for the drive directions, not through ``rotation_about``.

    Positive yaw is clockwise from above. Positive pitch nods ``+Z`` toward ``−Y``.
    """
    yaw, pitch = np.radians(yaw_deg), np.radians(pitch_deg)
    about_yaw = np.array(
        [
            [np.cos(yaw), np.sin(yaw), 0.0],
            [-np.sin(yaw), np.cos(yaw), 0.0],
            [0.0, 0.0, 1.0],
        ]
    )
    about_pitch = np.array(
        [
            [1.0, 0.0, 0.0],
            [0.0, np.cos(pitch), -np.sin(pitch)],
            [0.0, np.sin(pitch), np.cos(pitch)],
        ]
    )
    return about_pitch @ about_yaw


#: The two measured points from the bench screenshots, in the sensor frame:
#: point, normal, mounting height, and the height the stages present. The mounting
#: height is the plate (8 mm) over the point's own height; the presented height is
#: what the plan records as 19.051 and 21.578 mm.
MEASURED = (
    ((4.872, 5.444, 16.176), (0.595, 0.673, 0.439), 13.444, 19.051),
    ((-2.698, 5.186, 18.228), (-0.343, 0.699, 0.627), 13.186, 21.578),
)


def _measured_stack() -> PlatformStack:
    """The bench's mounting: the plate is the mounting pose's z, 8 mm."""
    return PlatformStack(sensor_offset_mm=(0.0, 0.0, 8.0))


def test_the_contact_height_is_the_height_after_the_stages_turn():
    """Defect: the pose reported the contact's height *before* the stages turned.

    On the bench that is the difference between 13.444 mm and 19.051 mm — the
    stages lift the contact several millimetres, and any press-axis target is
    derived from the height the contact is *presented* at, not from where it sits
    with the stages at zero.
    """
    stack = _measured_stack()
    for point, normal, mounting, presented in MEASURED:
        pose = stack.solve(np.array(point), np.array(normal) / np.linalg.norm(normal))
        assert pose.mounted_point_mm[2] == pytest.approx(mounting, abs=5e-4)
        assert pose.contact_z_mm == pytest.approx(presented, abs=5e-4)

        # The same height again, from a rotation built here rather than by the
        # solver: the landing point of the reported angles.
        landed = _independent_rotation(pose.yaw_deg, pose.pitch_deg) @ stack.setup_point(
            np.array(point)
        )
        assert pose.contact_z_mm == pytest.approx(float(landed[2]), abs=1e-9)
        assert pose.contact_point_mm == pytest.approx(
            (stack.tool_xy_mm[0], stack.tool_xy_mm[1], float(landed[2])), abs=1e-9
        )
        assert pose.contact_point_mm[2] > pose.mounted_point_mm[2]


def test_the_pose_aligns_the_normal_and_keeps_the_body_up():
    """Task-1 acceptance: independent rotation, tool axis and the platform's up.

    "Up" is the direction the sensing face nominally aims — platform +Z, which is
    where the mount sends the sensor's own up axis — not a horizontal axis of the
    platform frame.
    """
    stack = _measured_stack()
    up = np.array([0.0, 0.0, 1.0])
    for point, normal, _mounting, _presented in MEASURED:
        pose = stack.solve(np.array(point), np.array(normal) / np.linalg.norm(normal))
        rotate = _independent_rotation(pose.yaw_deg, pose.pitch_deg)

        aligned = rotate @ stack.setup_normal(np.array(normal))
        angle = np.degrees(np.arccos(np.clip(float(aligned @ up), -1.0, 1.0)))
        assert angle <= 1e-5

        # ``uprightness`` is that same rotation carried onto the up axis: the
        # component left pointing up, 1.0 when nothing has tipped at all.
        assert stack.uprightness(pose.yaw_deg, pose.pitch_deg) == pytest.approx(
            float((rotate @ up)[2]), abs=1e-12
        )
        assert 0.0 < stack.uprightness(pose.yaw_deg, pose.pitch_deg) <= 1.0
        assert pose.upright


def test_both_exact_poses_of_a_measured_point_leave_the_sensor_face_up():
    """Defect: the up test measured the body against a *horizontal* axis.

    The two exact poses of a measured point are one pose rolled 180 degrees about
    the vertical: same contact, same height, mirrored azimuth. Neither turns the
    sensing face over, so flagging one of them inverted was the bug — and the
    panel, the preview and the guards all read that flag.
    """
    stack = PlatformStack()
    first = _independent_rotation(-143.580, 47.692)
    second = _independent_rotation(36.420, -47.692)
    assert second == pytest.approx(rotation_about("z", 180.0) @ first, abs=1e-9)

    for yaw_deg, pitch_deg in (
        (36.420, -47.692),
        (-143.580, 47.692),
        (-61.319, 45.636),
        (118.681, -45.636),
    ):
        assert stack.uprightness(yaw_deg, pitch_deg) > 0.0
        assert stack.ends_upright(yaw_deg, pitch_deg)


def test_equivalent_poses_are_chosen_by_the_machine_not_by_the_residual():
    """Two exact poses differ by float noise; that must not decide the answer.

    The right-hand screenshot point, both ways round: 84 degrees of stage travel
    against 191, and the shorter one carries the marginally *larger* residual.
    """
    short = Branch(
        yaw_deg=36.420, pitch_deg=-47.692, residual_deg=1e-9, upright=True, in_range=True
    )
    long_way = Branch(
        yaw_deg=-143.580, pitch_deg=47.692, residual_deg=0.0, upright=True, in_range=True
    )
    assert PlatformStack.select_branch([long_way, short]) is short
    assert PlatformStack.branch_reason([long_way, short], short) == "smallest"


def test_a_materially_better_alignment_still_wins():
    """Equivalent is a tolerance, not an excuse to ignore the residual."""
    coarse = Branch(yaw_deg=0.0, pitch_deg=0.0, residual_deg=3.0, upright=True, in_range=True)
    exact = Branch(yaw_deg=80.0, pitch_deg=40.0, residual_deg=0.05, upright=True, in_range=True)
    assert PlatformStack.select_branch([coarse, exact]) is exact
    assert PlatformStack.branch_reason([coarse, exact], exact) == "alignment"


def test_real_roundoff_does_not_choose_a_half_turn_over_the_shorter_pose():
    stack = PlatformStack(pitch_limit_deg=90)
    normal = np.array([-0.499590704094966, 0.6734102426572587, -0.5449107940444822])
    unit = stack.setup_normal(normal)
    branches = stack.alignment_branches(unit, stack.tool_direction)
    short = min(branches, key=lambda branch: branch.travel_deg)
    for branch in branches:
        turned = stack.platform_rotation(branch.yaw_deg, branch.pitch_deg) @ unit
        np.testing.assert_allclose(turned, [0, 0, 1], atol=1e-12)
    assert stack.select_branch(branches) is short


def test_the_branch_keys_are_applied_in_the_order_the_machine_cares_about():
    """The envelope, then which way the body ends up, then the shorter turn."""
    inside = Branch(
        yaw_deg=-61.319, pitch_deg=45.636, residual_deg=0.0, upright=True, in_range=True
    )
    outside = Branch(
        yaw_deg=0.0, pitch_deg=0.0, residual_deg=0.0, upright=True, in_range=False
    )
    assert PlatformStack.select_branch([inside, outside]) is inside
    assert PlatformStack.branch_reason([inside, outside], inside) == "in_range"

    upright = Branch(
        yaw_deg=118.681, pitch_deg=-45.636, residual_deg=0.0, upright=True, in_range=True
    )
    rolled = Branch(
        yaw_deg=10.0, pitch_deg=10.0, residual_deg=0.0, upright=False, in_range=True
    )
    assert PlatformStack.select_branch([upright, rolled]) is upright
    assert PlatformStack.branch_reason([upright, rolled], upright) == "upright"


def test_a_lone_branch_is_its_own_reason_and_an_empty_list_is_refused():
    only = Branch(yaw_deg=10.0, pitch_deg=10.0, residual_deg=0.0, upright=True, in_range=True)
    assert PlatformStack.select_branch([only]) is only
    assert PlatformStack.branch_reason([only], only) == "only"
    with pytest.raises(ValueError):
        PlatformStack.select_branch([])