---

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

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

# Пины для каждого колеса (пара: вперёд/назад)
LF = (9, 11)    # Left Front
LR = (25, 8)    # Left Rear
RF = (26, 19)   # Right Front
RR = (20, 16)   # Right Rear

all_pins = LF + LR + RF + RR
GPIO.setup(all_pins, GPIO.OUT)

def set_wheel(pins, direction):
    """direction: 1 — вперёд, -1 — назад, 0 — стоп"""
    fwd, bwd = pins
    if direction > 0:
        GPIO.output(fwd, GPIO.HIGH)
        GPIO.output(bwd, GPIO.LOW)
    elif direction < 0:
        GPIO.output(fwd, GPIO.LOW)
        GPIO.output(bwd, GPIO.HIGH)
    else:
        GPIO.output(fwd, GPIO.LOW)
        GPIO.output(bwd, GPIO.LOW)

def stop_all():
    for pins in (LF, LR, RF, RR):
        set_wheel(pins, 0)

def forward():
    set_wheel(LF, 1); set_wheel(LR, 1)
    set_wheel(RF, 1); set_wheel(RR, 1)

def backward():
    set_wheel(LF, -1); set_wheel(LR, -1)
    set_wheel(RF, -1); set_wheel(RR, -1)

def turn_left():
    set_wheel(LF, -1); set_wheel(LR, -1)
    set_wheel(RF, 1);  set_wheel(RR, 1)

def turn_right():
    set_wheel(LF, 1);  set_wheel(LR, 1)
    set_wheel(RF, -1); set_wheel(RR, -1)

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

def main():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)

    print("Управление: W — вперёд, S — назад, A — влево, D — вправо")
    print("Пробел — стоп, Q — выход\n")

    current_cmd = None

    try:
        tty.setraw(fd)
        while True:
            key = get_key(0.1)
            if key is None:
                continue

            key = key.lower()

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

            elif key == 'w' and current_cmd != 'w':
                forward()
                current_cmd = 'w'
                print("Вперёд")

            elif key == 's' and current_cmd != 's':
                backward()
                current_cmd = 's'
                print("Назад")

            elif key == 'a' and current_cmd != 'a':
                turn_left()
                current_cmd = 'a'
                print("Влево")

            elif key == 'd' and current_cmd != 'd':
                turn_right()
                current_cmd = 'd'
                print("Вправо")

            elif key == ' ' and current_cmd != 'stop':
                stop_all()
                current_cmd = 'stop'
                print("Стоп")

    except KeyboardInterrupt:
        print("\nПрервано.")
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        stop_all()
        GPIO.cleanup()
        print("GPIO очищены.")

if __name__ == "__main__":
    main()