{{BEGIN_REPLACE}}

global steptime = get_steptime()
textmsg("steptime=", steptime)
global MULT_jointstate = {{JOINT_STATE_REPLACE}}

global SERVO_STOPPED = -2
global SERVO_UNINITIALIZED = -1
global SERVO_IDLE = 0
global SERVO_RUNNING = 1
global cmd_servo_state = SERVO_UNINITIALIZED
global cmd_servo_q = get_actual_joint_positions()
global cmd_servo_q_last = get_actual_joint_positions()
def set_servo_setpoint(q):
  enter_critical
  cmd_servo_state = SERVO_RUNNING
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = q
  exit_critical
end

def extrapolate():
  enter_critical
  diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]
  exit_critical

  return cmd_servo_q
end

thread servoThread():
  state = SERVO_IDLE
  while state > SERVO_STOPPED:
    enter_critical
    q = cmd_servo_q
    do_extrapolate = False
    if (cmd_servo_state == SERVO_IDLE):
      do_extrapolate = True
    end
    state = cmd_servo_state
    if cmd_servo_state > SERVO_UNINITIALIZED:
      cmd_servo_state = SERVO_IDLE
    end
    exit_critical
    if do_extrapolate:
      textmsg("No new setpoint received. Extrapolating.")
      q = extrapolate()
      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
    elif state == SERVO_RUNNING:
      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
    else:
      sync()
    end
  end
  textmsg("servo thread ended")
  stopj(0.1)
end
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")

thread_servo = run servoThread()
keepalive = -2
params_mult = socket_read_binary_integer(6+1, "reverse_socket")
keepalive = params_mult[7]
while keepalive > 0:
  socket_send_line(1, "reverse_socket")
  params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
  if params_mult[0] > 0:
    keepalive = params_mult[7]
    q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
    set_servo_setpoint(q)
  else:
    keepalive = keepalive - 1
  end
end
textmsg("Stopping communication and servoing")
enter_critical
  cmd_servo_state = SERVO_STOPPED
exit_critical
sleep(.1)
socket_close()
kill thread_servo