from machine import Pin
import utime

MOTOR_LEFT_DIR = True  # False if Reverse
MOTOR_RIGHT_DIR = True  # False if Reverse
ENCODER_LEFT_COUNT = 0
ENCODER_RIGHT_COUNT = 0

ENCODER_LEFT_PIN = Pin(4, Pin.IN)
ENCODER_RIGHT_PIN = Pin(5, Pin.IN)


def handle_encoder_left(pin):
    global ENCODER_LEFT_COUNT
    if MOTOR_LEFT_DIR:
        ENCODER_LEFT_COUNT += 1
    else:
        ENCODER_LEFT_COUNT -= 1


def handle_encoder_right(pin):
    global ENCODER_RIGHT_COUNT
    if MOTOR_RIGHT_DIR:
        ENCODER_RIGHT_COUNT += 1
    else:
        ENCODER_RIGHT_COUNT -= 1


def main():
    global MOTOR_LEFT_DIR
    ENCODER_LEFT_PIN.irq(trigger=Pin.IRQ_RISING, handler=handle_encoder_left)
    ENCODER_RIGHT_PIN.irq(trigger=Pin.IRQ_RISING, handler=handle_encoder_right)

    while True:
        print("LEFT: {} , RIGHT: {}".format(ENCODER_LEFT_COUNT, ENCODER_RIGHT_COUNT))
        utime.sleep(3)
        print("LEFT: {} , RIGHT: {}".format(ENCODER_LEFT_COUNT, ENCODER_RIGHT_COUNT))
        utime.sleep(3)
        print("LEFT: {} , RIGHT: {}".format(ENCODER_LEFT_COUNT, ENCODER_RIGHT_COUNT))
        utime.sleep(3)
        print("SWITCH DIR")
        MOTOR_LEFT_DIR = not MOTOR_LEFT_DIR