"""
test_mode_logic.py — Unit tests for saltybot_mode_switch pure logic.

Covers:
  - ch6_wants_auto / sticks_active helpers
  - ramp_alpha / alpha_for_mode
  - led_pattern
  - step_state state machine (all transitions)
  - blend_twist

No ROS2 runtime required.
"""

import sys
import os
import math

sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))

import pytest

from saltybot_mode_switch.mode_logic import (
    alpha_for_mode,
    blend_twist,
    ch6_wants_auto,
    led_pattern,
    ramp_alpha,
    step_state,
    sticks_active,
)

# ─────────────────────────────────────────────────────────────────────────────
# Helpers
# ─────────────────────────────────────────────────────────────────────────────

def _step(
    mode="RC",
    ramp_elapsed=0.0,
    ramp_s=0.5,
    ch6_auto=False,
    sticks_now_active=False,
    neutral_elapsed=0.0,
    return_delay=2.0,
    slam_ok=True,
    rc_link_ok=True,
    override_active=False,
):
    return step_state(
        current_mode=mode,
        ramp_elapsed_s=ramp_elapsed,
        ramp_s=ramp_s,
        ch6_auto=ch6_auto,
        sticks_now_active=sticks_now_active,
        sticks_neutral_elapsed_s=neutral_elapsed,
        return_delay_s=return_delay,
        slam_ok=slam_ok,
        rc_link_ok=rc_link_ok,
        override_active=override_active,
    )


# ─────────────────────────────────────────────────────────────────────────────
# ch6_wants_auto
# ─────────────────────────────────────────────────────────────────────────────

class TestCh6WantsAuto:

    def test_high_value_returns_true(self):
        assert ch6_wants_auto([0.0, 0.0, 0.0, 0.0, 0.0, 1.0], ch6_idx=5) is True

    def test_low_value_returns_false(self):
        assert ch6_wants_auto([0.0, 0.0, 0.0, 0.0, 0.0, -1.0], ch6_idx=5) is False

    def test_mid_value_below_threshold(self):
        assert ch6_wants_auto([0.0, 0.0, 0.0, 0.0, 0.0, 0.4], ch6_idx=5) is False

    def test_exactly_at_threshold(self):
        # 0.5 > 0.5 is False; require strictly above
        assert ch6_wants_auto([0.0, 0.0, 0.0, 0.0, 0.0, 0.5], ch6_idx=5) is False

    def test_just_above_threshold(self):
        assert ch6_wants_auto([0.0, 0.0, 0.0, 0.0, 0.0, 0.51], ch6_idx=5) is True

    def test_out_of_range_index(self):
        assert ch6_wants_auto([1.0], ch6_idx=10) is False

    def test_empty_axes(self):
        assert ch6_wants_auto([], ch6_idx=0) is False

    def test_custom_threshold(self):
        assert ch6_wants_auto([0.0, 0.3], ch6_idx=1, threshold=0.2) is True
        assert ch6_wants_auto([0.0, 0.3], ch6_idx=1, threshold=0.5) is False


# ─────────────────────────────────────────────────────────────────────────────
# sticks_active
# ─────────────────────────────────────────────────────────────────────────────

class TestSticksActive:

    def test_all_zero_not_active(self):
        assert sticks_active([0.0, 0.0, 0.0, 0.0], (0, 1, 2, 3)) is False

    def test_one_stick_above_threshold(self):
        assert sticks_active([0.0, 0.15, 0.0, 0.0], (0, 1, 2, 3)) is True

    def test_all_exactly_at_threshold_not_active(self):
        axes = [0.10, 0.10, 0.10, 0.10]
        assert sticks_active(axes, (0, 1, 2, 3), threshold=0.10) is False

    def test_just_above_threshold_active(self):
        assert sticks_active([0.0, 0.0, 0.0, 0.11], (0, 1, 2, 3)) is True

    def test_negative_deflection_detected(self):
        assert sticks_active([-0.5, 0.0, 0.0, 0.0], (0,)) is True

    def test_out_of_range_index_skipped(self):
        # index 5 is out of range for a 4-element list — should not crash
        assert sticks_active([0.0, 0.0, 0.0, 0.0], (0, 1, 5)) is False

    def test_only_monitored_axes_checked(self):
        # axes[4] = 0.9 but only (0,1,2,3) are monitored
        assert sticks_active([0.0, 0.0, 0.0, 0.0, 0.9], (0, 1, 2, 3)) is False


