import sys, tty, termios, select
from gpiozero import AngularServo
from gpiozero.pins.pigpio import PiGPIOFactory
from gpiozero import Motor
from time import sleep

pigpio_factory = PiGPIOFactory()

servo1 = AngularServo(13, min_angle=-15, max_angle=15,
                     min_pulse_width=0.0010, max_pulse_width=0.0020,
                     pin_factory=pigpio_factory)
servo2 = AngularServo(6, min_angle=-15, max_angle=15,
                     min_pulse_width=0.0010, max_pulse_width=0.0020,
                     pin_factory=pigpio_factory)
servo3 = AngularServo(5, min_angle=-15, max_angle=15,
                     min_pulse_width=0.0010, max_pulse_width=0.0020,
                     pin_factory=pigpio_factory)
motor1 = Motor(17, 18, pin_factory=pigpio_factory)

angle1 = 0
angle2 = 0
angle3 = 0
mspeed = 0.1
minA1 = servo1.min_angle
maxA1 = servo1.max_angle
minA2 = servo2.min_angle
maxA2 = servo2.max_angle
minA3 = servo3.min_angle
maxA3 = servo3.max_angle
mstatus = 0

def get_key(timeout=0.05):
    """Non-blocking single-key read with timeout"""
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(fd)
        rlist, _, _ = select.select([sys.stdin], [], [], timeout)
        if rlist:
            return sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return None

def motor_restart():
    if mstatus == 0:
        motor1.forward(speed=mspeed)
    elif mstatus == 1:
        motor1.backward(speed=mspeed)

print("Hold W/S to move dive planes, A/D to move rudder, R to reset servos, Ctrl+C to quit")

try:
    while True:
        key = get_key()
        if key == '\x03':  # Ctrl+C
            break
        elif key == 'r':
            angle1 = 0
            angle2 = 0
            angle3 = 0
            motor1.stop()
        elif key == 'g':
            motor1.stop()
        elif key == 'w':
            angle1 += 1  # adjust this for speed
            angle2 += 1
        elif key == 's':
            angle1 -= 1
            angle2 -= 1
        elif key == 'a':
            angle3 += 1
        elif key == 'd':
            angle3 -= 1
        elif key == 'f':
            mstatus = 0
            motor1.forward(speed=mspeed)
        elif key == 'v':
            mstatus = 1
            motor1.backward(speed=mspeed)
        elif key == 'k':
            mspeed-=0.1
            mspeed = max(0.0, min(1.0, mspeed))
            motor_restart()
            print(f"Motor speed: {mspeed}")
        elif key == 'l':
            mspeed += 0.1
            mspeed = max(0.0, min(1.0, mspeed))
            motor_restart()
            print(f"Motor speed: {mspeed}")



        # Clamp and apply
        angle1 = max(minA1, min(maxA1, angle1))
        servo1.angle = angle1
        print(f"\rAngle1: {angle1:>4}°", end='', flush=True)
        
        angle2 = max(minA2, min(maxA2, angle2))
        servo2.angle = angle2
        print(f"\rAngle2: {angle2:>4}°", end='', flush=True)

        angle3 = max(minA3, min(maxA3, angle3))
        servo3.angle = angle3
        print(f"\rAngle3: {angle3:>4}°", end='', flush=True)

        sleep(0.05)  # smooth motion interval

except KeyboardInterrupt:
    pass

finally:
    servo1.angle = 0
    servo2.angle = 0
    motor1.stop()
    mspeed = 0.1
    print("\nServo reset to 0°, motor stopped, exiting...")