import pygame
import math
import time
import random
import pylab
from Queue import PriorityQueue

# the units are (cm or pixel), and (degree) 
# The coordinate of PyGame is:
#    ---> x
#    '
#    '
#    y
# the upperleft pixel is (0,0)
# Note: use function PointDisplay() to convert to PyGame coordinate
#       use function PointActual()  to convert to the actual coordinate

# The size of the display window
DISP_WIDTH = 500
DISP_HEIGHT = 300

dtheta1 = 3.
dtheta2 = 6.
dtheta3 = 9.

mintheta1 = 0.
mintheta2 = -180.
mintheta3 = -180.
maxtheta1 = 180.
maxtheta2 = 180.
maxtheta3 = 180.

animation_delay = 0.025

ntheta1 = int((maxtheta1-mintheta1)/dtheta1)
ntheta2 = int((maxtheta2-mintheta2)/dtheta2)
ntheta3 = int((maxtheta3-mintheta3)/dtheta3)
	
class PathGraph:

    # The nodes that make up the graph.  Contain minimal information, namely an id and the elements in the "coordinates".
    class PathGraphNode:
        def __init__(self, nid, coords = [], adjs = [], state = []):
            self.nid = nid # Node id
            self.coords = coords # The coordinates of the node.  Typically posX, posY
            self.adjs = adjs # List of adjacencies.  (adjacent node id, cost)
            self.state = state # A list of the states of the node.

        # Return the node's id
        def getID(self):
            return self.nid

        # Get a particular coordinate of the node
        def getCoordinate(self, coord=0):
            return self.coords[coord]

        # Get all the coordinates of the node
        def getCoordinates(self):
            return self.coords

        # Get how many coordinates there are in the node
        def getNumCoordinates(self):
            return len(self.coords)

        # Get the list of Adjacencies
        def getAdjacencies(self):
            return self.adjs

        # Return if a given node with id nid is adjacent to this node
        def isAdjacent(self, nid):
            for (n, d) in self.adjs:
                if n == nid:
                    return True
            return False

        # get the state stateNum.  Returns None if not found.
        def getState(self, stateNum=0):
            if self.hasState(stateNum):
                return self.state[stateNum]
            else:
                return None
				
        # Set the state stateNum to the stateVal.
        # If stateNum doesn't exist, create it.
        def setState(self, stateVal, stateNum=0):
            if self.hasState(stateNum):
                self.state[stateNum] = stateVal
            else:
                while len(self.state) < stateNum -1:
                    self.state.append(None)
                self.state.append(stateVal)

        # Add a new state to the statelist, and assign it value stateVal.
        # Returns the state number created.
        def addState(self, stateVal):
            self.state.append(stateVal)
            return len(self.state) - 1

        # Return True if the stateNum exists, False otherwise.
        def hasState(self, stateNum=0):
            return len(self.state) > stateNum

        # Set a particular coordinate of the node
        def setCoordinate(self, val, coord=0):
            coords[coord]=val
            return self.coords[coord]

        # Set all the coordinates of the node
        def setCoordinates(self,somecoords):
            self.coords = somecoords
            return self.coords

        # Returns a string representation of the node
        def coordinateString(self):
            retStr = ""
            for i in self.coords:
                retStr += str(i) + " "
            return retStr.rstrip(' ')

    # Creates a new graph
    def __init__(self, coordCount, nodeList=[], nodeDict = {}):
        self.nodeList = nodeList
        self.coords = coordCount
        self.nodeDict = nodeDict

    # Get the node with coordinates nodeCoords
    def getNodeID(self, nodeCoords):
        return self.nodeDict[nodeCoords]

    # Get the adjacency list of node ID nid
    def getAdjacencies(self, nid):
        return self.nodeList[nid].getAdjacencies()

    # Gets the cost of the adjacency between node id nid1 and node id nid2.
    # Returns None if no such cost.
    def getAdjacencyCost(self, nid1, nid2):
        adjs = self.getAdjacencies(nid1)
        for (n, d) in adjs:
            if n == nid2:
                return d
        return None

    # Get the node list
    def getNodes(self):
        return self.nodeList

    # Get the node with id nid
    def getNode(self, nid):
        return self.nodeList[nid]

    # Get a node by the coords.  Returns None is there is no such node
    def getNodeByCoords(self, coords):
        return self.nodeDict[coords]

    # Get the number of nodes.
    def getNodeCount(self):
        return len(self.nodeList)

    # Get the coordinates of node with id nid
    def getNodeCoords(self, nid):
        return self.nodeList[nid].coords

    # Get the node dictionary
    def getNodeDict(self):
        return self.nodeDict

    # Get the id of a node that is adjacent in the specified direction to the node id nid.
    # Returns None if there is no adjacency.
    def getAdjNID(self, nid, direction):
        node = self.nodeList[nid]
        tgt = self.nodeDict(node.getCoordinates() + direcion)
        if tgt != None and node.isAdjacent(tgt):
            return tgt
        else:
            return None
            

    # Print out the graph.
    def printGraph(self):
        for i in range(len(self.nodeList)):
            print "Node " + str(self.nodeList[i].nid) + " at " + self.nodeList[i].coordinateString()
            for j in range(len(self.nodeList[i].adjs)):
                print "\tAdjacent to " + str(self.nodeList[i].adjs[j][0]) + " with cost " + str(self.nodeList[i].adjs[j][1])


    # Add a new node to the list, with the given coordinates and ajacencies.
    # Returns the new node's id
    def addNode(self, coordinates, adjacencies):
        nid = len(self.nodeList)
        self.nodeList.append(PathGraph.PathGraphNode(nid,coordinates, adjacencies))
        for adj in adjacencies:
            nbr = adj[0]
            edge = ( nid, adj[1] )
            self.nodeList[nbr].adjs.append(edge)
        self.nodeDict[coordinates] = nid
        return nid

