from math import sqrt, atan2, pi

class waypt_t:
	def __init__(self, x, y, z, radius=1, depth_band=1):
		#= <pydoc> ===========================================
		"""purpose:
				creates waypoint object from waypt_t(x,y,z) call
				uses cartesian coordinates (z up)
		exceptions:
				none"""
		#= </pydoc> ==========================================
		self.x = x
		self.y = y
		self.z = z
		self.radius = radius
		self.depth_band = depth_band
	

	def met(self, current_x, current_y, current_z):
		#= <pydoc> ===========================================
		"""purpose:
				returns waypoint criteria satisfaction (met: yes/no)
				takes cartesian x,y,z
		exceptions:
				none"""
		#= </pydoc> ==========================================
		cx, cy, cz = current_x, current_y, current_z

		dist_radial = sqrt((self.x-cx)**2 + (self.y-cy)**2)
		dist_depth = abs(self.z-cz)
		if((dist_radial <= self.radius) and (dist_depth <= self.depth_band)):
			# Goal met
			return True
		return False


	def bearing(self, current_x, current_y):
		#= <pydoc> ===========================================
		"""purpose:
				returns relevant bearing in radians
				takes cartesian x,y for current coordinates
		exceptions:
				none"""
		#= </pydoc> ==========================================
		dx = self.x - current_x
		dy = self.y - current_y

		return (-atan2(dy, dx) + pi/2.0) % (2*pi)


	def __str__(self):
		return '('+str(self.x)+', '+str(self.y)+', '+str(self.z)+', r['+str(self.radius)+'], b['+str(self.depth_band)+'])'
