"""
mode_logic.py — Pure RC/Autonomous mode-switch logic for saltybot.

All functions are stateless or operate on plain Python types so the full
state machine can be exercised in unit tests without a ROS2 runtime.

Mode vocabulary
---------------
<<<<<<< HEAD
  "RC"            — ESP32 BALANCE executing RC pilot commands; Jetson cmd_vel blocked.
  "RAMP_TO_AUTO"  — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s.
  "AUTO"          — ESP32 BALANCE executing Jetson cmd_vel; RC sticks idle.
=======
  "RC"            — ESP32-S3 executing RC pilot commands; Jetson cmd_vel blocked.
  "RAMP_TO_AUTO"  — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s.
  "AUTO"          — ESP32-S3 executing Jetson cmd_vel; RC sticks idle.
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
  "RAMP_TO_RC"    — Transitioning AUTO→RC; blend_alpha 1.0→0.0 over ramp_s.

Blend alpha
-----------
  The node publishes blend_alpha ∈ [0, 1] in /saltybot/control_mode.
  The cmd_vel_mux_node scales autonomous cmd_vel by alpha before publishing
  to /cmd_vel (the bridge input).  alpha=0 ⟹ no autonomous motion.

RC override
-----------
  Any stick magnitude > override_threshold while in AUTO instantly forces RC
  (alpha → 0 immediately; no ramp).  _override_active is set so the
  return-to-auto path requires sticks to be neutral for return_delay seconds.

CH6 / toggle
------------
  ch6_auto=True  means the pilot has requested AUTO (switch up/high).
  ch6_auto=False means the pilot has requested RC   (switch down/low).
  Pure CH6 transitions (no stick activity) always ramp gracefully.

Safety interlocks
-----------------
  • No AUTO without slam_ok (valid SLAM fix received recently).
  • Instant drop to RC when rc_link_ok goes False (RC link lost).
  These checks are applied at every control tick.
"""

import math

# ─── Constants ──────────────────────────────────────────────────────────────

MODES = ("RC", "RAMP_TO_AUTO", "AUTO", "RAMP_TO_RC")

LED_PATTERNS = {
    "rc_normal":      "solid_yellow",
    "rc_no_slam":     "solid_yellow",       # no slam needed in RC
    "auto_ok":        "solid_green",
    "auto_no_slam":   "blink_orange_fast",  # shouldn't happen (interlock), but defensive
    "ramp_to_auto":   "blink_green_slow",
    "ramp_to_rc":     "blink_yellow_slow",
    "no_rc_link":     "blink_red_fast",
}

# ─── Stick / CH6 helpers ────────────────────────────────────────────────────

def ch6_wants_auto(axes: list, ch6_idx: int, threshold: float = 0.5) -> bool:
    """
    Return True when CH6 (aux switch) is in the AUTO position.

    Typical CRSF→Joy mapping: AUX2 = axes[5].
    Switch up (high) = +1.0 → AUTO; switch down (low) = -1.0 → RC.
    threshold default 0.5 gives hysteresis against mid-travel noise.
    """
    if ch6_idx < 0 or ch6_idx >= len(axes):
        return False
    return float(axes[ch6_idx]) > threshold


def sticks_active(
    axes: list,
    stick_indices: tuple,
    threshold: float = 0.10,
) -> bool:
    """
    Return True when any monitored stick axis exceeds threshold magnitude.

    stick_indices: tuple of Joy axes indices for the four sticks
                   (roll, pitch, throttle, yaw).  Default CRSF mapping:
                   (0, 1, 2, 3).
    threshold: 10 % of full deflection (issue spec).
    """
    return any(
        abs(float(axes[i])) > threshold
        for i in stick_indices
        if i < len(axes)
    )


# ─── Alpha / ramp helpers ───────────────────────────────────────────────────

def ramp_alpha(elapsed_s: float, ramp_s: float) -> float:
    """
    Compute forward blend alpha [0, 1] linearly over ramp_s seconds.

    elapsed_s : seconds since ramp start.
    ramp_s    : total ramp duration (seconds).
    """
    if ramp_s <= 0.0:
        return 1.0
    return min(1.0, elapsed_s / ramp_s)


def alpha_for_mode(mode: str, elapsed_s: float, ramp_s: float) -> float:
    """
    Return the blend alpha appropriate for `mode`.

      RC           → 0.0
      RAMP_TO_AUTO → 0.0 … 1.0  (linear with elapsed)
      AUTO         → 1.0
      RAMP_TO_RC   → 1.0 … 0.0  (linear with elapsed)
    """
    if mode == "RC":
        return 0.0
    if mode == "AUTO":
        return 1.0
    if mode == "RAMP_TO_AUTO":
        return ramp_alpha(elapsed_s, ramp_s)
    if mode == "RAMP_TO_RC":
        return 1.0 - ramp_alpha(elapsed_s, ramp_s)
    return 0.0