def nidReturn(n1,n2,n3):
	return n1*ntheta2*ntheta3+n2*ntheta3+n3
		
def coordsReturn(n1,n2,n3):
	return [mintheta1+n1*dtheta1,mintheta2+n2*dtheta2,mintheta3+n3*dtheta3]	

#Creates and returns a graph object for the mapFile passed in.
def createGraph():
		
	nodeList = []
	nodeDict = {}
	coordCount = 0
	
	nid = 0

	for x1 in range(0,ntheta1):
		for x2 in range(0,ntheta2):
			for x3 in range(0,ntheta3):
				
				index = nid
				nid += 1
				coords = tuple(coordsReturn(x1,x2,x3))
				
				nodeDict[coords] = index
				adjs = []
				
				if x1>0 and not arm.checkPossibleCollision(coordsReturn(x1-1,x2,x3)):
						adjs.append((int(nidReturn(x1-1,x2,x3)),dtheta1))
				if x1<ntheta1-1 and not arm.checkPossibleCollision(coordsReturn(x1+1,x2,x3)):
						adjs.append((int(nidReturn(x1+1,x2,x3)),dtheta1))
				if x2>0 and not arm.checkPossibleCollision(coordsReturn(x1,x2-1,x3)):
						adjs.append((int(nidReturn(x1,x2-1,x3)),dtheta2))
				if x2<ntheta2-1 and not arm.checkPossibleCollision(coordsReturn(x1,x2+1,x3)):
						adjs.append((int(nidReturn(x1,x2+1,x3)),dtheta2))
				if x3>0 and not arm.checkPossibleCollision(coordsReturn(x1,x2,x3-1)):
						adjs.append((int(nidReturn(x1,x2,x3-1)),dtheta3))
				if x3<ntheta3-1 and not arm.checkPossibleCollision(coordsReturn(x1,x2,x3+1)):
						adjs.append((int(nidReturn(x1,x2,x3+1)),dtheta3))

				intermediate = PathGraph.PathGraphNode(int(index), coords, adjs, [])
				nodeList.append(intermediate)
				coordCount += 1

	return PathGraph(coordCount, nodeList, nodeDict)

# Returns the euclidean distance between two node id's in the given graph
def euclidDist(a,b, graph):
	sum = 0
	for i in range(3):
		sum += (graph.getNode(a).coords[i] - graph.getNode(b).coords[i]) ** 2
	return float(math.sqrt(sum))

