--- a/src/lifter.tp	Sun Jul 15 20:30:46 2012 -0700
+++ b/src/lifter.tp	Sun Jul 15 21:56:43 2012 -0700
@@ -1,46 +1,90 @@
 #{
-	pqueue <- {
-		normalnode <- :pri val {
-			#{
-				priority <- pri
-				value <- val
-				next <- false
-				higherPriority? <- :other {
-					priority > (other priority)
+	swap <- :arr from to {
+		a <- arr get: from
+		b <- arr get: to
+		arr set: from b
+		arr set: to a
+	}
+	
+	median <- :arr idx1 idx2 idx3 {
+		val1 <- (arr get: idx1) heuristic
+		val2 <- (arr get: idx2) heuristic
+		val3 <- (arr get: idx3) heuristic
+		
+		if: val2 > val1 {
+			if: val3 > val2 {
+				idx2
+			} else: {
+				if: val3 > val1 {
+					idx3
+				} else: {
+					idx1
 				}
-				if:else <- :self trueblock :elseblock {
-					trueblock:
+			}
+		} else: {
+			//val1 >= val2
+			if: val3 > val1 {
+				idx1
+			} else: {
+				//val1 >= val3
+				if: val3 > val2 {
+					idx3
+				} else: {
+					idx2
 				}
 			}
 		}
-		head <- #{
-			higherPriority? <- :other {false}
-			next <- { self }
-			value <- { false }
-		}
-		#{
-			take <- {
-				cur <- head
-				head <- cur next
-				cur value
+	}
+	
+	partition <- :arr left right pivotidx {
+		pivotval <- (arr get: pivotidx) heuristic
+		//move pivot to end
+		swap: arr pivotidx right
+		i <- left
+		storeidx <- left
+		while: { i < right } do: {
+			if: ((arr get: i) heuristic) < pivotval {
+				swap: arr storeidx i
+				storeidx <- storeidx + 1
 			}
-			insert:atPriority <- :val pri {
-				node <- normalnode: pri val
-				cur <- head
-				last <- false
-				while: {cur higherPriority?: node} do: {
-					last <- cur
-					cur <- cur next
-				}
-				if: last {
-					node next!: (last next)
-					last next!: node
-				} else: {
-					node next!: head
-					head <- node
-				}
-				self
+			i <- i + 1
+		}
+		swap: arr storeidx right
+		storeidx
+	}
+	
+	//quickselect shamelessly translated from pseudocode on Wikipedia
+	select <- :arr left right n {
+		pivotidx <- median: arr left right (left + (right - left) / 2)
+		newpivotidx <- partition: arr left right pivotidx
+		pivotdist <- newpivotidx - left + 1
+		while: { pivotdist != n } do: {
+			if: n < pivotdist {
+				right <- newpivotidx - 1
+			} else: {
+				n <- n - pivotdist
+				left <- newpivotidx + 1
 			}
+			pivotidx <- median: arr left right (left + (right - right) / 2)
+			newpivotidx <- partition: arr left right pivotidx
+			pivotdist <- newpivotidx - left + 1
+		}
+		newpivotidx
+	}
+	
+	topN <- :arr n {
+		curidx <- (select: arr 0 (arr length) - 1 ((arr length) - n)) + 1
+		newarr <- #[]
+		while: { curidx < (arr length) } do: {
+			newarr append: (arr get: curidx)
+			curidx <- curidx + 1
+		}
+		newarr
+	}
+	
+	printArr <- :arr {
+		foreach: arr :idx el {
+			print: "" . idx . ": " . (el heuristic) . "\n"
 		}
 	}
 	
@@ -55,10 +99,9 @@
 	moveFinder <- :field {
 		#{
 			curbest <- (field clone) advance: "A"
-			playfield <- field
+			states <- #[field]
 			bestMove:withMaxSteps <- :self :max{
 				n <- 0
-				states <- #[playfield]
 				while: { if: (states length) > 0 { if: n < max { not: (curbest succeeded) } } } do: {
 					nextstates <- #[]
 					foreach: states :idx curstate {
@@ -86,21 +129,14 @@
 				if: (curbest succeeded) {
 					false
 				} else: {
-					if: (states length) > 0 {
-						bestofcur <- states get: 0
-						n <- 1
-						while: { n < (states length) } do: {
-							curstate <- states get: n
-							if: ((curstate score) > (bestofcur score)) {
-								bestofcur <- curstate
-							}
-							n <- n + 1
-						}
-						playfield <- bestofcur
-						true
-					}
+					(states length) > 0
 				}
 			}
+			cullStatesTo <- :n {
+				print: "culling " . (states length) . " to " . n . "\n"
+				states <- topN: states n
+				print: "states length is now " . (states length) . "\n"
+			}
 		}
 	}
 	
@@ -112,15 +148,40 @@
 		os write: 2 "height: " . (string: (initial height)) . "\n"
 		
 		finder <- moveFinder: initial
-		while: { bestMove: finder withMaxSteps: 5 } do: {
+		initmaxsteps <- 6
+		maxsteps <- initmaxsteps
+		while: { bestMove: finder withMaxSteps: maxsteps } do: {
+			best <- -1000000
+			bestscore <- -1000000
+			foreach: (finder states) :idx el {
+				h <- (el heuristic)
+				s <- (el score)
+				if: (h > best) {
+					best <- h
+				}
+				if: (s > bestscore) {
+					bestscore <- s
+				}
+			}
+			finder cullStatesTo: 8
+			maxsteps <- initmaxsteps - 1
 			os write: 2 "--------iteration results-------\n"
 			os write: 2 "Best:\n"
 			(finder curbest) printGrid
-			os write: 2 "Current:\n"
-			(finder playfield) printGrid
+			os write: 2 "Current before cull\n"
+			os write: 2 " Best Heuristic: " . best . "\n"
+			os write: 2 " Best Score: " . bestscore . "\n"
+			os write: 2 "After cull:\n"
+			foreach: (finder states) :idx el{
+				os write: 2 " " . idx . " Heuristic: " . (el heuristic) . "\n"
+				os write: 2 " " . idx . " Score: " . (el score) . "\n"
+			}
+			//os write: 2 "Current:\n"
+			//(finder playfield) printGrid
 		}
 		os write: 2 "---------------\n"
 		os write: 2 "End Best:\n"
 		(finder curbest) printGrid
+		
 	}
 }

--- a/src/sim.tp	Sun Jul 15 20:30:46 2012 -0700
+++ b/src/sim.tp	Sun Jul 15 21:56:43 2012 -0700
@@ -59,6 +59,7 @@
 					x <- 0
 					y <- 0
 					isrobot <- { true }
+					navigable <- { false }
 					eq <- :other { id = (other id) }
 					collected <- 0
 					heldBreath <- 0
@@ -129,7 +130,9 @@
 			_nextGrid <- #[]
 			_robot <- null
 			_ended <- false
-			
+			_maxmoves <- in_width * in_height
+			_heuristicValid <- false
+			_heuristic <- 0
 			_succeeded <- false
 			ret <- #{
 				grid <- in_grid
@@ -190,6 +193,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)
@@ -202,6 +234,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}
@@ -260,11 +304,15 @@
 					grid <- _nextGrid
 					_nextGrid <- tmp
 				}
+				abort <- {
+					_ended <- true
+					addPoints: (_robot collected) * 25
+				}
 				advance <- :roboCmd {
+					_heuristicValid <- false
 					if: roboCmd = "A" {
-						_ended <- true
 						moves append: roboCmd
-						addPoints: (_robot collected) * 25
+						abort
 					}
 					
 					if: (not: _ended) {
@@ -274,6 +322,9 @@
 						doUpdate:
 						checkForDeath:
 						swapGrids:
+						if: (moves length) >= _maxmoves {
+							abort
+						}
 					}
 					self
 				}