"""
  Launching interface node with connecting skills
  ROS 2 launch program for Robossembler
  ```bash
  ros2 launch rbs_bt_executor bt_path:=</path/to/skills_json> mode:=benchmark

  @shalenikol release 0.1
  @shalenikol release 0.2 mode
  @shalenikol release 0.3 {"bt_path": bt_path} # included as default path
  @shalenikol release 0.4 bt_path : either name of skills config file or path to default skills config file
"""
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(f: str) -> list:
    if not os.path.isfile(f):
        return []

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

    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)
    skills_cfg_file = bt_path if bt_path.endswith(".json") else os.path.join(bt_path, FILE_SKILLS)

    mode = LaunchConfiguration("mode")
    mode = mode.perform(context)

    skills = get_skill_list(skills_cfg_file)

    rbs_interface = Node(
        package="rbs_bt_executor",
        executable="rbs_interface.py",
        parameters = [{"bt_path": skills_cfg_file},{"mode": mode},{"use_sim_time": True}]
    )
    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)])