from waypoint import waypt_t
from math import *

class invalidGoal(Exception):
# invalid goal specification exception
	def __init__(self, value):
		self.value = value
	def __str__(self):
		return repr(self.value)

class goals_t:
	def __init__(self, goal_list, default_radius=1, default_depth_band=1):
		#= <pydoc> ===========================================
		"""purpose:	
				creates goal implementation
				takes list of goal tuples in either of two formats
					(x,y,z):      default radius and depth band)
					(x,y,z,r,b):  custom radius and depth band
		exceptions:
					invalidGoal:  used when invalid goal is specified"""
		#= </pydoc> ==========================================
		self.goals = []
		self.i = 0

		for goal in goal_list:
			try:
				tmp = self.create(goal, default_radius, default_depth_band)
			except invalidGoal:
				raise

			self.goals.append(tmp)


	def last(self):
		#= <pydoc> ===========================================
		"""purpose:	
				return last goal waypoint
		exceptions:
				IndexError:	if no last waypoint"""
		#= </pydoc> ==========================================
		if self.i > 0 and self.i <= len(self.goals):
			return self.goals[self.i-1]
		else:
			raise IndexError


	def current(self):
		#= <pydoc> ===========================================
		"""purpose:	
				return current goal waypoint
		exceptions:
				IndexError:	if mission is complete"""
		#= </pydoc> ==========================================
		if self.complete():
			raise IndexError
		else:
			return self.goals[self.i]


	def next(self, n=1):
		#= <pydoc> ===========================================
		"""purpose:	
				increment to next waypoint (happily exceeds list end)
				or, if n is specified, skip ahead n steps
		exceptions:
				none"""
		#= </pydoc> ==========================================
		self.i += n


	def complete(self):
		#= <pydoc> ===========================================
		"""purpose:	
				return mission completion status
		exceptions:
				none"""
		#= </pydoc> ==========================================
		if self.i >= len(self.goals):
			return True
		return False


	def met(self, x, y, z):
		#= <pydoc> ===========================================
		"""purpose:	
				return current goal status
		exceptions:
				none"""
		#= </pydoc> ==========================================
		return self.current().met(x,y,z)


	def legBearing(self, current_x, current_y):
		#= <pydoc> ===========================================
		"""purpose:	
				compute bearing based on offset from current leg
				if we haven't met the start point, or have completed
					the mission, we simply return the bearing to the
					relevant point
		exceptions:
				none"""
		#= </pydoc> ==========================================
		try:
			start	= self.last()
			end		= self.current()

			legvec	= [end.x-start.x, end.y-start.y]
			myvec	= [current_x-start.x, current_y-start.y]

			legang	= atan2(legvec[1], legvec[0])

			# compute "err," the perpindicular distance to
			# the leg vector
			legmag	= sqrt(pow(legvec[0],2) + pow(legvec[1], 2))
			uleg	= [legvec[0]/float(legmag), legvec[1]/float(legmag)]

			udot	= uleg[0]*myvec[0]+uleg[1]*myvec[1]
			proj	= [udot*uleg[0], udot*uleg[1]]

			perp	= [myvec[0]-proj[0], myvec[1]-proj[1]]
			err		= sqrt(pow(perp[0],2)+pow(perp[1],2))

			# then, using 'err,' compute a bearing that will
			# pull us back onto the leg
			bearing	= min(err, pi/2.0)
			if (atan2(myvec[1], myvec[0]) - atan2(legvec[1],legvec[0]))%(2*pi) > pi:
				# if this was true, then we're on a certain side of the vector, and
				# must correct in the opposite direction to get back to the vector
				bearing *= -1

			# convert to NED bearing for the result
			return -(legang - bearing) + pi/2.0

		except IndexError:
			# couldn't make a leg, return the direct bearing 
			# to the coordinate of interest
			try:
				point = self.last()
			except IndexError:
				point = self.current()
			return point.bearing(current_x, current_y)


	def create(self, goal, default_radius, default_depth_band):
		#= <pydoc> ===========================================
		"""purpose:	
				return waypt_t version of 'goal'
				takes goal tuple in either of two formats
					(x,y,z):      default radius and depth band)
					(x,y,z,r,b):  custom radius and depth band
		exceptions:
				none"""
		#= </pydoc> ==========================================
		if len(goal) == 3:
			tmp = waypt_t(	goal[0],goal[1],goal[2],
							radius=default_radius, 
							depth_band=default_depth_band)
		elif len(goal) == 5:
			tmp = waypt_t(	goal[0],goal[1],goal[2],
							radius=goal[3],
							depth_band=goal[4])
		else:
			# invalid goal specified
			raise invalidGoal

		return tmp


	def insert(	self, goal, absolutePosition=-1, relativePosition=-1,
				default_radius=1, default_depth_band=1):
		#= <pydoc> ===========================================
		"""purpose:	
				insert goal into existing goal list
				takes goal with either of two position types
					absolutePosition (zero is first)
					relativePosition (zero is current)
		 exceptions:
				invalidGoal: if goal is incorrectly specified
				valueError:  if position is not specified"""
		#= </pydoc> ==========================================
		try:
			tmp = self.create(goal, default_radius, default_depth_band)
		except invalidGoal:
			raise

		if absolutePosition>=0:
			position	= absolutePosition
			pre			= self.goals[:position]
			post		= self.goals[position:]

		elif relativePosition>=0:
			position	= relativePosition
			pre			= self.goals[:self.i+position]
			post		= self.goals[self.i+position:]

		else:
			raise ValueError

		self.goals = pre + [tmp] + post


	def __str__(self):
		res = ''
		for goal in self.goals:
			res = res + '\t' + goal.__str__() + ',\n'
		res = res[:-2]
		return '['+res+'\t]'
