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

import pytest

from dora_teleop import mapping
from dora_teleop.mapping import VX, VY, VZ, WX, WY, WZ
from tests.conftest import 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(stick(axes=[0, 0, -1.0, 0, 0, 1.0]).axes, [], config)
    down = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, -1.0]).axes, [], 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(stick(axes=[0, 0, 1.0, 0, 0, 1.0]).axes, [], 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(stick(axes=[0, 0, -1, 1.0, -1.0, -1]).axes, [], 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, [False] * 5 + [True], config)
    left = mapping.velocity(stick().axes, [False] * 4 + [True, False], 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(stick(axes=[1.0, -1.0, -1, 0, 0, 1.0]).axes, [], 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