"""
cmd_vel_mux_node.py — Blend-alpha cmd_vel multiplexer (Issue #104).

Sits between the Nav2 / person-follower stack and the cmd_vel_bridge_node,
scaling the autonomous cmd_vel output by the blend_alpha published by
mode_switch_node so that the RC → AUTO handoff is smooth over 500 ms.

Topic graph
-----------
  Nav2 / follower → /cmd_vel_auto ──┐
                                     ├── cmd_vel_mux → /cmd_vel → bridge
  mode_switch → /saltybot/control_mode (blend_alpha)

In RC mode (blend_alpha ≈ 0) the node publishes Twist(0,0) so the bridge
receives zeros — this is harmless because the bridge's mode gate already
<<<<<<< HEAD
prevents autonomous commands when the ESP32 BALANCE is in RC_MANUAL.
=======
prevents autonomous commands when the ESP32-S3 is in RC_MANUAL.
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)

The bridge's existing ESC ramp handles hardware-level smoothing;
the blend_alpha here provides the higher-level cmd_vel policy ramp.

Parameters
----------
  cmd_vel_timeout_s  Deadman: zero output if /cmd_vel_auto goes silent  (0.5 s)
  control_rate       Output publish rate (Hz)                           (20.0)
"""

import json
import time

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

from geometry_msgs.msg import Twist
from std_msgs.msg import String

from saltybot_mode_switch.mode_logic import blend_twist

_BIG = 1e9


class CmdVelMuxNode(Node):

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

        self.declare_parameter("cmd_vel_timeout_s", 0.5)
        self.declare_parameter("control_rate",      20.0)

        self._timeout      = self.get_parameter("cmd_vel_timeout_s").value
        self._rate         = self.get_parameter("control_rate").value

        reliable = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
        )

        # ── State ─────────────────────────────────────────────────────────────
        self._auto_linear_x:  float = 0.0
        self._auto_angular_z: float = 0.0
        self._blend_alpha:    float = 0.0
        self._last_auto_t:    float = 0.0

        # ── Subscriptions ─────────────────────────────────────────────────────
        self.create_subscription(
            Twist, "/cmd_vel_auto", self._auto_cb, reliable)
        self.create_subscription(
            String, "/saltybot/control_mode", self._mode_cb, reliable)

        # ── Publisher ─────────────────────────────────────────────────────────
        self._pub = self.create_publisher(Twist, "/cmd_vel", reliable)

        # ── Timer ─────────────────────────────────────────────────────────────
        self._timer = self.create_timer(1.0 / self._rate, self._publish_cb)

        self.get_logger().info(
            f"CmdVelMuxNode ready  rate={self._rate:.0f}Hz  "
            f"timeout={self._timeout:.1f}s"
        )

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

    def _auto_cb(self, msg: Twist) -> None:
        self._auto_linear_x  = msg.linear.x
        self._auto_angular_z = msg.angular.z
        self._last_auto_t    = time.monotonic()

    def _mode_cb(self, msg: String) -> None:
        try:
            data = json.loads(msg.data)
            self._blend_alpha = float(data.get("blend_alpha", 0.0))
        except (json.JSONDecodeError, TypeError, ValueError):
            self._blend_alpha = 0.0

    # ── 20 Hz publish ─────────────────────────────────────────────────────────

    def _publish_cb(self) -> None:
        now = time.monotonic()

        # Deadman: zero if autonomous source went silent
        auto_age = (now - self._last_auto_t) if self._last_auto_t > 0.0 else _BIG
        if auto_age > self._timeout:
            linear_x, angular_z = 0.0, 0.0
        else:
            linear_x, angular_z = blend_twist(
                self._auto_linear_x,
                self._auto_angular_z,
                self._blend_alpha,
            )

        twist          = Twist()
        twist.linear.x  = linear_x
        twist.angular.z = angular_z
        self._pub.publish(twist)


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

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


if __name__ == "__main__":
    main()
