"""Pose maths and the tool-tip offset. No robot needed."""

import numpy as np
import pytest

from dora_ulite6.controller import interpolate, shortest_turn
from dora_ulite6.pose import RobotPose, with_tool_offset


def test_from_sequence_accepts_a_position_alone():
    pose = RobotPose.from_sequence([10, 20, 30])

    assert pose.as_list() == [10, 20, 30, 180.0, 0.0, 0.0]


def test_from_sequence_round_trips_a_full_pose():
    values = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]

    assert RobotPose.from_sequence(values).as_list() == values


@pytest.mark.parametrize(
    ("start", "target", "expected"),
    [(-178.0, 180.0, -2.0), (180.0, -178.0, 2.0), (0.0, 90.0, 90.0), (0.0, 270.0, -90.0)],
)
def test_shortest_turn_never_takes_the_long_way(start, target, expected):
    assert shortest_turn(start, target) == pytest.approx(expected)


def test_interpolate_turns_the_short_way_across_the_wrap():
    halfway = interpolate([0, 0, 0, -178, 0, 0], [0, 0, 0, 180, 0, 0], 0.5)

    assert halfway[3] == pytest.approx(-179.0)


def test_interpolate_walks_the_straight_line_in_position():
    halfway = interpolate([0, 0, 0, 180, 0, 0], [100, 200, 300, 180, 0, 0], 0.5)

    assert halfway[:3] == pytest.approx([50, 100, 150])


def test_tool_down_offset_lowers_the_tip():
    tip = with_tool_offset(RobotPose(300, 0, 250, roll=180, pitch=0, yaw=0), 61.0)

    assert tip.z == pytest.approx(250 - 61.0)
    assert (tip.x, tip.y) == pytest.approx((300, 0))


def test_offset_follows_the_tool_axis_not_the_base_axis():
    # Pitched 90° from tool-down, the tool points along +x rather than -z.
    tip = with_tool_offset(RobotPose(300, 0, 250, roll=0, pitch=90, yaw=0), 61.0)

    assert tip.x == pytest.approx(300 + 61.0)
    assert tip.z == pytest.approx(250)


def test_a_zero_offset_returns_the_pose_untouched():
    pose = RobotPose(1, 2, 3, 4, 5, 6)

    assert with_tool_offset(pose, 0.0) is pose


def test_the_offset_inverts_exactly():
    # move_to relies on this: it takes the offset back off a tip pose to get the
    # flange pose the controller wants.
    pose = RobotPose(123, -45, 210, roll=170, pitch=12, yaw=-30)

    there = with_tool_offset(pose, 61.0)
    back = with_tool_offset(there, -61.0)

    np.testing.assert_allclose(back.as_list(), pose.as_list(), atol=1e-9)