--- a/src/sim.tp	Sun Jul 15 18:37:36 2012 -0700
+++ b/src/sim.tp	Sun Jul 15 21:42:46 2012 -0700
@@ -59,6 +59,7 @@
 					x <- 0
 					y <- 0
 					isrobot <- { true }
+					navigable <- { false }
 					eq <- :other { id = (other id) }
 					collected <- 0
 					heldBreath <- 0
@@ -131,7 +132,8 @@
 			_robot <- null
 			_ended <- false
 			_maxmoves <- in_width * in_height
-			
+			_heuristicValid <- false
+			_heuristic <- 0
 			_succeeded <- false
 			ret <- #{
 				grid <- in_grid
@@ -189,6 +191,35 @@
 					}
 					cur
 				}
+				distanceFrom:to <- :x y celltype {
+					//print: "calculating distance from " . x . ", " . y . " to " . celltype . "\n"
+					moves <- validMoves: x y
+					curdist <- 0
+					visited <- _nextGrid
+					foreach: grid :idx el {
+						visited set: idx false
+					}
+					notfound <- true
+					while: { if: notfound { (moves length) > 0 } } do: {
+						nextmoves <- #[]
+						curdist <- curdist + 1
+						foreach: moves :idx move {
+							curpos <- move index
+							if: (not: (visited get: curpos)) {
+								if: ((grid get: curpos) eq: celltype) {
+									notfound <- false
+								} else: {
+									visited set: curpos true
+									foreach: (validMoves: (calcX: curpos) (calcY: curpos)) :idx move {
+										nextmoves append: move
+									}
+								}
+							}
+						}
+						moves <- nextmoves
+					}
+					curdist
+				}
 				getRobot <- { _robot }
 				updatePos <- :obj Index {
 					obj x!: (calcX: Index)
@@ -201,6 +232,18 @@
 				moves <- #[]
 				score <- 0
 				maxScore <- { score + (lambdaCount - (_robot collected)) * 25 + lambdaCount * 50 }
+				heuristic <- {
+					if: (not: _heuristicValid) {
+						dest <- if: (_robot collected) = lambdaCount {
+							cellTypes openLift
+						} else: {
+							cellTypes lambda
+						}
+						_heuristic <- score - (distanceFrom: (_robot x) (_robot y) to: dest)
+						_heuristicValid <- true
+					}
+					_heuristic
+				}
 				addPoints <- :points { score <- score + points }
 				ended <- {_ended}
 				succeeded <- {_succeeded}
@@ -256,6 +299,7 @@
 					addPoints: (_robot collected) * 25
 				}
 				advance <- :roboCmd {
+					_heuristicValid <- false
 					if: roboCmd = "A" {
 						moves append: roboCmd
 						abort