class ArmPlanning():

	def __init__(self, length, angles, colors):
		pygame.init()
		self.screen = pygame.display.set_mode((DISP_WIDTH, DISP_HEIGHT))
		self.screen.fill((255,255,255))

		self.length = length
		self.angles = angles #joint angles
		self.colors = colors #the color for each link
		self.dof = len(angles) #degree of freedom
		self.points = [self.PointDisplay(0, 0)]*(self.dof+1) # shift to the center
		self.obstacles = []
		self.goal = []
		self.lines = [pygame.rect.Rect(0,0,0,0)] * self.dof
		self.obstacle_rects = []
		self.goal_rect = []
		self.hit_ground = False
		self.delta = 15.
		self.r_inner = self.length[1]-self.length[2]
		if self.r_inner<0:
			self.r_inner = 0.0
		self.r_outer = self.length[1]+self.length[2]
		self.r_outer = self.r_outer-self.delta
		self.r_inner = self.r_inner+self.delta
		
		self.optimum_dist = math.sqrt(self.length[1]**2+self.length[2]**2)
		self.max_dist = self.length[2]+self.length[1]
		self.min_dist = self.length[2]-self.length[1]
		self.goalpt = []
		
		self.imagecounter = 0

  # Convert a point, (x,y) Tuple, to PyGame coordinate
	def PointDisplay(self, x, y): return (DISP_WIDTH/2 + x, DISP_HEIGHT - y)

  # Convert a point, (x,y) Tuple, to actual coordinate
	def PointActual(self, x, y): return (x - DISP_WIDTH/2, DISP_HEIGHT - y)

  # update the joint angels with the new values for joint indices in the list
  # e.g. updateArm([new_angle2, new_angle3], [1,2])
	def updateArm(self, angles, indices):
		for i in range(0, len(angles)):
			self.angles[indices[i]] = angles[i]

	# draw the whole arm
	def drawFK(self):
		
		self.screen.fill ((255, 255, 255)) # clean the screen first
		Aprev = pylab.mat(pylab.identity(4))
		origin = self.PointActual(self.points[0][0], self.points[0][1])
		for i in range(0, self.dof):
			theta = math.radians(self.angles[i])
			A = Aprev * pylab.mat([[math.cos(theta), -math.sin(theta), 0.0, self.length[i]*math.cos(theta)],
								   [math.sin(theta),  math.cos(theta), 0.0, self.length[i]*math.sin(theta)],
								   [0.0,              0.0,             1.0, 0.0],
								   [0.0,              0.0,             0.0, 1.0]])
			Aprev = A
			# the angle for each joint is the sum of all the previous joints' angles
			self.points[i+1] = self.PointDisplay(origin[0]+A[0,3],origin[1]+A[1,3])
			self.lines[i] = pygame.draw.line(self.screen, colors[i], self.points[i], self.points[i+1], 5)
			# check if it collides with ground
			if self.points[i+1][1] > DISP_HEIGHT:
				self.hit_ground = True

		# draw the obstacles
		for i in range(0, len(self.obstacles)):
			self.obstacle_rects[i] = pygame.draw.circle(self.screen, self.obstacles[i][0], self.obstacles[i][1], self.obstacles[i][2], 0)

		# draw the goal
		self.goal_rect = pygame.draw.circle(self.screen,self.goal[0],self.goal[1],self.goal[2],0)

		pygame.display.update()
		
		fname = 'animation/image' + '%04d' % self.imagecounter + '.jpg'
		screenshot = pygame.display.get_surface()
		pygame.image.save(screenshot, fname)
		self.imagecounter += 1

	def addObstacle(self, color, loc, radius):
		obs = (color, self.PointDisplay(loc[0],loc[1]), radius)
		self.obstacles.append(obs)
		self.obstacle_rects.append(pygame.rect.Rect(0,0,0,0))

	def setGoal(self, color, loc, radius):
		self.goal = (color, self.PointDisplay(loc[0],loc[1]), radius)
		print "Goal is now: ",loc," or in graphics space: ",self.PointDisplay(loc[0],loc[1])
		print "Wait for the path..."
		self.goal_rect = pygame.rect.Rect(0,0,0,0)
		self.goalpt = [loc[0],loc[1]]

  # check each link if it collides with ground or obstacles
	def checkCollision(self):
		for l in self.lines:
			if (l.collidelist(self.obstacle_rects) != -1):
				return True
		return False
		
	def checkPossibleCollision(self,thetas):
		
		if thetas[0]>180. or thetas[0]<0.0:
			return True
		
		collision = False
		poss_pts = pylab.zeros((len(thetas)+1,2))
		Aprev = pylab.mat(pylab.identity(4))
		poss_pts[0] = self.PointActual(self.points[0][0], self.points[0][1])
		
		for i in range(0, len(thetas)):
			theta = math.radians(thetas[i])
			A = Aprev * pylab.mat([[math.cos(theta), -math.sin(theta), 0.0, self.length[i]*math.cos(theta)],
								   [math.sin(theta),  math.cos(theta), 0.0, self.length[i]*math.sin(theta)],
								   [0.0,              0.0,             1.0, 0.0],
								   [0.0,              0.0,             0.0, 1.0]])
			Aprev = A
			if poss_pts[0][1]+A[1,3]<0.0:
				return True
			poss_pts[i+1] = self.PointDisplay(poss_pts[0][0]+A[0,3],poss_pts[0][1]+A[1,3])
			
			x1 = poss_pts[i  ][0]
			x2 = poss_pts[i+1][0]
			y1 = poss_pts[i  ][1]
			y2 = poss_pts[i+1][1]

			for j in self.obstacles:
				x0 = j[1][0]
				y0 = j[1][1]
				# From wolfram online:
				d = abs((x2-x1)*(y1-y0)-(x1-x0)*(y2-y1))/((x2-x1)**2+(y2-y1)**2)**(0.5)
				d1 = ((x1-x0)**2+(y1-y0)**2)**(0.5)
				d2 = ((x2-x0)**2+(y2-y0)**2)**(0.5)
				obs_rad = j[2]
				if d<obs_rad and (d1 < self.length[i]+obs_rad and d2 < self.length[i]+obs_rad):
					return True
					
		return collision

	# Find the distance between two points:
	def distPts(self,pt1,pt2):
		return ((pt1[0]-pt2[0])**2+(pt1[1]-pt2[1])**2)**0.5

	# Given a node along the arm, determine the coordinates
	def pointLocate(self,n):
		Aprev = pylab.mat(pylab.identity(4))
		origin = (0,0)
		for i in range(0, n):
			theta = math.radians(self.angles[i])
			A = Aprev * pylab.mat([[math.cos(theta), -math.sin(theta), 0.0, self.length[i]*math.cos(theta)],
								   [math.sin(theta),  math.cos(theta), 0.0, self.length[i]*math.sin(theta)],
								   [0.0,              0.0,             1.0, 0.0],
								   [0.0,              0.0,             0.0, 1.0]])
			Aprev = A
		if n==0:
			return origin
		else:
			return [origin[0]+A[0,3],origin[1]+A[1,3]]

	def pointLocate2(self,n):
		Aprev = pylab.mat(pylab.identity(4))
		origin = (0,0) #self.PointActual(self.points[0][0], self.points[0][1])
		for i,thet in enumerate(n):
			theta = math.radians(thet)
			A = Aprev * pylab.mat([[math.cos(theta), -math.sin(theta), 0.0, self.length[i]*math.cos(theta)],
								   [math.sin(theta),  math.cos(theta), 0.0, self.length[i]*math.sin(theta)],
								   [0.0,              0.0,             1.0, 0.0],
								   [0.0,              0.0,             0.0, 1.0]])
			Aprev = A
		return [origin[0]+A[0,3],origin[1]+A[1,3]]

	def Jacobian(self):
		# since we're 2-D, every k vector is the same
		# see page 113 of chapter 5 (Spong)
		k = pylab.mat([[0.], [0.], [1.0]])
		Aprev = pylab.mat(pylab.identity(3))
		Jomega = pylab.mat(pylab.identity(3))
		Jnu = pylab.mat(pylab.identity(3))
		J = pylab.empty((6,self.dof))
		origin = self.PointActual(self.points[0][0], self.points[0][1])
		for i in range(0, self.dof):
			theta = math.radians(self.angles[i])
			A = Aprev * pylab.mat([[math.cos(theta), -math.sin(theta), 0.0],
								   [math.sin(theta),  math.cos(theta), 0.0],
								   [0.0,              0.0,             1.0]])
			rhoz = A*k
			Jomega[:,i] = rhoz
			pointvec = pylab.mat([[arm.pointLocate(i+1)[0]],[arm.pointLocate(i+1)[1]],[0.0]])
			odiff = A*pointvec
			odiff = pylab.mat([[arm.pointLocate(i+1)[0]-arm.pointLocate(i)[0]],[arm.pointLocate(i+1)[1]-arm.pointLocate(i)[1]],[0.0]])
			Jnu[:,i] = pylab.transpose(pylab.cross(pylab.transpose(rhoz),pylab.transpose(odiff)))
		J[0:3,0:self.dof] = Jnu
		J[3:6,0:self.dof] = Jomega
		return pylab.mat(J) #[Jnu,Jomega]
		
	def Jstar(self,J,lmbd):
		# ASSUME W1 is IDENTITY
		w1 = pylab.mat(pylab.identity(6))
		w2 = pylab.mat(pylab.identity(3))
		if True:
			w2[0,0] = 10.1
			w2[1,1] = 1.0
			w2[2,2] = 0.1
		return pylab.linalg.inv(pylab.transpose(J)*w1*J + lmbd*w2) * pylab.transpose(J)*w1 # * pylab.identity(6)

	# This is mostly from the Spong paper on inverse kinematics (chapter 4)
	# Variable names are from figure 4.8
	def find2thetas(self,point,a2,a3,Plus):
		# We will solve the problem in figure 4.8, assuming the base of
		# our two joints is at (0,0), adjusting later
		pt1 = (0,0)
		pt2 = point
		D = (-a2**2-a3**2 + arm.distPts(pt1,pt2)**2)/2.0/a2/a3
		if abs(D)>1.0:  # We don't want to take the square root of a neg. number so let's bail
			return [0,0,False]
		if Plus:
			theta3 = math.atan2((1.0-D**2)**0.5,D)
		else:
			theta3 = math.atan2(-(1.0-D**2)**0.5,D)
		theta2 = math.atan2(pt2[1]-pt1[1],pt2[0]-pt1[0]) - math.atan2(a3*math.sin(theta3),self.length[0]+a3*math.cos(theta3))
		return [math.degrees(theta2),math.degrees(theta3),True]
		
	# Given an angle for the first segment, return the location of the end of the first segment
	def armOne(self,theta1):
		theta1 = math.radians(theta1)
		return [math.cos(theta1)*self.length[0],math.sin(theta1)*self.length[0]]

	# Find the (up to) 4 possible configurations to reach the goal, while
	# trying to preserve our "optimum dist" -- typically right angle b/t last 2 joints
	def solveDestination(self,toSkip):

		located = False
		a3 = self.optimum_dist
		delta = 1.0
		
		failures = 0
		dests = []
		while len(dests)==0:

			if arm.distPts((0,0),(self.goalpt[0],self.goalpt[1]))>self.length[0]+a3:
				a3 = a3 + delta
				if a3>self.length[1]+self.length[2]:
					print "We got ourselves in a hell of a pickle (cannot reach dest)"
					raw_input()
				continue

			[t1,t23,Success] = arm.find2thetas((self.goalpt[0],self.goalpt[1]),self.length[0],a3,True)
			checkSuccess = Success
			
			# CONFIGURATION #1:
			[t2,t3,Success] = arm.find2thetas((self.goalpt[0]-arm.armOne(t1)[0],self.goalpt[1]-arm.armOne(t1)[1]),self.length[1],self.length[2],True)
			t2 = t2 - t1
			if t2<mintheta2: t2 = t2 + 360.0
			if t2>maxtheta2: t2 = t2 - 360.0
			if t3<mintheta3: t3 = t3 + 360.0
			if t3>maxtheta3: t3 = t3 - 360.0
			if not(arm.checkPossibleCollision([t1,t2,t3])) and checkSuccess and Success:
				dests.append(int(nidReturn(int((t1-mintheta1)/dtheta1),int((t2-mintheta2)/dtheta2),int((t3-mintheta3)/dtheta3))))
				
			# CONFIGURATION #2:
			[t2,t3,Success] = arm.find2thetas((self.goalpt[0]-arm.armOne(t1)[0],self.goalpt[1]-arm.armOne(t1)[1]),self.length[1],self.length[2],False)
			t2 = t2 - t1
			if t2<mintheta2: t2 = t2 + 360.0
			if t2>maxtheta2: t2 = t2 - 360.0
			if t3<mintheta3: t3 = t3 + 360.0
			if t3>maxtheta3: t3 = t3 - 360.0
			if not(arm.checkPossibleCollision([t1,t2,t3])) and checkSuccess and Success:
				dests.append(int(nidReturn(int((t1-mintheta1)/dtheta1),int((t2-mintheta2)/dtheta2),int((t3-mintheta3)/dtheta3))))

			[t1,t23,Success] = arm.find2thetas((self.goalpt[0],self.goalpt[1]),self.length[0],a3,False)
			checkSuccess = Success

			# CONFIGURATION #3:
			[t2,t3,Success] = arm.find2thetas((self.goalpt[0]-arm.armOne(t1)[0],self.goalpt[1]-arm.armOne(t1)[1]),self.length[1],self.length[2],True)
			t2 = t2 - t1
			if t2<mintheta2: t2 = t2 + 360.0
			if t2>maxtheta2: t2 = t2 - 360.0
			if t3<mintheta3: t3 = t3 + 360.0
			if t3>maxtheta3: t3 = t3 - 360.0
			if not(arm.checkPossibleCollision([t1,t2,t3])) and checkSuccess and Success:
				dests.append(int(nidReturn(int((t1-mintheta1)/dtheta1),int((t2-mintheta2)/dtheta2),int((t3-mintheta3)/dtheta3))))

			# CONFIGURATION #4:
			[t2,t3,Success] = arm.find2thetas((self.goalpt[0]-arm.armOne(t1)[0],self.goalpt[1]-arm.armOne(t1)[1]),self.length[1],self.length[2],False)
			t2 = t2 - t1
			if t2<mintheta2: t2 = t2 + 360.0
			if t2>maxtheta2: t2 = t2 - 360.0
			if t3<mintheta3: t3 = t3 + 360.0
			if t3>maxtheta3: t3 = t3 - 360.0
			if not(arm.checkPossibleCollision([t1,t2,t3])) and checkSuccess and Success:
				dests.append(int(nidReturn(int((t1-mintheta1)/dtheta1),int((t2-mintheta2)/dtheta2),int((t3-mintheta3)/dtheta3))))

			# This is a bit of an experimental option in case we've previously
			# returned configurations that are impossible to reach
			if len(dests)==0 or toSkip>0:
				dests = []
				toSkip -= 1
				a3 = self.optimum_dist + (failures*delta)*(-1)**(failures)
				failures += 1
				print "new a3: ",a3
				print "failures: ",failures
				raw_input()
				if a3<self.r_inner or a3>self.r_outer:
					print "WHOA NELLIE! You are SOL",a3,delta
					raw_input()
				continue

		return dests


