---

import asyncio
import json
import struct
import numpy as np
import cv2
import websockets
from playwright.sync_api import sync_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 as e:
        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}...")
        with sync_playwright() as p:
            browser = p.chromium.launch(headless=True)
            page = browser.new_page()
            page.goto(CAM_URL)
            
            # Ждем появления тега video
            video_element = page.wait_for_selector("video", timeout=10000)
            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:
                    # 1. Получаем скриншот текущего кадра из WebRTC video-тега
                    img_bytes = video_element.screenshot()
                    nparr = np.frombuffer(img_bytes, np.uint8)
                    frame = cv2.imdecode(nparr, cv2.IMREAD_COLOR)

                    # 2. Рисуем подсказку поверх видео
                    cv2.putText(frame, f"Speed: Vx={current_vx:.1f}, Wz={current_wz:.1f}", 
                                (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
                    
                    cv2.imshow("RoboMarvel Live Camera & Control", frame)

                    # 3. Обработка клавиш в окне OpenCV
                    key = cv2.waitKey(20) & 0xFF

                    if key == ord('w') or key == ord('W') or key == 230:  # 'ц' на русской раскладке
                        current_vx, current_wz = LINEAR_SPEED, 0.0
                    elif key == ord('s') or key == ord('S') or key == 251:
                        current_vx, current_wz = -LINEAR_SPEED, 0.0
                    elif key == ord('a') or key == ord('A') or key == 244:
                        current_vx, current_wz = 0.0, ANGULAR_SPEED
                    elif key == ord('d') or key == ord('D') or key == 226:
                        current_vx, current_wz = 0.0, -ANGULAR_SPEED
                    elif key == ord(' '):  # Пробел - СТОП
                        current_vx, current_wz = 0.0, 0.0
                    elif key == 27 or key == ord('q'):  # ESC или Q
                        break

                    # Отправляем текущую скорость роботу
                    await send_twist(current_vx, current_wz)

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

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