"""The closed-form solve: exactness, agreement with the sweep, and what it reports.

The sweep in ``test_kinematics.py`` is the reference this path is checked against.
It is slow but line-by-line verifiable, and solvers in this file have been
confidently wrong five times; a fast path with no reference is how that happens.
"""

from __future__ import annotations

import math
from pathlib import Path

import numpy as np
import pytest

from orisys_autocal.motion.kinematics import (
    PlatformStack,
    Branch,
    rotation_about,
)
from orisys_autocal.sensor import load_bundle

#: The revision the bench confirmed. Skipped rather than failed when absent: the
#: export lives in a sibling design repo, and a machine without it has not broken
#: AutoCal.
BUNDLE = Path(r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute")


@pytest.fixture(scope="module")
def bundle():
    if not BUNDLE.is_dir():
        pytest.skip(f"no sensor export at {BUNDLE}")
    return load_bundle(BUNDLE)


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 _apply(stack: PlatformStack, unit: np.ndarray, yaw_deg: float, pitch_deg: float) -> np.ndarray:
    """Re-apply a returned pose with the library's own rotations."""
    return (
        rotation_about(stack.pitch_axis, pitch_deg)
        @ rotation_about(stack.yaw_axis, yaw_deg)
        @ unit
    )


# ------------------------------------------------------------------ exactness


def test_the_closed_form_is_exact_on_random_normals():
    """Two atan2 calls should land on the answer, not near it."""
    stack = PlatformStack()
    worst = 0.0
    for unit in _random_units(2000, seed=20240517):
        branches = stack.alignment_branches(unit, stack.tool_direction)
        assert branches, f"no branch found for {unit}"
        worst = max(worst, min(branch.residual_deg for branch in branches))
    assert worst < 1e-4, f"closed form left {worst:.3e} deg on the table"


def test_every_branch_reproduces_its_own_claimed_residual():
    """The number reported must be the number the rotations actually give."""
    stack = PlatformStack()
    for unit in _random_units(200, seed=7):
        for branch in stack.alignment_branches(unit, stack.tool_direction):
            landed = _apply(stack, unit, branch.yaw_deg, branch.pitch_deg)
            measured = math.degrees(
                math.acos(
                    max(-1.0, min(1.0, float(landed @ stack.tool_direction)))
                )
            )
            # ``acos`` near 1 loses about sqrt(eps) in radians because the
            # derivative blows up there; 1e-5 deg is the floating-point floor,
            # not slack in the answer.
            assert measured == pytest.approx(branch.residual_deg, abs=1e-5)


# ------------------------------------------------------------------ agreement


def test_the_closed_form_is_no_worse_than_the_sweep():
    """The sweep is the reference; the fast path may beat it but never lose."""
    stack = PlatformStack()
    for unit in _random_units(400, seed=99):
        fast = stack._search_alignment(unit, stack.tool_direction)  # noqa: SLF001
        branches = stack.alignment_branches(unit, stack.tool_direction)
        best = min(branch.residual_deg for branch in branches)
        assert best <= fast[2] + 1e-9, (
            f"closed form left {best:.6f} deg where the sweep found {fast[2]:.6f}"
        )


def test_the_two_methods_agree_inside_the_envelope():
    stack = PlatformStack()
    for unit in _random_units(200, seed=1234):
        analytical = stack.solve(np.zeros(3), unit, method="analytical")
        search = stack.solve(np.zeros(3), unit, method="search")
        assert analytical.residual_deg <= search.residual_deg + 1e-6
        for pose in (analytical, search):
            assert abs(pose.yaw_deg) <= stack.yaw_limit_deg + 1e-6
            assert abs(pose.pitch_deg) <= stack.pitch_limit_deg + 1e-6


# ----------------------------------------------------------- axis equivalence


def test_the_two_pitch_bindings_are_the_same_family():
    """Confirmed at the bench: a 90 degree yaw takes one binding to the other.

    ``R_y(p)·R_z(t)`` and ``R_x(p)·R_z(t − 90)`` agree to machine precision. So
    which axis is called pitch is a naming and zero-offset choice, not a
    correctness question — the two reach exactly the same set of alignments.
    """
    unit = np.array([0.3, 0.5, -0.81])
    unit = unit / np.linalg.norm(unit)
    tool = np.array([0.0, 0.0, -1.0])
    worst = 0.0
    for yaw in (-120.0, -45.0, 0.0, 37.0, 90.0, 150.0):
        for pitch in (-90.0, -20.0, 0.0, 15.0, 75.0):
            about_y = rotation_about("y", pitch) @ rotation_about("z", yaw) @ unit
            about_x = rotation_about("x", pitch) @ rotation_about("z", yaw - 90.0) @ unit
            worst = max(worst, abs(float(about_y @ tool) - float(about_x @ tool)))
    assert worst == pytest.approx(0.0, abs=1e-12), (
        f"the two pitch bindings diverge in alignment by {worst:.3e}"
    )


def test_the_offset_between_the_bindings_is_a_constant_ninety_degrees():
    """A constant shift, so it is a commissioning zero rather than a correction."""
    unit = np.array([0.3, 0.5, -0.81])
    unit = unit / np.linalg.norm(unit)
    tool = np.array([0.0, 0.0, -1.0])
    for yaw in (-120.0, -30.0, 0.0, 45.0, 120.0):
        for pitch in (-90.0, 0.0, 25.0, 75.0):
            about_y = rotation_about("y", pitch) @ rotation_about("z", yaw) @ unit
            same = rotation_about("x", pitch) @ rotation_about("z", yaw - 90.0) @ unit
            assert float(about_y @ tool) == pytest.approx(float(same @ tool), abs=1e-12)


# ---------------------------------------------------------------- reporting


def test_the_normal_is_aligned_to_plus_z_not_minus_z(bundle):
    """The regression that would have caught the sensor being turned over.

    ``approach_sign`` was ``-1``, which asked the solver to bring every normal onto
    **-Z** — the sensing face is the *top* surface of a sensor mounted at the bottom
    of its shell, so it points up, and the tool feeds down onto it. Nothing caught
    it because ``arccos`` is sign-blind: a face turned fully over still scored
    ``0.0000`` degrees, and every test here asserted only that the residual was
    small. The failure showed up exclusively as the model flipping upside down.

    So this asserts the *direction* the normal lands in, which is the thing the
    residual cannot express.
    """
    stack = PlatformStack()
    assert stack.approach_sign == 1
    assert np.allclose(stack.tool_direction, [0.0, 0.0, 1.0])

    for normal in bundle.vertex_normals():
        unit = stack.normal_to_platform(normal)
        unit = unit / np.linalg.norm(unit)
        pose = stack.solve(np.zeros(3), normal)
        # Rotate in the **platform** frame, which is where the residual is measured.
        # ``stage_rotation`` is the same rotation conjugated into the mesh's frame,
        # so using it here compares a sensor-frame vector against a platform-frame
        # target and reports a mismatch that does not exist.
        landed = _apply(stack, unit, pose.yaw_deg, pose.pitch_deg)
        landed = landed / np.linalg.norm(landed)
        # Aligned with the face's aim, not merely parallel to its line.
        assert landed[2] > 0.0, (
            f"the normal landed on {np.round(landed, 3)} — pointing away from +Z"
        )


def test_the_nominal_pose_is_upright(bundle):
    """No rotation is the commissioned pose, and it must read as upright."""
    stack = PlatformStack()
    assert stack.uprightness(0.0, 0.0) == pytest.approx(1.0)


def test_the_surface_normals_point_up_so_the_face_faces_the_tool(bundle):
    """A sanity check on the geometry the polarity has to agree with.

    The mean normal has a positive platform-Z component: the patch faces upward.
    If this ever inverted, ``approach_sign`` would need revisiting rather than the
    solver.
    """
    stack = PlatformStack()
    platform = np.array(
        [stack.normal_to_platform(n) for n in bundle.vertex_normals()]
    )
    platform /= np.linalg.norm(platform, axis=1)[:, None]
    assert float(platform[:, 2].mean()) > 0.0, (
        f"mean platform-Z component of the normals is {platform[:, 2].mean():+.3f}"
    )


def test_few_vertices_end_turned_over_and_those_are_edge_on(bundle):
    """Turning the face over is the failure mode; measure it and bound it.

    The remainder are vertices whose normals are 60-90 degrees off the tool axis —
    the rim of the dome. Aligning an edge-on face has to roll the body about that
    normal, and the sign is decided by rounding, so they cannot be pushed to zero
    and should not be asserted away.
    """
    stack = PlatformStack()
    normals = bundle.vertex_normals()
    flipped = 0
    for normal in normals:
        pose = stack.solve(np.zeros(3), normal)
        if stack.uprightness(pose.yaw_deg, pose.pitch_deg) < -0.5:
            flipped += 1
    assert flipped <= len(normals) * 0.03, (
        f"{flipped} of {len(normals)} vertices ended turned over"
    )


def test_a_pose_reports_whether_it_is_upright():
    """A pose can align the normal and still turn the sensor over."""
    stack = PlatformStack()
    seen = {True: 0, False: 0}
    for unit in _random_units(300, seed=3):
        pose = stack.solve(np.zeros(3), unit)
        seen[pose.upright] += 1
        # Whatever it says must match the measured orientation.
        assert pose.upright == (stack.uprightness(pose.yaw_deg, pose.pitch_deg) >= 0.0)
    assert seen[True] > 0


def test_an_out_of_range_pose_is_reported_and_never_silently_clamped():
    """Tight travel must give a measured residual, not a pose the stage cannot reach."""
    stack = PlatformStack(pitch_limit_deg=45.0)
    for unit in _random_units(50, seed=5):
        pose = stack.solve(np.zeros(3), unit)
        assert abs(pose.pitch_deg) <= 45.0 + 1e-6
        assert abs(pose.yaw_deg) <= stack.yaw_limit_deg + 1e-6
        if not pose.in_range:
            assert "travel" in pose.reason


def test_nothing_is_reported_out_of_range_when_the_travel_is_ample():
    stack = PlatformStack()
    for unit in _random_units(300, seed=11):
        pose = stack.solve(np.zeros(3), unit)
        assert pose.in_range, f"{unit} reported out of range at full travel"
        assert pose.reason == "" or "indeterminate" in pose.reason


def test_reachability_agrees_with_the_envelope():
    """Every normal is reachable when the yaw can turn a full half-turn."""
    stack = PlatformStack()
    for unit in _random_units(500, seed=13):
        reachable, why = stack.reachable(unit, stack.tool_direction)
        assert reachable, f"{unit} unreachable: {why}"
    # And with the pitch stage able to turn, no normal is rejected either.
    assert stack.reachable(
        np.array([0.0, 0.0, 1.0]), stack.tool_direction
    )[0]


# --------------------------------------------------------------------- poles


@pytest.mark.parametrize(
    "normal",
    [
        np.array([0.0, 1.0, 0.0]),   # on the tool axis
        np.array([0.0, -1.0, 0.0]),  # on the tool axis, other way
        np.array([1.0, 0.0, 0.0]),   # on the yaw plane
        np.array([0.0, 0.0, 1.0]),   # on the yaw axis
    ],
)
def test_poles_still_return_a_usable_pose(normal):
    stack = PlatformStack()
    pose = stack.solve(np.zeros(3), normal)
    assert pose.ok
    assert math.isfinite(pose.yaw_deg) and math.isfinite(pose.pitch_deg)
    landed = _apply(stack, stack.normal_to_platform(normal) / np.linalg.norm(
        stack.normal_to_platform(normal)
    ), pose.yaw_deg, pose.pitch_deg)
    measured = math.degrees(
        math.acos(max(-1.0, min(1.0, float(landed @ stack.tool_direction))))
    )
    assert measured <= stack.tolerance_deg + 1e-6


def test_the_degenerate_normal_is_still_refused():
    stack = PlatformStack()
    pose = stack.solve(np.zeros(3), np.zeros(3))
    assert not pose.ok
    assert "degenerate" in pose.reason


# ----------------------------------------------------------------- selection


def test_select_branch_prefers_in_range_then_upright_then_small():
    reachable_upright = Branch(10.0, 5.0, 1e-6, True, True)
    reachable_inverted = Branch(10.0, 5.0, 1e-6, False, True)
    unreachable_upright = Branch(179.0, 5.0, 1e-6, True, False)

    # In range beats everything: an inverted pose the machine can reach is worth
    # more than an upright one it cannot.
    assert PlatformStack.select_branch(
        [unreachable_upright, reachable_inverted, reachable_upright]
    ) is reachable_upright
    # Among reachable poses, upright wins.
    assert PlatformStack.select_branch(
        [reachable_inverted, reachable_upright]
    ) is reachable_upright
    # Among poses that are out of range and one of them upright, upright wins.
    unreachable_inverted = Branch(179.0, 5.0, 1e-6, False, False)
    assert PlatformStack.select_branch(
        [unreachable_inverted, unreachable_upright]
    ) is unreachable_upright
    # A materially better alignment beats an upright pose.
    much_better = Branch(10.0, 5.0, 0.0, False, True)
    worse_upright = Branch(10.0, 5.0, 5.0, True, True)
    assert PlatformStack.select_branch([much_better, worse_upright]) is much_better
    # And with everything else equal, the smaller move wins.
    smaller = Branch(5.0, 5.0, 1e-6, True, True)
    larger = Branch(60.0, 5.0, 1e-6, True, True)
    assert PlatformStack.select_branch([larger, smaller]) is smaller


def test_an_unknown_method_is_refused():
    stack = PlatformStack()
    with pytest.raises(ValueError):
        stack.solve(np.zeros(3), np.array([0.0, 1.0, 0.0]), method="nonsense")