"""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 (
    Branch,
    PlatformStack,
    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 same rotation the drives execute."""
    return stack.platform_rotation(yaw_deg, pitch_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():
    """``R_y(p)·R_z(t)`` and ``R_x(p)·R_z(t − 90)`` reach the same alignments.

    This is a property of the right-hand matrices. The drive does not get to
    pick either binding: pitch is about ``x`` with the measured command sign.
    """
    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_no_vertex_ends_turned_over(bundle):
    """Turning the face over is the failure mode; with the right up axis it is zero.

    ``uprightness`` used to compare the platform's Y axis, so 47 of these vertices read
    as turned over even though they only rolled about the vertical. Against the
    post-mount nominal up (the tool axis) no vertex ends inverted, and this now fails
    if the reference regresses rather than reporting a small count as acceptable.
    """
    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 == 0, 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")