#!/usr/bin/env python3
"""
ArduPilot SITL + Gazebo Launch File
Works with Gazebo Harmonic/Garden and ArduPilot SITL.

This launch file:
1. Starts Gazebo with ArduPilot-compatible drone model
2. Sets up ROS-Gazebo bridge for telemetry
3. Optionally starts ArduPilot SITL

NOTE: ArduPilot SITL integration requires the ardupilot_gazebo plugin.
Install from: https://github.com/ArduPilot/ardupilot_gazebo
"""

import os
import shutil
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
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', 'ardupilot_drone.sdf')
    
    # Check for ArduPilot Gazebo plugin
    plugin_paths = [
        os.path.expanduser("~/ardupilot_gazebo/build"),
        "/opt/ardupilot_gazebo/lib",
        os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", ""),
    ]
    ardupilot_plugin_found = any(os.path.exists(p) for p in plugin_paths if p)
    
    # Determine Gazebo command
    if shutil.which('gz'):
        gz_cmd = ['gz', 'sim', '-r', world_file]
    elif shutil.which('ign'):
        gz_cmd = ['ign', 'gazebo', '-r', world_file]
    else:
        gz_cmd = ['gz', 'sim', '-r', world_file]
    
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    start_sitl = LaunchConfiguration('start_sitl', default='false')
    
    # Set plugin path for ArduPilot
    env = os.environ.copy()
    plugin_search_paths = ':'.join([p for p in plugin_paths if p and os.path.exists(p)])
    if plugin_search_paths:
        env['GZ_SIM_SYSTEM_PLUGIN_PATH'] = plugin_search_paths + ':' + env.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '')
    
    actions = [
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='true',
            description='Use simulation clock'
        ),
        
        DeclareLaunchArgument(
            'start_sitl',
            default_value='false',
            description='Start ArduPilot SITL automatically'
        ),
        
        # Start Gazebo with ArduPilot world
        ExecuteProcess(
            cmd=gz_cmd,
            output='screen',
            additional_env=env
        ),
        
        # ROS-Gazebo Bridge for telemetry and commands
        # Delayed start to wait for Gazebo
        TimerAction(
            period=2.0,
            actions=[
                Node(
                    package='ros_gz_bridge',
                    executable='parameter_bridge',
                    name='gz_bridge',
                    arguments=[
                        # Rover velocity commands (ROS to Gazebo)
                        '/rover/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
                        # Odometry (from Gazebo to ROS) - fallback if MAVLink not used
                        '/model/drone/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
                        # Camera (from Gazebo to ROS)
                        '/drone/camera@sensor_msgs/msg/Image[gz.msgs.Image',
                        # IMU (from Gazebo to ROS)
                        '/imu@sensor_msgs/msg/Imu[gz.msgs.IMU',
                        # Clock
                        '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
                    ],
                    parameters=[{'use_sim_time': use_sim_time}],
                    output='screen'
                ),
            ]
        ),
    ]
    
    return LaunchDescription(actions)


if __name__ == '__main__':
    print("=" * 60)
    print("  ArduPilot SITL + Gazebo Launch File")
    print("=" * 60)
    print()
    print("This is a ROS 2 launch file for ArduPilot integration.")
    print()
    print("Prerequisites:")
    print("  1. Gazebo (gz sim or ign gazebo)")
    print("  2. ros_gz_bridge package")
    print("  3. ArduPilot Gazebo plugin (optional, for SITL)")
    print()
    print("Usage:")
    print("  ros2 launch gazebo/launch/ardupilot_drone.launch.py")
    print()
    print("Then in another terminal:")
    print("  # Start SITL")
    print("  sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON")
    print()
    print("  # Or use the integrated runner")
    print("  python run_ardupilot.py --no-sitl")
    print()
    print("Manual Gazebo Start:")
    
    if shutil.which('gz'):
        print("  gz sim -r gazebo/worlds/ardupilot_drone.sdf")
    else:
        print("  ign gazebo gazebo/worlds/ardupilot_drone.sdf")