from osc4py3.as_eventloop import *
#from osc4py3 import oscdispatch
import time
from osc4py3 import oscmethod as osm

# Define the OSC message handler
def joint_states_handler(address):
    time.sleep(1)  # Simulate processing time
    print(f"Received OSC message at {address}")

# Start the OSC system
osc_startup()

# Create an OSC UDP server
osc_udp_server("127.0.0.1", 8000, "osc_server")

# Register handlers for different joint state messages
osc_method("/joint_states", joint_states_handler, argscheme=osm.OSCARG_ADDRESS)
osc_method("/joint_states/*/position", joint_states_handler,argscheme=osm.OSCARG_ADDRESS)
osc_method("/joint_states/*/velocity", joint_states_handler,argscheme=osm.OSCARG_ADDRESS)
osc_method("/joint_states/*/effort", joint_states_handler,argscheme=osm.OSCARG_ADDRESS)

print("OSC Receiver is listening on 127.0.0.1:8000...\nPress Ctrl+C to exit.")

# Keep the server running
try:
    while True:
        osc_process()
except KeyboardInterrupt:
    print("\nShutting down OSC receiver...")
    osc_terminate()