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

    #osc_udp_client("192.168.1.24", 8000, "osc_client")
    osc_udp_client("127.0.0.1", 8000, "osc_client")
    # Example joint positions to send
    joint_positions1 = [0.4,0.4, 1.0, 0.0, 0.0, 0.0, 3]
    joint_positions2 = [0.4,-0.4, 0.2, 0.0,0.0, 0.0]
    joint_positions3 = [0.4,-0.4, 0.6, 0.0, 0.0, 0.0]#, 6.0]
    joint_positions4 = [0.4,0.4, 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("/tcp_coordinates", None, joint_positions1)
    osc_send(msg, "osc_client")
    osc_process()
    print(time.time())
    print(joint_positions1)
    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()