import os
import yaml

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

from ament_index_python import get_package_share_directory


def load_yaml(package_name: str, file_path: str):
    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 f:
            return yaml.safe_load(f)
    except EnvironmentError:
        return None


def load_file(package_name: str, file_path: str) -> str:
    package_path = get_package_share_directory(package_name)
    absolut_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolut_file_path, "r") as f:
            return f.read()
    except EnvironmentError:
        return None

def generate_launch_description():

    # Launch arguments
    launch_args = []

    launch_args.append(
        DeclareLaunchArgument(
                name="robot_description",
                description="Robot description XML file."
        )
    )

    """launch_args.append(
        DeclareLaunchArgument(
                name="robot_name",
                default_value="rasmt",
                description=""
        )
    )"""

    launch_args.append(
        DeclareLaunchArgument(
                name="sim",
                default_value="true",
                description="Launch robot in simulation or on real setup."
        )
    )

        # Evaluate frequently used variables
    #robot_name = LaunchConfiguration("robot_name").perform()

    # Configure robot_description
    robot_description = {"robot_description": LaunchConfiguration("robot_description")}

    # Robot semantics SRDF
    robot_description_semantic = {
        "robot_description_semantic": load_file("rasmt_moveit_config", "config/rasmt.srdf")
    }

    # Kinematics
    kinematics_yaml = load_yaml("rasmt_moveit_config", "config/kinematics.yaml")
    
    # Update group name

    # Joint limits
    robot_description_planning = {
        "robot_description_planning": PathJoinSubstitution(
            [
                FindPackageShare("rasmt_moveit_config"),
                "config/joint_limits.yaml"
            ]
        )
    }

    # Planning
    ompl_yaml = load_yaml("rasmt_moveit_config", "config/ompl_planning.yaml")

    planning = {
        "move_group": {
            "planning_plugin": "ompl_interface/OMPLPlanner",
            "request_adapters": "default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
            "start_state_max_bounds_error": 0.1
        }
    }

    # Trajectory execution
    trajectory_execution = {
        "moveit_manage_controllers": True,
        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
        "trajectory_execution.allowed_goal_duration_margin": 0.5,
        "trajectory_execution.allowed_start_tolerance": 0.01,
    }

    # Controllers
    controllers_yaml = load_yaml(
            "rasmt_moveit_config",
            "config/rasmt_moveit_controllers.yaml"
    )

    moveit_controllers = {"moveit_simple_controller_manager": controllers_yaml,
                          "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"}

    # Planning scene
    planning_scene_monitor_parameters = {"publish_planning_scene": True,
                                         "publish_geometry_updates": True,
                                         "publish_state_updates": True,
                                         "publish_transforms_updates": True}

    # Time configuration
    use_sim_time = {"use_sime_time": LaunchConfiguration("sim")}

    # Prepare move group node
    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        arguments=["--ros-args"],
        parameters=[
            robot_description,
            robot_description_semantic,
            kinematics_yaml,
            robot_description_planning,
            ompl_yaml,
            planning,
            trajectory_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
            use_sim_time
        ]
    )

    # RViz
    rviz_config = PathJoinSubstitution([FindPackageShare("rasmt_moveit_config"), "config/rasmt_moveit.rviz"])

    rviz = Node(
        package="rviz2",
        executable="rviz2",
        parameters=[
            robot_description,
            robot_description_semantic,
            kinematics_yaml,
            planning,
            use_sim_time
        ],
        arguments=[
            '-d', rviz_config
        ]
    )

    # move_topose_action_server = Node(
    #     package="robossembler_servers",
    #     executable="move_topose_action_server",
    #     name="move_to_pose_moveit",
    #     parameters=[
    #         robot_description,
    #         robot_description_semantic,
    #         kinematics_yaml,
    #         robot_description_planning,
    #         ompl_yaml,
    #         planning,
    #         trajectory_execution,
    #         moveit_controllers,
    #         planning_scene_monitor_parameters,
    #         use_sim_time
    #     ]
    # )

    # move_cartesian_path_action_server = Node(
    #     package="robossembler_servers",
    #     executable="move_cartesian_path_action_server",
    #     name="move_cartesian_path_action_server",
    #     parameters=[
    #         robot_description,
    #         robot_description_semantic,
    #         kinematics_yaml,
    #         robot_description_planning,
    #         ompl_yaml,
    #         planning,
    #         trajectory_execution,
    #         moveit_controllers,
    #         planning_scene_monitor_parameters,
    #         use_sim_time
    #     ]
    # )

    launch_nodes = []
    launch_nodes.append(rviz)
    launch_nodes.append(move_group_node)
    # launch_nodes.append(move_topose_action_server)
    # launch_nodes.append(move_cartesian_path_action_server)
   # launch_nodes.append(move_to_joint_state_action_server)



    return LaunchDescription(launch_args + launch_nodes)