#! /usr/bin/python3
import time
import socket
import math

s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
host ="192.168.1.101"
port =30002
s.connect((host,port))


c=0
ycenter=0.2
zcenter=0.5

scale=0.2

frequency=20
lookahead_time= 0.05  # range [0.03,0.2]
servo_t = 1.0/frequency*2 + lookahead_time
gain = 100 #range [100,2000]

last_send = 0
while True:

    if time.time()>=last_send + (1.0/frequency):
        last_send=time.time()
        #text="movel(p[-0.5,"+str(ycenter+math.cos(c)*scale)+","+str(zcenter+math.sin(c)*scale)+",0.0,3.14159,0.0], a=1.2, v=0.1, t=0.2, r=0.02)\n"
        #text="movep(p[-0.5,"+str(ycenter+math.cos(c)*scale)+","+str(zcenter+math.sin(c)*scale)+",0.0,3.14159,0.0], a=0.5, v=0.1, r=0.05)\n"
        text="servoj(get_inverse_kin(p[-0.5,"+str(ycenter+math.cos(c)*scale)+","+str(zcenter+math.sin(c)*scale)+",0.0,3.14159,0.0]), t="+str(servo_t)+", lookahead_time="+str(lookahead_time)+", gain="+str(gain)+")\n"
        s.send(text.encode())
        print(text)
        
        #time.sleep(1.0/frequency)
        c+=0.1
        c%=2*3.1415

s.close ()