from ament_index_python.packages import get_package_share_directory
import os
from clearpath_config.common.utils.yaml import read_yaml
from clearpath_config.clearpath_config import ClearpathConfig

from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    OpaqueFunction
)

from launch.substitutions import (
    LaunchConfiguration,
    PathJoinSubstitution
)

from launch_ros.actions import Node

from nav2_common.launch import RewrittenYaml


ARGUMENTS = [
    DeclareLaunchArgument('use_sim_time', default_value='false',
                          choices=['true', 'false'],
                          description='Use sim time'),
    DeclareLaunchArgument('setup_path',
                          default_value='/etc/clearpath/',
                          description='Clearpath setup path')
]


def launch_setup(context, *args, **kwargs):
    # Packages
    package_config_path = get_package_share_directory('locker98_tools_bringup')

    # Launch Configurations
    use_sim_time = LaunchConfiguration('use_sim_time')
    setup_path = LaunchConfiguration('setup_path')

    # Read robot YAML
    config = read_yaml(setup_path.perform(context) + 'robot.yaml')
    # Parse robot YAML into config
    clearpath_config = ClearpathConfig(config)

    namespace = clearpath_config.system.namespace
    platform_model = clearpath_config.platform.get_platform_model()

    file_parameters = os.path.join(
        package_config_path,
        'config',
        platform_model,
        'slam.yaml'
    )



    rewritten_parameters = RewrittenYaml(
        source_file=file_parameters,
        root_key=namespace,
        param_rewrites={},
        convert_types=True
    )

    slam = Node(
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox',
        namespace=namespace,
        output='screen',
        parameters=[
          rewritten_parameters,
          {'use_sim_time': use_sim_time}
        ],
        remappings=[
          ('/tf', 'tf'),
          ('/tf_static', 'tf_static'),
          ('/scan', 'sensors/lidar3d_0/scan'),
          ('/map', 'map'),
          ('/map_metadata', 'map_metadata'),
        ]
    )

    return [slam]


def generate_launch_description():
    ld = LaunchDescription(ARGUMENTS)
    ld.add_action(OpaqueFunction(function=launch_setup))
    return ld