diff --git a/compiler/router/cell.py b/compiler/router/cell.py
index 1e5da018..e695122f 100644
--- a/compiler/router/cell.py
+++ b/compiler/router/cell.py
@@ -11,36 +11,24 @@ class cell:
         self.blocked = False
         self.source = False
         self.target = False
+        # -1 means it isn't visited yet
+        self.min_cost = -1 
 
-
-    def get_color(self):
-        r=g=b=0
-        count=0
-        # Blues are horizontal
+    def get_type(self):
         if self.blocked:
-            [r1,g1,b1] = ImageColor.getrgb("Green")
-            r+=r1
-            g+=g1
-            b+=b1
-            count+=1
+            return "X"
 
-        if self.source or self.target:
-            [r1,g1,b1] = ImageColor.getrgb("Red")
-            r+=r1
-            g+=g1
-            b+=b1
-            count+=1
+        if self.source:
+            return "S"
+
+        if self.target:
+            return "T"
 
         if self.path:
-            [r1,g1,b1] = ImageColor.getrgb("Blue")
-            r+=r1
-            g+=g1
-            b+=b1
-            count+=1
+            return "P"
 
-        if count>0:
-            return [int(r/count),int(g/count),int(b/count)]
-        else:
-            return [255,255,255]
-            
-            
+        # We can display the cost of the frontier
+        if self.min_cost > 0:
+            return str(self.min_cost)
+        
+        return "."
diff --git a/compiler/router/grid.py b/compiler/router/grid.py
index 51ae278b..68d543b8 100644
--- a/compiler/router/grid.py
+++ b/compiler/router/grid.py
@@ -1,10 +1,11 @@
 import numpy as np
-from PIL import Image
+import string
 from itertools import tee
 import debug
 from vector3d import vector3d
-
 from cell import cell
+import os
+
 try:
     import Queue as Q  # ver. < 3.0
 except ImportError:
@@ -19,81 +20,172 @@ class grid:
 
     def __init__(self):
         """ Create a routing map of width x height cells and 2 in the z-axis. """
-        self.NONPREFERRED_COST = 5
-        self.VIA_COST = 3
+
+        # costs are relative to a unit grid
+        # non-preferred cost allows an off-direction jog of 1 grid
+        # rather than 2 vias + preferred direction (cost 5)
+        self.VIA_COST = 2
+        self.NONPREFERRED_COST = 4
+        
+        # list of the source/target grid coordinates
         self.source = []
         self.target = []
-        self.blocked = []
 
-        # let's leave the map sparse, cells are created on demand
+        # let's leave the map sparse, cells are created on demand to reduce memory
         self.map={}
 
         # priority queue for the maze routing
         self.q = Q.PriorityQueue()
         
+
+    def reinit(self):
+        """ Reinitialize everything for a new route. """
+        
+        self.convert_path_to_blockages()
+        
+        self.convert_pins_to_blockages()
+
+        # clear source and target pins
+        self.source=[]
+        self.target=[]
+        
+        # clear the queue 
+        while (not self.q.empty()):
+            self.q.get(False)
+
+    def view(self):
+        """
+        View the data as text array.
+        """
+        #os.system('clear')
+          
+        xmin=-10
+        xmax=25
+        ymin=-10
+        ymax=25
+        for v in self.map.keys():
+            xmin = min(xmin,v.x)
+            xmax = max(xmax,v.x)
+            ymin = min(ymin,v.y)
+            ymax = max(ymax,v.y)
+
+        xoffset=0
+        if xmin < 0:
+            xoffset=xmin
+        yoffset=0
+        if ymin < 0:
+            yoffset=ymin
+
+        v_map = {}
+        h_map = {}
+        
+        fieldwidth = 3
+        for h in self.map.keys():
+            fieldwidth = max(fieldwidth,len(self.map[h].get_type()))
+        for v in self.map.keys():
+            fieldwidth = max(fieldwidth,len(self.map[v].get_type()))
+            
+        # for x in range(width):
+        #     for y in range(height):
+        #         v_map[x,y]="."
+        #         h_map[x,y]="."
+                
+        #         h = vector3d(x+xoffset,y+yoffset,0)
+        #         v = vector3d(x+xoffset,y+yoffset,1)
+                
+        #         if (h in self.map.keys()):
+        #             h_map[x,y] = self.map[h].get_type()
+        #             fieldwidth = max(fieldwidth,len(h_map[x,y]))
                     
+        #         if (v in self.map.keys()):               
+        #             v_map[x,y] = self.map[v].get_type()
+        #             fieldwidth = max(fieldwidth,len(v_map[x,y]))                    
 
