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="sirius_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")},
            {"crosshair_alpha": 1.0},
            {"sof_cos_sim": 0.99},
            {"fe_distance": 10.0},
        ],
        remappings=[
            ("raw_image", "/sirius_bot/camera/image_view"),
            ("camera_info", "/sirius_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'))
    )


    pkg_gen_location_on_mesh = FindPackageShare(package="").find("gen_locations_on_mesh")

    mesh_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "sirius_scene_floor.ply"])

    cache_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "cache.pkl"])

    start_gen_location_on_mesh_cmd = Node(
        name='gen_locations_on_mesh',
        package='gen_locations_on_mesh',
        executable='gen_locations_on_mesh',
        parameters=[{'mesh_filename': mesh_path,
                   'cache_filename': cache_path,
                   'init_number_of_points': 100,
                   'max_z_coord': 3.}])

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