import time
import board
import usb_cdc
import pwmio
import digitalio

pwm_X = pwmio.PWMOut(board.GP17, frequency=1000, duty_cycle=0)
pwm_Y = pwmio.PWMOut(board.GP18, frequency=1000, duty_cycle=0)
pen = digitalio.DigitalInOut(board.GP16)
pen.direction = digitalio.Direction.OUTPUT
pen.value = False  # Set pen to up position

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]}")
    pwm_X.duty_cycle = int(parts[0])
    pwm_Y.duty_cycle = int(parts[1])
    pen.value = True if int(parts[2]) == 1 else False

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(0.5)
        usb_cdc.data.write(b"OK\n")
    time.sleep(0.1)