                              (let [base (env/get-var s [:base])]
                                (if (= base dst)
                                   [[]]
-                                 [[(ReachHLA env [0 0]) (make-unpark s) 
-                                   (NavHLA dst) (make-park s)]])))
+                                 [[(make-reach-hla env [0 0]) (make-unpark s) 
+                                   (make-nav-hla env dst) (make-park s)]])))
                             (cycle-level- [s] nil)))
 ; manhattan + park (if appl)
 ; pess? SLD?
 
+(defn move-to-front [x s]
+  (if (some #{x} s)
+      (cons x (remove #{x} s))
+    s))
+
 ;; TODO: check out state abstraction here
-
+;; Note: since we may not sample current position, have to special case.
 (defn make-go-grasp-hla [env o] 
  (reify :as this
   env/Action                (action-name [] [:go-grasp o])

  hierarchy/HighLevelAction (immediate-refinements- [s]
                             (if (= dst (env/get-var s [:base]))
                               [[]]
                               (for [dir dirs] [(make-base-dir s dir) this])))
                            (cycle-level- [s] 1)))
; manhattan
; pess? SLD? (try direct line, if occ then -inf).

                              (conj (reach-context s o-dst) [:holding]))
  hierarchy/HighLevelAction (immediate-refinements- [s]
                             (let [const (env/get-var s :const)
                                   base  (env/get-var s [:base])
                                   free  (get const [:freespace])]
                               (for [dst (->> (get const [:base-offsets])
                                              (map #(add-pos % o-dst))
                                              (filter free)
                                              (move-to-front base)
                                              (take (get const [:n-base-samples])))]
                                 [(make-move-base-hla env dst) (make-drop-at-hla env o o-dst)])))
                            (cycle-level- [s] nil)))


(defn make-go-drop-hla [env o] 
 (reify :as this
