import serial
import struct
import time
from vpython import box, vector, color
colorIndexes = [0,0,0,0,0]
colorIndexes[0] = vector(0,0,0)
colorIndexes[1] = vector(0,1,0)
colorIndexes[2] = vector(0.25,0.125,0)
colorIndexes[3] = vector(0.5,0.5,0.5)
colorIndexes[4] = vector(0,0,1)


drawn_blocks = []
ser = serial.Serial('COM3', 115200, timeout=1)
time.sleep(2)
while True:
    if ser.in_waiting:
        line_bytes = ser.readline()  # read until \n
        try:
            line = line_bytes.decode('ascii').strip()
            if line == "RDY":
                print("Connected")
                break
        except UnicodeDecodeError:
            
            continue

ser.reset_input_buffer()

       
while True:
    if ser.in_waiting >= 4:
        blockData = ser.read(4)
        pX, pY, pZ, blockType = struct.unpack('<bbbb', blockData)
        print(pX,pY,pZ,blockType)
        colorDefined = colorIndexes[blockType]
        b = box(pos=vector(pX,pY,pZ),size=vector(1,1,1),color=colorDefined)
    else:
        time.sleep(0.01)