#!/usr/bin/env python3
"""Full Simulation Launch - GPS-Denied Navigation."""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import os


def generate_launch_description():
    pkg_share = FindPackageShare('uav_ugv_simulation').find('uav_ugv_simulation')
    
    world_arg = DeclareLaunchArgument(
        'world',
        default_value=os.path.join(pkg_share, 'worlds', 'empty_custom.world'),
        description='Path to world file'
    )
    
    use_nvidia_arg = DeclareLaunchArgument(
        'use_nvidia', default_value='true', description='Enable NVIDIA GPU'
    )
    
    headless_arg = DeclareLaunchArgument(
        'headless', default_value='false', description='Run without GUI'
    )
    
    use_ground_truth_arg = DeclareLaunchArgument(
        'use_ground_truth', default_value='true', description='Use Gazebo ground truth'
    )
    
    # Gazebo Harmonic (gz sim) instead of Gazebo Classic
    gazebo = ExecuteProcess(
        cmd=[
            'gz', 'sim', '-v4', '-r',
            os.path.expanduser('~/ardupilot_gazebo/worlds/iris_runway.sdf')
        ],
        output='screen',
        additional_env={
            'GZ_SIM_RESOURCE_PATH': f"{pkg_share}/models:{os.path.expanduser('~/ardupilot_gazebo/models')}:{os.path.expanduser('~/ardupilot_gazebo/worlds')}",
            'GZ_SIM_SYSTEM_PLUGIN_PATH': os.path.expanduser('~/ardupilot_gazebo/build')
        }
    )
    
    ardupilot_uav = TimerAction(
        period=5.0,  # Wait for Gazebo to initialize
        actions=[
            ExecuteProcess(
                cmd=[
                    'sim_vehicle.py',
                    '-v', 'ArduCopter',
                    '-f', 'gazebo-iris',
                    '--model', 'JSON',
                    '--no-mavproxy',  # Don't start MAVProxy, MAVROS will connect
                    '-I0',
                ],
                cwd=os.path.expanduser('~/ardupilot'),
                output='screen',
                additional_env={
                    'PATH': os.environ.get('PATH', '') + ':' + os.path.expanduser('~/ardupilot/Tools/autotest')
                }
            )
        ]
    )
    
    mavros_uav = TimerAction(
        period=15.0,  # Wait for ArduPilot SITL to start
        actions=[
            Node(
                package='mavros',
                executable='mavros_node',
                name='mavros_uav',
                namespace='uav',
                output='screen',
                parameters=[
                    os.path.join(pkg_share, 'config', 'mavros_params.yaml'),
                    {
                        'fcu_url': 'tcp://127.0.0.1:5760',  # SITL TCP port
                        'gcs_url': '',
                        'target_system_id': 1,
                        'target_component_id': 1,
                        'fcu_protocol': 'v2.0'
                    }
                ]
            )
        ]
    )
    
    visual_odom_node = TimerAction(
        period=10.0,
        actions=[
            Node(
                package='uav_ugv_simulation',
                executable='visual_odom_node',
                name='visual_odom_node',
                namespace='uav',
                output='screen',
                parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')]
            )
        ]
    )
    
    geofence_node = TimerAction(
        period=12.0,
        actions=[
            Node(
                package='uav_ugv_simulation',
                executable='geofence_node',
                name='geofence_monitor',
                namespace='uav',
                output='screen',
                parameters=[os.path.join(pkg_share, 'config', 'geofence_params.yaml')]
            )
        ]
    )
    
    vision_nav_node = TimerAction(
        period=14.0,
        actions=[
            Node(
                package='uav_ugv_simulation',
                executable='vision_nav_node',
                name='vision_nav_node',
                namespace='uav',
                output='screen',
                parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')]
            )
        ]
    )
    
    uav_controller = TimerAction(
        period=15.0,
        actions=[
            Node(
                package='uav_ugv_simulation',
                executable='uav_controller',
                name='uav_controller',
                namespace='uav',
                output='screen',
                parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')]
            )
        ]
    )
    
    failsafe_handler = TimerAction(
        period=16.0,
        actions=[
            Node(
                package='uav_ugv_simulation',
                executable='failsafe_handler',
                name='failsafe_handler',
                output='screen',
                parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')]
            )
        ]
    )
    
    return LaunchDescription([
        world_arg,
        use_nvidia_arg,
        headless_arg,
        use_ground_truth_arg,
        gazebo,
        ardupilot_uav,
        mavros_uav,
        visual_odom_node,
        geofence_node,
        vision_nav_node,
        uav_controller,
        failsafe_handler,
    ])