#!/usr/bin/env python3
"""
Voice Command Router (Issue #491)
Receives transcribed text and routes to appropriate action commands.
Uses fuzzy matching to handle natural speech variations.
"""

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from rapidfuzz import process, fuzz
import json


class VoiceCommand:
    """Voice command definition with fuzzy matching patterns."""
    def __init__(self, name, patterns, action_type, topic=None, service=None, args=None):
        self.name = name
        self.patterns = patterns  # List of recognized variations
        self.action_type = action_type  # 'publish' or 'service'
        self.topic = topic
        self.service = service
        self.args = args or {}


class VoiceCommandRouter(Node):
    """Routes voice commands to appropriate ROS2 actions."""

    def __init__(self):
        super().__init__('voice_command_router')

        # Define all voice commands with fuzzy matching patterns
        self.commands = self._init_commands()

        # Create subscription to transcribed speech
        self.transcription_sub = self.create_subscription(
            String,
            '/speech_recognition/transcribed_text',
            self.transcription_callback,
            10
        )

        # Create publisher for parsed voice commands
        self.command_pub = self.create_publisher(String, '/saltybot/voice_command', 10)

        # Create publishers for direct command topics
        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
        self.action_pub = self.create_publisher(String, '/saltybot/action_command', 10)

        # Create service clients (lazily initialized on first use)
        self.service_clients = {}

        self.get_logger().info('Voice command router initialized')

    def _init_commands(self):
        """Initialize voice command definitions."""
        return {
            'follow_me': VoiceCommand(
                'follow_me',
                ['follow me', 'follow me please', 'start following', 'come with me', 'follow'],
                'publish',
                topic='/saltybot/action_command',
                args={'action': 'follow_person'}
            ),
            'stop': VoiceCommand(
                'stop',
                ['stop', 'halt', 'hold', 'pause', 'freeze', 'dont move'],
                'publish',
                topic='/cmd_vel',
                args={'linear': {'x': 0.0, 'y': 0.0}, 'angular': {'z': 0.0}}
            ),
            'go_home': VoiceCommand(
                'go_home',
                ['go home', 'return home', 'back home', 'go to dock', 'charge', 'return to dock'],
                'publish',
                topic='/saltybot/action_command',
                args={'action': 'return_home'}
            ),
            'patrol': VoiceCommand(
                'patrol',
                ['patrol', 'start patrol', 'begin patrol', 'patrol mode', 'autonomous mode'],
                'publish',
                topic='/saltybot/action_command',
                args={'action': 'start_patrol'}
            ),
            'come_here': VoiceCommand(
                'come_here',
                ['come here', 'come to me', 'approach', 'move closer', 'get closer'],
                'publish',
                topic='/saltybot/action_command',
                args={'action': 'approach_person'}
            ),
            'sit': VoiceCommand(
                'sit',
                ['sit', 'sit down', 'sit please', 'lower yourself'],
                'publish',
                topic='/saltybot/action_command',
                args={'action': 'sit'}
            ),
            'spin': VoiceCommand(
                'spin',
                ['spin', 'spin around', 'rotate', 'turn around', 'twirl'],
                'publish',
                topic='/saltybot/action_command',
                args={'action': 'spin', 'count': 1}
            ),
            'dance': VoiceCommand(
                'dance',
                ['dance', 'dance with me', 'do a dance', 'move to music', 'groove'],
                'publish',
                topic='/saltybot/action_command',
                args={'action': 'dance'}
            ),
            'take_photo': VoiceCommand(
                'take_photo',
                ['take a photo', 'take a picture', 'photo', 'picture', 'take photo', 'smile'],
                'service',
                service='/photo/capture',
                args={'save': True}
            ),
            'whats_that': VoiceCommand(
                'whats_that',
                ['whats that', 'what is that', 'identify', 'recognize', 'what do you see'],
                'publish',
                topic='/saltybot/action_command',
                args={'action': 'identify_object'}
            ),
            'battery_status': VoiceCommand(
                'battery_status',
                ['battery status', 'battery level', 'how much battery', 'check battery', 'whats my battery'],
                'publish',
                topic='/saltybot/action_command',
                args={'action': 'report_battery'}
            ),
        }

    def transcription_callback(self, msg):
        """Handle incoming transcribed text from speech recognition."""
        transcribed_text = msg.data.lower().strip()
        self.get_logger().info(f'Received transcription: "{transcribed_text}"')

        # Try to match against known commands using fuzzy matching
        command = self.match_command(transcribed_text)

        if command:
            self.get_logger().info(f'Matched command: {command.name}')
            self.execute_command(command)
        else:
            self.get_logger().warning(f'No matching command found for: "{transcribed_text}"')

    def match_command(self, text, threshold=80):
        """
        Use fuzzy matching to find best matching command.
        Returns the best matching VoiceCommand or None.
        """
        best_score = 0
        best_command = None

        for cmd_name, cmd in self.commands.items():
            # Try matching against each pattern for this command
            for pattern in cmd.patterns:
                score = fuzz.token_set_ratio(text, pattern)

                if score > best_score and score >= threshold:
                    best_score = score
                    best_command = cmd

        return best_command

    def execute_command(self, command: VoiceCommand):
        """Execute the matched voice command."""
        try:
            if command.action_type == 'publish':
                self.publish_command(command)
            elif command.action_type == 'service':
                self.call_service(command)

            # Always publish to /saltybot/voice_command for monitoring
            cmd_msg = String(data=json.dumps({
                'command': command.name,
                'action_type': command.action_type,
                'timestamp': self.get_clock().now().to_msg(),
            }))
            self.command_pub.publish(cmd_msg)

        except Exception as e:
            self.get_logger().error(f'Error executing command {command.name}: {e}')

    def publish_command(self, command: VoiceCommand):
        """Publish command to appropriate topic."""
        if not command.topic:
            return

        if command.topic == '/cmd_vel':
            # Publish Twist message for velocity commands
            msg = Twist()
            msg.linear.x = command.args['linear'].get('x', 0.0)
            msg.linear.y = command.args['linear'].get('y', 0.0)
            msg.angular.z = command.args['angular'].get('z', 0.0)
            self.cmd_vel_pub.publish(msg)

        else:
            # Publish String message with command data
            msg = String(data=json.dumps(command.args))
            self.action_pub.publish(msg)

    def call_service(self, command: VoiceCommand):
        """Call a service for the command."""
        if not command.service:
            return

        try:
            # Lazy initialize service client
            if command.service not in self.service_clients:
                from std_srvs.srv import Empty
                client = self.create_client(Empty, command.service)
                self.service_clients[command.service] = client

            client = self.service_clients[command.service]

            # Wait for service to be available
            if not client.wait_for_service(timeout_sec=2.0):
                self.get_logger().warning(f'Service {command.service} not available')
                return

            # Call service
            from std_srvs.srv import Empty
            request = Empty.Request()
            future = client.call_async(request)
            rclpy.spin_until_future_complete(self, future)

        except Exception as e:
            self.get_logger().error(f'Error calling service {command.service}: {e}')


def main(args=None):
    rclpy.init(args=args)
    node = VoiceCommandRouter()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
