"""Stick deflection to base-frame velocity: units, signs and the shaping curve."""

import pytest

from dora_teleop import mapping
from dora_teleop.mapping import LAYOUT, VX, VY, VZ, WX, WY, WZ
from tests.conftest import axes_for, buttons_for, stick


def test_a_stick_at_rest_asks_for_nothing(config):
    velocity = mapping.velocity(stick().axes, [], config)

    assert velocity == [0.0] * 6
    assert not mapping.is_moving(velocity)


def test_pushing_the_stick_up_moves_the_arm_along_positive_x(config):
    # Up is negative in SDL, and up should send the arm away from the operator.
    velocity = mapping.velocity(stick(axes=[0.0, -1.0]).axes, [], config)

    assert velocity[VX] == pytest.approx(config.speed_mm_s)
    assert velocity[VY] == 0.0


def test_pushing_the_stick_right_moves_the_arm_along_positive_y(config):
    velocity = mapping.velocity(stick(axes=[1.0, 0.0]).axes, [], config)

    assert velocity[VY] == pytest.approx(config.speed_mm_s)


def test_the_right_trigger_lifts_and_the_left_one_lowers(config):
    up = mapping.velocity(axes_for(right_trigger=1.0), [], config)
    down = mapping.velocity(axes_for(left_trigger=1.0), [], config)

    assert up[VZ] == pytest.approx(config.speed_mm_s)
    assert down[VZ] == pytest.approx(-config.speed_mm_s)


def test_both_triggers_together_cancel(config):
    velocity = mapping.velocity(axes_for(left_trigger=1.0, right_trigger=1.0), [], config)

    assert velocity[VZ] == 0.0


def test_an_untouched_trigger_does_not_read_as_half_pressed(config):
    # SDL reports a trigger from -1 to 1, and one that has never moved sometimes
    # reads 0 until it does. Treating that as half travel would lift the arm.
    assert mapping.trigger(-1.0) == 0.0
    assert mapping.trigger(0.0) == pytest.approx(0.5)
    assert mapping.trigger(1.0) == 1.0


def test_the_right_stick_turns_the_tool_in_degrees_per_second(config):
    velocity = mapping.velocity(axes_for(right_x=1.0, right_y=-1.0), [], config)

    assert velocity[WX] == pytest.approx(config.speed_deg_s)
    assert velocity[WY] == pytest.approx(config.speed_deg_s)


def test_the_bumpers_yaw_in_opposite_directions(config):
    right = mapping.velocity(stick().axes, buttons_for(yaw_right=True), config)
    left = mapping.velocity(stick().axes, buttons_for(yaw_left=True), config)

    assert right[WZ] == pytest.approx(config.speed_deg_s)
    assert left[WZ] == pytest.approx(-config.speed_deg_s)


def test_several_axes_move_at_once(config):
    # The whole reason a pad beats the console's four-sector dial: the dial can
    # only ever ask for one axis.
    velocity = mapping.velocity(axes_for(left_x=1.0, left_y=-1.0, right_trigger=1.0), [], config)

    assert velocity[VX] > 0
    assert velocity[VY] > 0
    assert velocity[VZ] > 0


def test_drift_inside_the_deadzone_is_ignored(config):
    # A worn stick that does not centre would otherwise drive the arm across the
    # table with nobody touching it.
    velocity = mapping.velocity(stick(axes=[config.deadzone / 2, 0.0]).axes, [], config)

    assert velocity == [0.0] * 6


def test_motion_starts_from_zero_just_outside_the_deadzone(config):
    # Without rescaling, the arm would jump to a fraction of full speed the
    # instant the stick left the dead band.
    just_outside = mapping.deadzone(config.deadzone + 1e-9, config.deadzone)

    assert just_outside == pytest.approx(0.0, abs=1e-6)


def test_the_deadzone_still_reaches_full_speed_at_full_deflection(config):
    assert mapping.deadzone(1.0, config.deadzone) == pytest.approx(1.0)
    assert mapping.deadzone(-1.0, config.deadzone) == pytest.approx(-1.0)


def test_expo_slows_small_deflections_without_capping_large_ones():
    assert mapping.expo(0.5, 0.0) == pytest.approx(0.5)
    assert mapping.expo(0.5, 1.0) == pytest.approx(0.125)
    assert mapping.expo(1.0, 1.0) == pytest.approx(1.0)
    assert mapping.expo(-0.5, 1.0) == pytest.approx(-0.125)