-    # def view(self,filename="test.png"):
-    #     """
-    #     View the data by creating an RGB array and mapping the data
-    #     structure to the RGB color palette.
-    #     """
 
-    #     v_map = np.zeros((self.width,self.height,3), 'uint8')
-    #     mid_map = np.ones((10,self.height,3), 'uint8')
-    #     h_map = np.ones((self.width,self.height,3), 'uint8')        
+            
+        # display lower layer
+        print '='*80
+        print '='*80
+        self.printgrid(0,xmin,xmax,ymin,ymax,fieldwidth)
+        print '='*80
+        self.printgrid(1,xmin,xmax,ymin,ymax,fieldwidth)
+        print '='*80
+        print '='*80
+        raw_input("Press Enter to continue...")
 
-    #     # We shouldn't have a path greater than 50% the HPWL
-    #     # so scale all visited indices by this value for colorization
-    #     for x in range(self.width):
-    #         for y in range(self.height):
-    #             h_map[x,y] = self.map[vector3d(x,y,0)].get_color()
-    #             v_map[x,y] = self.map[vector3d(x,y,1)].get_color()
-    #             # This is just for scale
-    #             if x==0 and y==0:
-    #                 h_map[x,y] = [0,0,0]
-    #                 v_map[x,y] = [0,0,0]
+    def printgrid(self,layer,xmin,xmax,ymin,ymax,fieldwidth):
+        """
+        Display a text representation of a layer of the routing grid.
+        """
+        print "".center(fieldwidth),
+        for x in range(xmin,xmax+1):
+            print str(x).center(fieldwidth),
+        print ""
+        for y in reversed(range(ymin,ymax+1)):
+            print str(y).center(fieldwidth),
+            for x in range(xmin,xmax+1):
+                n = vector3d(x,y,layer)
+                if n in self.map.keys():
+                    print self.map[n].get_type().center(fieldwidth),
+                else:
+                    print ".".center(fieldwidth),
+            print ""
         
-    #     v_img = Image.fromarray(v_map, 'RGB').rotate(90)
-    #     #v_img.show()
-    #     mid_img = Image.fromarray(mid_map, 'RGB').rotate(90)        
-    #     h_img = Image.fromarray(h_map, 'RGB').rotate(90)
-    #     #h_img.show()
         
-    #     # concatenate them into a plot with the two layers
-    #     img = Image.new('RGB', (2*self.width+10, self.height))
-    #     img.paste(h_img, (0,0))
-    #     img.paste(mid_img, (self.width,0))        
-    #     img.paste(v_img, (self.width+10,0))
-    #     #img.show()
-    #     img.save(filename)
-
-    def set_property(self,ll,ur,z,name,value=True):
+    def add_blockage(self,ll,ur,z):
+        debug.info(3,"Adding blockage ll={0} ur={1} z={2}".format(str(ll),str(ur),z))
         for x in range(int(ll[0]),int(ur[0])+1):
             for y in range(int(ll[1]),int(ur[1])+1):
                 n = vector3d(x,y,z)
                 self.add_map(n)
-                setattr (self.map[n], name, True)
-                if n not in getattr(self, name):
-                    getattr(self, name).append(n)
-
-    def add_blockage(self,ll,ur,z):
-        debug.info(3,"Adding blockage ll={0} ur={1} z={2}".format(str(ll),str(ur),z))
-        self.set_property(ll,ur,z,"blocked")
+                self.map[n].blocked=True
+                
 
     def set_source(self,ll,ur,z):
         debug.info(1,"Adding source ll={0} ur={1} z={2}".format(str(ll),str(ur),z))
-        self.set_property(ll,ur,z,"source")
-
+        for x in range(int(ll[0]),int(ur[0])+1):
+            for y in range(int(ll[1]),int(ur[1])+1):
+                n = vector3d(x,y,z)
+                self.add_map(n)
+                self.map[n].source=True
+                # Can't have a blocked target otherwise it's infeasible
+                self.map[n].blocked=False
+                self.source.append(n)
 
     def set_target(self,ll,ur,z):
         debug.info(1,"Adding target ll={0} ur={1} z={2}".format(str(ll),str(ur),z))
-        self.set_property(ll,ur,z,"target")
+        for x in range(int(ll[0]),int(ur[0])+1):
+            for y in range(int(ll[1]),int(ur[1])+1):
+                n = vector3d(x,y,z)
+                self.add_map(n)
+                self.map[n].target=True
+                # Can't have a blocked target otherwise it's infeasible
+                self.map[n].blocked=False
+                self.target.append(n)                
 
