#!/usr/bin/env python3
"""UGV-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",
        }
    )
    
    ugv_spawn = TimerAction(
        period=3.0,
        actions=[
            ExecuteProcess(
                cmd=[
                    'ros2', 'run', 'gazebo_ros', 'spawn_entity.py',
                    '-entity', 'ugv',
                    '-file', os.path.join(pkg_share, 'models', 'custom_ugv', 'model.sdf'),
                    '-x', '5.0', '-y', '0.0', '-z', '0.1'
                ],
                output='screen'
            )
        ]
    )
    
    ugv_controller = TimerAction(
        period=5.0,
        actions=[
            Node(
                package='uav_ugv_simulation',
                executable='ugv_controller',
                name='ugv_controller',
                namespace='ugv',
                output='screen',
                parameters=[os.path.join(pkg_share, 'config', 'ugv_params.yaml')]
            )
        ]
    )
    
    return LaunchDescription([
        world_arg,
        gazebo,
        ugv_spawn,
        ugv_controller,
    ])