import socket
import time
from oscpy import *


SEND_IP = "172.18.0.2"
SEND_PORT = 8000
RECV_IP = "172.18.0.1"
RECV_PORT = 7000

pub = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sub = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

msg = Message("/joint_positions", [-1]*6).encode()
n = pub.sendto(msg, (SEND_IP, SEND_PORT))
if n > 0:
    print(f"INFO: Sent {n} bytes")

time.sleep(10)

msg = Message("/tcp_coordinates", [-1]*6).encode()
n = pub.sendto(msg, (SEND_IP, SEND_PORT))
if n > 0:
    print(f"INFO: Sent {n} bytes")

time.sleep(10)

sub.bind((RECV_IP, RECV_PORT))
joint_names = []
found = False
while not found:
    data, addr = sub.recvfrom(5*1024)
    if "#bundle" in data[:8].decode():
        b = Bundle().decode(data)
        for msg in b.msgs:
            if "/joint_state/name" in msg.address:
                joint_names = msg.args
                found = True

if joint_names:
    msg = Message(f"/joint_position/{joint_names[2]}", [ 0 ])
    n = pub.sendto(msg.encode(), (SEND_IP, SEND_PORT))
    if n > 0:
        print(f"INFO: Sent {n} bytes")