#!/usr/bin/env python3

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import TimerAction
from launch_ros.actions import Node


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

    # node specific parameters
    config_file1 = os.path.join(
        get_package_share_directory("basler_camera_launcher"), "config", "camera1.yaml"
    )
    config_file2 = os.path.join(
        get_package_share_directory("basler_camera_launcher"), "config", "camera2.yaml"
    )
    config_file3 = os.path.join(
        get_package_share_directory("basler_camera_launcher"), "config", "camera3.yaml"
    )
    node_name1 = "pylon_ros2_camera_node1"
    node_name2 = "pylon_ros2_camera_node2"
    node_name3 = "pylon_ros2_camera_node3"

    # common parameters
    namespace = "my_camera"
    mtu_size = 1500
    startup_user_set = "CurrentSetting"
    enable_status_publisher = True
    enable_current_params_publisher = True

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

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

    # node1
    pylon_ros2_camera_node1 = Node(
        package="pylon_ros2_camera_wrapper",
        namespace=namespace,
        executable="pylon_ros2_camera_wrapper",
        name=node_name1,
        output="screen",
        respawn=respawn,
        emulate_tty=True,
        prefix=launch_prefix,
        parameters=[
            config_file1,
            {
                "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,
            },
        ],
    )
    # node2
    pylon_ros2_camera_node2 = Node(
        package="pylon_ros2_camera_wrapper",
        namespace=namespace,
        executable="pylon_ros2_camera_wrapper",
        name=node_name2,
        output="screen",
        respawn=respawn,
        emulate_tty=True,
        prefix=launch_prefix,
        parameters=[
            config_file2,
            {
                "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,
            },
        ],
    )
    # node3
    pylon_ros2_camera_node3 = Node(
        package="pylon_ros2_camera_wrapper",
        namespace=namespace,
        executable="pylon_ros2_camera_wrapper",
        name=node_name3,
        output="screen",
        respawn=respawn,
        emulate_tty=True,
        prefix=launch_prefix,
        parameters=[
            config_file3,
            {
                "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(pylon_ros2_camera_node1)
    # Add the second node with a delay
    ld.add_action(
        TimerAction(
            period=5.0,  # 5 seconds delay
            actions=[pylon_ros2_camera_node2],
        )
    )
    # Add the third node with a delay
    ld.add_action(
        TimerAction(
            period=10.0,  # Additional 5 seconds delay after the second node, making it 10 seconds after the first
            actions=[pylon_ros2_camera_node3],
        )
    )

    return ld