#!/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='iris_runway.sdf',
        description='World file name'
    )
    
    # Gazebo Harmonic
    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 = TimerAction(
        period=5.0,
        actions=[
            ExecuteProcess(
                cmd=[
                    'sim_vehicle.py', '-v', 'ArduCopter', '-f', 'gazebo-iris',
                    '--model', 'JSON', '--no-mavproxy', '-I0',
                ],
                cwd=os.path.expanduser('~/ardupilot'),
                output='screen',
                additional_env={
                    'PATH': os.environ.get('PATH', '') + ':' + os.path.expanduser('~/ardupilot/Tools/autotest')
                }
            )
        ]
    )
    
    mavros = TimerAction(
        period=15.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': 'tcp://127.0.0.1:5760'}
                ]
            )
        ]
    )
    
    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,
    ])