#!/usr/bin/env python3
"""
Gazebo Launch File - Drone Landing Simulation
Launches Gazebo with the world and spawns the drone.
"""

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node


def generate_launch_description():
    """Generate the launch description."""
    
    script_dir = os.path.dirname(os.path.abspath(__file__))
    gazebo_dir = os.path.dirname(script_dir)
    
    world_file = os.path.join(gazebo_dir, 'worlds', 'drone_landing.sdf')
    model_path = os.path.join(gazebo_dir, 'models')
    drone_model = os.path.join(model_path, 'drone', 'model.sdf')
    
    gz_resource_path = os.environ.get('GZ_SIM_RESOURCE_PATH', '')
    if model_path not in gz_resource_path:
        os.environ['GZ_SIM_RESOURCE_PATH'] = f"{model_path}:{gz_resource_path}"
    
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    headless = LaunchConfiguration('headless', default='false')
    
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='true',
            description='Use simulation (Gazebo) clock'
        ),
        DeclareLaunchArgument(
            'headless',
            default_value='false',
            description='Run Gazebo in headless mode (no GUI)'
        ),
        
        ExecuteProcess(
            cmd=['gz', 'sim', '-r', world_file],
            output='screen',
            additional_env={'GZ_SIM_RESOURCE_PATH': os.environ.get('GZ_SIM_RESOURCE_PATH', '')}
        ),
        
        ExecuteProcess(
            cmd=[
                'gz', 'service', '-s', '/world/drone_landing_world/create',
                '--reqtype', 'gz.msgs.EntityFactory',
                '--reptype', 'gz.msgs.Boolean',
                '--timeout', '5000',
                '--req', f'sdf_filename: "{drone_model}", name: "drone"'
            ],
            output='screen'
        ),
        
        Node(
            package='ros_gz_bridge',
            executable='parameter_bridge',
            name='gz_bridge',
            arguments=[
                '/drone/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist',
                '/model/drone/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry',
                '/drone/imu@sensor_msgs/msg/Imu@gz.msgs.IMU',
                '/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock',
            ],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'
        ),
    ])


if __name__ == '__main__':
    print("This is a ROS 2 launch file. Run with:")
    print("  ros2 launch <package_name> drone_landing.launch.py")
    print("")
    print("Or use the standalone script:")
    print("  python3 gazebo_bridge.py")