---

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

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

# Пины: (forward, backward, enable)
LF = (9,  11, 13)   # Левое переднее
LR = (25, 24, 18)   # Левое заднее
RF = (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

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 get_key(timeout=0.1):
    r, _, _ = select.select([sys.stdin], [], [], timeout)
    if r:
        return sys.stdin.read(1)
    return None

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

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

    current_cmd = None
    stop_all()

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

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

            elif k == 'w' and current_cmd != 'w':
                forward(); current_cmd = 'w'
                print(f"Вперёд ({speed}%)")

            elif k == 's' and current_cmd != 's':
                backward(); current_cmd = 's'
                print(f"Назад ({speed}%)")

            elif k == 'a' and current_cmd != 'a':
                turn_left(); current_cmd = 'a'
                print(f"Влево ({speed}%)")

            elif k == 'd' and current_cmd != 'd':
                turn_right(); current_cmd = 'd'
                print(f"Вправо ({speed}%)")

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

            elif k in ('+', '='):
                speed = min(MAX_SPEED, speed + STEP)
                print(f"Скорость: {speed}%")
                if current_cmd == 'w': forward()
                elif current_cmd == 's': backward()
                elif current_cmd == 'a': turn_left()
                elif current_cmd == 'd': turn_right()

            elif k == '-':
                speed = max(MIN_SPEED, speed - STEP)
                print(f"Скорость: {speed}%")
                if current_cmd == 'w': forward()
                elif current_cmd == 's': backward()
                elif current_cmd == 'a': turn_left()
                elif current_cmd == 'd': turn_right()

    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()