# The base pathfinding class.  Inherit and overwrite FindPath to implement a pathfinding algorithm.
class Pathfind:

    # Creates a new instance of the pathfinding class, over the specified PathGraph.
    # Does not have to be linked to a gui.

    def __init__(self, graph, gui=None):
        self.graph = graph
        self.gui = gui
        self.path = None
        self.pathLen = -1
        self.drawnObjects = []

    # Find a path from the starting node id to the goal node id.
    # Such a path found is a list of node id's, which form a valid path from startNodeID
    # to goalNodeID, such that each node represented in the array is adjacent to its
    # immediate neighbors in the list.  List is NOT returned, instead set it to self.path
    # Default behavior is to fail to find a path.  Overwrite this in path algorithms implemented.
    def FindPath(self, startNodeID, goalNodeID):
        self.path = None
        return

    # Returns a list of tuples of imformation about the path.
    # The list is of 2-tuples.  Entry 1 is the label, entry 2 is the information.
    # Default behavior is to return the length.  Overwrite this is desired in path algorithms implemented.
    def GetStats(self):
        if self.path == None:
            return [("No Path Found", "")]

        pathStats = []
		
        length = 0
        for nodeID, adjNodeID in zip(self.path[:-1], self.path[1:]):
            adjCost = self.graph.getAdjacencyCost(nodeID, adjNodeID)
            if adjCost:
                length += adjCost
        pathStats.append(("Path Length:", round(length,3)))
		
        return pathStats
		
	#############################################
	# Nothing needs to be overwritten past here #
	#############################################
		
    # Set which gui this is attached to.
    def SetGui(self, gui):
        self.gui = gui

    # Smooth the path.  Currently does not work as intended.
    def Smooth(self):
        if self.path:
            #pathobjects = []
            thisnodem2=[] # node-2
            thisnodem1=[] # node-1
            xm1 = -1
            ym1 = -1
            xp1 = -1
            yp1 = -1
            xavg = -1
            yavg = -1
            for index in range(1,len(self.path)-1,2): # starting at 1, ending at next to last, in steps of 2
                nodeID = self.path[index]
                thisnode = self.graph.getNode(nodeID)
                (x,y) = thisnode.getCoordinates()
                prevnode = self.graph.getNode(nodeID-1)
                (xm1,ym1) = prevnode.getCoordinates()
                nextnode = self.graph.getNode(nodeID+1)
                (xp1,yp1) = nextnode.getCoordinates()

                xavg = 0.5* (xm1 + xp1)
                yavg = 0.5* (ym1 + yp1)

                x2 = int(0.5+(x - 2.0*(x-xavg)))
                y2 = int(0.5+(y - 2.0*(y-yavg)))
                targetnode = self.graph.nodeDict.get((x2,y2),None)
                if (targetnode != None):
                    print x, ' -> ', x2, ', ', y, ' -> ',y2
                    self.path[index] = targetnode
                else:
                    print "No change: ",x,", ",y
        else:
            return

    # If there is an attached gui, draw the list of nodeID's in drawList.  color and fill are the colors desired, 
    # height and width control the size of the rectangles drawn.  If clear is False, does not remove any previous
    # results from calling this.
    # This is used by the "animation" feature from the gui, to animate your planning algorithm.
    def DrawList(self, drawList, height=0, width=0, color='green', fill=None, clear=True):
        if self.gui and self.gui.planDraw.get():
            if clear:
                self.ClearDraws()
            for node in drawList:
                (x,y) = self.graph.getNode(node).getCoordinates()
                self.drawnObjects.append(self.gui.map.create_rectangle(x+width,y+height,x-width,y-height,outline=color, fill=fill))

            self.gui.tkroot.update_idletasks()

    # Clear all the graphics from calling DrawList
    def ClearDraws(self):
        if self.drawnObjects:
            for obj in self.drawnObjects:
                self.gui.map.delete(obj)
            self.drawnObjects = []
    
    # Return the path found (or the lack of such)
    def ReturnPath(self):
        return self.path

