--- a/src/sim.tp	Mon Jul 16 02:20:38 2012 -0700
+++ b/src/sim.tp	Mon Jul 16 04:03:03 2012 -0700
@@ -32,7 +32,7 @@
 				string <- idStr
 				isrobot <- { false }
 				eq <- :other { id = (other id) }
-				navigable <- { cannav }				
+				navigable <- cannav
 			}
 			typedict set: (ret id) ret
 			ret
@@ -179,9 +179,7 @@
 						} 
 					}}
 					here <- calcIndex: x y
-					//TODO: Add wait move when rocks are in motion
-					//(amove: here "W") 
-					cur <- #[(amove: here "A")]
+					cur <- #[(amove: here "A") (amove: here "W")]
 					up <- amove: (calcIndex: x y + 1) "U"
 					down <- amove: (calcIndex: x y - 1) "D"
 					left <- amove: (calcIndex: x - 1 y) "L"
@@ -194,7 +192,10 @@
 					cur
 				}
 				distanceFrom:to <- :x y celltype {
-					//print: "calculating distance from " . x . ", " . y . " to " . celltype . "\n"
+					//debugLog: "calculating distance from " . x . ", " . y . " to " . celltype . "\n"
+					if: (celltype eq: (cellTypes closedLift)) {
+						celltype navigable!: true
+					}
 					moves <- validMoves: x y
 					curdist <- 0
 					visited <- _nextGrid
@@ -205,14 +206,17 @@
 					while: { if: notfound { (moves length) > 0 } } do: {
 						nextmoves <- #[]
 						curdist <- curdist + 1
+						//debugLog: "moves at distance " . curdist . "\n"
 						foreach: moves :idx move {
 							curpos <- move index
+							//debugLog: "" . move . " " . (grid get: curpos) . "\n"
 							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
 									}
 								}
@@ -220,7 +224,14 @@
 						}
 						moves <- nextmoves
 					}
-					curdist
+					if: (celltype eq: (cellTypes closedLift)) {
+						celltype navigable!: false
+					}
+					if: notfound {
+						-1
+					} else: {
+						curdist
+					}
 				}
 				getRobot <- { _robot }
 				updatePos <- :obj Index {
@@ -237,11 +248,32 @@
 				heuristic <- {
 					if: (not: _heuristicValid) {
 						dest <- if: (_robot collected) = lambdaCount {
-							cellTypes openLift
+							dist <- (distanceFrom: (_robot x) (_robot y) to: (cellTypes openLift))
+							if: dist < 0 {
+								//debugLog: "open lift unreachable\n"
+								_heuristic <- (_robot collected) * 50 -  (moves length)
+							} else: {
+								//debugLog: "open lift unreachable at distance" . dist . "\n"
+								_heuristic <- (_robot collected) * 75 - dist -  (moves length)
+							}
 						} else: {
-							cellTypes lambda
+							mult <- 0
+							liftdist <- (distanceFrom: (_robot x) (_robot y) to: (cellTypes closedLift))
+							if: liftdist < 0 {
+								mult <- 50
+							} else: {
+								mult <- 75
+							}
+							lambdadist <- (distanceFrom: (_robot x) (_robot y) to: (cellTypes lambda))
+							if: lambdadist < 0 {
+								//debugLog: "lambda unreachable with lift multilier " . mult . "\n"
+								_heuristic <- score
+							} else: {
+								//debugLog: "lambda reachable at distance " . lambdadist . " lift multilier " . mult . "\n"
+								_heuristic <- (_robot collected) * mult - lambdadist - (moves length)
+							}
 						}
-						_heuristic <- score - (distanceFrom: (_robot x) (_robot y) to: dest)
+						//_heuristic <- (_robot collected) * 75 - (distanceFrom: (_robot x) (_robot y) to: (cellTypes openLift) -  (moves length)
 						_heuristicValid <- true
 					}
 					_heuristic
@@ -309,21 +341,34 @@
 					addPoints: (_robot collected) * 25
 				}
 				advance <- :roboCmd {
-					_heuristicValid <- false
-					if: roboCmd = "A" {
-						moves append: roboCmd
-						abort
-					}
+					if: roboCmd = "?" {
+						os write: 2 "valid moves: "
+						valid <- validMoves: (_robot x) (_robot y)
+						foreach: valid :idx el {
+							os write: 2 (el cmd)
+						}
+						os write: 2 "\n"
+					} else: {
+						if: roboCmd = "h" {
+							os write: 2 "heuristic: " . heuristic . "\n"
+						} else: {
+							_heuristicValid <- false
+							if: roboCmd = "A" {
+								moves append: roboCmd
+								abort
+							}
 					
-					if: (not: _ended) {
-						_robot doCmd: roboCmd
-						score <- score - 1
-						moves append: roboCmd
-						doUpdate:
-						checkForDeath:
-						swapGrids:
-						if: (moves length) >= _maxmoves {
-							abort
+							if: (not: _ended) {
+								_robot doCmd: roboCmd
+								score <- score - 1
+								moves append: roboCmd
+								doUpdate:
+								checkForDeath:
+								swapGrids:
+								if: (moves length) >= _maxmoves {
+									abort
+								}
+							}
 						}
 					}
 					self