import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess, IncludeLaunchDescription
from launch.launch_description_sources import (
    AnyLaunchDescriptionSource,
    PythonLaunchDescriptionSource,
)
from launch_ros.actions import Node


def generate_launch_description():
    # Resolve the world file inside the package
    world_path = os.path.join(
        get_package_share_directory("museum_guide"),
        "worlds",
        "museum_world.sdf",
    )

    # 1. Start Gazebo by including ros_gz_sim's own launch file
    start_gazebo_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory("ros_gz_sim"),
                "launch",
                "gz_sim.launch.py",
            )
        ),
        launch_arguments={"gz_args": f"-r -s {world_path}"}.items(),
    )

    # 2. Start the Gazebo websocket bridge for Foxglove's sim panel
    gzlaunch_path = os.path.join(
        get_package_share_directory("museum_guide"),
        "launch",
        "websocket.gzlaunch",
    )

    start_gz_websocket_cmd = ExecuteProcess(
        cmd=["gz-launch", gzlaunch_path],
        output="log",
    )

    # 3. Spawn the TurtleBot3 at its starting pose
    sdf_path = os.path.join(
        get_package_share_directory("museum_guide"),
        "models",
        "turtlebot3_burger",
        "turtlebot3_burger.sdf",
    )

    spawn_robot_cmd = Node(
        package="ros_gz_sim",
        executable="create",
        arguments=[
            "-world",
            "museum_world",
            "-name",
            "turtlebot3",
            "-x",
            "0",
            "-y",
            "8",
            "-z",
            "0.1",
            "-file",
            sdf_path,
        ],
    )

    # 4. Bridge Gazebo topics to ROS2 topics
    start_topic_bridge_cmd = Node(
        package="ros_gz_bridge",
        executable="parameter_bridge",
        arguments=[
            "/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan",
            "/joint_states@sensor_msgs/msg/JointState[gz.msgs.Model",
            "/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist",
            "/odom@nav_msgs/msg/Odometry[gz.msgs.Odometry",
        ],
    )

    # 5. Run robot_state_publisher
    urdf_path = os.path.join(
        get_package_share_directory("museum_guide"),
        "models",
        "turtlebot3_burger",
        "turtlebot3_burger.urdf",
    )
    with open(urdf_path, "r") as f:
        robot_description = f.read()

    start_robot_state_publisher_cmd = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": robot_description}],
    )

    # 6. Include foxglove_bridge's launch file (XML — AnyLaunchDescriptionSource handles it)
    start_foxglove_cmd = IncludeLaunchDescription(
        AnyLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory("foxglove_bridge"),
                "launch",
                "foxglove_bridge_launch.xml",
            )
        ),
    )

    # 7. Include your five robot nodes (its params_file argument is exposed at the top level)
    start_nodes_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory("museum_guide"),
                "launch",
                "nodes.launch.py",
            )
        ),
    )

    ld = LaunchDescription()
    ld.add_action(start_gazebo_cmd)
    ld.add_action(start_gz_websocket_cmd)
    ld.add_action(spawn_robot_cmd)
    ld.add_action(start_topic_bridge_cmd)
    ld.add_action(start_robot_state_publisher_cmd)
    ld.add_action(start_foxglove_cmd)
    ld.add_action(start_nodes_cmd)

    return ld
