from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

from pathlib import Path


def generate_launch_description():

    basedir = FindPackageShare("").find("toid_lidar")

    lidar_config = PathJoinSubstitution([basedir, "params/lidar.yaml"])
    rviz_config = PathJoinSubstitution([basedir, "rviz/conf.rviz"])

    visualize = LaunchConfiguration("visualize")
    draw_markers = LaunchConfiguration("draw_markers")
    use_closest = LaunchConfiguration("use_closest")
    lidar_frame = LaunchConfiguration("lidar_frame")

    return LaunchDescription(
        [
            DeclareLaunchArgument(
                "visualize",
                default_value="False",
                description="Whether to launch rviz2",
            ),
            DeclareLaunchArgument(
                "draw_markers", default_value="False", description="Draw markers"
            ),
            DeclareLaunchArgument(
                "use_closest",
                default_value="True",
                description="Use closest point for calibration",
            ),
            DeclareLaunchArgument(
                "lidar_frame",
                default_value="lidar_frame",
                description="TF frame of the lidar",
            ),
            Node(
                package="toid_lidar",
                executable="toid_lidar",
                output="screen",
                parameters=[
                    lidar_config,
                    {"closest": use_closest, "draw_markers": draw_markers},
                ],
            ),
            Node(
                package="rplidar_ros",
                executable="rplidar_composition",
                output="screen",
                parameters=[lidar_config, {"frame_id": lidar_frame}],
            ),
            Node(
                package="rviz2",
                executable="rviz2",
                name="rviz2",
                output="screen",
                arguments=["-d", rviz_config],
                condition=IfCondition(visualize),
            ),
        ]
    )