#!/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 = ExecuteProcess(
        cmd=['gazebo', '--verbose', LaunchConfiguration('world')],
        output='screen',
        additional_env={
            'GAZEBO_MODEL_PATH': f"{pkg_share}/models:" + os.path.expanduser('~/ardupilot_gazebo/models'),
            'GAZEBO_RESOURCE_PATH': f"{pkg_share}/worlds:" + os.path.expanduser('~/ardupilot_gazebo/worlds')
        }
    )
    
    ardupilot_uav = TimerAction(
        period=3.0,
        actions=[
            ExecuteProcess(
                cmd=[
                    'sim_vehicle.py',
                    '-v', 'ArduCopter',
                    '-f', 'gazebo-iris',
                    '--model', 'JSON',
                    '--map', '--console',
                    '-I0',
                    '--out', '127.0.0.1:14550',
                    '--out', '127.0.0.1:14551',
                    '--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm')
                ],
                cwd=os.path.expanduser('~/ardupilot/ArduCopter'),
                output='screen'
            )
        ]
    )
    
    mavros_uav = TimerAction(
        period=8.0,
        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': 'udp://:14550@127.0.0.1:14555',
                        '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,
    ])