# A-Star pathfinding algorithm.
# A greedy algorithm that uses an admissable heuristic, so it finds a guaranteed shortest path.
# http://en.wikipedia.org/wiki/A-star
# Find the shortest path between the node id start and the node id goal, using A-Star.
# Mostly copied from the PathPlanning code
def FindPath(graph, start, goal):
	
	visited = []
	if graph != None:
		for i in range(graph.getNodeCount()):
			#print i
			visited.append(None)

	toVisit = PriorityQueue()

	# Start of the algorithm
	visited[start] = (0, None)
	toVisit.put( (0 + euclidDist(start, goal, graph), start, None) )

	while not toVisit.empty(): # Start of the main loop
		nextTuple = toVisit.get() #Get the next node

		next = nextTuple[1]
		nextDist = visited[next][0] # Total distance travelled to get to here.
		
		if next == goal: # Success!  We reached our destination!
			lastPathNode = goal # Construct the path list
			pathCost = visited[goal][0]  # 0 denotes cost to get to this node
			path = []
			while visited[lastPathNode][1] != None:
				path.append(lastPathNode)
				lastPathNode = visited[lastPathNode][1] # 1 denotes the node we just came from
			path.append(start)
			path.reverse()
			return path
		for adjacency in graph.getAdjacencies(next): # For each neightbor, see if have a path to
			# use this assignment if you want the cost to be the associated with the angle swept out
			#nextNbr, cost = adjacency[0], adjacency[1]
			# use this assignment if you want the cost to be the associated with the discretization of the joints
			nextNbr, cost = adjacency[0], 1.0
			totalTravel = nextDist + cost
			if visited[nextNbr] == None: # The neighbor is unvisited
				visited[nextNbr] = ( totalTravel, next ) # Mark the node as visited
				goalDist = euclidDist(nextNbr, goal, graph) # Estimated distance (euclidean)
				toVisit.put( (totalTravel + goalDist, nextNbr, next) ) # Add node to priority queue
			elif totalTravel < visited[nextNbr][0]: # The neighbor has been visited, but that is shorter
														 # if we've found a shorter route to nextNbr
				visited[nextNbr] = (totalTravel, next) # then replace old route to nextNbr with new
				goalDist = euclidDist(nextNbr, goal, graph)
				toVisit.put( (totalTravel + goalDist, nextNbr, next) )
	
	print 'FYI: to visit is EMPTY!'

	return []

