"""
Full ROV autonomy stack bringup.
Launches all subsystems: MAVROS, navigation, control, perception, mission.

Usage:
  ros2 launch rov_bringup rov_full.launch.py env:=dev
  ros2 launch rov_bringup rov_full.launch.py env:=field
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os


def generate_launch_description():
    # Package directories
    nav_pkg = get_package_share_directory('rov_navigation')
    ctrl_pkg = get_package_share_directory('rov_control')
    perc_pkg = get_package_share_directory('rov_perception')
    mission_pkg = get_package_share_directory('rov_mission')
    bringup_pkg = get_package_share_directory('rov_bringup')

    env_arg = DeclareLaunchArgument(
        'env', default_value='dev',
        description='Deployment environment: dev, pool, field'
    )

    blueos_ip_arg = DeclareLaunchArgument(
        'blueos_ip', default_value='192.168.1.x',
        description='IP address of BlueOS (RPi4)'
    )

    return LaunchDescription([
        env_arg,
        blueos_ip_arg,

        # MAVROS — bridge to ArduSub on RPi4
        Node(
            package='mavros',
            executable='mavros_node',
            name='mavros',
            output='screen',
            parameters=[{
                'fcu_url': ['udp://', LaunchConfiguration('blueos_ip'), ':14550@14555'],
                'gcs_url': '',
                'target_system_id': 1,
                'target_component_id': 1,
            }],
        ),

        # Navigation stack
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(nav_pkg, 'launch', 'navigation.launch.py')
            ),
        ),

        # Control stack (failsafe first, then motion)
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(ctrl_pkg, 'launch', 'control.launch.py')
            ),
        ),

        # Perception stack
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(perc_pkg, 'launch', 'perception.launch.py')
            ),
        ),

        # Mission executor
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(mission_pkg, 'launch', 'mission.launch.py')
            ),
        ),
    ])