import os
import yaml

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return file.read()
    except EnvironmentError:
        # parent of IOError, OSError *and* WindowsError where available
        return None


def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:
        # parent of IOError, OSError *and* WindowsError where available
        return None


def generate_launch_description():

    pkg_dir = get_package_share_directory('rbs_task_planner')
    bt_exec_dir = get_package_share_directory('rbs_bt_executor')
    namespace = LaunchConfiguration('namespace')
    assemble_dir = os.path.join(
        get_package_share_directory("rbs_task_planner"), "example", "sdf_models"
    )
    
    declare_namespace_cmd = DeclareLaunchArgument(
        name="namespace",
        default_value='',
        description='Namespace')

    plansys2_cmd = IncludeLaunchDescription(
      PythonLaunchDescriptionSource(os.path.join(
        get_package_share_directory('plansys2_bringup'),
        'launch',
        'plansys2_bringup_launch_monolithic.py')),
        launch_arguments={
          'model_file': pkg_dir + '/domain/atomic_domain.pddl',
          'namespace': namespace
        }.items())
    assemble = Node(
        package='plansys2_bt_actions',
        executable='bt_action_node',
        name='assemble',
        namespace=namespace,
        output='screen',
        parameters=[
          pkg_dir + '/config/params.yaml',
          {
            'action_name': 'assemble',
            'bt_xml_file': bt_exec_dir + '/bt_trees/assemble.xml',
          }
        ])

    assemble_state = Node(
        package="rbs_skill_servers",
        executable="assemble_state_service_server",
        output="screen",
        parameters=[
            {'assemble_prefix': 'ASSEMBLE_'},
            {'assemble_dir': assemble_dir}
        ]
    )
    ld = LaunchDescription()
    # Args
    ld.add_action(declare_namespace_cmd)
    # Nodes
    ld.add_action(plansys2_cmd)
    ld.add_action(assemble)
    ld.add_action(assemble_state)

    return ld