#!/usr/bin/env python3
"""
Gazebo Launch File - Drone Landing Simulation
Launches Gazebo with the world and spawns the drone.
Supports both 'gz' (newer) and 'ign' (Fortress) commands.
"""

import os
import shutil
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 get_gazebo_command():
    """Determine whether to use 'gz' or 'ign' command."""
    if shutil.which('gz'):
        return 'gz'
    elif shutil.which('ign'):
        return 'ign'
    else:
        # Default to gz, will fail with helpful error
        return 'gz'


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', '')
    ign_resource_path = os.environ.get('IGN_GAZEBO_RESOURCE_PATH', '')
    combined_path = f"{model_path}:{gz_resource_path}:{ign_resource_path}"
    
    os.environ['GZ_SIM_RESOURCE_PATH'] = combined_path
    os.environ['IGN_GAZEBO_RESOURCE_PATH'] = combined_path
    
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    headless = LaunchConfiguration('headless', default='false')
    
    # Detect which Gazebo command is available
    gz_cmd = get_gazebo_command()
    
    # For ign command, use 'gazebo' subcommand; for gz, use 'sim'
    if gz_cmd == 'ign':
        sim_cmd = [gz_cmd, 'gazebo', '-r', world_file]
        spawn_cmd = [
            gz_cmd, 'service', '-s', '/world/drone_landing_world/create',
            '--reqtype', 'ignition.msgs.EntityFactory',
            '--reptype', 'ignition.msgs.Boolean',
            '--timeout', '5000',
            '--req', f'sdf_filename: "{drone_model}", name: "drone"'
        ]
    else:
        sim_cmd = [gz_cmd, 'sim', '-r', world_file]
        spawn_cmd = [
            gz_cmd, '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"'
        ]
    
    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=sim_cmd,
            output='screen',
            additional_env={
                'GZ_SIM_RESOURCE_PATH': combined_path,
                'IGN_GAZEBO_RESOURCE_PATH': combined_path
            }
        ),
        
        ExecuteProcess(
            cmd=spawn_cmd,
            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.")
    print()
    print("Usage:")
    print("  ros2 launch gazebo/launch/drone_landing.launch.py")
    print()
    print("Or start Gazebo manually:")
    
    gz_cmd = get_gazebo_command()
    if gz_cmd == 'ign':
        print("  ign gazebo gazebo/worlds/drone_landing.sdf")
    else:
        print("  gz sim gazebo/worlds/drone_landing.sdf")
    
    print()
    print("Then run: python run_gazebo.py")