                    (ParentEntry (env/initial-state e) 0 nil nil) 0))
     (loop []
       (if (queues/pq-empty? queue) nil
-        (let [[best neg-rew] (queues/pq-remove-min-with-cost! queue)
-;              fresh?         (= :fresh @(:remaining-parents-atom best))
-;              rma            (:result-map-atom (:sanode best))
-;              old-rew        (get @rma best)
-              
-              ]          
-;            (assert (<= neg-rew 10))
- ;           (println neg-rew (queues/pq-size queue) (env/action-name (:action (:sanode best))) (spos (:state best)) (spos (:state (first (:parent-set (:sanode best))))) #_ (:state best))
-            (if (nil? (:sanode (first (:parent-set (:sanode best)))))
-                (- neg-rew)
-              (let [init-hash   (hash best)
-                    cpa-val     (:remaining-parents best)
-;                    old-results @(:result-map-atom (:sanode best))
-                    cur-parents (if (= :fresh cpa-val) 
-                                  (do (swap! (:result-map-atom (:sanode best)) assoc-safe >=
-                                             (:state best) (:reward-to-state best))
-                                      @(:parent-vec-atom (:sanode best))) 
-                                  cpa-val)
-                    best-rew    (- 0 neg-rew (:reward-to-state best))
-                    [good-parents bad-parents] (split-with #(= (second %) best-rew) cur-parents)]
-;                (println (map second bad-parentsc))
-                (util/assert-is (seq good-parents)  "%s" [(keyword? cpa-val) neg-rew (:reward-to-state best) best-rew (map second cur-parents) (env/action-name (:action (:sanode best))) (count cur-parents) (count (:parent-vec-atom (:sanode best)))])
-;                (reset! (:remaining-parents-atom best) bad-parents)
-;                (assert (not (= init-hash (hash best))))
-                (when (seq bad-parents)
-                  (let [nxt (GQEntry (:state best) (:reward-to-state best) (:sanode best) bad-parents)]
-;                  (when-not (nil? (queues/g-pq-priority queue best))
-;                    (let [on (queues/g-pq-key queue best)]
-;                      (dr/debug-repl)))
-                    (util/assert-is (nil? (queues/g-pq-priority queue nxt)) "%s" [(:reward-to-state best) (second (first bad-parents))  (count cur-parents) (count bad-parents) (map second cur-parents) (count @(:parent-vec-atom (:sanode best)))])
-                    (queues/pq-replace! queue nxt 
-                                        (- 0 (:reward-to-state best) (second (first bad-parents))))))
-                (doseq [[parent parent-reward] good-parents]
-                  (pp #_ queues/pq-add-all! queue
-                    (update-parent cache parent parent-reward 
-                                   (:state best) (:reward-to-state best) (:sanode best))))
-                (recur))))))))
+        (let [[best neg-rew] (queues/pq-remove-min-with-cost! queue)]                    
+          (if (nil? (:sanode (first (:parent-set (:sanode best))))) ; solution state
+              (- neg-rew)
+            (let [b-s  (:state best), b-rts (:reward-to-state best), b-sa (:sanode best)
+                  [b-gp b-bp] (split-with #(= (second %) (- 0 neg-rew b-rts))
+                                (if (= :fresh (:remaining-parents best))
+                                    (do (swap! (:result-map-atom b-sa) assoc-new b-s b-rts)
+                                        @(:parent-vec-atom b-sa))
+                                  (:remaining-parents best)))] 
+              (assert (seq b-gp))
+              (when (seq b-bp)
+                (queues/pq-replace! queue (GQEntry b-s b-rts b-sa b-bp) 
+                                    (- 0 b-rts (second (first b-bp)))))
+              (doseq [[p p-r] b-gp]
+                (queues/pq-add-all! queue (update-parent cache p p-r b-s b-rts b-sa)))
+              (recur))))))))
 
 
 
+;                (util/assert-is (seq good-parents)  "%s" [(keyword? cpa-val) neg-rew (:reward-to-state best) best-rew (map second cur-parents) (env/action-name (:action (:sanode best))) (count cur-parents) (count (:parent-vec-atom (:sanode best)))])
+
+;                    (util/assert-is (nil? (queues/g-pq-priority queue nxt)) "%s" [(:reward-to-state best) (second (first bad-parents))  (count cur-parents) (count bad-parents) (map second cur-parents) (count @(:parent-vec-atom (:sanode best)))])
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Core Algorithm  ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;; TODO: sort out rewards.

(defn get-sa-node [#^HashMap cache a parent-entry pre-reward]
  "Create a new sa-node, or returned the cached copy if it exists."

        context (env/precondition-context a s)
        cache-key [(env/action-name a) (env/extract-context s context)]
        cache-val (.get cache cache-key)]
    (when cache-val (assert (<= pre-reward (second (last @(:parent-vec-atom cache-val))))))
    (cond (and cache-val (.contains #^HashSet (:parent-set cache-val) parent-entry))
            []  
          cache-val
            (do (swap! (:parent-vec-atom cache-val) conj [parent-entry pre-reward])
                (.add  #^HashSet (:parent-set cache-val) parent-entry)
                (for [[ss sr] @(:result-map-atom cache-val)]
                  [(GQEntry ss sr cache-val [[parent-entry pre-reward]]) (- 0 pre-reward sr)]))
          :else 
            (let [s      (env/get-logger s)
                  nd     (make-sa-node context a parent-entry pre-reward)]
              (.put cache cache-key nd)
              (if (env/primitive? a)
                  (when (env/applicable? a s)
                    (let [[ss sr] (env/successor a (env/get-logger s ))]
                      [[(GQEntry ss sr nd :fresh) (- 0 pre-reward sr)]]))
                (apply concat
                  (for [ref (hierarchy/immediate-refinements a s)]
                    (if (empty? ref)
                        [[(GQEntry s 0 nd :fresh) (- pre-reward)]]
                      (get-sa-node cache (first ref) (ParentEntry s 0 (next ref) nd) pre-reward)))))))))

(defn update-parent [cache parent-entry parent-pre-reward new-state new-reward child-sanode]
  (let [new-effects (env/extract-effects new-state (:context child-sanode))
        final-state  (env/apply-effects (:state parent-entry) new-effects)
        actions      (:remaining-actions parent-entry)
        rts          (:reward-to-state parent-entry)
        ]
    (if (empty? actions)
       [[(GQEntry final-state (+ rts new-reward) (:sanode parent-entry) :fresh)
         (- 0 parent-pre-reward new-reward)]]
      (get-sa-node cache (first actions) 
        (ParentEntry final-state (+ rts new-reward) (next actions) (:sanode parent-entry))
        (+ parent-pre-reward new-reward)))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;    Top-level    ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;



(defn sahucs-inverted [henv]
  (let [e     (hierarchy/env henv)
