import time
import board
import usb_cdc
import pwmio
import digitalio

settling_time = 0.01 # Measured on the scope
pen_time = 0.1

# GP20 is purple
pwm_Y = pwmio.PWMOut(board.GP20, frequency=1000, duty_cycle=0)
# GP10 is blue
pwm_X = pwmio.PWMOut(board.GP10, frequency=1000, duty_cycle=0)
pen = digitalio.DigitalInOut(board.GP8)

def set_pen_state(enable_low):
    if enable_low:
        pen.direction = digitalio.Direction.OUTPUT
        pen.value = False  # Set to LOW
    else:
        pen.direction = digitalio.Direction.INPUT # Set to HI-Z
    time.sleep(pen_time)
        

def message_handler(msg):
    parts = msg.split(',')
    if len(parts) != 3:
        print("Invalid message format")
        return
    print(f"X: {parts[0]}, Y: {parts[1]}, Pen: {parts[2]}")
    pen_state = True if int(parts[2]) == 1 else False
    set_pen_state(pen_state)
    # Set pen before setting the PWM values to not get weird lines. 
    pwm_X.duty_cycle = int(parts[0])
    pwm_Y.duty_cycle = int(parts[1])

print("Booted")
while True:
    if usb_cdc.data.in_waiting:
        data = usb_cdc.data.read(usb_cdc.data.in_waiting)
        message = data.decode('utf-8').strip()
        message_handler(message)
        time.sleep(settling_time)
        usb_cdc.data.write(b"OK\n")
    time.sleep(0.01)