 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 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."

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;   Helpers       ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defn assoc-safe [m pred k v]
  (if (contains? m k) 
      (do (assert (pred (get m k) v)) m)
    (assoc m k v)))

(defn spos [s]  (try  (map #(env/get-var s %) '[[atx] [aty]]) (catch Exception e)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Data Structures ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

                   (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)]                    
          (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)))])