from osc4py3.as_eventloop import *
from osc4py3 import oscbuildparse
import time
import roboticstoolbox as rtb

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

    # Make client channels to send packets
<<<<<<< HEAD
    osc_udp_client("localhost", 8000, "osc_client")
=======
    osc_udp_client("192.168.1.24", 8000, "osc_client")
>>>>>>> c77d75859fda8ac862323fbd1993c35f7f931973

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

    msg = oscbuildparse.OSCMessage("/joint_trajectory", None, [joint_positions1, joint_positions2])#, joint_positions3, joint_positions4, joint_positions5])
    osc_send(msg, "osc_client")
    osc_process()
    print("Sending joint positions")
    '''
    time.sleep(2)
    msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions2)
    osc_send(msg, "osc_client")
    osc_process()
    print("Sent joint positions2")
    time.sleep(3)
    msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions3)
    osc_send(msg, "osc_client")
    osc_process()
    print("Sent joint positions3")
    time.sleep(3)
    msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions4)
    osc_send(msg, "osc_client")
    osc_process()
    print("Sent joint positions4")
    time.sleep(3)
    msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions5)
    osc_send(msg, "osc_client")
    osc_process()
    print("Sent joint positions5")
    time.sleep(3)
    '''
    osc_terminate()

if __name__ == "__main__":
    main()
