---

import pygame
import pygame.camera # Используем встроенный модуль камер Pygame
import RPi.GPIO as GPIO
import sys
import time

# --- Настройка GPIO ---
GPIO.setmode(GPIO.BCM)
motor_pins = [26, 19, 13, 16, 27, 20, 23, 21]
SERVO_Y_PIN = 18

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)

# Переменные состояния
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, 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)

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

# --- Инициализация Pygame и Камеры (без CV2) ---
pygame.init()
pygame.camera.init()

# Пытаемся найти камеру
cam_list = pygame.camera.list_cameras()
if cam_list:
    cam = pygame.camera.Camera(cam_list[0], (640, 480))
    cam.start()
else:
    print("Камера не найдена!")
    sys.exit()

screen = pygame.display.set_mode((640, 480))
pygame.display.set_caption("Robot: No CV2 / No NumPy")

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

clock = pygame.time.Clock()

try:
    while True:
        # 1. Захват кадра средствами Pygame (медленнее, чем CV2, но без зависимостей)
        if cam.query_image():
            image = cam.get_image()
            screen.blit(image, (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. Колеса (Joystick)
        if joystick:
            ax_y, ax_x = joystick.get_axis(1), joystick.get_axis(0)
            if ax_y < -0.3: move_forward()
            elif ax_y > 0.3: move_backward()
            elif ax_x < -0.4: move_right()
            elif ax_x > 0.4: move_left()
            else: stop_all_motors()

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

except KeyboardInterrupt:
    pass
finally:
    if cam: cam.stop()
    stop_all_motors()
    pwm_y.stop()
    GPIO.cleanup()
    pygame.quit()