import os
import yaml

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
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():

     # planning_context
    robot_description_config = load_file(
        "rasms_description", "urdf/rasms_description.urdf.xacro"
    )
    robot_description = {"robot_description": robot_description_config}

    robot_description_semantic_config = load_file(
        "rasms_moveit_config", "srdf/rasms_description.srdf"
    )
    robot_description_semantic = {
        "robot_description_semantic": robot_description_semantic_config
    }

    kinematics_yaml = load_yaml(
        "rasms_moveit_config", "config/kinematics.yml"
    )

    pkg_dir = get_package_share_directory('rasms_manipulator')
    namespace = LaunchConfiguration('namespace')

    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Namespace')

    # stdout_linebuf_envvar = SetEnvironmentVariable(
    #     'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')

    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 + '/pddl/domain.pddl',
          'namespace': namespace
        }.items())

    move_1 = Node(
        package='plansys2_bt_actions',
        executable='bt_action_node',
        name='move',
        namespace=namespace,
        output='screen',
        parameters=[
          pkg_dir + '/config/params.yaml',
          robot_description, 
          robot_description_semantic, 
          kinematics_yaml,
          {
            'action_name': 'move',
            'bt_xml_file': pkg_dir + '/behavior_trees_xml/move.xml'
          }
        ])
    transport_1_cmd = Node(
        package='rasms_manipulator',
        executable='rasms_action_node',
        name='transport_1',
        namespace=namespace,
        output='screen',
        parameters=[])   # Create the launch description and populate
    
    ld = LaunchDescription()

    # ld.add_action(stdout_linebuf_envvar)
    ld.add_action(declare_namespace_cmd)

    # Declare the launch options
    ld.add_action(plansys2_cmd)
    ld.add_action(move_1)
    ld.add_action(transport_1_cmd)
  
    return ld