# ─────────────────────────────────────────────────────────────────────────────
# ramp_alpha / alpha_for_mode
# ─────────────────────────────────────────────────────────────────────────────

class TestRampAlpha:

    def test_zero_elapsed_returns_zero(self):
        assert ramp_alpha(0.0, 0.5) == pytest.approx(0.0)

    def test_full_elapsed_returns_one(self):
        assert ramp_alpha(0.5, 0.5) == pytest.approx(1.0)

    def test_beyond_duration_clamped_to_one(self):
        assert ramp_alpha(1.0, 0.5) == pytest.approx(1.0)

    def test_halfway(self):
        assert ramp_alpha(0.25, 0.5) == pytest.approx(0.5)

    def test_zero_duration_returns_one(self):
        assert ramp_alpha(0.0, 0.0) == pytest.approx(1.0)


class TestAlphaForMode:

    def test_rc_returns_zero(self):
        assert alpha_for_mode("RC", 0.3, 0.5) == 0.0

    def test_auto_returns_one(self):
        assert alpha_for_mode("AUTO", 0.3, 0.5) == 1.0

    def test_ramp_to_auto_mid(self):
        assert alpha_for_mode("RAMP_TO_AUTO", 0.25, 0.5) == pytest.approx(0.5)

    def test_ramp_to_rc_mid(self):
        assert alpha_for_mode("RAMP_TO_RC", 0.25, 0.5) == pytest.approx(0.5)

    def test_ramp_to_auto_start(self):
        assert alpha_for_mode("RAMP_TO_AUTO", 0.0, 0.5) == pytest.approx(0.0)

    def test_ramp_to_rc_start(self):
        assert alpha_for_mode("RAMP_TO_RC", 0.0, 0.5) == pytest.approx(1.0)

    def test_ramp_to_auto_complete(self):
        assert alpha_for_mode("RAMP_TO_AUTO", 0.5, 0.5) == pytest.approx(1.0)

    def test_ramp_to_rc_complete(self):
        assert alpha_for_mode("RAMP_TO_RC", 0.5, 0.5) == pytest.approx(0.0)

    def test_unknown_mode_returns_zero(self):
        assert alpha_for_mode("UNKNOWN", 0.3, 0.5) == 0.0


# ─────────────────────────────────────────────────────────────────────────────
# led_pattern
# ─────────────────────────────────────────────────────────────────────────────

class TestLedPattern:

    def test_no_rc_link_overrides_all(self):
        for mode in ("RC", "RAMP_TO_AUTO", "AUTO", "RAMP_TO_RC"):
            assert led_pattern(mode, slam_ok=True, rc_link_ok=False) == "blink_red_fast"

    def test_rc_mode_yellow(self):
        assert led_pattern("RC", slam_ok=True, rc_link_ok=True) == "solid_yellow"

    def test_ramp_to_rc_yellow(self):
        assert led_pattern("RAMP_TO_RC", slam_ok=True, rc_link_ok=True) == "solid_yellow"

    def test_auto_ok_green(self):
        assert led_pattern("AUTO", slam_ok=True, rc_link_ok=True) == "solid_green"

    def test_auto_no_slam_orange(self):
        assert led_pattern("AUTO", slam_ok=False, rc_link_ok=True) == "blink_orange_fast"

    def test_ramp_to_auto_blink_green(self):
        assert led_pattern("RAMP_TO_AUTO", slam_ok=True, rc_link_ok=True) == "blink_green_slow"

    def test_rc_slam_state_irrelevant(self):
        p1 = led_pattern("RC", slam_ok=True,  rc_link_ok=True)
        p2 = led_pattern("RC", slam_ok=False, rc_link_ok=True)
        assert p1 == p2  # SLAM doesn't affect RC LED


