"""
mode_switch_node.py — Autonomous/RC mode switch for saltybot (Issue #104).

Inputs
------
  /rc/joy                  (sensor_msgs/Joy)
      CRSF channel axes:
        axes[ch6_axis]        AUX2 — mode toggle  (+1=AUTO, -1=RC)
        axes[stick_axes...]   Roll/Pitch/Throttle/Yaw — override detection

  /saltybot/balance_state  (std_msgs/String JSON)
<<<<<<< HEAD
      Parsed for RC link health (field "rc_link") and ESP32 BALANCE mode.
=======
      Parsed for RC link health (field "rc_link") and ESP32-S3 mode.
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)

  <slam_fix_topic>         (geometry_msgs/PoseWithCovarianceStamped)
      Any message received within slam_fix_timeout_s → SLAM fix valid.

Outputs
-------
  /saltybot/control_mode   (std_msgs/String JSON)
      {
        "mode":           "RC" | "RAMP_TO_AUTO" | "AUTO" | "RAMP_TO_RC",
        "blend_alpha":    0.0 … 1.0,
        "slam_ok":        bool,
        "rc_link_ok":     bool,
        "override_active": bool
      }

  /saltybot/led_pattern    (std_msgs/String)
      LED pattern name consumed by the LED controller node.

State machine
-------------
  See mode_logic.py for the full transition table.

  RC           — pilot in control; blend_alpha = 0.0
  RAMP_TO_AUTO — transitioning; blend_alpha 0→1 over ramp_duration_s (500 ms)
  AUTO         — Jetson in control; blend_alpha = 1.0
  RAMP_TO_RC   — transitioning; blend_alpha 1→0 over ramp_duration_s (500 ms)

Safety interlocks
-----------------
  • Any stick > override_threshold while AUTO/RAMP_TO_AUTO → instant RC.
  • rc_link lost (Joy silent > rc_link_timeout_s) → instant RC.
  • AUTO not entered without valid SLAM fix.
  • SLAM fix lost while AUTO → RAMP_TO_RC.

Parameters
----------
  ch6_axis            Joy axes index for the mode-toggle switch   (default 5)
  stick_axes          Space-separated Joy indices for 4 sticks    (default "0 1 2 3")
  override_threshold  Stick magnitude for instant RC override      (default 0.10)
  return_delay_s      Sticks-neutral duration before re-engage    (default 2.0 s)
  ramp_duration_s     RC↔AUTO blend ramp duration                 (default 0.5 s)
  rc_link_timeout_s   Joy silence threshold → RC link lost        (default 0.5 s)
  slam_fix_topic      Pose topic for SLAM fix detection           (default /slam_toolbox/pose_with_covariance_stamped)
  slam_fix_timeout_s  Pose silence threshold → SLAM fix lost      (default 3.0 s)
  control_rate        State machine tick rate (Hz)                (default 20.0)
"""

import json
import time

import rclpy
from rclpy.node import Node
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, DurabilityPolicy

from geometry_msgs.msg import PoseWithCovarianceStamped
from sensor_msgs.msg import Joy
from std_msgs.msg import String

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

_BIG = 1e9   # sentinel: "never received"


