import search
reload(search)

class XYThGridPlanner:
    def __init__(self, gridMap):
        self.gridMap = gridMap

    # Given init (ix, iy, idir) and goal (gx, gy, gdir) in XY grid coordinates
    # and idir, gdir cardinal directions
    def makePlan(self, init, goal):
        # Successor function
        def s((i,j,h)):
            # Turn left and turn right
            successors = [(i, j, (h+1)%4), (i, j, (h-1)%4)]
            if h==0: # north
                moveAhead = (i,j+1)
            elif h==1: # east
                moveAhead = (i+1,j)
            elif h==2: # south
                moveAhead = (i,j-1)
            else: # west
                moveAhead = (i-1,j)
            if self.gridMap.free(moveAhead):
                successors.append(moveAhead+(h,))
            return successors
    
        def g(state):
            return state == goal
    
        result = search.breadthFirstDP(init, g, s)
        if result:
            return result
        else:
            print "Couldn't get from ", init, " to ", goal
            return None

    # Plan is a list of coordinate triples
    def drawPlan(self, plan, w):
        w.clear()
        self.gridMap.draw(w)
        blockSize = 3
        for step in plan:
            (sx,sy,sth) = self.gridMap.indicesAndDirToPose(step)
            if step == plan[0]:
                botColor = "DarkGreen"
            elif step == plan[-1]:
                botColor = "Gold"
            else:
                botColor = "Blue"
            w.drawRobotWithNose(sx, sy, sth, color = botColor, \
                                size = blockSize)



    
        
        
