import urx, math, math3d

class UR5Exception(Exception):
	pass

class DemoController(object):
	"""Implements control of the UR5 in cylinder coordinates (corresponding to attached table).
	   -Checks bounds of the arc section
	   -Methods for placing blocks
	"""

	def __init__(self, config):
		"""Initializes the controller with parameters specified in config"""
		self.debug = config.DEBUG
		try:
			self.robot = urx.Robot(config.UR5_IP, useRTInterface=config.USE_FORCE_MONITOR) #TODO or hostname
		except Exception, e:
			raise UR5Exception('Robot initiation failed: %s' % str(e))

		# Set cylinder parameters
		self.quadrant = config.TABLE_QUADRANT
		self.phi = self.quadrant*math.pi/2
		self.cyl_offset = config.TABLE_ORIGO_OFFSET
		self.cyl_ox = self.cyl_offset*math.cos(self.phi)
		self.cyl_oy = self.cyl_offset*math.sin(self.phi)

		self.r_min = config.R_MIN
		self.r_max = config.R_MAX
		self.theta_min = config.TABLE_EDGE_LEFT
		self.theta_max = config.TABLE_EDGE_RIGHT
		self.z_min = config.Z_MIN
		self.z_max = config.Z_MAX

		self.j_home = config.QUADRANT_JOINT_CONFIG

		self.vel = config.VELOCITY
		self.acc = config.ACCELERATION
		self.dec = config.DECELERATION

		# Move to reference coordinate system base
		self.set_pose_home()

		# Set reference coordinate system parameters
		csys = math3d.Transform()
		csys.orient.rotate_zb(self.phi)
		self.robot.set_csys("csys", csys)
		self.trans_base = self.robot.get_transform()
		self.cyl_ox = self.cyl_offset
		self.cyl_oy = 0
		


	def set_pose_home(self):
		"""Move to "home" configuration. The resulting pose ensures correct orientation."""
		try:
			self.robot.movej(self.j_home, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
			pose = self.robot.getl()
			pose[0] = self.r_min*math.cos(self.phi) + self.cyl_ox
			pose[1] = self.r_min*math.sin(self.phi) + self.cyl_oy
			pose[2] = self.z_min
			self.robot.movel(pose, acc=0.1, vel=0.2, radius=0, wait=True, relative=False)
			self.current_cyl = (self.r_min, 0, self.z_min)
		except Exception, e:
			raise UR5Exception('Move to home configuration failed: %s' % str(e))


	def cylinder2pose(self, r, theta, z):
		"""Translate table cylinder coordinates to robot cartesian pose.
		   (r, theta) is given in TABLE coordinates.
		"""
		trans = math3d.Transform(self.trans_base) #deep copy
		trans.orient.rotate_zb(theta)
		trans.pos = math3d.Vector(r*math.cos(theta) + self.cyl_ox,
								  r*math.sin(theta) + self.cyl_oy,
								  z)
		return trans.pose_vector.tolist()


	def movel(self, r, theta, z):
		pose = self.cylinder2pose(r, theta, z)
		self.robot.movel(pose, acc=self.acc, vel=self.vel, radius=0, wait=True, relative=False)
		self.current_cyl = (r, theta, z)


	def movec(self, r, theta, z):
		#Note: r should be the same.
		curr_theta = self.current_cyl[1]
		pose_via = self.cylinder2pose(r, curr_theta+(theta-curr_theta)/2, z)
		pose = self.cylinder2pose(r, theta, z)
		self.robot.movec(pose_via, pose, acc=self.acc, vel=self.vel, radius=0, wait=True)
		self.current_cyl = (r, theta, z)

	def movec_hax(self, r, theta, z):
		"""Since movec is unreliable in reaching the end pose: linearize the arc."""
		curr_r, curr_theta, curr_z = self.current_cyl
		dtheta = theta - curr_theta
		if dtheta > 0:
			theta_incr = math.pi/41
		else:
			theta_incr = -math.pi/41
		if abs(dtheta) < abs(theta_incr):
			self.movel(r, theta, z)
			return

		pose_list = []
		segments = int(math.floor(dtheta/theta_incr))
		r_incr = (r-curr_r)/(segments+1)
		z_incr = (z-curr_z)/(segments+1)
		for i in range(1, segments+1):
			pose_list.append(self.cylinder2pose(curr_r + i*r_incr,
												curr_theta + i*theta_incr,
												curr_z + i*z_incr))
		pose_list.append(self.cylinder2pose(r, theta, z))

		#debug
		if self.debug:
			print "pose list for movec_hax"
			for p in pose_list:
				print [('%.3f' % i) for i in p]

		self.robot.movels(pose_list, acc=self.acc, vel=self.vel, radius=0.05, wait=True)
		self.current_cyl = (r, theta, z)



	def cleanup(self):
		self.robot.cleanup()