import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.substitutions import LaunchConfiguration
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}"
    )

    world_path = LaunchConfiguration("world_path")
    map_name = LaunchConfiguration("map_name")
    use_sim_time = LaunchConfiguration("use_sim_time")
    loop = LaunchConfiguration("loop")

    declarations = [
        DeclareLaunchArgument(
            "world_path",
            default_value="",
            description="Full path to world folder, containing navigation related parameters",
        ),
        DeclareLaunchArgument(
            "map_name",
            default_value="",
            description="Basename of the map yaml file",
        ),
        DeclareLaunchArgument(
            "use_sim_time",
            default_value="false",
            description="Use simulation (Gazebo) clock if true",
        ),
        DeclareLaunchArgument(
            "loop",
            default_value="false",
            description="Use loop for waypoint handler",
        ),
    ]

    load_nodes = GroupAction(
        actions=[
            Node(
                package="waypoint_handler",
                executable="waypoint_handler_node",
                name="waypoint_handler_node",
                output="screen",
                parameters=[
                    {
                        "use_sim_time": use_sim_time,
                        "loop": loop,
                    }
                ],
                arguments=["--ros-args", "--log-level", DEFAULT_LOG_LEVEL],
            ),
            Node(
                package="waypoint_handler",
                executable="stairs_check_node",
                name="stairs_check_node",
                output="screen",
                arguments=["--ros-args", "--log-level", DEFAULT_LOG_LEVEL],
                parameters=[
                    {
                        "use_sim_time": use_sim_time,
                        "world_path": world_path,
                        "map_name": map_name,
                        "debug_vis_enabled": True,
                    },
                ],
            ),
        ],
    )

    return LaunchDescription([*declarations, load_nodes])