import os
import tempfile

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, TimerAction, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

from ament_index_python.packages import get_package_share_directory
import xacro


def generate_launch_description():
    pkg_trolley = get_package_share_directory("trolley")
    xacro_path = os.path.join(pkg_trolley, "urdf", "miniBox.urdf.xacro")
    pkg_gazebo = get_package_share_directory("gazebo_ros")
    controllers_yaml = os.path.join(pkg_trolley, "config", "ros2_controllers.yaml")

    # 解析 xacro，传入 controllers_yaml 路径
    robot_description = xacro.process_file(
        xacro_path,
        mappings={"controllers_yaml": controllers_yaml}
    ).toxml()

    # ★ 将 urdf 写入固定路径临时文件（不是随机名），方便调试
    urdf_tmp_path = "/tmp/box_bot.urdf"
    with open(urdf_tmp_path, 'w') as f:
        f.write(robot_description)

    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_gazebo, "launch", "gazebo.launch.py")
        )
    )

    # ★ robot_state_publisher 使用文件路径，不传字符串参数
    rsp = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="screen",
        parameters=[{
            "use_sim_time": True,
            "robot_description": robot_description,
        }],
        # ★ 关键：通过环境变量设置 RCL 参数长度限制
        additional_env={"RCUTILS_LOGGING_BUFFERED_STREAM": "1"},
    )

    spawn = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        output="screen",
        arguments=[
            "-file", urdf_tmp_path,
            "-entity", "box_bot",
            "-z", "0.05",
        ],
    )

    spawn_jsb = Node(
        package="controller_manager",
        executable="spawner",
        output="screen",
        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
    )

    spawn_dd = Node(
        package="controller_manager",
        executable="spawner",
        output="screen",
        arguments=["diff_drive_controller", "--controller-manager", "/controller_manager"],
    )

    return LaunchDescription([
        gazebo,
        rsp,
        TimerAction(period=3.0, actions=[spawn]),
        TimerAction(period=6.0, actions=[spawn_jsb, spawn_dd]),
    ])