import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
from launch.substitutions import (
    LaunchConfiguration,
    PathJoinSubstitution,
    PythonExpression,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

DEFAULT_LOG_LEVEL = "info"


def generate_launch_description():
    # log format
    os.environ["RCUTILS_CONSOLE_OUTPUT_FORMAT"] = (
        "[{time}] [{name}] [{severity}] {message}"
    )

    # Get the launch directory
    bringup_dir = get_package_share_directory("nav_launch")

    world_path = LaunchConfiguration("world_path")
    map_name = LaunchConfiguration("map")
    use_sim_time = LaunchConfiguration("use_sim_time")
    imu_topic = LaunchConfiguration("imu_topic")
    points_topic = LaunchConfiguration("points_topic")

    declarations = [
        DeclareLaunchArgument(
            "world_path",
            default_value=os.path.join(bringup_dir, "config", "turtlebot3_world"),
            description="Full path to world folder, containing navigation related parameters",
        ),
        DeclareLaunchArgument(
            "map",
            default_value="turtlebot3_world",
            description="Basename of the map file (.pcd and .yaml should be named the same)",
        ),
        DeclareLaunchArgument(
            "use_sim_time",
            default_value="false",
            description="Use simulation (Gazebo) clock if true",
        ),
        DeclareLaunchArgument(
            "imu_topic",
            default_value="imu",
            description="imu topic to use for hdl localization",
        ),
        DeclareLaunchArgument(
            "points_topic",
            default_value="velodyne_points",
            description="pointcloud topic to use for hdl localization",
        ),
    ]

    # Map fully qualified names to relative ones so the node's namespace can be prepended.
    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
    # https://github.com/ros/geometry2/issues/32
    # https://github.com/ros/robot_state_publisher/pull/30
    # TODO(orduno) Substitute with `PushNodeRemapping`
    #              https://github.com/ros2/launch_ros/issues/56
    remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]

    lifecycle_nodes = ["map_server"]

    map_pcd_basename = PythonExpression(["'", map_name, ".pcd'"])
    map_yaml_basename = PythonExpression(["'", map_name, ".yaml'"])
    map_pcd_path = PathJoinSubstitution([world_path, "map", map_pcd_basename])
    map_yaml_path = PathJoinSubstitution([world_path, "map", map_yaml_basename])
    hdl_params_path = PathJoinSubstitution(
        [world_path, "param", "hdl_localization.yaml"]
    )

    hdl_localization_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [
                get_package_share_directory("hdl_localization"),
                "/launch/hdl_localization.launch.py",
            ]
        ),
        launch_arguments={
            "use_sim_time": use_sim_time,
            "globalmap_pcd": map_pcd_path,
            "config_path": hdl_params_path,
            "points_topic": points_topic,
            "imu_topic": imu_topic,
        }.items(),
    )

    load_nodes = GroupAction(
        actions=[
            Node(
                package="nav2_map_server",
                executable="map_server",
                name="map_server",
                output="screen",
                parameters=[
                    {"use_sim_time": use_sim_time, "yaml_filename": map_yaml_path}
                ],
                arguments=["--ros-args", "--log-level", DEFAULT_LOG_LEVEL],
                remappings=remappings,
            ),
            Node(
                package="nav2_lifecycle_manager",
                executable="lifecycle_manager",
                name="lifecycle_manager_localization",
                output="screen",
                arguments=["--ros-args", "--log-level", DEFAULT_LOG_LEVEL],
                parameters=[
                    {"use_sim_time": use_sim_time},
                    {"autostart": True},
                    {"node_names": lifecycle_nodes},
                ],
            ),
        ],
    )

    return LaunchDescription([*declarations, hdl_localization_launch, load_nodes])