 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;    Top-level    ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 
+(defn pp [q xs]
+  ;(println "adding" (count xs)) 
+  (queues/pq-add-all! q xs))
 
 (defn sahucs-inverted [henv]
   (let [e     (hierarchy/env henv)

    (.add hs init-parent-entry)
    (SANode context a (atom init-result-map) (atom [[init-parent-entry ip-reward]]) hs)))

(deftype GQEntry [effects reward-to-state sanode remaining-parents-atom] :as this
    Object
    (equals [y] (and (= effects (:effects y)) (identical? sanode (:sanode y))))
    (hashCode [] (unchecked-add (int (hash effects))
                                (unchecked-multiply (int 13) (System/identityHashCode sanode)))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


(defn get-sa-node [#^HashMap cache a parent-entry pre-reward]
  "Create a new sa-node, or returned the cached copy if it exists."
  (let [s       (:state parent-entry)
        context (env/precondition-context a s)
        cache-key [(env/action-name a) (env/extract-context s context)]
        cache-val (.get cache cache-key)]
;    (println "get-sa" (env/action-name a) (when cache-val "t") (when (and cache-val (.contains (:parent-set cache-val) parent-entry)) "t") pre-reward (:reward-to-state parent-entry))
    (when cache-val (assert (<= pre-reward (second (last @(:parent-vec-atom cache-val))))))
    (cond (and cache-val (.contains (:parent-set cache-val) parent-entry))
          []  
          cache-val
          (do (swap! (:parent-vec-atom cache-val) conj [parent-entry pre-reward])
              (.add  (:parent-set cache-val) parent-entry)
;              (println "REHIT" (env/action-name (:action cache-val)) (count @(:result-map-atom cache-val)))              
              (for [[e sr] @(:result-map-atom cache-val)]
                [(GQEntry e sr cache-val (atom [[parent-entry pre-reward]]))
                 (- 0 pre-reward sr)]))
          :else 
          (let [s      (env/get-logger s)]
            (if (env/primitive? a)
              (if (env/applicable? a s)
                (let [[ss sr] (env/successor a (env/get-logger s ))
                      e       (env/extract-effects ss context)
                      nd      (make-sa-node context a {e sr} parent-entry pre-reward)]
;                  (println "app")
                  (.put cache cache-key nd)
                  [[(GQEntry e sr nd (atom :fresh))
                    (- 0 pre-reward sr)]])
                (do ;(println "NA")
                    (.put cache cache-key (make-sa-node context a {} parent-entry pre-reward))
                    nil))
              (let [nd (make-sa-node context a {} parent-entry pre-reward)]
                (.put cache cache-key nd)
                (doall 
                 (mapcat 
                  (fn [ref] 
                    (if (empty? ref)
                      [[(GQEntry (env/extract-effects s context) 0 nd (atom :fresh)) (- pre-reward)]]
                      (get-sa-node cache (first ref) (ParentEntry s 0 (next ref) nd) pre-reward)))
                  (hierarchy/immediate-refinements a s)))))))))

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