class ModeSwitchNode(Node):

    def __init__(self):
        super().__init__("mode_switch")

        # ── Parameters ────────────────────────────────────────────────────────
        self.declare_parameter("ch6_axis",           5)
        self.declare_parameter("stick_axes",         "0 1 2 3")
        self.declare_parameter("override_threshold",  0.10)
        self.declare_parameter("return_delay_s",      2.0)
        self.declare_parameter("ramp_duration_s",     0.5)
        self.declare_parameter("rc_link_timeout_s",   0.5)
        self.declare_parameter("slam_fix_topic",
                               "/slam_toolbox/pose_with_covariance_stamped")
        self.declare_parameter("slam_fix_timeout_s",  3.0)
        self.declare_parameter("control_rate",       20.0)

        self._reload_params()

        # ── State ─────────────────────────────────────────────────────────────
        self._mode: str           = "RC"
        self._override_active: bool = False
        self._ramp_start: float   = 0.0      # monotonic time ramp began
        self._sticks_neutral_since: float | None = None
        self._last_joy_t: float   = 0.0      # monotonic; 0 = never
        self._last_slam_t: float  = 0.0
        self._joy_axes: list      = []
        self._esp32_mode: int     = 0        # from balance_state JSON

        # ── QoS ───────────────────────────────────────────────────────────────
        best_effort = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=5,
        )
        reliable = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
        )
        # SLAM toolbox uses transient_local for its pose topic
        transient = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            depth=1,
        )

        # ── Subscriptions ─────────────────────────────────────────────────────
        self.create_subscription(Joy, "/rc/joy", self._joy_cb, best_effort)
        self.create_subscription(
            String, "/saltybot/balance_state", self._balance_cb, reliable)

        slam_topic = self.get_parameter("slam_fix_topic").value
        # Try transient_local first (SLAM Toolbox); fall back gracefully if not
        try:
            self.create_subscription(
                PoseWithCovarianceStamped, slam_topic,
                self._slam_cb, transient)
        except Exception:
            self.create_subscription(
                PoseWithCovarianceStamped, slam_topic,
                self._slam_cb, reliable)

        # ── Publishers ────────────────────────────────────────────────────────
        self._mode_pub = self.create_publisher(
            String, "/saltybot/control_mode", reliable)
        self._led_pub  = self.create_publisher(
            String, "/saltybot/led_pattern", reliable)

        # ── Control timer ─────────────────────────────────────────────────────
        self._timer = self.create_timer(
            1.0 / self._p["control_rate"], self._tick)

        self.get_logger().info(
            f"ModeSwitchNode ready  "
            f"ramp={self._p['ramp_duration_s']:.1f}s  "
            f"override_thresh={self._p['override_threshold']:.0%}  "
            f"return_delay={self._p['return_delay_s']:.1f}s  "
            f"slam_topic={slam_topic}"
        )

    # ── Parameter reload ──────────────────────────────────────────────────────

    def _reload_params(self):
        raw_stick_axes = self.get_parameter("stick_axes").value
        self._p = {
            "ch6_axis":           int(self.get_parameter("ch6_axis").value),
            "stick_indices":      tuple(int(x) for x in raw_stick_axes.split()),
            "override_threshold": float(self.get_parameter("override_threshold").value),
            "return_delay_s":     float(self.get_parameter("return_delay_s").value),
            "ramp_duration_s":    float(self.get_parameter("ramp_duration_s").value),
            "rc_link_timeout_s":  float(self.get_parameter("rc_link_timeout_s").value),
            "slam_fix_timeout_s": float(self.get_parameter("slam_fix_timeout_s").value),
            "control_rate":       float(self.get_parameter("control_rate").value),
        }

    # ── Callbacks ─────────────────────────────────────────────────────────────

    def _joy_cb(self, msg: Joy) -> None:
        self._joy_axes  = list(msg.axes)
        self._last_joy_t = time.monotonic()

    def _balance_cb(self, msg: String) -> None:
        try:
            data = json.loads(msg.data)
            # "mode" is a label string; map back to int for reference
            mode_label = data.get("mode", "RC_MANUAL")
            self._esp32_mode = {"RC_MANUAL": 0, "RC_ASSISTED": 1,
                                "AUTONOMOUS": 2}.get(mode_label, 0)
        except (json.JSONDecodeError, TypeError):
            pass

    def _slam_cb(self, msg: PoseWithCovarianceStamped) -> None:
        self._last_slam_t = time.monotonic()

    # ── 20 Hz control tick ────────────────────────────────────────────────────

    def _tick(self) -> None:
        self._reload_params()
        now = time.monotonic()
        p   = self._p

        # ── Derive inputs ─────────────────────────────────────────────────────
        joy_age    = (now - self._last_joy_t) if self._last_joy_t > 0.0 else _BIG
        slam_age   = (now - self._last_slam_t) if self._last_slam_t > 0.0 else _BIG
        rc_link_ok = joy_age <= p["rc_link_timeout_s"]
        slam_ok    = slam_age <= p["slam_fix_timeout_s"]

        axes = self._joy_axes
        ch6  = ch6_wants_auto(axes, p["ch6_axis"])
        active = sticks_active(axes, p["stick_indices"], p["override_threshold"])

        # Track neutral timer
        if active:
            self._sticks_neutral_since = None
        elif self._sticks_neutral_since is None:
            self._sticks_neutral_since = now

        neutral_elapsed = (
            (now - self._sticks_neutral_since)
            if self._sticks_neutral_since is not None else 0.0
        )

        ramp_elapsed = now - self._ramp_start

        # ── State machine step ────────────────────────────────────────────────
        prev_mode = self._mode
        next_mode, next_override, instant = step_state(
            current_mode=self._mode,
            ramp_elapsed_s=ramp_elapsed,
            ramp_s=p["ramp_duration_s"],
            ch6_auto=ch6,
            sticks_now_active=active,
            sticks_neutral_elapsed_s=neutral_elapsed,
            return_delay_s=p["return_delay_s"],
            slam_ok=slam_ok,
            rc_link_ok=rc_link_ok,
            override_active=self._override_active,
        )

        if next_mode != prev_mode:
            # Log transitions; reset ramp timer on ramp entry
            self.get_logger().info(
                f"Mode: {prev_mode} → {next_mode}"
                f"  slam_ok={slam_ok} rc_link={rc_link_ok}"
                f"  override={next_override}"
            )
            if next_mode in ("RAMP_TO_AUTO", "RAMP_TO_RC"):
                self._ramp_start = now
                ramp_elapsed = 0.0

        self._mode            = next_mode
        self._override_active = next_override

        # ── Compute blend alpha ───────────────────────────────────────────────
        current_ramp_elapsed = now - self._ramp_start
        alpha = alpha_for_mode(next_mode, current_ramp_elapsed, p["ramp_duration_s"])
        if instant:
            alpha = 0.0

        # ── Log steady warnings ───────────────────────────────────────────────
        if next_mode == "AUTO" and not slam_ok:
            self.get_logger().warn(
                "In AUTO but SLAM fix lost — will exit AUTO",
                throttle_duration_sec=2.0)
        if not rc_link_ok:
            self.get_logger().warn(
                "RC link lost — forced RC mode",
                throttle_duration_sec=1.0)

        # ── Publish /saltybot/control_mode ────────────────────────────────────
        payload = {
            "mode":            next_mode,
            "blend_alpha":     round(alpha, 4),
            "slam_ok":         slam_ok,
            "rc_link_ok":      rc_link_ok,
            "override_active": next_override,
        }
        mode_msg      = String()
        mode_msg.data = json.dumps(payload)
        self._mode_pub.publish(mode_msg)

        # ── Publish /saltybot/led_pattern ─────────────────────────────────────
        pattern      = led_pattern(next_mode, slam_ok, rc_link_ok)
        led_msg      = String()
        led_msg.data = pattern
        self._led_pub.publish(led_msg)


# ── Entry point ───────────────────────────────────────────────────────────────

def main(args=None):
    rclpy.init(args=args)
    node = ModeSwitchNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == "__main__":
    main()
