"""
rosbridge.launch.py — rosbridge WebSocket server + compressed image transport

Starts two things:
  1. rosbridge_websocket — WebSocket server on port 9090 (roslibjs-compatible)
  2. image_transport/republish × 4 — compresses each CSI camera's raw stream
     /camera/<name>/image_raw  →  /camera/<name>/image_raw/compressed

Prerequisites (already installed in saltybot/ros2-humble:jetson-orin):
  apt install ros-humble-rosbridge-server
  apt install ros-humble-image-transport-plugins   # supplies compressed plugin

Browser connection:
  var ros = new ROSLIB.Ros({ url: 'ws://<jetson-ip>:9090' });

Topic whitelist is configured in config/rosbridge_params.yaml.

Bandwidth tips:
  - Subscribe to /camera/<name>/image_raw/compressed, not raw images.
  - Use throttle_rate in ROSLIB.Topic to cap camera update rate:
      new ROSLIB.Topic({ ... messageType: 'sensor_msgs/CompressedImage',
                         throttle_rate: 200 })  // max 5 fps per camera
  - Subscribe to /map with throttle_rate: 5000 (0.2 Hz is enough for display).

Usage:
  ros2 launch saltybot_bringup rosbridge.launch.py

Verify:
  ros2 topic echo /rosbridge_websocket/status
  # In browser console:
  #   var ros = new ROSLIB.Ros({ url: 'ws://jetson.local:9090' });
  #   ros.on('connection', () => console.log('connected'));
"""

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


# Camera names matching saltybot_cameras/launch/csi_cameras.launch.py
# Topics: /camera/<name>/image_raw  (published by v4l2_camera_node)
_CAMERAS = ['front', 'left', 'rear', 'right']

# JPEG quality for compressed output (0–100).
# 75 = good quality/size trade-off at 640×480: ~15–25 KB/frame.
# Lower to 50 for tighter bandwidth budgets; raise to 90 for inspection use.
_JPEG_QUALITY = 75


def generate_launch_description():
    pkg_share = get_package_share_directory('saltybot_bringup')
    params_file = os.path.join(pkg_share, 'config', 'rosbridge_params.yaml')

    # ── rosbridge WebSocket server ────────────────────────────────────────────
    rosbridge = Node(
        package='rosbridge_server',
        executable='rosbridge_websocket',
        name='rosbridge_websocket',
        parameters=[params_file],
        output='screen',
    )

    # ── Compressed image republishers ─────────────────────────────────────────
    # image_transport/republish subscribes to raw sensor_msgs/Image and
    # re-publishes as sensor_msgs/CompressedImage using the compressed plugin.
    #
    # Node arguments: ['raw', 'compressed']
    #   'raw'        — input transport type
    #   'compressed' — output transport type
    #
    # Topic remappings:
    #   in              →  /camera/<name>/image_raw
    #   out/compressed  →  /camera/<name>/image_raw/compressed
    #
    # Parameter jpeg_quality controls JPEG encoder quality for the
    # compressed publisher.  The full parameter path in ROS2 is:
    #   /<node_name>/compressed/jpeg_quality
    republishers = [
        Node(
            package='image_transport',
            executable='republish',
            name=f'compress_{name}',
            arguments=['raw', 'compressed'],
            remappings=[
                ('in',             f'/camera/{name}/image_raw'),
                ('out/compressed', f'/camera/{name}/image_raw/compressed'),
            ],
            parameters=[{
                'compressed.jpeg_quality': _JPEG_QUALITY,
            }],
            output='screen',
        )
        for name in _CAMERAS
    ]

    # ── D435i colour republisher (Issue #177) ────────────────────────────────
    d435i_color = Node(
        package='image_transport',
        executable='republish',
        name='compress_d435i_color',
        arguments=['raw', 'compressed'],
        remappings=[
            ('in',             '/camera/color/image_raw'),
            ('out/compressed', '/camera/color/image_raw/compressed'),
        ],
        parameters=[{'compressed.jpeg_quality': _JPEG_QUALITY}],
        output='screen',
    )

    # ── D435i depth republisher (Issue #177) ─────────────────────────────────
    # Depth stream as compressedDepth (PNG16) — preserves uint16 depth values.
    # Browser displays as greyscale PNG (darker = closer).
    d435i_depth = Node(
        package='image_transport',
        executable='republish',
        name='compress_d435i_depth',
        arguments=['raw', 'compressedDepth'],
        remappings=[
            ('in',                  '/camera/depth/image_rect_raw'),
            ('out/compressedDepth', '/camera/depth/image_rect_raw/compressed'),
        ],
        output='screen',
    )

    return LaunchDescription([rosbridge] + republishers + [d435i_color, d435i_depth])