# ─── LED pattern ────────────────────────────────────────────────────────────

def led_pattern(mode: str, slam_ok: bool, rc_link_ok: bool) -> str:
    """Return the LED pattern name for the current operating state."""
    if not rc_link_ok:
        return LED_PATTERNS["no_rc_link"]
    if mode in ("RC", "RAMP_TO_RC"):
        return LED_PATTERNS["rc_normal"]
    if mode == "RAMP_TO_AUTO":
        return LED_PATTERNS["ramp_to_auto"]
    if mode == "AUTO":
        return LED_PATTERNS["auto_ok"] if slam_ok else LED_PATTERNS["auto_no_slam"]
    return LED_PATTERNS["no_rc_link"]


# ─── State machine step ──────────────────────────────────────────────────────

def step_state(
    current_mode: str,
    ramp_elapsed_s: float,
    ramp_s: float,
    ch6_auto: bool,
    sticks_now_active: bool,
    sticks_neutral_elapsed_s: float,
    return_delay_s: float,
    slam_ok: bool,
    rc_link_ok: bool,
    override_active: bool,
) -> tuple:
    """
    Compute (next_mode, next_override_active, instant_to_rc).

    Parameters
    ----------
    current_mode            : current mode string
    ramp_elapsed_s          : seconds since last ramp start
    ramp_s                  : configured ramp duration
    ch6_auto                : True = pilot switch requests AUTO
    sticks_now_active       : True = at least one stick > override_threshold
    sticks_neutral_elapsed_s: seconds since all sticks went neutral (0 if still active)
    return_delay_s          : seconds of stick-neutral required to re-engage AUTO
    slam_ok                 : True = SLAM fix is valid
    rc_link_ok              : True = CRSF link alive
    override_active         : True = currently in stick-override RC

    Returns
    -------
    next_mode           : new mode string
    next_override_active: updated override flag
    instant_to_rc       : True when dropping to RC without ramp (safety / stick override)
    """
    instant_to_rc = False

    # ── Safety: RC link lost → instant RC ────────────────────────────────────
    if not rc_link_ok:
        return "RC", False, True

    # ── Compute override transitions ──────────────────────────────────────────
    next_override = override_active
    if sticks_now_active and current_mode in ("AUTO", "RAMP_TO_AUTO", "RAMP_TO_RC"):
        # Stick input while AUTO or any transition → instant RC override
        next_override = True
        return "RC", True, True

    if not sticks_now_active and sticks_neutral_elapsed_s >= return_delay_s:
        # Sticks have been neutral long enough — clear override
        next_override = False

    # ── Ramp completion ───────────────────────────────────────────────────────
    ramp_done = ramp_elapsed_s >= ramp_s - 1e-9   # epsilon guards float accumulation

    # ── Transitions per state ─────────────────────────────────────────────────
    if current_mode == "RC":
        if ch6_auto and slam_ok and not next_override:
            return "RAMP_TO_AUTO", next_override, False
        return "RC", next_override, False

    if current_mode == "RAMP_TO_AUTO":
        if not ch6_auto:
            return "RAMP_TO_RC", next_override, False
        if not slam_ok:
            return "RAMP_TO_RC", next_override, False
        if ramp_done:
            return "AUTO", next_override, False
        return "RAMP_TO_AUTO", next_override, False

    if current_mode == "AUTO":
        if not ch6_auto:
            return "RAMP_TO_RC", next_override, False
        if not slam_ok:
            return "RAMP_TO_RC", next_override, False
        return "AUTO", next_override, False

    if current_mode == "RAMP_TO_RC":
        if ch6_auto and slam_ok and not next_override:
            return "RAMP_TO_AUTO", next_override, False
        if ramp_done:
            return "RC", next_override, False
        return "RAMP_TO_RC", next_override, False

    return "RC", next_override, True   # unknown state → safe fallback


# ─── cmd_vel blending ───────────────────────────────────────────────────────

def blend_twist(
    auto_linear_x: float,
    auto_angular_z: float,
    alpha: float,
) -> tuple:
    """
    Scale autonomous cmd_vel by blend alpha.

    Returns (linear_x, angular_z) for the blended output.
    alpha=0 → zeros; alpha=1 → full autonomous.
    """
    a = max(0.0, min(1.0, alpha))
    return auto_linear_x * a, auto_angular_z * a