+    def convert_pins_to_blockages(self):
+        """
+        Convert all the pins to blockages and reset the pin sets.
+        """
+        for p in self.map.values():
+            if (p.source or p.target):
+                p.blocked=True
+
+    def convert_path_to_blockages(self):
+        """
+        Convert the routed path to blockages and reset the path.
+        """
+        for p in self.map.values():
+            if (p.path):
+                p.path=False
+                p.blocked=True
+        
+            
     def set_path(self,path):
         """ 
         Mark the path in the routing grid for visualization
         """
+        self.path=path
         for p in path:
             self.map[p].path=True
         
@@ -105,7 +197,7 @@ class grid:
         # We set a cost bound of 2.5 x the HPWL for run-time. This can be 
         # over-ridden if the route fails due to pruning a feasible solution.
         if (cost_bound==0):
-            cost_bound = 2.5*self.cost_to_target(self.source[0])
+            cost_bound = self.cost_to_target(self.source[0])*self.NONPREFERRED_COST
             
         # Make sure the queue is empty if we run another route
         while not self.q.empty():
@@ -119,7 +211,8 @@ class grid:
         # Keep expanding and adding to the priority queue until we are done
         while not self.q.empty():
             (cost,path) = self.q.get()
-            debug.info(2,"Expanding: cost=" + str(cost) + " " + str(path))
+            debug.info(2,"Queue size: size=" + str(self.q.qsize()) + " " + str(cost))            
+            debug.info(3,"Expanding: cost=" + str(cost) + " " + str(path))
             
             # expand the last element
             neighbors =  self.expand_dirs(path)
@@ -129,17 +222,25 @@ class grid:
                 newpath = path + [n]
                 if n not in self.map.keys():
                     self.map[n]=cell()
-                self.map[n].visited=True
 
                 # check if we hit the target and are done
                 if self.is_target(n):
                     return (newpath,self.cost(newpath))
-                else:
-                    # potential path cost + predicted cost
-                    cost = self.cost(newpath) +  self.cost_to_target(n)
+                elif not self.map[n].visited:
+                    # current path cost + predicted cost
+                    current_cost = self.cost(newpath)
+                    target_cost = self.cost_to_target(n)                    
+                    predicted_cost = current_cost +  target_cost
                     # only add the cost if it is less than our bound
-                    if (cost < cost_bound):
-                        self.q.put((cost,newpath))
+                    if (predicted_cost < cost_bound):
+                        if (self.map[n].min_cost==-1 or current_cost<self.map[n].min_cost):
+                            self.map[n].visited=True
+                            self.map[n].min_path = newpath
+                            self.map[n].min_cost = predicted_cost
+                            debug.info(3,"Enqueuing: cost=" + str(current_cost) + "+" + str(target_cost) + " " + str(newpath))
+                            # add the cost to get to this point if we haven't reached it yet
+                            self.q.put((predicted_cost,newpath))
+            #self.view()
 
         debug.error("Unable to route path. Expand area?",-1)
 
@@ -209,36 +310,54 @@ class grid:
         Cost so far will be the length of the path.
         """
         debug.info(4,"Initializing queue.")
+
+        # uniquify the source (and target while we are at it)
+        self.source = list(set(self.source))
+        self.target = list(set(self.target))
+        
         for s in self.source:
             cost = self.cost_to_target(s)
             debug.info(4,"Init: cost=" + str(cost) + " " + str([s]))
             self.q.put((cost,[s]))
 
+    def hpwl(self, src, dest):
+        """ 
+        Return half perimeter wire length from point to another.
+        Either point can have positive or negative coordinates. 
+        Include the via penalty if there is one.
+        """
+        hpwl = max(abs(src.x-dest.x),abs(dest.x-src.x))
+        hpwl += max(abs(src.y-dest.y),abs(dest.y-src.y))
+        hpwl += max(abs(src.z-dest.z),abs(dest.z-src.z))
+        if src.x!=dest.x or src.y!=dest.y:
+            hpwl += self.VIA_COST
+        return hpwl
+            
     def cost_to_target(self,source):
         """
-        Find the cheapest HPWL distance to any target point
+        Find the cheapest HPWL distance to any target point ignoring 
+        blockages for A* search.
         """
-        cost = source.hpwl(self.target[0])
+        cost = self.hpwl(source,self.target[0])
         for t in self.target:
-            cost = min(source.hpwl(t),cost)
+            cost = min(self.hpwl(source,t),cost)
         return cost
 
 
-    
-
     def cost(self,path):
         """ 
         The cost of the path is the length plus a penalty for the number
