"""voice_command_router.py — Command execution router for SaltyBot (Issue #409).

Subscribes to /saltybot/voice_command and executes commands by publishing
appropriate navigation and system commands. Includes TTS confirmation via Piper.

ROS2 topics
-----------
Subscribe: /saltybot/voice_command            (saltybot_social_msgs/VoiceCommand)
Publish:   /saltybot/cmd_vel                  (geometry_msgs/Twist) for nav commands
Publish:   /saltybot/tts_text                 (std_msgs/String) for TTS output

Parameters
----------
min_confidence          float   0.70    Only execute commands above this confidence
enable_tts             bool    true    Enable TTS confirmation
battery_topic          str     "/battery_status"  Topic for battery status
"""

from __future__ import annotations

import subprocess
from typing import Dict, Callable, Optional

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy

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


class VoiceCommandRouter(Node):
    """Routes voice commands to appropriate ROS2 topics/services."""

    def __init__(self) -> None:
        super().__init__("voice_command_router")

        # ── Parameters ──────────────────────────────────────────────────────
        self.declare_parameter("min_confidence", 0.70)
        self.declare_parameter("enable_tts", True)

        self._min_conf: float = self.get_parameter("min_confidence").value
        self._enable_tts: bool = self.get_parameter("enable_tts").value

        # ── Reliable QoS ────────────────────────────────────────────────────
        qos = QoSProfile(depth=10)
        qos.reliability = QoSReliabilityPolicy.RELIABLE

        self._cmd_vel_pub = self.create_publisher(Twist, "/saltybot/cmd_vel", qos)
        self._tts_pub = self.create_publisher(String, "/saltybot/tts_text", qos)

        self._cmd_sub = self.create_subscription(
            VoiceCommand,
            "/saltybot/voice_command",
            self._on_voice_command,
            qos,
        )

        # ── Command handlers ────────────────────────────────────────────────
        self._handlers: Dict[str, Callable] = {
            "nav.follow_me": self._handle_follow_me,
            "nav.stop": self._handle_stop,
            "nav.come_here": self._handle_come_here,
            "nav.go_home": self._handle_go_home,
            "system.battery": self._handle_battery,
            "nav.spin": self._handle_spin,
            "system.quiet_mode": self._handle_quiet_mode,
        }

        self.get_logger().info("voice_command_router ready")

    def _on_voice_command(self, msg: VoiceCommand) -> None:
        """Process incoming voice command."""
        if msg.confidence < self._min_conf:
            self.get_logger().debug(
                f"Ignoring '{msg.intent}' (confidence={msg.confidence:.2f} < {self._min_conf})"
            )
            return

        self.get_logger().info(f"Executing command: {msg.intent}")

        if msg.intent == "fallback":
            self._tts_speak("I didn't understand that command. Try 'follow me', 'stop', or 'go home'.")
            return

        handler = self._handlers.get(msg.intent)
        if handler:
            handler()
        else:
            self.get_logger().warn(f"No handler for intent: {msg.intent}")

    # ── Navigation Command Handlers ─────────────────────────────────────────

    def _handle_follow_me(self) -> None:
        """Start following the speaker."""
        self._tts_speak("Following you.")
        # Publish a marker to indicate follow mode
        # (actual following behavior is handled by nav stack)
        twist = Twist()
        twist.linear.x = 0.5  # Start moving forward
        self._cmd_vel_pub.publish(twist)

    def _handle_stop(self) -> None:
        """Stop all motion."""
        self._tts_speak("Stopping.")
        twist = Twist()  # Zero velocity stops motion
        self._cmd_vel_pub.publish(twist)

    def _handle_come_here(self) -> None:
        """Come towards the speaker."""
        self._tts_speak("Coming to you.")
        # Command navigation system to approach speaker
        twist = Twist()
        twist.linear.x = 0.3  # Approach speed
        self._cmd_vel_pub.publish(twist)

    def _handle_go_home(self) -> None:
        """Return to base/dock."""
        self._tts_speak("Going home.")
        # Trigger homing routine (specific intent/service call)
        # For now, just indicate intention

    def _handle_battery(self) -> None:
        """Announce battery status."""
        # In a real implementation, would read from battery_status topic
        self._tts_speak("Battery status: Full charge. Ready to go.")

    def _handle_spin(self) -> None:
        """Execute a spin."""
        self._tts_speak("Spinning.")
        twist = Twist()
        twist.angular.z = 1.0  # Spin at 1 rad/s
        self._cmd_vel_pub.publish(twist)
        # Would need a timer to stop spin after timeout

    def _handle_quiet_mode(self) -> None:
        """Suppress TTS output."""
        self._enable_tts = False
        if self._enable_tts:  # Announce before going quiet
            self._tts_speak("Quiet mode enabled.")

    # ── TTS Helper ──────────────────────────────────────────────────────────

    def _tts_speak(self, text: str) -> None:
        """Queue text for TTS output via Piper."""
        if not self._enable_tts:
            return

        try:
            # Try to speak using piper command if available
            subprocess.Popen(
                ["echo", text, "|", "piper", "--model", "en_US-ryan-high"],
                stdout=subprocess.DEVNULL,
                stderr=subprocess.DEVNULL,
            )
        except (FileNotFoundError, OSError):
            # Fall back to publishing message (external TTS node will handle)
            pass

        # Always publish the text message for external TTS nodes
        msg = String()
        msg.data = text
        self._tts_pub.publish(msg)
        self.get_logger().info(f"[TTS] {text}")


def main(args=None) -> None:
    rclpy.init(args=args)
    node = VoiceCommandRouter()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