# ─────────────────────────────────────────────────────────────────────────────
# step_state — RC transitions
# ─────────────────────────────────────────────────────────────────────────────

class TestStepStateFromRC:

    def test_rc_stays_rc_by_default(self):
        mode, override, instant = _step(mode="RC")
        assert mode == "RC"
        assert not instant

    def test_rc_to_ramp_when_ch6_auto(self):
        mode, override, instant = _step(mode="RC", ch6_auto=True)
        assert mode == "RAMP_TO_AUTO"
        assert not instant

    def test_rc_stays_rc_when_ch6_auto_but_no_slam(self):
        mode, _, _ = _step(mode="RC", ch6_auto=True, slam_ok=False)
        assert mode == "RC"

    def test_rc_stays_rc_when_ch6_auto_but_no_rc_link(self):
        mode, _, instant = _step(mode="RC", ch6_auto=True, rc_link_ok=False)
        assert mode == "RC"
        assert instant  # RC link lost → instant

    def test_rc_stays_rc_when_override_active_and_sticks_not_neutral(self):
        mode, _, _ = _step(
            mode="RC", ch6_auto=True, slam_ok=True,
            override_active=True, neutral_elapsed=1.0, return_delay=2.0)
        assert mode == "RC"

    def test_rc_to_ramp_after_override_clears(self):
        # override clears after sticks neutral >= return_delay
        mode, override, _ = _step(
            mode="RC", ch6_auto=True, slam_ok=True,
            override_active=True, neutral_elapsed=2.5, return_delay=2.0)
        assert mode == "RAMP_TO_AUTO"
        assert not override


# ─────────────────────────────────────────────────────────────────────────────
# step_state — RAMP_TO_AUTO transitions
# ─────────────────────────────────────────────────────────────────────────────

class TestStepStateFromRampToAuto:

    def test_continues_ramping(self):
        mode, _, _ = _step(mode="RAMP_TO_AUTO", ramp_elapsed=0.2, ramp_s=0.5,
                           ch6_auto=True)
        assert mode == "RAMP_TO_AUTO"

    def test_completes_to_auto(self):
        mode, _, _ = _step(mode="RAMP_TO_AUTO", ramp_elapsed=0.5, ramp_s=0.5,
                           ch6_auto=True)
        assert mode == "AUTO"

    def test_stick_override_instant_rc(self):
        mode, override, instant = _step(
            mode="RAMP_TO_AUTO", ch6_auto=True,
            sticks_now_active=True)
        assert mode == "RC"
        assert override is True
        assert instant is True

    def test_ch6_low_aborts_to_ramp_rc(self):
        mode, _, _ = _step(mode="RAMP_TO_AUTO", ch6_auto=False, ramp_elapsed=0.1)
        assert mode == "RAMP_TO_RC"

    def test_slam_lost_aborts_to_ramp_rc(self):
        mode, _, _ = _step(mode="RAMP_TO_AUTO", ch6_auto=True, slam_ok=False)
        assert mode == "RAMP_TO_RC"

    def test_rc_link_lost_instant_rc(self):
        mode, _, instant = _step(mode="RAMP_TO_AUTO", ch6_auto=True, rc_link_ok=False)
        assert mode == "RC"
        assert instant is True


# ─────────────────────────────────────────────────────────────────────────────
# step_state — AUTO transitions
# ─────────────────────────────────────────────────────────────────────────────

