import subprocess
import sys
import time
import serial
import struct
import os
import glob

############################################################
# 1) PROCESS ARGUMENT
############################################################
if len(sys.argv) < 2:
    print("Usage: python3 getprox.py <MAC-address>")
    sys.exit(1)

MAC = sys.argv[1]


############################################################
# 2) HELPER: RUN BLUEUTIL COMMANDS
############################################################
def run(cmd):
    return subprocess.run(cmd, shell=True, capture_output=True, text=True).stdout.strip()


def wait_until_disconnected(mac, timeout=60):
    for _ in range(timeout):
        status = run(f"blueutil --is-connected {mac}")
        if status == "0":
            return True
        time.sleep(1)
    return False


############################################################
# 3) SEQUENCE: UNPAIR → PAIR → WAIT UNTIL DISCONNECTED → CONNECT
############################################################
print(f"=== Unpairing {MAC} ===")
run(f"blueutil --unpair {MAC}")
time.sleep(2)

print(f"=== Pairing {MAC} ===")
run(f"blueutil --pair {MAC}")
time.sleep(2)

print("=== Waiting until device DISCONNECTS after auto-connect ===")
if not wait_until_disconnected(MAC):
    print("Timeout: device did not disconnect after pairing.")
else:
    print("Device is now disconnected (as expected).")

print(f"=== Connecting manually to {MAC} ===")
run(f"blueutil --connect {MAC}")
time.sleep(2)

if run(f"blueutil --is-connected {MAC}") != "1":
    print("ERROR: Could not connect to device.")
    sys.exit(1)

print("Successfully connected! The SPP port should now exist.\n")


############################################################
# 4) FIND THE /dev/cu.e-puck2_<ID> PORT AUTOMATICALLY
############################################################
print("Searching for SPP serial port...")

ports = glob.glob("/dev/cu.e-puck2_*")
if not ports:
    print("ERROR: No e-puck2 serial port found.")
    sys.exit(1)

SERIAL_PORT = ports[0]
print(f"Using serial port: {SERIAL_PORT}")


############################################################
# 5) OPEN SERIAL PORT AND READ PROXIMITY DATA
############################################################
ser = serial.Serial(SERIAL_PORT, 115200, timeout=0.5)

try:
    while True:
        message = struct.pack(">bb", -ord('N'), 0)
        ser.write(message)

        reply = ser.read()
        while len(reply) < 16:  # 8 sensors × 2 bytes
            reply += ser.read()
            print("len = " + str(len(reply)))

        reply = struct.unpack('@HHHHHHHH', reply)
        print("prox:", reply)

        time.sleep(0.2)

finally:
    ser.close()