# Pathfinding Crap
# BY:scabootssca
# scabootssca@yahoo.com

import Blender
import operator

### Generate The Node Tree
pathTree = Blender.Mesh.Get("path")

def dist(a,b):
	return ((a[0]-b[0])**2+(a[1]-b[1])**2+(a[2]-b[2])**2)**.5

class point:
	def __init__(self,co):
		self.neighbors = {}
		self.distance = 0
		self.co = co
		
class path:
	def __init__(self,points,distance,length):
		self.points = points
		self.distance = distance
		self.length = length
	
def generateTree(pathTree):
	points = {}
	for v in pathTree.verts:
		points[v.index] = point(v.co)
		#print v.index,v.sel
	
	for edge in pathTree.edges:
		distance = dist(edge.v1.co,edge.v2.co)
		points[edge.v1.index].neighbors[edge.v2.index] = distance
		points[edge.v2.index].neighbors[edge.v1.index] = distance
	return points

### Extra Stuff
def getClosestTo(pos,points):
	closestPoint = 0
	closestPos = dist(pos,points[closestPoint].co)
	for pointTest in points:
		if dist(pos,points[pointTest].co) < closestPos:
			closestPoint = pointTest
			closestPos = dist(pos,points[pointTest].co)
	return closestPoint
			
def getPath(start,end,points):
	p = [] # create a list P
	
	# estimate The Distance
	endPoint = points[end]
	for pointTest in points.values():
		pointTest.distance = dist(endPoint.co,pointTest.co)
	
	# add the start node S, to P giving it one element
	p.append(path([start],0,0))
	
	print "-"*30
	
	iter = 0
	while True:
		iter += 1
		#if iter > 10000: print "itered out"; break
	
		# Until first path of P ends with G, or P is empty
		if not p: print "not p"; break
		elif p[0].points[-1] == end: print "found end On Iter:%i"%iter; break
	
		# extract the first path from P
		test = p.pop(0)
	
		# extend first path one step to all neighbors creating X new paths
		news = []
		for neighbor in points[test.points[-1]].neighbors:
			new = path(test.points[:],test.distance,0)
			new.points.append(neighbor)
			new.distance += points[test.points[-1]].neighbors[neighbor]
			new.length = new.distance+points[neighbor].distance
			news.append(new)
	
		# reject all paths with loops
		tmp = news[:]
		for pos,new in enumerate(news):
			if len(new.points) != len(set(new.points)): #passes if there is a duplicate
				tmp.remove(new)
		news = tmp[:]
	
		# for all paths that end at the same node, keep only the shortest one.
		same = {}
		for pos,new in enumerate(news):
			try: same[new.points[-1]].append(new)
			except KeyError: same[new.points[-1]] = [new]
	
		for sameNum in same:
			lists = same[sameNum]
			lowest = lists[0]
			for pathTest in lists:
				if pathTest.length < lowest.length:
					lowest = pathTest
			same[sameNum] = lowest
	
		# add each remaining new path to of P
		for pathTest in same.values(): p.append(pathTest)
		
	
		#for new in news: p.append(new)
	
		#Sort all paths by total distance travelled, shortest first.
		p.sort(key=operator.attrgetter("length"))#,reverse=1)
	return p[0]

			
### Solve
cont = GameLogic.getCurrentController()
endOwn = cont.getOwner()
startMove = cont.getActuator("start")
startOwn = startMove.getOwner()
trackOwn = cont.getActuator("trackOwn").getOwner()
track = cont.getActuator("track")

if not hasattr(GameLogic,"points"):
	GameLogic.points = generateTree(pathTree)

start = getClosestTo(startOwn.getPosition(),GameLogic.points)#0
end = getClosestTo(endOwn.getPosition(),GameLogic.points)#28

startOwn.r = dist(eval(startOwn.lastEndPos),endOwn.getPosition())
if dist(eval(startOwn.lastEndPos),endOwn.getPosition()) > .2:
	startOwn.resolve = 1


if startOwn.resolve:# cont.getSensor("solveKey").isPositive():
	startOwn.isSolving = 1
	startOwn.resolve = 0

	GameLogic.path = getPath(start,end,GameLogic.points)
	startOwn.lastEndPos = str(endOwn.getPosition())
	
if startOwn.isSolving:
	
	speed = .01
	cur = GameLogic.points[GameLogic.path.points[0]].co
	pos = startOwn.getPosition()
	dist = dist(cur,pos)
	if dist < speed:
		del GameLogic.path.points[0]
		if GameLogic.path.points:
			trackOwn.setPosition(GameLogic.points[GameLogic.path.points[0]].co)
		else:
			startOwn.isSolving = 0
			GameLogic.addActiveActuator(startMove,0)
	else:
	
		startMove.setDLoc(0,speed,0,1)	
		GameLogic.addActiveActuator(startMove,1)
		GameLogic.addActiveActuator(track,1)