import os

import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

# Vi tri lap dat tung frame lidar so voi base_link: frame -> (x, y, z, yaw) met/rad.
# Frame khong liet ke o day mac dinh dat tai goc base_link (0,0,0,0) -> van co TF
# nen RViz khong con vut message. Sua toa do thuc te tai day.
MOUNT_POSES = {
    'front': (0.0, 0.0, 0.0, 0.0),
    'rear': (0.0, 0.0, 0.0, 0.0),
    'sick1': (0.0, 0.00, 0.0, 0.0),
}


def lidars_from_config(params_file):
    """Doc config.yaml -> danh sach dict thong tin moi lidar trong 'lidars'."""
    with open(params_file, 'r') as f:
        data = yaml.safe_load(f)
    params = data['lidarlib_node']['ros__parameters']
    lidars = []
    for name in params.get('lidars', []):
        block = params.get(name, {}) or {}
        lidars.append({
            'name': name,
            'brand': block.get('brand', '?'),
            'model': block.get('model', '?'),
            'ip': block.get('ip', '?'),
            'port': block.get('port', '?'),
            'inverted': block.get('inverted', False),
            'topic': block.get('topic', f'scan_{name}'),
            'frame_id': block.get('frame_id', name),
        })
    return lidars


def lidar_frames_from_config(params_file):
    """Doc config.yaml -> danh sach (frame_id) cua moi lidar trong 'lidars'."""
    return [ld['frame_id'] for ld in lidars_from_config(params_file)]


def generate_launch_description():
    pkg = get_package_share_directory('lidarlib_ros')
    params_file = os.path.join(pkg, 'config', 'lidar.yaml')
    rviz_config = os.path.join(pkg, 'rviz', 'lidar.rviz')

    use_rviz = LaunchConfiguration('rviz')

    lidars = lidars_from_config(params_file)

    # In thong tin cau hinh ra man hinh de tien theo doi/test.
    log_nodes = [
        LogInfo(msg='========== LIDAR LAUNCH INFO =========='),
        LogInfo(msg=f'Params file : {params_file}'),
        LogInfo(msg=f'RViz config : {rviz_config}'),
        LogInfo(msg=f'So luong lidar: {len(lidars)}'),
        LogInfo(msg='----------------------------------------'),
    ]

    # Tu sinh 1 static TF cho MOI frame lidar co trong config -> them lidar la co TF.
    tf_nodes = []
    for ld in lidars:
        frame = ld['frame_id']
        x, y, z, yaw = MOUNT_POSES.get(frame, (0.0, 0.0, 0.0, 0.0))
        default_pose = frame not in MOUNT_POSES
        log_nodes.append(LogInfo(
            msg=(f"[{ld['name']}] brand={ld['brand']} model={ld['model']} "
                 f"ip={ld['ip']}:{ld['port']} inverted={ld['inverted']} "
                 f"topic={ld['topic']} frame={frame} "
                 f"TF(x={x} y={y} z={z} yaw={yaw})"
                 f"{'  <-- mac dinh (0,0,0), sua trong MOUNT_POSES' if default_pose else ''}")))
        tf_nodes.append(Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name=f'base_to_{frame}',
            arguments=[str(x), str(y), str(z), str(yaw), '0', '0', 'base_link', frame],
        ))

    log_nodes.append(LogInfo(msg='========================================'))

    return LaunchDescription([
        DeclareLaunchArgument(
            'rviz', default_value='true',
            description='Mo RViz2 de hien thi LaserScan'),

        *log_nodes,

        # Node cau noi lidarlib -> sensor_msgs/LaserScan (nhieu lidar 1 node)
        Node(
            package='lidarlib_ros',
            executable='lidarlib_node',
            name='lidarlib_node',
            output='screen',
            parameters=[params_file],
        ),

        *tf_nodes,

        # RViz2 voi config san
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config],
            condition=IfCondition(use_rviz),
            output='screen',
        ),
    ])