-        of vias.
-        We assume that non-preferred direction is penalized 2x.
+        of vias. We assume that non-preferred direction is penalized.
         """
+
         # Ignore the source pin layer change, FIXME?
         def pairwise(iterable):
             "s -> (s0,s1), (s1,s2), (s2, s3), ..."
             a, b = tee(iterable)
             next(b, None)
             return zip(a, b)
-        
+
+
         plist = pairwise(path)
         cost = 0
         for p0,p1 in plist:
@@ -250,7 +369,7 @@ class grid:
                 cost += self.NONPREFERRED_COST if (p0.z == 0) else 1
             else:
                 debug.error("Non-changing direction!")
-        
+
         return cost
     
     def get_inertia(self,p0,p1):
diff --git a/compiler/router/router.py b/compiler/router/router.py
index d8d5e632..5ff7e4f9 100644
--- a/compiler/router/router.py
+++ b/compiler/router/router.py
@@ -118,6 +118,7 @@ class router:
         for layer in self.layers:
             self.write_obstacle(self.top_name)
 
+            
     def clear_pins(self):
         """
         Reset the source and destination pins to start a new routing.
@@ -126,8 +127,13 @@ class router:
         Clear other pins from blockages?
         
         """
-        self.source = []
-        self.dest = []
+
+        self.pin_names = []
+        self.pin_shapes = {}
+        self.pin_layers = {}
+        self.all_pin_shapes = []
+        
+        self.rg.reinit()
         
 
     def route(self, layers, src, dest):
@@ -136,7 +142,11 @@ class router:
         the simplified rectilinear path.
         """
         # Clear the pins if we have previously routed
-        self.clear_pins()
+        if (hasattr(self,'rg')):
+            self.num=self.num+1
+            self.clear_pins()
+        else:
+            self.num=0
 
         # Set up layers and track sizes
         self.set_layers(layers)
@@ -152,15 +162,15 @@ class router:
         
         self.find_blockages()
 
-        #self.rg.view("preroute.png")
-
+        self.rg.view()
+        
         # returns the path in tracks
         (self.path,cost) = self.rg.route()
         debug.info(1,"Found path: cost={0} ".format(cost))
         debug.info(2,str(self.path))
         self.set_path(self.path)
+        self.rg.view()        
         
-        #self.rg.view("postroute.png")
         return 
 
     def add_route(self,cell):
@@ -197,6 +207,7 @@ class router:
             c=contact(self.layers, (1, 1))
             via_offset = vector(-0.5*c.width,-0.5*c.height)
             cell.add_via(self.layers,abs_path[0]+via_offset)
+
         # Check if a via is needed at the end point
         if (contracted_path[-1].z!=self.target_pin_layer):
             # offset this by 1/2 the via size
diff --git a/compiler/router/tests/05_two_nets_test.py b/compiler/router/tests/05_two_nets_test.py
index c9b67d2c..57fbf0d4 100644
--- a/compiler/router/tests/05_two_nets_test.py
+++ b/compiler/router/tests/05_two_nets_test.py
@@ -55,14 +55,15 @@ class two_nets_test(unittest.TestCase):
                 layer_stack =("metal1","via1","metal2")
                 r.route(layer_stack,src="A",dest="B")
                 r.add_route(self)
+                #self.gds_write("temp1.gds")
                 
-                #r.route(layer_stack,src="C",dest="D")
-                #r.add_route(self)
+                r.route(layer_stack,src="C",dest="D")
+                r.add_route(self)
+                #self.gds_write("temp2.gds")
 
 
         
         r = routing("test1", "05_two_nets_test")
-        r.gds_write("temp.gds")        
         self.local_check(r)
         
         # fails if there are any DRC errors on any cells
diff --git a/compiler/router/vector3d.py b/compiler/router/vector3d.py
index 655e8ff3..b84f2eda 100644
--- a/compiler/router/vector3d.py
+++ b/compiler/router/vector3d.py
@@ -136,10 +136,3 @@ class vector3d():
         """ Min of both values """
         return vector3d(min(self.x,other.x),min(self.y,other.y),min(self.z,other.z))
 
-    def hpwl(self, other):
-        """ Return half perimeter wire length from point to another.
-        Either point can have positive or negative coordinates. """
-        hpwl = max(abs(self.x-other.x),abs(other.x-self.x))
-        hpwl += max(abs(self.y-other.y),abs(other.y-self.y))
-        hpwl += max(abs(self.z-other.z),abs(other.z-self.z))  
-        return hpwl