"""
autonomous_mode.launch.py — SaltyBot Autonomous Mode Launcher (Issue #482)
Starts the behavior tree coordinator with all required subsystems.
"""

from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
import os

def generate_launch_description():
    """Generate launch description for autonomous mode."""

    saltybot_bringup_dir = FindPackageShare('saltybot_bringup')
    bt_xml_dir = PathJoinSubstitution([saltybot_bringup_dir, 'behavior_trees'])

    return LaunchDescription([

        # ────────────────────────────────────────────────────────────────────────
        # Behavior Tree Executor Node (Nav2 BT framework)
        # ────────────────────────────────────────────────────────────────────────
        Node(
            package='saltybot_bt_executor',
            executable='behavior_tree_executor',
            name='autonomous_coordinator',
            output='screen',
            parameters=[{
                'bt_xml_filename': PathJoinSubstitution([
                    bt_xml_dir, 'autonomous_coordinator.xml'
                ]),
                'blackboard_variables': {
                    'battery_level': 100.0,
                    'person_detected': False,
                    'obstacle_state': 'clear',
                    'current_mode': 'idle',
                    'home_pose': {
                        'header': {
                            'frame_id': 'map',
                            'stamp': {'sec': 0, 'nsec': 0}
                        },
                        'pose': {
                            'position': {'x': 0.0, 'y': 0.0, 'z': 0.0},
                            'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
                        }
                    }
                },
                'enable_groot_monitoring': True,
                'groot_port': 5555,
            }],
            remappings=[
                ('/clicked_point', '/bt/clicked_point'),
                ('/goal_pose', '/bt/goal_pose'),
            ],
        ),

        # ────────────────────────────────────────────────────────────────────────
        # Battery Monitor (publishes to /battery_state for BT)
        # ────────────────────────────────────────────────────────────────────────
        Node(
            package='saltybot_battery_monitor',
            executable='battery_monitor_node',
            name='battery_monitor',
            output='screen',
            parameters=[{
                'update_rate': 1.0,  # Hz
                'critical_threshold': 10.0,
                'warning_threshold': 20.0,
            }],
        ),

        # ────────────────────────────────────────────────────────────────────────
        # Person Detector (publishes detected persons for BT)
        # ────────────────────────────────────────────────────────────────────────
        Node(
            package='saltybot_perception',
            executable='person_detector',
            name='person_detector',
            output='screen',
        ),

        # ────────────────────────────────────────────────────────────────────────
        # Obstacle Monitor (LIDAR-based obstacle detection)
        # ────────────────────────────────────────────────────────────────────────
        Node(
            package='saltybot_lidar_avoidance',
            executable='obstacle_monitor',
            name='obstacle_monitor',
            output='screen',
            parameters=[{
                'danger_distance': 0.3,
                'warning_distance': 0.6,
            }],
        ),

        # ────────────────────────────────────────────────────────────────────────
        # Autonomy Mode Manager (handles mode transitions, safety checks)
        # ────────────────────────────────────────────────────────────────────────
        Node(
            package='saltybot_mode_switch',
            executable='autonomous_mode_manager',
            name='autonomous_mode_manager',
            output='screen',
            parameters=[{
                'auto_return_battery': 20.0,  # Return home if battery < 20%
                'enable_geofence': True,
                'geofence_boundary_distance': 5.0,
            }],
        ),

        # ────────────────────────────────────────────────────────────────────────
        # Person Follower (follows detected persons)
        # ────────────────────────────────────────────────────────────────────────
        Node(
            package='saltybot_follower',
            executable='person_follower',
            name='person_follower',
            output='screen',
            parameters=[{
                'follow_distance': 1.5,
                'max_linear_vel': 0.5,
                'max_angular_vel': 1.0,
            }],
        ),

        # ────────────────────────────────────────────────────────────────────────
        # Curiosity Behavior (autonomous exploration)
        # ────────────────────────────────────────────────────────────────────────
        Node(
            package='saltybot_curiosity',
            executable='curiosity_explorer',
            name='curiosity_explorer',
            output='screen',
            parameters=[{
                'exploration_mode': 'active',
                'max_exploration_distance': 2.0,
                'idle_timeout': 60.0,  # Start exploration after 60s idle
            }],
        ),

        # ────────────────────────────────────────────────────────────────────────
        # Gesture Recognition (for interactive mode)
        # ────────────────────────────────────────────────────────────────────────
        Node(
            package='saltybot_gesture_recognition',
            executable='gesture_recognition_node',
            name='gesture_recognition',
            output='screen',
            parameters=[{
                'min_confidence': 0.6,
            }],
        ),

        # ────────────────────────────────────────────────────────────────────────
        # TTS Service (for greetings and social interaction)
        # ────────────────────────────────────────────────────────────────────────
        Node(
            package='saltybot_tts_service',
            executable='tts_service',
            name='tts_service',
            output='screen',
            parameters=[{
                'voice_speed': 1.0,
                'voice_pitch': 1.0,
            }],
        ),

        # ────────────────────────────────────────────────────────────────────────
        # Emergency Stop Monitor (highest priority safety)
        # ────────────────────────────────────────────────────────────────────────
        Node(
            package='saltybot_emergency_stop',
            executable='emergency_stop_monitor',
            name='emergency_stop_monitor',
            output='screen',
        ),

    ])