import time
import board
import digitalio
import pwmio

# ------------------ Motor A (Front Left) ------------------
in1 = digitalio.DigitalInOut(board.GP16)
in2 = digitalio.DigitalInOut(board.GP17)
in1.direction = digitalio.Direction.OUTPUT
in2.direction = digitalio.Direction.OUTPUT

ena_pwm = pwmio.PWMOut(board.GP18, frequency=1000)
ena_pwm.duty_cycle = 65535

# ------------------ Motor B (Front Right) ------------------
in3 = digitalio.DigitalInOut(board.GP19)
in4 = digitalio.DigitalInOut(board.GP20)
in3.direction = digitalio.Direction.OUTPUT
in4.direction = digitalio.Direction.OUTPUT

enb_pwm = pwmio.PWMOut(board.GP21, frequency=1000)
enb_pwm.duty_cycle = 65535

# ------------------ Motor C (Back Left) ------------------
in5 = digitalio.DigitalInOut(board.GP22)
in6 = digitalio.DigitalInOut(board.GP26)
in5.direction = digitalio.Direction.OUTPUT
in6.direction = digitalio.Direction.OUTPUT

enc_pwm = pwmio.PWMOut(board.GP27, frequency=1000)
enc_pwm.duty_cycle = 65535

# ------------------ Motor D (Back Right) ------------------
in7 = digitalio.DigitalInOut(board.GP28)
in8 = digitalio.DigitalInOut(board.GP10)
in7.direction = digitalio.Direction.OUTPUT
in8.direction = digitalio.Direction.OUTPUT

end_pwm = pwmio.PWMOut(board.GP15, frequency=1000)
end_pwm.duty_cycle = 65535

# ------------------ Movement Functions ------------------
def forward():
    # Front wheels (A & B): forward
    in1.value = True
    in2.value = False
    in3.value = True
    in4.value = False
    # Back wheels (C & D): reverse
    in5.value = False
    in6.value = True
    in7.value = False
    in8.value = True
    print("Forward (C & D reversed)")

def backward():
    # Front wheels: reverse
    in1.value = False
    in2.value = True
    in3.value = False
    in4.value = True
    # Back wheels: forward
    in5.value = True
    in6.value = False
    in7.value = True
    in8.value = False
    print("Backward (C & D reversed)")

def stop():
    in1.value = False
    in2.value = False
    in3.value = False
    in4.value = False
    in5.value = False
    in6.value = False
    in7.value = False
    in8.value = False
    print("Stopped")

# ------------------ Main Loop ------------------
while True:
    forward()
    time.sleep(2)

    backward()
    time.sleep(2)

    stop()
    time.sleep(2)