from helpfunctions import *

class OSCSend:
    from pythonosc import udp_client 

    def __init__(self,_frequency_oscsend=50):
        self.oscreceivers = []
        self.frequency_oscsend=_frequency_oscsend
        self.last_sendosc=0
        
    def setupReceivers(self,serverips,port):
        self.oscreceivers = []
        
        # Set up OSC client 
        for cip in serverips:
            print("connecting to "+str(cip))
            connection = self.udp_client.SimpleUDPClient(cip, port) 
            self.oscreceivers.append(connection)


    def sendMessage(self,address,value):
        for c in self.oscreceivers:   
            c.send_message(address,value)
    
    def send(self,looptime,q,tcp):
    
        if looptime-self.last_sendosc<1.0/self.frequency_oscsend:            
            return
        
        self.last_sendosc=looptime


        for c in self.oscreceivers:                
            joint_deg=[toDeg(x) for x in q]
            c.send_message("/j",joint_deg)
            rx,ry,rz=RPYtoVec(tcp[3],tcp[4],tcp[5])
            tcp_deg=[tcp[0],tcp[1],tcp[2],toDeg(rx),toDeg(ry),toDeg(rz)]
            #tcp_deg=[tcp[0],tcp[1],tcp[2],toDeg(tcp[3]),toDeg(tcp[4]),toDeg(tcp[5])]
            c.send_message("/tcp", tcp_deg)