# Get from one configuration to an adjacent configuration by incrementally
# moving between each set of angles
def nextWaypoint(start,goal):

	delta_steps = 10
	
	da1 = (goal[0]-start[0])/delta_steps
	da2 = (goal[1]-start[1])/delta_steps
	da3 = (goal[2]-start[2])/delta_steps	
	a1 = start[0]
	a2 = start[1]
	a3 = start[2]
	
	for i in range(delta_steps):
		a1 += da1
		a2 += da2
		a3 += da3
		arm.updateArm([a1,a2,a3], [0,1,2])
		arm.drawFK()
		time.sleep(animation_delay)

# Use the Jacobian method to proceed from wherever the end effector is
# to the location specified by X
# -> Typically used to make fine adjustments to make the last little
#    movement to reach the goal
def nextJacobian(X):

	close_enough = 0.5  # This is the criteria for deciding if we're close enough

	# initialize current angles
	atrack1 = arm.angles[0]
	atrack2 = arm.angles[1]
	atrack3 = arm.angles[2]
	
	# determine the direction the end effector needs to go
	vector_diff = pylab.array([X[0],X[1]]) - pylab.array(arm.pointLocate(3))
	
	while pylab.linalg.norm(vector_diff)>close_enough:
		
		J = arm.Jacobian()
		Js = arm.Jstar(J,1.0)		
		vel = vector_diff/pylab.linalg.norm(vector_diff)
		speed = 50.0
		dang = Js * pylab.mat([[vel[0]*speed], [vel[1]*speed], [0.0], [0.0], [0.0], [0.0]])
		atrack1 += dang[0]
		atrack2 += dang[1]
		atrack3 += dang[2]
		arm.updateArm([atrack1,atrack2,atrack3], [0,1,2])
		arm.drawFK()
		time.sleep(animation_delay)
		vector_diff = pylab.array([X[0],X[1]]) - pylab.array(arm.pointLocate(3))


