from pythonosc import dispatcher
from pythonosc import osc_server
from pythonosc.osc_bundle import OscBundle
from pythonosc.osc_message import OscMessage

def joint_handler(address, *args):
    if isinstance(args[0], OscBundle):
        bundle = args[0]
        print(f"Received OSC bundle with timestamp: {bundle.timestamp}")
        for element in bundle.bundle_elements:
            if isinstance(element, OscMessage):
                print(f"Message: {element.address}, Args: {element.params}")

def main():
    ip = "0.0.0.0"  # Listen on all network interfaces
    port = 5005    # Must match the sender's port in `joint_state_osc.py`

    # Create dispatcher and register callback
    disp = dispatcher.Dispatcher()
    disp.map("/joint_states/*", joint_handler)

    server = osc_server.ThreadingOSCUDPServer((ip, port), disp) # Start OSC server
    
    try:
        server.serve_forever()  # Keep server running
    except KeyboardInterrupt:
        print("")

if __name__ == "__main__":
    main()
