---

import pygame
import RPi.GPIO as GPIO
import cv2
import numpy as np
import sys

# --- Настройки 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 + [SERVO_Y_PIN]:
    GPIO.setup(pin, GPIO.OUT)
    if pin != SERVO_Y_PIN: GPIO.output(pin, GPIO.LOW)

# Инициализация ШИМ для серво
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 update_servo(angle):
    global current_angle
    current_angle = max(0, min(180, angle))
    duty = current_angle / 18 + 2.5
    pwm_y.ChangeDutyCycle(duty)

def stop_all_motors():
    for pin in motor_pins: GPIO.output(pin, 0)

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)

# --- Инициализация Pygame и Камеры ---
pygame.init()
cap = cv2.VideoCapture(0)
screen = pygame.display.set_mode((640, 480))
pygame.display.set_caption("Robot Control: Joystick Wheels + WASD Servo")

joystick = None
if pygame.joystick.get_count() > 0:
    joystick = pygame.joystick.Joystick(0)
    joystick.init()

clock = pygame.time.Clock()

try:
    while True:
        # 1. Захват видео
        ret, frame = cap.read()
        if ret:
            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()

        # Вывод текста
        font = pygame.font.SysFont("Arial", 20)
        img = font.render(f"Servo Y: {int(current_angle)}° | Step: {current_step:.1f}", True, (255, 255, 0))
        screen.blit(img, (10, 10))

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

except KeyboardInterrupt:
    pass
finally:
    cap.release()
    stop_all_motors()
    pwm_y.stop()
    GPIO.cleanup()
    pygame.quit()