from machine import Pin
from time import sleep
from servo import Servo
from encoder import RotaryEncoder


servo1 = Servo(12)
servo2 = Servo(14)
motion_sensor = Pin(34, Pin.IN)


direction = None


def rotation_handler(enc_direction):
    global direction
    direction = enc_direction 
    if direction == RotaryEncoder.ROT_CW:
        servo1.rotate(9)
        servo2.rotate(-9)
    else:
        servo1.rotate(-9)
        servo2.rotate(9)


myEncoder = RotaryEncoder(26, 27, rotation_handler)

# Main function
def main():
    global direction
    while True:
        sleep(0.1)  # Add slight delay for stability
        if motion_sensor.value() == 1:
            # Debugging output
            print("Motion detected! Opening bridge.")
            
            # Open the bridge when motion is detected
            servo1.move(180)
            servo2.move(0)  # Ensure this line is reached
            
            # Debugging output
            print("Servo1 moved to 180, Servo2 moved to -180.")
        elif direction is not None:
            # Allow manual control via rotary encoder
            if direction == RotaryEncoder.ROT_CW:
                servo1.move(0)
                servo2.move(180)
            elif direction == RotaryEncoder.ROT_CCW:
                servo1.move(180)
                servo2.move(0)
            direction = None  # Reset direction to avoid repeated actions
        else:
            # Default position when no input
            servo1.move(90)
            servo2.move(90)

# Entry point
if __name__ == '__main__':
    print("Hello, ESP32!")
    main()