#!/usr/bin/env python3

import os

from ament_index_python.packages import get_package_share_directory

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


def generate_launch_description():
    # adapt if needed
    debug = False
    respawn = False

    default_config_file = os.path.join(
        get_package_share_directory("basler_camera_launcher"), "config", "camera2.yaml"
    )

    # launch configuration variables
    node_name = LaunchConfiguration("node_name")
    namespace = LaunchConfiguration("namespace")

    config_file = LaunchConfiguration("config_file")

    mtu_size = LaunchConfiguration("mtu_size")
    startup_user_set = LaunchConfiguration("startup_user_set")
    enable_status_publisher = LaunchConfiguration("enable_status_publisher")
    enable_current_params_publisher = LaunchConfiguration(
        "enable_current_params_publisher"
    )

    # launch arguments
    declare_node_name_cmd = DeclareLaunchArgument(
        "node_name",
        default_value="pylon_ros2_camera_node2",
        description="Name of the wrapper node.",
    )

    declare_namespace_cmd = DeclareLaunchArgument(
        "namespace",
        default_value="my_camera",
        description="Id of the camera. Used as node namespace.",
    )

    declare_config_file_cmd = DeclareLaunchArgument(
        "config_file",
        default_value=default_config_file,
        description="Camera parameters structured in a .yaml file.",
    )

    declare_mtu_size_cmd = DeclareLaunchArgument(
        "mtu_size",
        default_value="1500",
        description="Maximum transfer unit size. To enable jumbo frames, set it to a high value (8192 recommended)",
    )

    declare_startup_user_set_cmd = DeclareLaunchArgument(
        "startup_user_set",
        # possible value: Default, UserSet1, UserSet2, UserSet3, CurrentSetting
        default_value="CurrentSetting",
        description="Specific user set defining user parameters to run the camera.",
    )

    declare_enable_status_publisher_cmd = DeclareLaunchArgument(
        "enable_status_publisher",
        default_value="true",
        description="Enable/Disable the status publishing.",
    )

    declare_enable_current_params_publisher_cmd = DeclareLaunchArgument(
        "enable_current_params_publisher",
        default_value="true",
        description="Enable/Disable the current parameter publishing.",
    )

    # log format
    os.environ["RCUTILS_CONSOLE_OUTPUT_FORMAT"] = (
        "{time} [{name}] [{severity}] {message}"
    )

    # see https://navigation.ros.org/tutorials/docs/get_backtrace.html
    if debug is True:
        launch_prefix = ["xterm -e gdb -ex run --args"]
    else:
        launch_prefix = ""

    # node
    pylon_ros2_camera_node = Node(
        package="pylon_ros2_camera_wrapper",
        namespace=namespace,
        executable="pylon_ros2_camera_wrapper",
        name=node_name,
        output="screen",
        respawn=respawn,
        emulate_tty=True,
        prefix=launch_prefix,
        parameters=[
            config_file,
            {
                "gige/mtu_size": mtu_size,
                "startup_user_set": startup_user_set,
                "enable_status_publisher": enable_status_publisher,
                "enable_current_params_publisher": enable_current_params_publisher,
            },
        ],
    )

    # Define LaunchDescription variable and return it
    ld = LaunchDescription()

    ld.add_action(declare_node_name_cmd)
    ld.add_action(declare_namespace_cmd)

    ld.add_action(declare_config_file_cmd)
    ld.add_action(declare_mtu_size_cmd)
    ld.add_action(declare_startup_user_set_cmd)
    ld.add_action(declare_enable_status_publisher_cmd)
    ld.add_action(declare_enable_current_params_publisher_cmd)

    ld.add_action(pylon_ros2_camera_node)

    return ld