from osc4py3.as_eventloop import *
from osc4py3 import oscbuildparse

def main():
    # Start the OSC system
    osc_startup()

    # Make client channels to send packets
    osc_udp_client("127.0.0.1", 8000, "osc_client")

    
    # Example joint positions to send
    joint_positions1 = [-0.5,-0.6, 0.2,0.0, 0.0, 0.0]
    joint_positions2 = [-0.5,-0.6, 0.6,0.0,0.0, 0.0]
    joint_positions3 = [-0.5,0.6, 0.6,0.0, 0.0, 0.0]
    joint_positions4 = [-0.5,0.6, 0.2,0.0, 0.0, 0.0]
    joint_positions5 = [-0.5,-0.6, 0.2,0.0, 0.0, 0.0]

    msg = oscbuildparse.OSCMessage("/joint_trajectory", None, [joint_positions1, joint_positions2, joint_positions3, joint_positions4, joint_positions5])
    print("Sending joint positions")
    # Send the OSC message
    osc_send(msg, "osc_client")
    print("Sent joint positions")
    osc_process()
    osc_terminate()

if __name__ == "__main__":
    main()