def test_a_pad_reporting_fewer_axes_is_read_as_centred_not_as_an_error(config):
    velocity = mapping.velocity([0.0, 0.0], [], config)

    assert velocity[VX] == 0.0
    assert velocity[WZ] == 0.0


# --- is the pad being used at all ---


def test_a_pad_at_rest_is_not_asking_for_the_arm():
    # SDL rests a trigger at -1.0, so a raw threshold reports every connected
    # pad as in use for ever: the pad claims the arm the moment it is plugged
    # in and GAMEPAD_IDLE_RELEASE_S never fires.
    assert mapping.is_active(stick()) is False


def test_an_absent_pad_is_not_asking_for_the_arm():
    assert mapping.is_active(stick(present=False)) is False


def test_a_nudged_stick_is_asking_for_the_arm():
    assert mapping.is_active(stick(axes=[0.5])) is True


def test_a_held_trigger_is_asking_for_the_arm():
    assert mapping.is_active(stick(axes=axes_for(left_trigger=1.0))) is True


def test_a_held_button_is_asking_for_the_arm():
    assert mapping.is_active(stick(buttons=buttons_for(grasp=True))) is True


def test_an_axis_the_layout_does_not_use_still_counts():
    # A pad reporting its d-pad as two extra axes is asking for the arm when
    # somebody presses it, whether or not this node maps it to anything.
    assert mapping.is_active(stick(axes=axes_for() + [1.0])) is True


# --- reading a pad that is not the layout's ideal one ---


def test_a_resting_pad_measures_as_resting():
    zeros = mapping.rest_zeros(axes_for())

    assert zeros == axes_for()


def test_a_small_tilt_is_accepted_as_rest():
    # A worn stick is an ordinary pad, not a broken one.
    assert mapping.rest_zeros(axes_for(left_x=0.2, right_y=-0.2))


def test_an_axis_resting_where_the_layout_says_it_cannot_is_refused():
    wrong = axes_for()
    wrong[LAYOUT.right_y] = -1.0

    with pytest.raises(ValueError, match="right stick Y"):
        mapping.rest_zeros(wrong)


def test_the_refusal_names_the_index_and_what_it_read():
    # It is read off a terminal by somebody deciding what to put in `Layout`.
    wrong = axes_for()
    wrong[LAYOUT.right_x] = 0.9

    with pytest.raises(ValueError, match=r"axis 2 rests at \+0.900"):
        mapping.rest_zeros(wrong)


def test_a_trigger_resting_at_either_end_is_accepted():
    # -1 is what SDL reports; 0 is what some pads report until it first moves.
    assert mapping.rest_zeros(axes_for(left_trigger=-1.0, right_trigger=0.0))


def test_an_axis_the_layout_does_not_use_still_has_to_rest():
    # A d-pad reported as two extra axes counts towards `is_active`, so one
    # stuck at an extreme would hold the arm as surely as a named one.
    with pytest.raises(ValueError, match="axis 6"):
        mapping.rest_zeros(axes_for() + [1.0])


def test_canonical_puts_a_tilted_stick_back_at_centre():
    tilted = axes_for(left_x=0.2)

    canonical = mapping.canonical(tilted, mapping.rest_zeros(tilted))

    assert canonical[LAYOUT.left_x] == pytest.approx(0.0)


def test_canonical_puts_a_trigger_back_at_its_own_rest():
    zeroed = axes_for(left_trigger=0.0, right_trigger=0.0)

    canonical = mapping.canonical(zeroed, mapping.rest_zeros(zeroed))

    assert canonical[LAYOUT.left_trigger] == pytest.approx(mapping.TRIGGER_REST)
    assert canonical[LAYOUT.right_trigger] == pytest.approx(mapping.TRIGGER_REST)


def test_a_trigger_measured_from_zero_still_reaches_full_travel():
    zeros = mapping.rest_zeros(axes_for(right_trigger=0.0))

    canonical = mapping.canonical(axes_for(right_trigger=1.0), zeros)

    assert mapping.trigger(canonical[LAYOUT.right_trigger]) == pytest.approx(1.0)


def test_a_calibrated_pad_at_rest_asks_for_nothing(config):
    tilted = axes_for(left_x=0.2, left_y=-0.15, right_trigger=0.0)

    canonical = mapping.canonical(tilted, mapping.rest_zeros(tilted))

    assert mapping.velocity(canonical, [], config) == [0.0] * 6