"""
  Launching interface node with connecting skills
  ROS 2 launch program for Robossembler

  @shalenikol release 0.1
  @shalenikol release 0.2 mode
"""
import os
import json

from launch_ros.actions import Node
from launch.actions import (DeclareLaunchArgument, OpaqueFunction)
from launch.substitutions import LaunchConfiguration
from launch import LaunchDescription

FILE_SKILLS  = "skills.json"
PARAM_SUFFIX = "_cfg"

def get_skill_list_(path: str) -> list:
    f = os.path.join(path, FILE_SKILLS)
    if not os.path.isfile(f):
        return []

    with open(f, "r") as fh:
        data = json.load(fh)
    #     str_data = fh.read()
    # data = json.loads(str_data)

    nn_skills = 0
    excluding = {}
    skills = []
    for skill in data["skills"]:
        node_name = skill["Module"]["node_name"]
        launch = skill["Launch"]
        p = launch["package"]
        e = launch["executable"]
        excluding[p+e] = None # unique key
        if len(excluding) == nn_skills:
            continue
        nn_skills += 1
        # skills.append(Node(package=p, executable=e))
        launch["parameters"] = [{node_name+PARAM_SUFFIX: json.dumps(skill)}]
        skills.append(Node(**launch))

    return skills

def launch_setup(context, *args, **kwargs):
    # Initialize Arguments
    bt_path = LaunchConfiguration("bt_path")
    bt_path = bt_path.perform(context)
    mode = LaunchConfiguration("mode")
    mode = mode.perform(context)

    skills = get_skill_list_(bt_path)

    rbs_interface = Node(
        package="rbs_bt_executor",
        executable="rbs_interface.py",
        parameters = [{"mode": mode},{"use_sim_time": True}]
        # parameters = [{"bt_path": bt_path}] # can be included as default path
    )
    nodes_to_start = [rbs_interface]
    return nodes_to_start + skills

def generate_launch_description():
    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument(
            "bt_path",
            default_value="",
            description="path to Behavior tree instance"
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "mode",
            default_value="",
            description="Run mode of the interface node"
        )
    )
    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])