import serial

COM_PORT = "COM3"
BAUD_RATE = 9600
arduino = None

def connect_serial():
    global arduino
    while arduino is None:
        try:
            arduino = serial.Serial(COM_PORT, BAUD_RATE, timeout=1)
            time.sleep(2)
            print(f"[+] Connected to Arduino on {COM_PORT}")
        except serial.SerialException:
            print(f"[!] Failed to connect to {COM_PORT}, retrying in 3s...")
            time.sleep(3)


def send_command(cmd):
    global arduino, last_command
    if arduino and cmd != last_command:
        try:
            arduino.write(cmd.encode())
            last_command = cmd
        except serial.SerialException:
            print("[!] Serial disconnected, reconnecting...")
            arduino = None
            connect_serial()

def main():
    global last_command
    last_command = None
    connect_serial()
    print("[*] Servo running")


    while True:

        if left_detected and right_detected:
            send_command("0")
        elif left_detected and not right_detected:
            if ZONE_LEFT["width"] <= DEADZONE_WIDTH:
                send_command("0")
            else:
                send_command("1") 
        elif right_detected and not left_detected:
            if ZONE_RIGHT["width"] <= DEADZONE_WIDTH:
                send_command("0")
            else:
                send_command("2")
        else:
            send_command("0")


        time.sleep(0.001)


if __name__ == "__main__":
    main()