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

import os
import json
from ament_index_python.packages import get_package_share_directory


share_dir = get_package_share_directory('vision_detect')
config_dir = os.path.join(share_dir, 'configs/launch_configs/crossboard_topic_pca.json')
with open(config_dir, "r") as f:
    configs = json.load(f)

def generate_launch_description():
    args_detect = [
        DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
        DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
        DeclareLaunchArgument('eye_in_left_hand_mat_path', default_value=configs['eye_in_left_hand_mat_path']),
        DeclareLaunchArgument('eye_in_right_hand_mat_path', default_value=configs['eye_in_right_hand_mat_path']),
        DeclareLaunchArgument('eye_to_hand_mat_path', default_value=configs['eye_to_hand_mat_path']),

        DeclareLaunchArgument('get_camera_mode', default_value=configs['get_camera_mode']),
        DeclareLaunchArgument('detect_mode', default_value=configs['detect_mode']),
        DeclareLaunchArgument('calculate_mode', default_value=configs['calculate_mode']),

        DeclareLaunchArgument("Topic_configs", default_value=json.dumps(configs['Topic_configs'])),
        DeclareLaunchArgument("Crossboard_configs", default_value=json.dumps(configs['Crossboard_configs'])),
        DeclareLaunchArgument("PCA_configs", default_value=json.dumps(configs['PCA_configs'])),
    ]

    def create_detect_node(context):
        output_boxes = LaunchConfiguration('output_boxes').perform(context)
        output_masks = LaunchConfiguration('output_masks').perform(context)
        eye_in_left_hand_mat_path = LaunchConfiguration('eye_in_left_hand_mat_path').perform(context)
        eye_in_right_hand_mat_path = LaunchConfiguration('eye_in_right_hand_mat_path').perform(context)
        eye_to_hand_mat_path = LaunchConfiguration('eye_to_hand_mat_path').perform(context)

        get_camera_mode = LaunchConfiguration('get_camera_mode').perform(context)
        detect_mode = LaunchConfiguration('detect_mode').perform(context)
        calculate_mode = LaunchConfiguration('calculate_mode').perform(context)

        Topic_configs = LaunchConfiguration('Topic_configs').perform(context)
        Crossboard_configs = LaunchConfiguration('Crossboard_configs').perform(context)
        PCA_configs = LaunchConfiguration('PCA_configs').perform(context)

        return [
            Node(
                package='vision_detect',
                executable='detect_node',
                name=configs['node_name'],
                parameters=[{
                    'output_boxes': output_boxes.lower() == 'true',
                    'output_masks': output_masks.lower() == 'true',
                    'eye_in_left_hand_mat_path': eye_in_left_hand_mat_path,
                    'eye_in_right_hand_mat_path': eye_in_right_hand_mat_path,
                    'eye_to_hand_mat_path': eye_to_hand_mat_path,

                    'get_camera_mode': get_camera_mode,
                    'detect_mode': detect_mode,
                    'calculate_mode': calculate_mode,

                    'Topic_configs': Topic_configs,
                    'Crossboard_configs': Crossboard_configs,
                    'PCA_configs': PCA_configs,
                }]
            )
        ]

    return LaunchDescription(args_detect + [
        OpaqueFunction(function=create_detect_node),
    ])
