---

import asyncio
import json
import struct
import numpy as np
import cv2
import websockets
from playwright.async_api import async_playwright

ROBOT_IP = "10.10.10.10"
WS_URL = f"ws://{ROBOT_IP}:8765"
CAM_URL = f"http://{ROBOT_IP}:8889/cam/"
CMD_TOPIC = "/cmd_vel"

LINEAR_SPEED = 0.25
ANGULAR_SPEED = 0.6

channel_id = 1
ws_conn = None

async def send_twist(vx, wz):
    if ws_conn 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)}
    }
    payload = json.dumps(twist_data).encode('utf-8')
    header = struct.pack("<BI", 1, channel_id)
    try:
        await ws_conn.send(header + payload)
    except Exception:
        pass

async def main():
    global ws_conn

    print(f"1. Подключение к Foxglove WebSocket {WS_URL}...")
    async with websockets.connect(WS_URL, subprotocols=["foxglove.websocket.v1"]) as ws:
        ws_conn = ws

        # Регистрируем топик скорости
        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("Управление подключено!")

        print(f"2. Подключение к видеопотоку WebRTC: {CAM_URL}...")
        async with async_playwright() as p:
            browser = await p.chromium.launch(headless=True)
            page = await browser.new_page()
            await page.goto(CAM_URL)

            # Ждем появления тега video
            video_element = await page.wait_for_selector("video", timeout=15000)
            
            print("\n==============================================")
            print(" ОКНО ВИДЕО ОТКРЫТО!")
            print(" Кликните на появившееся окно с видео:")
            print("   W - вперед | S - назад | A/D - повороты")
            print("   Пробел - остановка | ESC/Q - выход")
            print("==============================================\n")

            current_vx = 0.0
            current_wz = 0.0

            try:
                while True:
                    # Снимаем текущий кадр из видео
                    img_bytes = await video_element.screenshot()
                    nparr = np.frombuffer(img_bytes, np.uint8)
                    frame = cv2.imdecode(nparr, cv2.IMREAD_COLOR)

                    if frame is not None:
                        # Текст со скоростью на кадре
                        status_text = f"Vx: {current_vx:.2f} | Wz: {current_wz:.2f}"
                        cv2.putText(frame, status_text, (10, 30), 
                                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        cv2.imshow("RoboMarvel Live Camera & Control", frame)

                    # Читаем клавиши в окне OpenCV
                    key = cv2.waitKey(20) & 0xFF

                    if key in (ord('w'), ord('W'), 230):      # W / ц
                        current_vx, current_wz = LINEAR_SPEED, 0.0
                    elif key in (ord('s'), ord('S'), 251):    # S / ы
                        current_vx, current_wz = -LINEAR_SPEED, 0.0
                    elif key in (ord('a'), ord('A'), 244):    # A / ф
                        current_vx, current_wz = 0.0, ANGULAR_SPEED
                    elif key in (ord('d'), ord('D'), 226):    # D / в
                        current_vx, current_wz = 0.0, -ANGULAR_SPEED
                    elif key == ord(' '):                     # Пробел - СТОП
                        current_vx, current_wz = 0.0, 0.0
                    elif key in (27, ord('q'), ord('Q')):     # ESC / Q - Выход
                        break

                    # Отправляем команду скорости
                    await send_twist(current_vx, current_wz)

            finally:
                await send_twist(0.0, 0.0)
                await browser.close()
                cv2.destroyAllWindows()

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