---

import RPi.GPIO as GPIO
import sys
import time
import termios
import tty
import select

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

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):
    set_motor(MOTOR_A, left_dir)
    set_motor(MOTOR_B, right_dir)


# ============ Чтение клавиш без Enter ============
def get_key(timeout=0.05):
    """Возвращает нажатую клавишу или None."""
    fd = sys.stdin.fileno()
    rlist, _, _ = select.select([sys.stdin], [], [], timeout)
    if rlist:
        return sys.stdin.read(1)
    return None


# ============ Основной цикл ============
old_settings = termios.tcgetattr(sys.stdin)

print("Управление: W = вперёд, S = назад, A = влево, D = вправо, Q = выход")

try:
    tty.setcbreak(sys.stdin.fileno())

    while True:
        key = get_key(timeout=0.05)

        if key is None:
            # Клавиша не нажата — стоп
            stop_all()
            continue

        key = key.lower()

        if key == 'q':
            print("\nВыход")
            break

        elif key == 'w':
            drive(1, 1)     # вперёд

        elif key == 's':
            drive(-1, -1)   # назад

        elif key == 'a':
            drive(-1, 1)    # влево (разворот на месте)

        elif key == 'd':
            drive(1, -1)    # вправо (разворот на месте)

        else:
            stop_all()

        time.sleep(0.02)

except KeyboardInterrupt:
    pass
finally:
    stop_all()
    GPIO.cleanup()
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
    sys.exit(0)