###############################################################
###############################################################
###############################################################

length = [100, 100, 50] #the length of links
#init_angles = [95, 60, 35] #joint angles
init_angles = [90, 70, 30] #joint angles
colors = [(0,0,255), (0,255,80), (255,0,0)] #the color for each link

obstacle = (-25.,75.)
obstacle = (110.,160.)
obstacle = (-60.,160.)
goal = (110.,170.)
input_angles = init_angles[:]
arm = ArmPlanning(length, init_angles[:], colors)
arm.addObstacle( (100,25,60), obstacle, int(20) )
arm.addObstacle( (100,25,60), (0.,160.), int(20) )
arm.setGoal( (250,250,50), goal, int(5) )

arm.drawFK()

graph = createGraph()
print "Graph Created."
raw_input()

shortest_length = 9999
path = []
toSkip = 0

while path==[]:

	dests = arm.solveDestination(toSkip)
	startnode = int(nidReturn(int((init_angles[0]-mintheta1)/dtheta1),int((init_angles[1]-mintheta2)/dtheta2),int((init_angles[2]-mintheta3)/dtheta3)))

	for d in dests:
		current_path = FindPath(graph,startnode,d)
		path_length = len(current_path)
		if path_length < shortest_length and path_length>1:
			path = current_path
			shortest_length = path_length
			goalnode = d
			
	toSkip += 1	
	pathn = 0

