#!/usr/bin/env python3
#
# Copyright 2023 CoAsia Nexell Co.,LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Authors: Biela Jo

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch.substitutions import ThisLaunchFileDir
from launch.substitutions import Command
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    LDS_MODEL = os.environ['LDS_MODEL']
    LDS_LAUNCH_FILE = '/hlds_laser.launch.py'

    # Dynamixel USB Port
    dynamixel_usb_port = LaunchConfiguration('dynamixel_usb_port', default='/dev/ttyUSB0')

    eutobot_param_dir = LaunchConfiguration(
        'eutobot_param_dir',
        default=os.path.join(
            get_package_share_directory('eutobot_bringup'),
            'param',
            'eutobot.yaml'))

    if LDS_MODEL == 'LDS-01':
        lidar_pkg_dir = LaunchConfiguration(
            'lidar_pkg_dir',
            default=os.path.join(get_package_share_directory('hls_lfcd_lds_driver'), 'launch'))
    elif LDS_MODEL == 'LDS-02':
        lidar_pkg_dir = LaunchConfiguration(
            'lidar_pkg_dir',
            default=os.path.join(get_package_share_directory('ld08_driver'), 'launch'))
        LDS_LAUNCH_FILE = '/ld08.launch.py'
    elif LDS_MODEL == 'YDLIDAR':
        lidar_pkg_dir = LaunchConfiguration(
            'lidar_pkg_dir',
            default=os.path.join(get_package_share_directory('ydlidar_ros2_driver'), 'launch'))
        LDS_LAUNCH_FILE = '/ydlidar_launch.py'
    else:
        lidar_pkg_dir = LaunchConfiguration(
            'lidar_pkg_dir',
            default=os.path.join(get_package_share_directory('hls_lfcd_lds_driver'), 'launch'))

    use_sim_time = LaunchConfiguration('use_sim_time', default='false')


    # YAHBOOM
    # yahboom_pkg_dir = LaunchConfiguration(
    #     'yahboom_pkg_dir',
    #     default=os.path.join(get_package_share_directory('yahboomcar_bringup'), 'launch'))


    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value=use_sim_time,
            description='Use simulation (Gazebo) clock if true'),

        DeclareLaunchArgument(
            'eutobot_param_dir',
            default_value=eutobot_param_dir,
            description='Full path to eutobot parameter file to load'),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [ThisLaunchFileDir(), '/eutobot_state_publisher.launch.py']),
            launch_arguments={'use_sim_time': use_sim_time}.items(),
        ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([lidar_pkg_dir, LDS_LAUNCH_FILE]),
            launch_arguments={'port': '/dev/ttySAC6', 'frame_id': 'base_scan'}.items(),
        ),

        # IncludeLaunchDescription(
        #     PythonLaunchDescriptionSource([yahboom_pkg_dir, "/yahboomcar_bringup_X3_launch.py"]),
        # ),

       Node(
           package='eutobot_node',
           executable='eutobot_ros',
           parameters=[eutobot_param_dir],
           arguments=['-i', dynamixel_usb_port],
           output='screen'),
    ])
