---

import pygame
import RPi.GPIO as GPIO
import cv2  # Добавили библиотеку для работы с камерой
import numpy as np
import sys
import time

# --- Настройка GPIO ---
GPIO.setmode(GPIO.BCM)

# Пины моторов (твои настройки)
motor_pins = [26, 19, 13, 16, 27, 20, 23, 21]
SERVO_Y_PIN = 18  # Пин для сервопривода FS90

for pin in motor_pins:
    GPIO.setup(pin, GPIO.OUT)
    GPIO.output(pin, GPIO.LOW)

GPIO.setup(SERVO_Y_PIN, GPIO.OUT)
pwm_y = GPIO.PWM(SERVO_Y_PIN, 50)
pwm_y.start(7.5)  # Центр (90 градусов)

# Переменные для серво и прогрессии
current_angle = 90.0
base_step = 1.0
multiplier = 1.08
current_step = base_step

# Отдельные переменные для моторов
M1, M2, M3, M4 = 26, 19, 13, 16
M5, M6, M7, M8 = 27, 20, 23, 21

# --- Функции управления ---
def stop_all_motors():
    for pin in motor_pins:
        GPIO.output(pin, GPIO.LOW)

def move_forward():
    GPIO.output([M2, M4, M6, M8], 1)
    GPIO.output([M1, M3, M5, M7], 0)

def move_backward():
    GPIO.output([M2, M4, M6, M8], 0)
    GPIO.output([M1, M3, M5, M7], 1)

def move_left():
    GPIO.output([M1, M4, M5, M8], 1)
    GPIO.output([M2, M3, M6, M7], 0)

def move_right():
    GPIO.output([M2, M3, M6, M7], 1)
    GPIO.output([M1, M4, M5, M8], 0)

def update_servo(angle):
    global current_angle
    current_angle = max(0, min(180, angle))
    duty = current_angle / 18 + 2.5
    pwm_y.ChangeDutyCycle(duty)

# --- Инициализация захвата видео ---
cap = cv2.VideoCapture(0)  # Захват с первой камеры
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

# --- Инициализация Pygame ---
pygame.init()
screen = pygame.display.set_mode((640, 480))
pygame.display.set_caption("Robot Control: Video + Joystick + WASD")

# Проверка джойстика
joystick = None
if pygame.joystick.get_count() > 0:
    joystick = pygame.joystick.Joystick(0)
    joystick.init()
    print("Джойстик подключен")
else:
    print("Джойстик не найден, управление только WASD")

clock = pygame.time.Clock()

try:
    while True:
        # 1. Видеозахват
        ret, frame = cap.read()
        if ret:
            # Конвертируем BGR в RGB и готовим для Pygame
            frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
            frame = np.rot90(frame) # Поворачиваем если нужно
            frame = pygame.surfarray.make_surface(frame)
            frame = pygame.transform.flip(frame, True, False)
            screen.blit(frame, (0, 0)) # Рисуем видео как фон

        # 2. Обработка системных событий
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                raise KeyboardInterrupt
            if event.type == pygame.KEYUP:
                if event.key in [pygame.K_a, pygame.K_d]:
                    current_step = base_step # Сброс прогрессии

        # 3. Управление СЕРВО (WASD)
        keys = pygame.key.get_pressed()
        if keys[pygame.K_w]:
            update_servo(180) # Максимум
        elif keys[pygame.K_s]:
            update_servo(0)   # Минимум
        elif keys[pygame.K_a] or keys[pygame.K_d]:
            if keys[pygame.K_a]:
                update_servo(current_angle - current_step)
            if keys[pygame.K_d]:
                update_servo(current_angle + current_step)
            current_step = min(current_step * multiplier, 15) # Ускорение

        # 4. Управление КОЛЕСАМИ (Джойстик)
        if joystick:
            axis_y = joystick.get_axis(1)
            axis_x = joystick.get_axis(0)
            
            if axis_y < -0.3: move_forward()
            elif axis_y > 0.3: move_backward()
            elif axis_x < -0.4: move_right()
            elif axis_x > 0.4: move_left()
            else: stop_all_motors()

        # 5. Вывод инфо поверх видео
        font = pygame.font.SysFont("Arial", 18)
        info = font.render(f"Servo Y: {int(current_angle)} | Step: {current_step:.1f}", True, (0, 255, 0))
        screen.blit(info, (10, 10))

        pygame.display.flip()
        clock.tick(30)

except KeyboardInterrupt:
    pass
finally:
    # Очистка
    cap.release()
    stop_all_motors()
    pwm_y.stop()
    GPIO.cleanup()
    pygame.quit()
    sys.exit()