from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.substitutions import (
    PathJoinSubstitution,
    LaunchConfiguration,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch.conditions import IfCondition
from launch_ros.actions import Node


def generate_launch_description():
    pkg_unity_robot_controller = FindPackageShare(package="").find("unity_robot_controller")

    # Launch arguments
    launch_args = [
        DeclareLaunchArgument(
            "use_sim_time",
            description="Use clock from simulation",
            default_value="True",
        ),
        DeclareLaunchArgument(
            "namespace", default_value="arctic_bot", description="Top-level namespace"
        ),
        DeclareLaunchArgument(
            "start_rviz", default_value="True", description="Use basic rviz"
        ),
    ]

    # Unity connection
    unity_client = Node(
        package="ros_tcp_endpoint",
        executable="default_server_endpoint",
        name="ros_tcp_endpoint",
        # output="screen",
        arguments=[
            "--ros-args",
            "-p",
            "ROS_IP:=127.0.0.1",
            "-p",
            "ROS_TCP_PORT:=10000",
        ],
    )

    # joy = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource(
    #         PathJoinSubstitution([pkg_unity_robot_controller, "launch", "joy.launch.py"])
    #     ),
    #     launch_arguments={"namespace": LaunchConfiguration("namespace")}.items(),
    # )

    # fire extingusher emulator
    fe_emulator = Node(
        package="unity_robot_controller",
        executable="fire_extinguisher_emulator_node",
        namespace=LaunchConfiguration("namespace"),
        parameters=[
            {"use_sim_time": LaunchConfiguration("use_sim_time")},
        ],
        remappings=[
            ("raw_image", "/arctic_bot/camera"),
            ("camera_info", "/arctic_bot/camera/camera_info")
        ]
    )

    rviz = Node(
        package="rviz2",
        executable="rviz2",
        parameters=[
            {"use_sim_time": LaunchConfiguration("use_sim_time")},
        ],
        arguments=[
            "-d",
            PathJoinSubstitution([pkg_unity_robot_controller, "config", "unity.rviz"]),
        ],
        condition=IfCondition(LaunchConfiguration('start_rviz'))
    )

    return LaunchDescription(
        launch_args
        + [
            unity_client,
            #joy,
            fe_emulator,
            rviz,
        ]
    )