target_reached = False

while not arm.hit_ground:
	
	for event in pygame.event.get():
		
		# Check to see if a new goal has been selected with the mouse
		if event.type == pygame.MOUSEBUTTONDOWN:
			
			goal = arm.PointActual(event.dict['pos'][0],event.dict['pos'][1])
			arm.setGoal( (250,250,50), goal, int(5) )
			new_path = []
			toSkip = 0
			
			# Loop until we have a new path
			while new_path==[]:

				dests = arm.solveDestination(toSkip)
				
				if goalnode in dests:
					new_path = path
					break

				shortest_length = 99999
				if pathn < len(path):
					startnode = path[pathn]
				else:
					startnode = goalnode
				for d in dests:
					current_path = FindPath(graph,startnode,d)
					path_length = len(current_path)
					if path_length < shortest_length and path_length>1:
						new_path = current_path
						shortest_length = path_length
						goalnode = d
				
				toSkip += 1
				pathn = 0
				
			path = new_path
			target_reached = False
			print "Nodes in our new path: ",path
			print "Hit RETURN to continue"
			raw_input()

	if len(path)>pathn:
		nextWaypoint(arm.angles,graph.getNodeCoords(path[pathn]))
		pathn += 1
		print arm.pointLocate(3)
	else:
		if not target_reached:
			nextJacobian(goal)
			print "Target Reached!"
			target_reached = True
	
	arm.drawFK()

print "Collided with something"
raw_input()