#!/usr/bin/env python3
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    camera_device_arg = DeclareLaunchArgument('camera_device', default_value='/dev/video0', description='Camera device path')
    phone_host_arg = DeclareLaunchArgument('phone_host', default_value='192.168.1.200', description='Phone IP for video bridge')
    phone_cam_enabled_arg = DeclareLaunchArgument('phone_cam_enabled', default_value='true', description='Enable phone camera node')
    gps_enabled_arg = DeclareLaunchArgument('gps_enabled', default_value='true', description='Enable GPS node')
    imu_enabled_arg = DeclareLaunchArgument('imu_enabled', default_value='true', description='Enable IMU node')
    chat_enabled_arg = DeclareLaunchArgument('chat_enabled', default_value='true', description='Enable OpenClaw chat node')
    bridge_url_arg = DeclareLaunchArgument('bridge_url', default_value='ws://saltylab-orin:9090', description='Jetson WebSocket bridge URL')

    camera_node = Node(
        package='saltybot_phone',
        executable='camera_node',
        name='camera_node',
        parameters=[{'camera_device': LaunchConfiguration('camera_device'), 'frame_width': 320, 'frame_height': 240, 'fps': 15}],
        output='screen',
    )

    gps_node = Node(
        package='saltybot_phone',
        executable='gps_node',
        name='gps_node',
        parameters=[{'gps_provider': 'termux', 'update_rate': 1.0}],
        output='screen',
    )

    imu_node = Node(
        package='saltybot_phone',
        executable='imu_node',
        name='imu_node',
        parameters=[{'imu_source': 'termux', 'update_rate': 50.0}],
        output='screen',
    )

    chat_node = Node(
        package='saltybot_phone',
        executable='openclaw_chat',
        name='openclaw_chat_node',
        parameters=[{'model_path': '/data/data/com.termux/files/home/.cache/openclaw/model.gguf', 'context_size': 256, 'top_p': 0.95, 'temperature': 0.7}],
        output='screen',
    )

    bridge_node = Node(
        package='saltybot_phone',
        executable='phone_bridge',
        name='phone_ws_bridge',
        parameters=[{'bridge_url': LaunchConfiguration('bridge_url'), 'reconnect_interval': 5.0}],
        output='screen',
    )

    phone_camera_node = Node(
        package='saltybot_phone',
        executable='phone_camera_node',
        name='phone_camera_node',
        parameters=[{
            'phone_host':         LaunchConfiguration('phone_host'),
            'ws_port':            8765,
            'http_port':          8766,
            'frame_id':           'phone_camera',
            'publish_raw':        True,
            'publish_compressed': True,
            'reconnect_s':        3.0,
            'latency_warn_ms':    200.0,
        }],
        output='screen',
    )

    return LaunchDescription([
        camera_device_arg, gps_enabled_arg, imu_enabled_arg, chat_enabled_arg,
        bridge_url_arg, phone_host_arg, phone_cam_enabled_arg,
        camera_node, gps_node, imu_node, chat_node, bridge_node, phone_camera_node,
    ])
