import lcm,time,random
from lcm_interfaces.diana_joint_states_t import diana_joint_states_t
import numpy as np

class LCM_pub(object):
    def __init__(self, name : str, udpm : str):
        self.lc = lcm.LCM(udpm)
        self.msg = diana_joint_states_t()
        self.name = name
        self.time = 0
        self.initMsg()

    def initMsg(self):
        self.msg.timestamp = self.time
        self.msg.num = 7
        self.msg.jntpos  = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        self.msg.name = "arm init"
        self.msg.enabled = True
        

    def set_msg(self,qpos : np.ndarray):
        self.time += 1
        for i in range(self.msg.num):
            self.msg.jntpos[i] = qpos[i]


    def publish(self):
        self.lc.publish(self.name, self.msg.encode())

if __name__ == "__main__":
    udp = "udpm://239.255.76.67:7667?ttl=255"
    pub = LCM_pub("diana_joint_state",udp)
    q  = np.array([0.0, 0.0, 0.0, 1.57, 0.0, 0.0, 0.0])
    qt  = np.array([0.0, 0.0, 0.0, 1.57, 0.0, 0.0, 0.0])
    upperlim= [ 2.56,  1.57,  2.56, 3.05,  3.05,  3.14,  3.14]
    lowerlim= [-2.56, -1.57, -2.56,  0.0, -3.05, -3.14, -3.14]
      
    # pub.update(left,right)
    while True:
        diff = 0.1
        for j in range(7):
            symbol = random.randint(0,1)
            if symbol == 0:
                symbol = -1.0
            else: symbol = 1.0
            # q[j] = random.uniform(lowerlim[j],upperlim[j])
            qt[j]  = q[j] + random.random()*random.uniform(0,diff)*symbol
            if qt[j] > upperlim[j]:
                qt[j] = upperlim[j]
            elif qt[j] < lowerlim[j]:
                qt[j] = lowerlim[j]

        pub.set_msg(qt)
        pub.publish()
        time.sleep(10)
             



