---

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

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

# Пины: (forward, backward, enable)
# Плата №1 — передняя ось
LF = (9,  11, 13)   # левое переднее
RF = (25, 24, 18)   # правое переднее
# Плата №2 — задняя ось
LR = (26, 19, 12)   # левое заднее
RR = (20, 16, 17)   # правое заднее

all_pins = [p for wheel in (LF, LR, RF, RR) for p in wheel]
GPIO.setup(all_pins, GPIO.OUT, initial=GPIO.LOW)

# ШИМ на каждом EN-пине
pwm = {
    13: GPIO.PWM(13, 1000),
    18: GPIO.PWM(18, 1000),
    12: GPIO.PWM(12, 1000),
    17: GPIO.PWM(17, 1000),
}
for p in pwm.values():
    p.start(0)

# Глобальные переменные
speed = 60
MIN_SPEED, MAX_SPEED, STEP = 20, 100, 10
current_cmd = None
last_key_time = 0.0
HOLD_TIMEOUT = 0.4   # сек: если клавиша не повторилась за это время — стоп

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

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

def forward():
    for w in (LF, LR, RF, RR):
        set_wheel(w, 1)

def backward():
    for w in (LF, LR, RF, RR):
        set_wheel(w, -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 apply_speed_change():
    """Переприменяет текущее движение с новой скоростью."""
    if current_cmd == 'w':   forward()
    elif current_cmd == 's': backward()
    elif current_cmd == 'a': turn_left()
    elif current_cmd == 'd': turn_right()

def get_key(timeout=0.05):
    """Читает один символ, если есть. Иначе None."""
    r, _, _ = select.select([sys.stdin], [], [], timeout)
    if not r:
        return None
    return sys.stdin.read(1)

def main():
    global speed, current_cmd, last_key_time
    fd = sys.stdin.fileno()
    old = termios.tcgetattr(fd)

    print("Управление (движение — только пока клавиша зажата):")
    print("  W / A / S / D   — движение")
    print("  ,   /   .       — скорость -10% / +10%")
    print("  Пробел          — стоп")
    print("  Q               — выход")
    print(f"Стартовая скорость: {speed}%\n")

    current_cmd = None
    last_key_time = 0.0
    stop_all()

    try:
        tty.setraw(fd)
        while True:
            key = get_key(0.05)
            now = time.time()

            if key is not None:
                k = key.lower()

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

                elif k in ('w', 's', 'a', 'd'):
                    last_key_time = now
                    if current_cmd != k:
                        if   k == 'w': forward()
                        elif k == 's': backward()
                        elif k == 'a': turn_left()
                        elif k == 'd': turn_right()
                        current_cmd = k
                        names = {'w':'Вперёд','s':'Назад','a':'Влево','d':'Вправо'}
                        print(f"{names[k]} ({speed}%)")

                elif k == '.':
                    speed = min(MAX_SPEED, speed + STEP)
                    print(f"Скорость: {speed}%")
                    apply_speed_change()

                elif k == ',':
                    speed = max(MIN_SPEED, speed - STEP)
                    print(f"Скорость: {speed}%")
                    apply_speed_change()

                elif k == ' ':
                    stop_all()
                    current_cmd = 'stop'
                    print("Стоп")

            # Таймаут удержания: если клавиша движения не повторилась — стоп
            if current_cmd in ('w', 's', 'a', 'd') and (now - last_key_time) > HOLD_TIMEOUT:
                stop_all()
                current_cmd = 'stop'
                print("Стоп (клавиша отпущена)")

    except KeyboardInterrupt:
        print("\nПрервано.")
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old)
        stop_all()
        for p in pwm.values():
            p.stop()
        GPIO.cleanup()
        print("GPIO очищены.")

if __name__ == "__main__":
    main()