from osc4py3.as_eventloop import *
from osc4py3 import oscmethod as osm
from osc4py3 import oscbuildparse
import time

def joint_states_handler(*args):
    """Handler function to process incoming joint states."""
    print(f"Received message")
    for arg in args:
        print(arg)

def main():
    ip = "0.0.0.0"  # IP address to listen on
    port = 8000       # Port to listen on

    # Start the OSC system
    osc_startup()

    # Make server channels to receive packets
    osc_udp_server(ip, port, "osc_server")

    # Associate Python functions with message address patterns
    osc_method("/joint_states", joint_states_handler, argscheme=osm.OSCARG_MESSAGE)

    print(f"Listening for OSC messages on {ip}:{port}...")

    try:
        # Run the event loop
        while True:
            osc_process()  # Process OSC messages
            time.sleep(0.01)  # Sleep to avoid high CPU usage

    except KeyboardInterrupt:
        print("")

    finally:
        # Properly close the system
        osc_terminate()

if __name__ == "__main__":
    main()