#!/usr/bin/env python3
"""UAV-only Simulation Launch."""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
from launch.substitutions import LaunchConfiguration
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'
    )
    
    gazebo = ExecuteProcess(
        cmd=['gazebo', '--verbose', LaunchConfiguration('world')],
        output='screen',
        additional_env={
            'GAZEBO_MODEL_PATH': f"{pkg_share}/models:" + os.path.expanduser('~/ardupilot_gazebo/models'),
        }
    )
    
    ardupilot = 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',
                    '--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm')
                ],
                cwd=os.path.expanduser('~/ardupilot/ArduCopter'),
                output='screen'
            )
        ]
    )
    
    mavros = TimerAction(
        period=8.0,
        actions=[
            Node(
                package='mavros',
                executable='mavros_node',
                name='mavros',
                namespace='uav',
                output='screen',
                parameters=[
                    os.path.join(pkg_share, 'config', 'mavros_params.yaml'),
                    {'fcu_url': 'udp://:14550@127.0.0.1:14555'}
                ]
            )
        ]
    )
    
    visual_odom = 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')]
            )
        ]
    )
    
    uav_controller = TimerAction(
        period=12.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')]
            )
        ]
    )
    
    return LaunchDescription([
        world_arg,
        gazebo,
        ardupilot,
        mavros,
        visual_odom,
        uav_controller,
    ])