prog

def driverProg():
  HOSTNAME = "10.0.2.97"
  MSG_OUT = 1
  MSG_QUIT = 2
  MSG_JOINT_STATES = 3
  MSG_MOVEJ = 4
  MSG_WAYPOINT_FINISHED = 5
  MSG_STOPJ = 6
  MULT_jointstate = 1000.0
  MULT_time = 1000000.0
  MULT_blend = 1000.0
  
  def send_out(msg):
    enter_critical
    socket_send_int(MSG_OUT)
    socket_send_string(msg)
    socket_send_string("~")
    exit_critical
  end
  
  def send_waypoint_finished(waypoint_id):
    enter_critical
    socket_send_int(MSG_WAYPOINT_FINISHED)
    socket_send_int(waypoint_id)
    exit_critical
  end
  
  thread statePublisherThread():
    def send_joint_state():
      q = get_joint_positions()
      qdot = get_joint_speeds()
      tau = get_joint_torques()
      enter_critical
      socket_send_int(MSG_JOINT_STATES)
      socket_send_int(floor(MULT_jointstate * q[0]))
      socket_send_int(floor(MULT_jointstate * q[1]))
      socket_send_int(floor(MULT_jointstate * q[2]))
      socket_send_int(floor(MULT_jointstate * q[3]))
      socket_send_int(floor(MULT_jointstate * q[4]))
      socket_send_int(floor(MULT_jointstate * q[5]))
      socket_send_int(floor(MULT_jointstate * qdot[0]))
      socket_send_int(floor(MULT_jointstate * qdot[1]))
      socket_send_int(floor(MULT_jointstate * qdot[2]))
      socket_send_int(floor(MULT_jointstate * qdot[3]))
      socket_send_int(floor(MULT_jointstate * qdot[4]))
      socket_send_int(floor(MULT_jointstate * qdot[5]))
      socket_send_int(floor(MULT_jointstate * tau[0]))
      socket_send_int(floor(MULT_jointstate * tau[1]))
      socket_send_int(floor(MULT_jointstate * tau[2]))
      socket_send_int(floor(MULT_jointstate * tau[3]))
      socket_send_int(floor(MULT_jointstate * tau[4]))
      socket_send_int(floor(MULT_jointstate * tau[5]))
      #socket_send_int(7895160)  # Recognizable  ".xxx" or 00787878
      exit_critical
    end
    #socket_open(HOSTNAME, 50002)
    send_joint_state()
    while True:
      send_joint_state()
      sync()
    end
    sync()
  end

  socket_open(HOSTNAME, 50001)
  socket_send_int(1)
  socket_send_string("hello~")

  thread_state = run statePublisherThread()
  
  #movej([2.2,0,-1.57,0,0,0],2)
  #movej([1.5,0,-1.57,0,0,0],2)
  
  while True:
    #send_out("Listening")
    ll = socket_read_binary_integer(1)

    if ll[0] == 0:
      #send_out("Received nothing")
    elif ll[0] > 1:
      send_out("Received too many things")
    else:
      mtype = ll[1]
      if mtype == MSG_QUIT:
        send_out("Received QUIT")
        break
      elif mtype == MSG_MOVEJ:
        send_out("Received movej")
	params_mult = socket_read_binary_integer(1+6+4)
	if params_mult[0] == 0:
	  send_out("Received no parameters for movej message")
	end

	# Unpacks the parameters
	waypoint_id = params_mult[1]
	q = [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,
             params_mult[7] / MULT_jointstate]
	a = params_mult[8] / MULT_jointstate
	v = params_mult[9] / MULT_jointstate
	t = params_mult[10] / MULT_time
	r = params_mult[11] / MULT_blend

	# Sends the command
	send_out("movej started")
	movej(q, a, v, t, r)
	send_waypoint_finished(waypoint_id)
	send_out("movej finished")
      elif mtype == MSG_STOPJ:
        send_out("Received stopj")
	stopj(1.0)
      else:
        send_out("Received unknown message type")
      end
    end
    #movej([2.2,0,-1.57,0,0,0],2)
    #movej([1.5,0,-1.57,0,0,0],2)
  end

  #sleep(1)
  kill thread_state
  socket_send_int(2)
end
driverProg()