#!/usr/bin/env python3
"""
Gazebo Launch File - Drone Landing Simulation
Works with Ignition Fortress (ROS 2 Humble)
"""

import os
import shutil
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    """Generate the launch description."""
    
    # Get paths
    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')
    
    # Determine which gazebo command is available
    if shutil.which('ign'):
        gz_cmd = ['ign', 'gazebo', '-r', world_file]
    elif shutil.which('gz'):
        gz_cmd = ['gz', 'sim', '-r', world_file]
    else:
        gz_cmd = ['ign', 'gazebo', '-r', world_file]
    
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='true',
            description='Use simulation clock'
        ),
        
        # Start Gazebo
        ExecuteProcess(
            cmd=gz_cmd,
            output='screen'
        ),
        
        # ROS-Gazebo Bridge
        Node(
            package='ros_gz_bridge',
            executable='parameter_bridge',
            name='gz_bridge',
            arguments=[
                # Velocity commands (bidirectional)
                '/drone/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist',
                # Odometry (from Gazebo to ROS)
                '/model/drone/odometry@nav_msgs/msg/Odometry[ignition.msgs.Odometry',
                # IMU (from Gazebo to ROS)
                '/drone/imu@sensor_msgs/msg/Imu[ignition.msgs.IMU',
                # Clock
                '/clock@rosgraph_msgs/msg/Clock[ignition.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:")
    if shutil.which('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")