class TestStepStateFromAuto:

    def test_auto_stays_auto(self):
        mode, _, _ = _step(mode="AUTO", ch6_auto=True)
        assert mode == "AUTO"

    def test_stick_override_instant_rc(self):
        mode, override, instant = _step(
            mode="AUTO", ch6_auto=True, sticks_now_active=True)
        assert mode == "RC"
        assert override is True
        assert instant is True

    def test_ch6_low_ramp_to_rc(self):
        mode, _, instant = _step(mode="AUTO", ch6_auto=False)
        assert mode == "RAMP_TO_RC"
        assert not instant

    def test_slam_lost_ramp_to_rc(self):
        mode, _, instant = _step(mode="AUTO", ch6_auto=True, slam_ok=False)
        assert mode == "RAMP_TO_RC"
        assert not instant

    def test_rc_link_lost_instant_rc(self):
        mode, _, instant = _step(mode="AUTO", ch6_auto=True, rc_link_ok=False)
        assert mode == "RC"
        assert instant is True


# ─────────────────────────────────────────────────────────────────────────────
# step_state — RAMP_TO_RC transitions
# ─────────────────────────────────────────────────────────────────────────────

class TestStepStateFromRampToRC:

    def test_continues_ramping(self):
        mode, _, _ = _step(mode="RAMP_TO_RC", ramp_elapsed=0.2, ramp_s=0.5)
        assert mode == "RAMP_TO_RC"

    def test_completes_to_rc(self):
        mode, _, _ = _step(mode="RAMP_TO_RC", ramp_elapsed=0.5, ramp_s=0.5)
        assert mode == "RC"

    def test_stick_override_instant_rc(self):
        mode, override, instant = _step(
            mode="RAMP_TO_RC", sticks_now_active=True, ch6_auto=True)
        assert mode == "RC"
        assert instant is True

    def test_ch6_still_auto_no_override_cancels_back(self):
        # CH6 still says AUTO, no override → cancel to RAMP_TO_AUTO
        mode, _, _ = _step(mode="RAMP_TO_RC", ch6_auto=True, slam_ok=True,
                           override_active=False, ramp_elapsed=0.1)
        assert mode == "RAMP_TO_AUTO"

    def test_rc_link_lost_instant_rc(self):
        _, _, instant = _step(mode="RAMP_TO_RC", rc_link_ok=False)
        assert instant is True


# ─────────────────────────────────────────────────────────────────────────────
# blend_twist
# ─────────────────────────────────────────────────────────────────────────────

class TestBlendTwist:

    def test_alpha_zero_zeros_output(self):
        lx, az = blend_twist(1.0, 0.5, 0.0)
        assert lx == pytest.approx(0.0)
        assert az == pytest.approx(0.0)

    def test_alpha_one_passes_through(self):
        lx, az = blend_twist(0.4, -0.7, 1.0)
        assert lx == pytest.approx(0.4)
        assert az == pytest.approx(-0.7)

    def test_alpha_half_scales(self):
        lx, az = blend_twist(1.0, 1.0, 0.5)
        assert lx == pytest.approx(0.5)
        assert az == pytest.approx(0.5)

    def test_alpha_above_one_clamped(self):
        lx, az = blend_twist(1.0, 1.0, 1.5)
        assert lx == pytest.approx(1.0)
        assert az == pytest.approx(1.0)

    def test_alpha_below_zero_clamped(self):
        lx, az = blend_twist(1.0, 1.0, -0.5)
        assert lx == pytest.approx(0.0)
        assert az == pytest.approx(0.0)

    def test_negative_cmd_vel_preserved(self):
        lx, az = blend_twist(-0.5, -1.0, 0.5)
        assert lx == pytest.approx(-0.25)
        assert az == pytest.approx(-0.5)


# ─────────────────────────────────────────────────────────────────────────────
# Integration scenarios
# ─────────────────────────────────────────────────────────────────────────────

