import sys, time, math, serial

if len(sys.argv) < 2:
    print("Usage: python read_vectors_friendly.py COMx")
    sys.exit(1)

PORT = sys.argv[1]
BAUD = 115200
HEADER = b"\x01\x02\x03\x04"
S12 = 0.13
THRESHOLD = 150

def s12(u):
    return u - 4096 if (u & 0x800) else u

def unpack_xyz(v0, v1, v2, v4, v5):
    x = s12((v0 << 4) | ((v4 & 0xF0) >> 4))
    y = s12((v1 << 4) |  (v4 & 0x0F))
    z = s12((v2 << 4) |  (v5 & 0x0F))
    return x, y, z

def vector_info(x, y, z):
    # Compute magnitude and angles
    r = math.sqrt(x*x + y*y + z*z)
    azimuth = math.degrees(math.atan2(y, x))      # 0° = +X, 90° = +Y
    elevation = math.degrees(math.asin(z / r)) if r > 1e-6 else 0.0

    # Human-readable direction hints
    horiz = ""
    if abs(azimuth) < 22.5: horiz = "→ right"
    elif 22.5 <= azimuth < 67.5: horiz = "↗ up-right"
    elif 67.5 <= azimuth < 112.5: horiz = "↑ up"
    elif 112.5 <= azimuth < 157.5: horiz = "↖ up-left"
    elif abs(azimuth) > 157.5: horiz = "← left"
    elif -157.5 <= azimuth < -112.5: horiz = "↙ down-left"
    elif -112.5 <= azimuth < -67.5: horiz = "↓ down"
    elif -67.5 <= azimuth < -22.5: horiz = "↘ down-right"

    if z > 100:
        vert = "⬆️  field coming out of board (north toward sensor)"
    elif z < -100:
        vert = "⬇️  field going into board (south toward sensor)"
    else:
        vert = "≈ in plane of board"

    return r, azimuth, elevation, horiz, vert

print(f"Opening {PORT} @ {BAUD}")
ser = serial.Serial(PORT, BAUD, timeout=0.1)
try:
    ser.setDTR(False)
except Exception:
    pass

buf = bytearray()
last_any = time.time()



try:
    Seen = False
    while True:
        chunk = ser.read(256)
        if chunk:
            buf += chunk
            last_any = time.time()
        else:
            if time.time() - last_any > 1.0:
                print("(no data yet)")
                last_any = time.time()

        

        while True:
            i = buf.find(HEADER)
            if i < 0:
                if len(buf) > 4096:
                    del buf[:len(buf)-16]
                break
            if len(buf) < i + 10:
                break

            start = i + 4
            payload = bytes(buf[start:start+6])
            del buf[:start+6]

            v0,v1,v2,v3,v4,v5 = payload
            x,y,z = unpack_xyz(v0,v1,v2,v4,v5)
            r, az, el, horiz, vert = vector_info(x, y, z)

            Bx = x * S12
            By = y * S12
            Bz = z * S12
            Bmag = math.sqrt(Bx*Bx + By*By + Bz*Bz)
            print(f"X={x:6d} Y={y:6d} Z={z:6d}  "
                  f"Bx={Bx:7.2f}mT By={By:7.2f}mT Bz={Bz:7.2f}mT |B|={Bmag:7.2f}mT  ")
            
            # if Bmag >= THRESHOLD and not Seen:
            #     print(f'Magnet close by')
            #     Seen = True
            # elif Bmag < THRESHOLD and Seen:
            #     print(f'Magnet moved away')
            #     Seen = False
            

except KeyboardInterrupt:
    pass
finally:
    try:
        ser.close()
    except Exception:
        pass