import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node, LoadComposableNodes
from launch_ros.descriptions import ParameterFile, ComposableNode
from nav2_common.launch import RewrittenYaml

DEFAULT_LOG_LEVEL = "info"


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

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

    world_path = LaunchConfiguration("world_path")
    use_sim_time = LaunchConfiguration("use_sim_time")
    cmd_vel_topic = LaunchConfiguration("cmd_vel_topic")

    # declare launch arguments
    declarations = [
        DeclareLaunchArgument(
            "world_path",
            default_value=os.path.join(nav_launch_dir, "config", "turtlebot3_world"),
            description="Full path to world folder, containing navigation related parameters",
        ),
        DeclareLaunchArgument(
            "use_sim_time",
            default_value="false",
            description="Use simulation (Gazebo) clock if true",
        ),
        DeclareLaunchArgument(
            "use_respawn",
            default_value="False",
            description="Whether to respawn if a node crashes. Applied when composition is disabled.",
        ),
        DeclareLaunchArgument(
            "cmd_vel_topic",
            default_value="kstack/set/cmd_vel",
            description="Topic to send command velocities on",
        ),
    ]

    lifecycle_nodes = [
        "controller_server",
        "smoother_server",
        "planner_server",
        "behavior_server",
        "bt_navigator",
    ]

    # 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"),
        ("cmd_vel", cmd_vel_topic),
    ]

    # Create our own temporary YAML files that include substitutions
    param_substitutions = {"use_sim_time": use_sim_time, "autostart": "True"}

    # Params for the controller server
    controller_params = ParameterFile(
        RewrittenYaml(
            source_file=PathJoinSubstitution([world_path, "param", "controllers.yaml"]),
            root_key="",
            param_rewrites=param_substitutions,
            convert_types=True,
        ),
        allow_substs=True,
    )

    costmap_params = ParameterFile(
        RewrittenYaml(
            source_file=PathJoinSubstitution(
                [world_path, "param", "costmap_params.yaml"]
            ),
            root_key="",
            param_rewrites=param_substitutions,
            convert_types=True,
        ),
        allow_substs=True,
    )

    # Params for the planner server
    planner_params = ParameterFile(
        RewrittenYaml(
            source_file=PathJoinSubstitution([world_path, "param", "planners.yaml"]),
            root_key="",
            param_rewrites=param_substitutions,
            convert_types=True,
        ),
        allow_substs=True,
    )

    # Params for the behavior server
    recovery_params = ParameterFile(
        RewrittenYaml(
            source_file=PathJoinSubstitution([world_path, "param", "recovery.yaml"]),
            root_key="",
            param_rewrites=param_substitutions,
            convert_types=True,
        ),
        allow_substs=True,
    )

    # Other navigation parameters
    # TODO: For now group them all into 1 file, may change in the future as needed
    other_params = ParameterFile(
        RewrittenYaml(
            source_file=PathJoinSubstitution(
                [world_path, "param", "other_nav_params.yaml"]
            ),
            root_key="",
            param_rewrites=param_substitutions,
            convert_types=True,
        ),
        allow_substs=True,
    )

    configured_params = [
        controller_params,
        planner_params,
        costmap_params,
        recovery_params,
        other_params,
    ]

    # Generic container needed to be loaded with all the parameters,
    # since loadComposableNode will choose parameters according to node name
    # and local_costmap + global_costmap is not included
    container = Node(
        name="nav2_container",
        package="rclcpp_components",
        executable="component_container_isolated",
        parameters=[*configured_params, {"autostart": True}],
        arguments=["--ros-args", "--log-level", DEFAULT_LOG_LEVEL],
        remappings=remappings,
        output="screen",
    )
    load_composable_nodes = LoadComposableNodes(
        target_container="/nav2_container",
        composable_node_descriptions=[
            ComposableNode(
                package="nav2_controller",
                plugin="nav2_controller::ControllerServer",
                name="controller_server",
                parameters=[controller_params, costmap_params],
                remappings=remappings,
            ),
            ComposableNode(
                package="nav2_smoother",
                plugin="nav2_smoother::SmootherServer",
                name="smoother_server",
                parameters=[other_params],
                remappings=remappings,
            ),
            ComposableNode(
                package="nav2_planner",
                plugin="nav2_planner::PlannerServer",
                name="planner_server",
                parameters=[planner_params, costmap_params],
                remappings=remappings,
            ),
            ComposableNode(
                package="nav2_behaviors",
                plugin="behavior_server::BehaviorServer",
                name="behavior_server",
                parameters=[recovery_params],
                remappings=remappings,
            ),
            ComposableNode(
                package="nav2_bt_navigator",
                plugin="nav2_bt_navigator::BtNavigator",
                name="bt_navigator",
                parameters=[other_params],
                remappings=remappings,
            ),
            ComposableNode(
                package="nav2_lifecycle_manager",
                plugin="nav2_lifecycle_manager::LifecycleManager",
                name="lifecycle_manager_navigation",
                parameters=[
                    {
                        "use_sim_time": use_sim_time,
                        "autostart": True,
                        "node_names": lifecycle_nodes,
                    }
                ],
            ),
        ],
    )

    return LaunchDescription([*declarations, container, load_composable_nodes])