class TestIntegrationScenarios:

    def test_full_rc_to_auto_ramp_cycle(self):
        """Simulate RC→RAMP_TO_AUTO→AUTO with 0.5s ramp at 20Hz."""
        mode      = "RC"
        override  = False
        ramp_s    = 0.5
        dt        = 1.0 / 20.0
        t_ramp    = 0.0

        # CH6 flipped to AUTO, SLAM ok
        mode, override, instant = step_state(
            mode, 0.0, ramp_s, True, False, 5.0, 2.0, True, True, override)
        assert mode == "RAMP_TO_AUTO"
        t_ramp = 0.0

        # Simulate 10 ticks (0.5s) → should complete
        for tick in range(10):
            t_ramp += dt
            mode, override, instant = step_state(
                mode, t_ramp, ramp_s, True, False, 5.0, 2.0, True, True, override)

        assert mode == "AUTO"
        assert alpha_for_mode("AUTO", t_ramp, ramp_s) == 1.0

    def test_stick_override_then_return_to_auto(self):
        """Stick override while AUTO → RC → neutral 2s → back to AUTO."""
        # Start in AUTO
        mode, override, instant = _step(
            mode="AUTO", ch6_auto=True, sticks_now_active=True)
        assert mode == "RC"
        assert override is True
        assert instant is True

        # Sticks neutral but < 2s — stays RC
        mode, override, _ = _step(
            mode="RC", ch6_auto=True, slam_ok=True,
            override_active=True, neutral_elapsed=1.0, return_delay=2.0)
        assert mode == "RC"

        # Sticks neutral >= 2s — override clears, enters ramp
        mode, override, _ = _step(
            mode="RC", ch6_auto=True, slam_ok=True,
            override_active=True, neutral_elapsed=2.0, return_delay=2.0)
        assert mode == "RAMP_TO_AUTO"
        assert override is False

    def test_slam_lost_during_auto_graceful_exit(self):
        """SLAM fix lost during AUTO → graceful RAMP_TO_RC (not instant)."""
        mode, _, instant = _step(mode="AUTO", ch6_auto=True, slam_ok=False)
        assert mode == "RAMP_TO_RC"
        assert not instant

    def test_rc_link_lost_always_instant(self):
        """RC link lost from any state → instant RC."""
        for start_mode in ("AUTO", "RAMP_TO_AUTO", "RAMP_TO_RC"):
            mode, _, instant = _step(mode=start_mode, ch6_auto=True, rc_link_ok=False)
            assert mode == "RC", f"from {start_mode}"
            assert instant is True, f"from {start_mode}"

    def test_blend_alpha_sequence_through_ramp(self):
        """Blend alpha is monotonically increasing during RAMP_TO_AUTO."""
        ramp_s = 0.5
        alphas = [
            alpha_for_mode("RAMP_TO_AUTO", t * 0.05, ramp_s)
            for t in range(11)   # 0.0 to 0.5 s in 0.05 s steps
        ]
        for a, b in zip(alphas, alphas[1:]):
            assert b >= a

    def test_ramp_to_rc_alpha_decreasing(self):
        """Blend alpha is monotonically decreasing during RAMP_TO_RC."""
        ramp_s = 0.5
        alphas = [
            alpha_for_mode("RAMP_TO_RC", t * 0.05, ramp_s)
            for t in range(11)
        ]
        for a, b in zip(alphas, alphas[1:]):
            assert b <= a

    def test_no_auto_without_slam(self):
        """From RC, ch6=AUTO but slam=False → cannot enter AUTO path."""
        mode, _, _ = _step(mode="RC", ch6_auto=True, slam_ok=False)
        assert mode == "RC"

    def test_no_auto_without_rc_link(self):
        """From RC, ch6=AUTO but rc_link=False → stays RC (instant)."""
        mode, _, instant = _step(mode="RC", ch6_auto=True, rc_link_ok=False)
        assert mode == "RC"
        assert instant is True
