import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    bringup_dir = get_package_share_directory("hik_camera_ros2_driver")
    cam1_params_file = LaunchConfiguration("cam1_params_file")
    cam2_params_file = LaunchConfiguration("cam2_params_file")
    cam3_params_file = LaunchConfiguration("cam3_params_file")
    log_level = LaunchConfiguration("log_level")

    stdout_linebuf_envvar = SetEnvironmentVariable(
        "RCUTILS_LOGGING_BUFFERED_STREAM", "1"
    )

    colorized_output_envvar = SetEnvironmentVariable("RCUTILS_COLORIZED_OUTPUT", "1")

    declare_cam1_params_file_cmd = DeclareLaunchArgument(
        "cam1_params_file",
        default_value=os.path.join(bringup_dir, "config", "camera_params_cam1.yaml"),
        description="Parameter file path for cam1 (serial: DA9492688)",
    )

    declare_cam2_params_file_cmd = DeclareLaunchArgument(
        "cam2_params_file",
        default_value=os.path.join(bringup_dir, "config", "camera_params_cam2.yaml"),
        description="Parameter file path for cam2 (serial: DB0174264)",
    )

    declare_cam3_params_file_cmd = DeclareLaunchArgument(
        "cam3_params_file",
        default_value=os.path.join(bringup_dir, "config", "camera_params_cam3.yaml"),
        description="Parameter file path for cam3 (serial: TODO — fill in camera_params_cam3.yaml)",
    )

    declare_log_level_cmd = DeclareLaunchArgument(
        "log_level", default_value="info", description="log level"
    )

    start_hik_camera1_cmd = Node(
        name="hik_camera_cam1",
        package="hik_camera_ros2_driver",
        executable="hik_camera_ros2_driver_node",
        parameters=[cam1_params_file],
        arguments=["--ros-args", "--log-level", log_level],
        output="screen",
    )

    start_hik_camera2_cmd = Node(
        name="hik_camera_cam2",
        package="hik_camera_ros2_driver",
        executable="hik_camera_ros2_driver_node",
        parameters=[cam2_params_file],
        arguments=["--ros-args", "--log-level", log_level],
        output="screen",
    )

    start_hik_camera3_cmd = Node(
        name="hik_camera_cam3",
        package="hik_camera_ros2_driver",
        executable="hik_camera_ros2_driver_node",
        parameters=[cam3_params_file],
        arguments=["--ros-args", "--log-level", log_level],
        output="screen",
    )

    ld = LaunchDescription()

    ld.add_action(stdout_linebuf_envvar)
    ld.add_action(colorized_output_envvar)
    ld.add_action(declare_cam1_params_file_cmd)
    ld.add_action(declare_cam2_params_file_cmd)
    ld.add_action(declare_cam3_params_file_cmd)
    ld.add_action(declare_log_level_cmd)
    ld.add_action(start_hik_camera1_cmd)
    ld.add_action(start_hik_camera2_cmd)
    ld.add_action(start_hik_camera3_cmd)

    return ld
