---

import asyncio
import json
import struct
from pynput import keyboard
import websockets

ROBOT_IP = "10.10.10.10"
WS_URL = f"ws://{ROBOT_IP}:8765"

# Если в Шаге 1 топик называется иначе — замените здесь:
CMD_TOPIC = "/cmd_vel"

LINEAR_SPEED = 0.2
ANGULAR_SPEED = 0.5

keys_pressed = set()
ws = None
channel_id = 1

async def advertise_channel():
    """Объявляем для Foxglove Bridge, что мы будем публиковать geometry_msgs/msg/Twist"""
    # Сообщение регистрации канала по протоколу Foxglove WebSocket
    advertise_msg = {
        "op": "advertise",
        "channels": [
            {
                "id": channel_id,
                "topic": CMD_TOPIC,
                "encoding": "json",
                "schemaName": "geometry_msgs/msg/Twist"
            }
        ]
    }
    await ws.send(json.dumps(advertise_msg))
    print(f"Канал публикации в {CMD_TOPIC} зарегистрирован!")

async def send_twist(vx, wz):
    if ws is None:
        return
    
    twist_data = {
        "linear": {"x": float(vx), "y": 0.0, "z": 0.0},
        "angular": {"x": 0.0, "y": 0.0, "z": float(wz)}
    }
    
    # Foxglove WebSocket binary publish message:
    # Байт 1: op code (1 = Client Publish)
    # Байты 2-5: uint32 channel_id (little-endian)
    # Остальные байты: payload (JSON-строка)
    payload = json.dumps(twist_data).encode('utf-8')
    header = struct.pack("<BI", 1, channel_id)
    binary_msg = header + payload

    try:
        await ws.send(binary_msg)
    except Exception as e:
        print(f"Ошибка отправки: {e}")

def on_press(key):
    try:
        if hasattr(key, 'char') and key.char:
            keys_pressed.add(key.char.lower())
    except AttributeError:
        pass

def on_release(key):
    try:
        if hasattr(key, 'char') and key.char:
            keys_pressed.discard(key.char.lower())
    except AttributeError:
        pass
    if key == keyboard.Key.esc:
        return False

async def main():
    global ws
    print(f"Подключение к {WS_URL}...")
    async with websockets.connect(WS_URL, subprotocols=["foxglove.websocket.v1"]) as websocket:
        ws = websocket
        await advertise_channel()
        
        print("\n--- Управление готово! ---")
        print("Зажмите W для движения вперед, A/D для поворота, ESC для выхода.")

        listener = keyboard.Listener(on_press=on_press, on_release=on_release)
        listener.start()

        try:
            while listener.is_alive():
                vx, wz = 0.0, 0.0
                if 'w' in keys_pressed:
                    vx += LINEAR_SPEED
                if 's' in keys_pressed:
                    vx -= LINEAR_SPEED
                if 'a' in keys_pressed:
                    wz += ANGULAR_SPEED
                if 'd' in keys_pressed:
                    wz -= ANGULAR_SPEED

                await send_twist(vx, wz)
                await asyncio.sleep(0.05)  # 20 Гц
        finally:
            await send_twist(0.0, 0.0)
            print("Остановлено.")

if __name__ == "__main__":
    asyncio.run(main())