#encoding: UTF-8
#!/usr/bin/env python2
import rospy
import time,os

from mira_communication.msg import MiraSetAngles, MiraSetCoords, MiraPumpStatus


class Movement(object):
	"""Tools class: Communication with mycobot."""
	def __init__(self):
		super(Movement, self).__init__()
		self.angle_pub = rospy.Publisher("mycobot/angles_goal", MiraSetAngles, queue_size=5)
		self.coord_pub = rospy.Publisher("mycobot/coords_goal", MiraSetCoords, queue_size=5)

		self.pump_pub = rospy.Publisher("mycobot/pump_status", MiraPumpStatus, queue_size=10)

		self.angles = MiraSetAngles()
		self.coords = MiraSetCoords()
		self.pump = MiraPumpStatus()

	def pub_coords(self, item, sp=20):
		self.coords.x = item[0]
		self.coords.y = item[1]
		self.coords.z = item[2]
		# self.coords.rx = item[3]
		# self.coords.ry = item[4]
		# self.coords.rz = item[5]
		self.coords.speed = sp
		# self.coords.model = m
		self.coord_pub.publish(self.coords)


	def pub_angles(self, item, sp):
		self.angles.joint_1 = item[0]
		self.angles.joint_2 = item[1]
		self.angles.joint_3 = item[2]
		# self.angles.joint_4 = item[3]
		# self.angles.joint_5 = item[4]
		# self.angles.joint_6 = item[5]
		self.angles.speed = sp
		self.angle_pub.publish(self.angles)


	def pub_pump(self, flag):
		self.pump.Status= flag
		# self.pump.Pin1 = Pin[0]
		# self.pump.Pin2 = Pin[1]
		self.pump_pub.publish(self.pump)