#!/usr/bin/python3
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import LifecycleNode
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument

import lifecycle_msgs.msg
import os


def generate_launch_description():
    decoder_dir_right = os.path.join(
        get_package_share_directory("lslidar_c16_decoder"),
        "params",
        "lslidar_c16_right.yaml",
    )
    decoder_dir_left = os.path.join(
        get_package_share_directory("lslidar_c16_decoder"),
        "params",
        "lslidar_c16_left.yaml",
    )
    decoder_dir_front1 = os.path.join(
        get_package_share_directory("lslidar_c16_decoder"),
        "params",
        "lslidar_c16_front1.yaml",
    )
    decoder_dir_front2 = os.path.join(
        get_package_share_directory("lslidar_c16_decoder"),
        "params",
        "lslidar_c16_front2.yaml",
    )

    decoder_node_right = LifecycleNode(
        package="lslidar_c16_decoder",
        executable="cloud_node",
        name="cloud_node",
        output="screen",
        emulate_tty=True,
        parameters=[decoder_dir_right],
    )
    driver_node_right = LifecycleNode(
        package="lslidar_c16_driver",
        executable="lslidar_node",
        name="lslidar_node",
        output="screen",
        emulate_tty=True,
        parameters=[decoder_dir_right],
    )

    decoder_node_left = LifecycleNode(
        package="lslidar_c16_decoder",
        executable="cloud_node",
        name="cloud_node",
        output="screen",
        emulate_tty=True,
        parameters=[decoder_dir_left],
    )
    driver_node_left = LifecycleNode(
        package="lslidar_c16_driver",
        executable="lslidar_node",
        name="lslidar_node",
        output="screen",
        emulate_tty=True,
        parameters=[decoder_dir_left],
    )

    decoder_node_front1 = LifecycleNode(
        package="lslidar_c16_decoder",
        executable="cloud_node",
        name="cloud_node",
        output="screen",
        emulate_tty=True,
        parameters=[decoder_dir_front1],
    )
    driver_node_front1 = LifecycleNode(
        package="lslidar_c16_driver",
        executable="lslidar_node",
        name="lslidar_node",
        output="screen",
        emulate_tty=True,
        parameters=[decoder_dir_front1],
    )

    decoder_node_front2 = LifecycleNode(
        package="lslidar_c16_decoder",
        executable="cloud_node",
        name="cloud_node",
        output="screen",
        emulate_tty=True,
        parameters=[decoder_dir_front2],
    )
    driver_node_front2 = LifecycleNode(
        package="lslidar_c16_driver",
        executable="lslidar_node",
        name="lslidar_node",
        output="screen",
        emulate_tty=True,
        parameters=[decoder_dir_front2],
    )

    return LaunchDescription(
        [
            # decoder_node_right,
            # driver_node_right,
            # decoder_node_left,
            # driver_node_left,
            decoder_node_front1,
            driver_node_front1,
            decoder_node_front2,
            driver_node_front2,
        ]
    )
