                             (cycle-level- [s] 2)
  env/AngelicAction         (optimistic-map [s]
                              {(env/set-vars s (env/make-finish-goal-state env))
-                              (let [objects (remaining-objects s)
-                                    object-rewards (for [o objects] [o (object-reward s o)])]
-                                (util/maximum-matching (cons :start nodes) (cons :end nodes)
-                                  (concat 
-                                   (for [[o or] object-rewards]
-                                     [:start o (+ or (start-reward s o))])
-                                   (for [o1 objects, [o2 or] object-rewards]
-                                     [n1 n2 (+ or (linkage-reward s o1 o2))])
-                                   (for [o objects] [o :finish (finish-reward s o)]))))})
+                              (let [objects (remaining-objects s), 
+                                    s1 (set (cons :start objects)), 
+                                    s2 (set (cons :finish objects))]
+                                (util/maximum-matching s1 s2 
+                                  (concat (for [o objects] [:start o (start-reward s o)])
+                                          (filter (fn [[n1 n2]] (and (s1 n1) (s2 n2))) match-edges))))})
                            (pessimistic-map [s] {}))))
 
 (defn make-2d-manipulation-hierarchy [env]

      (and (can-directly-nav-dir? free-fn l1 [x2 y1] [-1 0])
           (can-directly-nav-dir? free-fn [x2 y1] l2 [0 (util/signum (- y2 y1))]))))

(defn can-reach-from? [s base dst-go]
  (let [const (env/get-var s :const)]
    (and (contains? (const [:legal-go]) dst-go)
         (contains? (const [:freespace]) (add-pos dst-go base)))))

(defn can-reach? [s dst-go] (can-reach-from? s (env/get-var s [:base]) dst-go))

(defn move-gripper-reward [src dst] (* move-gripper-step-reward (manhattan-distance src dst)))


;                                  {}))



;; TODO: check out state abstraction here
;; Note: since we may not sample current position, have to special case.

(defn possible-grasp-base-pos 
  ([s grasp-pos] 
     (possible-grasp-base-pos (env/get-var s :const) (env/get-var s [:base]) grasp-pos))
  ([const base grasp-pos]
     (let [free  (get const [:freespace])
           candidates (->> (get const [:base-offsets])
                           (map #(add-pos % grasp-pos))
                           (filter free))]
       (if (some #{base} candidates)
         (cons base (remove #{base} (take (get const [:n-base-samples]) candidates)))
         (take (get const [:n-base-samples]) candidates)))))

;(defn possible-putdown-base-pos [const init-base init-loc drop-loc]
;  (let [free  (get const [:freespace])
;        pickup-candidates (--> (get const [:base-offsets])
;                               (map #(add-pos % init-loc))
;                               (filter free))
;        selected-pickups (util/cons-when (when (some #{init-base} pickup-candidates) init-base)
;                                         (take (get const [:n-base-samples]) candidates))
;        candidates (->> (get const [:base-offsets])
;                        (map #(add-pos % grasp-pos))
;                        (filter free))
;        candidate-set (set candidates)]
;    (distinct (concat (filter candidate-set selected-pickups) (take (get const [:n-base-samples]) candidates)))))


;; TODO: pess
