"""Launch three Hikrobot cameras for one-time checkerboard extrinsics."""

from pathlib import Path

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch_ros.parameter_descriptions import ParameterValue


VIEWS = ("front", "side", "top")


def _launch(context):
    cameras = []
    rectifiers = []
    serials = {}
    for view in VIEWS:
        serial = LaunchConfiguration(f"{view}_camera_serial").perform(context)
        if not serial:
            raise RuntimeError(f"{view}_camera_serial is required")
        serials[view] = serial
        namespace = f"/g20_extrinsics/{view}/camera"
        cameras.append(
            Node(
                package="linkerhand_calibration",
                executable="hikrobot_camera_node",
                name="hikrobot_camera",
                namespace=namespace,
                output="screen",
                emulate_tty=True,
                parameters=[
                    {
                        "serial_number": serial,
                        "expected_model": LaunchConfiguration("camera_model"),
                        "camera_name": LaunchConfiguration(
                            f"{view}_camera_name"
                        ),
                        "frame_id": f"g20_extrinsics_{view}_optical_frame",
                        "image_width": 1624,
                        "image_height": 1240,
                        "frame_rate": ParameterValue(
                            LaunchConfiguration("camera_frame_rate"),
                            value_type=float,
                        ),
                        "exposure_time_us": ParameterValue(
                            LaunchConfiguration("exposure_time_us"),
                            value_type=float,
                        ),
                        "gain_db": ParameterValue(
                            LaunchConfiguration("gain_db"), value_type=float
                        ),
                        "auto_exposure": False,
                        "camera_info_url": LaunchConfiguration(
                            f"{view}_camera_info_url"
                        ),
                    }
                ],
            )
        )
        rectifiers.append(
            ComposableNode(
                package="image_proc",
                plugin="image_proc::RectifyNode",
                name=f"rectify_{view}",
                namespace=namespace,
                remappings=[
                    ("image", f"{namespace}/image_raw"),
                    ("camera_info", f"{namespace}/camera_info"),
                    ("image_rect", f"{namespace}/image_rect"),
                ],
                parameters=[{"queue_size": 1}],
                extra_arguments=[{"use_intra_process_comms": True}],
            )
        )
    container = ComposableNodeContainer(
        name="g20_extrinsics_vision",
        namespace="/",
        package="rclcpp_components",
        executable="component_container_mt",
        composable_node_descriptions=rectifiers,
        output="screen",
    )
    solver = Node(
        package="linkerhand_calibration",
        executable="three_camera_extrinsics_node",
        name="g20_camera_extrinsics",
        output="screen",
        emulate_tty=True,
        parameters=[
            {
                "output_file": LaunchConfiguration("output_file"),
                "checkerboard_columns": ParameterValue(
                    LaunchConfiguration("checkerboard_columns"), value_type=int
                ),
                "checkerboard_rows": ParameterValue(
                    LaunchConfiguration("checkerboard_rows"), value_type=int
                ),
                "square_size_m": ParameterValue(
                    LaunchConfiguration("square_size_m"), value_type=float
                ),
                "enable_gui": ParameterValue(
                    LaunchConfiguration("enable_gui"), value_type=bool
                ),
                "gui_refresh_hz": ParameterValue(
                    LaunchConfiguration("gui_refresh_hz"), value_type=float
                ),
                "maximum_reprojection_rms_px": ParameterValue(
                    LaunchConfiguration("maximum_reprojection_rms_px"),
                    value_type=float,
                ),
                "maximum_candidate_pair_reprojection_rms_px": ParameterValue(
                    LaunchConfiguration(
                        "maximum_candidate_pair_reprojection_rms_px"
                    ),
                    value_type=float,
                ),
                "maximum_single_camera_reprojection_rms_px": ParameterValue(
                    LaunchConfiguration(
                        "maximum_single_camera_reprojection_rms_px"
                    ),
                    value_type=float,
                ),
                "auto_capture_default": ParameterValue(
                    LaunchConfiguration("auto_capture_default"),
                    value_type=bool,
                ),
                "auto_capture_stable_seconds": ParameterValue(
                    LaunchConfiguration("auto_capture_stable_seconds"),
                    value_type=float,
                ),
                **{
                    f"{view}_camera_serial": serials[view]
                    for view in VIEWS
                },
            }
        ],
    )
    return [*cameras, container, solver]


def generate_launch_description() -> LaunchDescription:
    package_share = Path(
        get_package_share_directory("linkerhand_calibration")
    )
    camera_info = Path.home() / ".ros" / "camera_info"
    return LaunchDescription(
        [
            # Keep the large-image transport deterministic even when the
            # calling shell selected another ROS 2 RMW implementation.
            SetEnvironmentVariable(
                name="RMW_IMPLEMENTATION",
                value="rmw_fastrtps_cpp",
            ),
            SetEnvironmentVariable(
                name="FASTDDS_DEFAULT_PROFILES_FILE",
                value=str(
                    package_share / "config" / "fastdds_large_images.xml"
                ),
            ),
            SetEnvironmentVariable(
                name="FASTRTPS_DEFAULT_PROFILES_FILE",
                value=str(
                    package_share / "config" / "fastdds_large_images.xml"
                ),
            ),
            DeclareLaunchArgument("front_camera_serial", default_value="DB2163742"),
            DeclareLaunchArgument("side_camera_serial", default_value="DB2163749"),
            DeclareLaunchArgument("top_camera_serial", default_value="DB2163739"),
            DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
            DeclareLaunchArgument("front_camera_name", default_value="hikrobot_front_DB2163742"),
            DeclareLaunchArgument("side_camera_name", default_value="hikrobot_side_DB2163749"),
            DeclareLaunchArgument("top_camera_name", default_value="hikrobot_top_DB2163739"),
            DeclareLaunchArgument("front_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163742.yaml")),
            DeclareLaunchArgument("side_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163749.yaml")),
            DeclareLaunchArgument("top_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163739.yaml")),
            DeclareLaunchArgument("camera_frame_rate", default_value="15.0"),
            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
            DeclareLaunchArgument("gain_db", default_value="0.0"),
            DeclareLaunchArgument("checkerboard_columns", default_value="8"),
            DeclareLaunchArgument("checkerboard_rows", default_value="5"),
            DeclareLaunchArgument("square_size_m", default_value="0.027"),
            DeclareLaunchArgument("enable_gui", default_value="true"),
            DeclareLaunchArgument("gui_refresh_hz", default_value="2.0"),
            DeclareLaunchArgument(
                "maximum_reprojection_rms_px", default_value="1.2"
            ),
            DeclareLaunchArgument(
                "maximum_candidate_pair_reprojection_rms_px",
                default_value="1.5",
            ),
            DeclareLaunchArgument(
                "maximum_single_camera_reprojection_rms_px",
                default_value="1.5",
            ),
            DeclareLaunchArgument("auto_capture_default", default_value="false"),
            DeclareLaunchArgument(
                "auto_capture_stable_seconds", default_value="1.0"
            ),
            DeclareLaunchArgument(
                "output_file",
                default_value=str(Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"),
            ),
            OpaqueFunction(function=_launch),
        ]
    )