---

import RPi.GPIO as GPIO
import pygame
import sys
import time

# ============ GPIO ============
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

# Каждый мотор — пара пинов (IN1, IN2) одного канала L298N.
# ENA/ENB предполагаются замкнутыми джампером (полная скорость).
MOTOR_A = (20, 16)   # левый мотор
MOTOR_B = (26, 19)   # правый мотор

ALL_MOTORS = [MOTOR_A, MOTOR_B]

# Сброс и инициализация пинов
GPIO.cleanup()
for motor in ALL_MOTORS:
    for pin in motor:
        GPIO.setup(pin, GPIO.OUT)
        GPIO.output(pin, GPIO.LOW)


# ============ Управление моторами ============
def set_motor(motor, direction):
    """direction: +1 вперёд, -1 назад, 0 стоп"""
    in1, in2 = motor
    if direction > 0:
        GPIO.output(in1, GPIO.HIGH)
        GPIO.output(in2, GPIO.LOW)
    elif direction < 0:
        GPIO.output(in1, GPIO.LOW)
        GPIO.output(in2, GPIO.HIGH)
    else:
        GPIO.output(in1, GPIO.LOW)
        GPIO.output(in2, GPIO.LOW)


def stop_all():
    for motor in ALL_MOTORS:
        set_motor(motor, 0)


def drive(left_dir, right_dir):
    """left_dir / right_dir: -1, 0, +1"""
    set_motor(MOTOR_A, left_dir)
    set_motor(MOTOR_B, right_dir)


def sign(value):
    if value > 0:
        return 1
    if value < 0:
        return -1
    return 0


# ============ Джойстик ============
pygame.init()
pygame.joystick.init()

if pygame.joystick.get_count() == 0:
    print("Джойстик не найден")
    GPIO.cleanup()
    pygame.quit()
    sys.exit(1)

joystick = pygame.joystick.Joystick(0)
joystick.init()
print(f"Джойстик: {joystick.get_name()}")

# ============ Настройки ============
DEADZONE = 0.2   # мёртвая зона стика
LOOP_HZ  = 50    # частота опроса, Гц
PERIOD   = 1.0 / LOOP_HZ

# ============ Основной цикл ============
try:
    while True:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                raise KeyboardInterrupt

        # Ось Y: вверх = -1, вниз = +1 -> инвертируем
        y = -joystick.get_axis(1)   # вперёд = +1, назад = -1
        x =  joystick.get_axis(0)   # вправо = +1, влево = -1

        # Мёртвая зона
        if abs(y) < DEADZONE: y = 0.0
        if abs(x) < DEADZONE: x = 0.0

        # Дифференциальный привод (танк)
        left  = max(-1.0, min(1.0, y + x))
        right = max(-1.0, min(1.0, y - x))

        drive(sign(left), sign(right))

        time.sleep(PERIOD)

except KeyboardInterrupt:
    pass
finally:
    stop_all()
    GPIO.cleanup()
    pygame.quit()
    sys.exit(0)