#!/usr/bin/python3
# -- coding: utf-8 --**
# Dual-camera (cam1 + cam2) launch. Unlike mapping_mid360s_cam{1,2,3}.launch.py
# (which each swap in a single alternative camera), this launches the
# multi-camera VIOManager path with both cameras subscribed simultaneously.
# See docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md.

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

def generate_launch_description():

    pkg_dir = get_package_share_directory("fast_livo")
    config_file_dir = os.path.join(pkg_dir, "config")
    rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz")
    urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf")

    mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml")
    camera_config_cmd = os.path.join(config_file_dir, "camera_dualcam_cam1_cam2.yaml")
    camera_array_config_cmd = os.path.join(config_file_dir, "extrin_dualcam_cam1_cam2.yaml")

    use_rviz_arg = DeclareLaunchArgument(
        "use_rviz", default_value="False", description="Whether to launch Rviz2"
    )
    mid360s_config_arg = DeclareLaunchArgument(
        "mid360s_params_file", default_value=mid360s_config_cmd,
        description="Full path to mid360s.yaml (scalar/common parameters)"
    )
    camera_config_arg = DeclareLaunchArgument(
        "camera_params_file", default_value=camera_config_cmd,
        description="Full path to the cam_0/cam_1 intrinsics yaml for parameter_blackboard"
    )
    camera_array_config_arg = DeclareLaunchArgument(
        "camera_array_config_file", default_value=camera_array_config_cmd,
        description="Full path to the yaml-cpp-parsed multi-camera extrinsics/topics array"
    )
    use_respawn_arg = DeclareLaunchArgument(
        "use_respawn", default_value="True", description="Whether to respawn if a node crashes."
    )

    mid360s_params_file = LaunchConfiguration("mid360s_params_file")
    camera_params_file = LaunchConfiguration("camera_params_file")
    camera_array_config_file = LaunchConfiguration("camera_array_config_file")
    use_respawn = LaunchConfiguration("use_respawn")

    return LaunchDescription([
        use_rviz_arg,
        mid360s_config_arg,
        camera_config_arg,
        camera_array_config_arg,
        use_respawn_arg,

        Node(
            package="robot_state_publisher",
            executable="robot_state_publisher",
            name="robot_state_publisher",
            parameters=[{"robot_description": open(urdf_file).read()}],
            output="screen"
        ),

        # Camera intrinsics for both cameras (cam_0/cam_1 namespaces), read
        # remotely by vk::camera_loader::loadFromRosNs.
        Node(
            package="demo_nodes_cpp",
            executable="parameter_blackboard",
            name="parameter_blackboard",
            parameters=[camera_params_file],
            output="screen"
        ),

        Node(
            package="fast_livo",
            executable="fastlivo_mapping",
            name="laserMapping",
            parameters=[
                mid360s_params_file,
                {
                    "common": {
                        "camera_config_path": camera_array_config_file,
                        # hik_camera_ros2_driver stamps images from the shared LiDAR-packet
                        # timestamp (see docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md), so two
                        # independently-read cameras can legitimately differ by close to one
                        # MID360 LiDAR period (~0.1s @ 10Hz). Loosen from Omni-LIVO's 1ms
                        # default until per-camera hardware trigger timestamps are wired up;
                        # tighten this once that's done.
                        "camera_time_tolerance": 0.12,
                    }
                },
                {"vio": {"enable_cross_camera_tracking": True}},
            ],
            additional_env={
                "LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0"
            },
            output="screen"
        ),

        Node(
            condition=IfCondition(LaunchConfiguration("use_rviz")),
            package="rviz2",
            executable="rviz2",
            name="rviz2",
            arguments=["-d", rviz_config_file],
            output="screen"
        ),
    ])
