#!/usr/bin/python

from numpy import *
from pylab import *
from mission.waypoint import waypt_t
from mission.goals import goals_t
from constants import *
from math import cos, sin, atan2, pi

def xytMult(xyt, p):
	b = xyt[2]
	c = cos(b)
	s = sin(b)
	r = [	c*p[0] - s*p[1] + xyt[0]*p[2] ,
			s*p[0] + c*p[1] + xyt[1]*p[2] ,
			1*p[2]						  ]
	return r

g = goals_t([(0,0,0), (2,0,0), (4,2,0), (4,4,0), (2,4,0), (0,2,0), (0,0,0)])

print "close plots for next, until exhausted (",len(g.goals)," total goals)"
while not g.complete():
	print "i: ",g.i+1, "::", g.current()
	step = .5
	p1 = [0,		0, 1]
	p2 = [step/2.0,	0, 1]
	xmin, xmax = -2, 7
	ymin, ymax = -2, 7
	x = arange(xmin, xmax, step)
	y = arange(ymin, ymax, step)
	figure
	hold = True
	for xi in x:
		for yi in y:
			# compute xyt.  note xytMult assumes cartesian theta
			xyt = [xi, yi, -g.legBearing(xi, yi)+pi/2.0]
			tp1 = xytMult(xyt, p1)
			tp2 = xytMult(xyt, p2)
			plot(	[tp1[0], tp2[0] ],
					[tp1[1], tp2[1] ] )
	try:
		s = g.last()
		e = g.current()
		plot([s.x], [s.y], 's')
		plot([e.x], [e.y], '^')
		plot([s.x, e.x],[s.y, e.y],'k')
	except IndexError:
		try:
			p = g.last()
		except IndexError:
			p = g.current()
		plot([p.x], [p.y], 's')
	axis('equal')

	show()

	g.next()
