             (for [dir dirs]
               [(make-base-dir s dir) (make-gripper-dir s dir)
                (make-pickup-dir s dir) (make-putdown-dir s dir)]))))
-     (into {[:parked?] true [:gxo] 0 [:gyo] 0} 
-           (for [[o _ goal] objects :when goal] [[:at-goal? o] true])))))
+     (into {[:parked?] true [:gripper-offset] [0 0]} 
+           (for [[o _ goal] objects :when goal] [[:at-goal? o] true]))))))
+
+; (print-state (initial-state (make-2d-manipulation-env [10 10] [1 1] [ [ [4 4] [6 6] ] ] [ [:a [5 5] #{ [4 4] } ] ] 2)))
+; (uniform-cost-search (make-2d-manipulation-env [10 10] [1 1] [ [ [4 4] [6 6] ] ] [ [:a [5 5] #{ [4 4] } ] ] 2))
+
+(comment 
+ (defn make-2d-manipulation-env [size start obstacles objects g-rad]
+   (let [[width height] size
+         [bx by]        start
+         border-regions [[[0 0] [0 height]] [[width 0] [width height]]
+                         [[0 0] [width 0]] [[0 height] [width height]]]
+         border-cells   (set (mapcat region-cells border-regions))
+         obstacle-cells (set (mapcat region-cells obstacles))
+         all-cells      (set (region-cells [[0 0] size]))
+         freespace      (clojure.set/difference all-cells border-cells obstacle-cells)
+         legal-go       (set (filter (fn [[x y]] (<= (+ (util/abs x) (util/abs y)) g-rad))
+                                     (region-cells [[(- g-rad) (- g-rad)] [g-rad g-rad]])))]
+     (env/SimpleFactoredEnv
+      (into {:const (into {[:size] size [:freespace] freespace [:legal-go] legal-go [:objects] objects
+                           [:map] (reduce (fn [m [x y]] (assoc-in m [y x] \.))
+                                          (reduce (fn [m [x y]] (assoc-in m [y x] \*))
+                                                  (vec (repeat (inc height) (vec (repeat (inc width) \ ))))
+                                                  border-cells)
+                                          obstacle-cells)}
+                          (for [[o _ goal] objects :when goal] [[:goal o] goal]))
+             [:bx] bx [:by] by [:gxo] 0 [:gyo] 0 [:holding] nil [:parked?] true}
+            (apply concat
+                   (for [[x y] all-cells]     [[:object-at x y] nil])
+                   (for [[x y] border-cells] [[:object-at x y] :border])
+                   (for [[o [ox oy] goal] objects]
+                     [[[:x o] ox] [[:y o] oy] 
+                      [[:at-goal? o] (if goal false true)] 
+                      [[:object-at ox oy] o]])))
+      (fn [s]
+        (filter #(and % (env/applicable? % s))
+                (apply concat
+                       [(make-park s) (make-unpark s)]
+                       (for [dir dirs]
+                         [(make-base-dir s dir) (make-gripper-dir s dir)
+                          (make-pickup-dir s dir) (make-putdown-dir s dir)]))))
+      (into {[:parked?] true [:gxo] 0 [:gyo] 0} 
+            (for [[o _ goal] objects :when goal] [[:at-goal? o] true]))))))
 
 (defn state-map [s]
   (let [const   (env/get-var s :const)
-        bx      (env/get-var s [:bx])
-        by      (env/get-var s [:by])
-        gxo     (env/get-var s [:gxo])
-        gyo     (env/get-var s [:gyo])]
+        base    (env/get-var s [:base])
+        go      (env/get-var s [:gripper-offset])]
     (reduce (partial apply assoc-in)
             (get const [:map])
             (concat 
-             [[[(+ bx gxo) (+ by gyo)] \G]
-              [[bx by]                 \B]]
+             [[(add-pos base go) \G]
+              [base              \B]]
              (for [[o] (get const [:objects])]
-               [[(env/get-var s [:x o]) (env/get-var s [:y o])] (first (name o))])))))
+               [(env/get-var s [:pos o]) (first (name o))])))))
 
 (defn state-str [s]
   (apply str (util/str-join "\n" (map (partial apply str) (state-map s))) "\n"

; pickup-l/r/d/u
; drop-l/r/d/u

(def dirs [[:l -1 0] [:r 1 0] [:u 0 -1] [:d 0 1]])

; Note, unlike previous domains, here still require precondition verification.


(defn print-state [s] (print (state-str s)))



(defn reach-context [s]
  (into #{[:bx] [:by] [:gxo] [:gyo]}
        (let [bx (env/get-var s [:bx])
              by (env/get-var s [:by])]
          (for [[xo yo] (util/safe-get (env/get-var s :const) [:legal-go])]
            [:object-at (+ bx xo) (+ by yo)]))))

(deftype ReachHLA [env dx dy] :as this
  env/Action                (action-name [] [:reach dx dy])
                            (primitive? [] false)
  env/ContextualAction      (precondition-context [s] (reach-context s))
  hierarchy/HighLevelAction (immediate-refinements- [s]
                             (if (and (= dx (env/get-var s [:bx])) 
                                      (= dy (env/get-var s [:by])))
                               [[]]
                               (for [dir dirs 
                                     :let [a (make-gripper-dir s dir)]
                                     :when (and a (env/applicable? a s))]
                                 [a this])))
                            (cycle-level- [s] 1))
; manhattan, SLD?  range?

(deftype GraspHLA [env o] :as this
  env/Action                (action-name [] [:grasp o])
                            (primitive? [] false)
  env/ContextualAction      (precondition-context [s] 
                              (conj (reach-context s) [:x o] [:y o] [:at-goal? o] [:holding]))
  hierarchy/HighLevelAction (immediate-refinements- [s]
                             (let [bx (env/get-var s [:bx]), by (env/get-var s [:by])
                                   ox (env/get-var s [:x o]), oy (env/get-var s [:y o])]
                               (for [[dirname dx dy] dirs]
                                 [(ReachHLA env (- ox dx) (- oy dy))
                                  (make-pickup dirname bx by (- ox dx) (- oy dy) o ox oy)])))
                            (cycle-level- [s] nil))
;manhattan
;pess: SLD?  


(deftype DropAtHLA [env o dx dy] :as this
  env/Action                (action-name [] [:dropat o dx dy])
                            (primitive? [] false)
  env/ContextualAction      (precondition-context [s] 
                              (conj (reach-context s) [:holding]))
  hierarchy/HighLevelAction (immediate-refinements- [s]
                             (let [bx (env/get-var s [:bx]), by (env/get-var s [:by])
                                   ox (env/get-var s [:x o]), oy (env/get-var s [:y o])]
                               (for [[dirname dx dy] dirs]
                                 [(ReachHLA env (- ox dx) (- oy dy))
                                  (make-pickup dirname bx by (- ox dx) (- oy dy) o ox oy)])))
                            (cycle-level- [s] nil))
;manhattan
;pess: SLD?  







;; TODO: should nav be done by external alg, (fairest between angelic/not) or explicit? 
(deftype NavHLA [env dx dy] :as this
  env/Action                (action-name [] [:nav dx dy])
                            (primitive? [] false)
  env/ContextualAction      (precondition-context [s] #{[:bx] [:by]})
  hierarchy/HighLevelAction (immediate-refinements- [s]
                             (if (and (= dx (env/get-var s [:bx])) 
                                      (= dy (env/get-var s [:by])))
                               [[]]
                               (for [dir dirs 
                                     :let [a (make-base-dir s dir)]
                                     :when (and a (env/applicable? a s))]
                                 [a this])))
                            (cycle-level- [s] 1))
; manhattan
; pess? SLD? (try direct line, if occ then -inf).

(deftype MoveBaseHLA [env dx dy] :as this
  env/Action                (action-name [] [:movebase dx dy])
                            (primitive? [] false)
  env/ContextualAction      (precondition-context [s] #{[:bx] [:by] [:parked?] [:gxo] [:gyo]})
  hierarchy/HighLevelAction (immediate-refinements- [s]
                             (let [bx (env/get-var s [:bx]), by (env/get-var s [:by])]
                               (if (and (= dx bx) (= dy by))
                                  [[]]
                                 [[(ReachHLA env bx by) (make-unpark s) 
                                   (NavHLA dx dy) (make-park s)]])))
                            (cycle-level- [s] nil))
; manhattan + park (if appl)
; pess? SLD?




; DropFrom
; Drop
; GoGrasp
; GoDropAt
; GoDrop
; MoveToGoal
; Act

;; Do we sample base, drop positions, or enumerate ? 




; (print-state (initial-state (make-2d-manipulation-env [10 10] [1 1] [ [ [4 4] [6 6] ] ] [ [:a [5 5] [ [4 4] [4 4] ] ] ] 2))) 