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,
    GroupAction,
    IncludeLaunchDescription,
    OpaqueFunction
)
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch.substitutions import (
    LaunchConfiguration,
    PathJoinSubstitution
)

from launch_ros.actions import PushRosNamespace


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
    pkg_nav2_bringup = get_package_share_directory('nav2_bringup')
    package_config_path = get_package_share_directory('locker98_tools_bringup')

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

    # 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,
        'localization.yaml'
    )
   


    launch_localization = PathJoinSubstitution(
      [pkg_nav2_bringup, 'launch', 'localization_launch.py'])

    localization = GroupAction([
        PushRosNamespace(namespace),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(launch_localization),
            launch_arguments=[
                ('namespace', namespace),
                ('map', map),
                ('use_sim_time', use_sim_time),
                ('params_file', file_parameters)
              ]
        ),
    ])

    return [localization]


def generate_launch_description():
    map_arg = DeclareLaunchArgument(
        'map',
        description='Full path to map yaml file to load')

    ld = LaunchDescription(ARGUMENTS)
    ld.add_action(map_arg)
    ld.add_action(OpaqueFunction(function=launch_setup))
    return ld