diff --git a/include/astar/heapset.h b/include/astar/heapset.h
index d126f55..a99a29a 100644
--- a/include/astar/heapset.h
+++ b/include/astar/heapset.h
@@ -5,86 +5,109 @@
 #include <set>
 #include <vector>
 
-template<class T, class SC = std::less<T>, class VC = std::less<T>>
-class HeapSet {
+enum class CloseOnPop : bool { FALSE, TRUE };
+
+template<class T>
+class Closeable {
+    public:
+        Closeable(const T& t, bool c = false) : t_(t), closed_(c) {}
+        operator T&() { return t_; }
+        operator const T&() const { return t_; }
+        bool operator!() const { return closed_; }
+    private:
+        T t_; bool closed_;
+};
+
+template<class T>
+class Wrapper {
+    public:
+        Wrapper(const T&, bool = false) {}
+        operator T&() { return t_; }
+        operator const T&() const { return t_; }
+        bool operator!() const { return false; }
+    private:
+        T t_;
+};
+
+template<class T, CloseOnPop COP> struct SetT {
+    using type = Wrapper<T>;
+};
+template<class T> struct SetT<T,CloseOnPop::TRUE> {
+    using type = Closeable<T>;
+};
+
+template<class T,
+    class SC = std::less<T>, class VC = std::less<T>,
+    CloseOnPop COP = CloseOnPop::FALSE>
+class CachingHeapSet {
     public:
         bool empty() const;
         const T& top() const;
         T pop();
         void push(const T& val);
-        void push(T&& val);
 
     private:
-        std::set<T,SC> states_;
+        using SetType = typename SetT<T,COP>::type;
+
+        std::set<SetType,SC> states_;
         std::priority_queue<T,std::vector<T>,VC> heap_;
         // returns true if second arg is higher priority than the first
         VC higher_priority_;
-        T pop_heap();
+        T pop_heap(CloseOnPop);
         void update_heap(const T& old, const T& updated);
 };
 
-template<class T, class SC, class VC>
-bool HeapSet<T,SC,VC>::empty() const {
+template<class T, class SC, class VC, CloseOnPop COP>
+bool CachingHeapSet<T,SC,VC,COP>::empty() const {
     return states_.empty();
 }
 
-template<class T, class SC, class VC>
-const T& HeapSet<T,SC,VC>::top() const {
+template<class T, class SC, class VC, CloseOnPop COP>
+const T& CachingHeapSet<T,SC,VC,COP>::top() const {
     return heap_.top();
 }
 
-template<class T, class SC, class VC>
-T HeapSet<T,SC,VC>::pop() {
-    T t = pop_heap();
-    states_.erase(t);
-    return std::move(t);
+template<class T, class SC, class VC, CloseOnPop COP>
+T CachingHeapSet<T,SC,VC,COP>::pop() {
+    return pop_heap(COP);
 }
 
-template<class T, class SC, class VC>
-void HeapSet<T,SC,VC>::push(const T& t) {
-    auto it_inserted = states_.insert(t);
+template<class T, class SC, class VC, CloseOnPop COP>
+void CachingHeapSet<T,SC,VC,COP>::push(const T& t) {
+    auto it_inserted = states_.insert(SetType{t});
+    bool inserted = it_inserted.second;
+    auto& tt = *it_inserted.first;
 
-    if (it_inserted.second) {
-        heap_.push(t);
+    if (inserted) {
+        heap_.push(tt);
     } else {
-        auto old = *it_inserted.first;
-        if (higher_priority_(old, t)) {
-            update_heap(old, t);
+        if (!tt && higher_priority_(tt, t)) {
+            update_heap(tt, t);
             auto it = states_.erase(it_inserted.first);
             states_.insert(it, t);
         }
     }
 }
 
-template<class T, class SC, class VC>
-void HeapSet<T,SC,VC>::push(T&& t) {
-    auto it_inserted = states_.insert(t);
-
-    if (it_inserted.second) {
-        heap_.push(std::forward<T>(t));
-    } else {
-        auto old = *it_inserted.first;
-        if (higher_priority_(old, t)) {
-            update_heap(old, t);
-            auto it = states_.erase(it_inserted.first);
-            states_.insert(it, std::forward<T>(t));
-        }
-    }
-}
-
-template<class T, class SC, class VC>
-T HeapSet<T,SC,VC>::pop_heap() {
+template<class T, class SC, class VC, CloseOnPop COP>
+T CachingHeapSet<T,SC,VC,COP>::pop_heap(CloseOnPop cop) {
     T t = heap_.top();
+    if (COP == CloseOnPop::TRUE && cop == CloseOnPop::TRUE) {
+        SetType val{t, true};
+        auto it = states_.find(val);
+        it = states_.erase(it);
+        states_.insert(it, val);
+    }
     heap_.pop();
     return std::move(t);
 }
 
-template<class T, class SC, class VC>
-void HeapSet<T,SC,VC>::update_heap(const T& old, const T& updated) {
+template<class T, class SC, class VC, CloseOnPop COP>
+void CachingHeapSet<T,SC,VC,COP>::update_heap(const T& old, const T& updated) {
     std::deque<T> queue;
 
     do {
-        queue.push_back(pop_heap());
+        queue.push_back(pop_heap(CloseOnPop::FALSE));
     } while (queue.back() != old);
 
     queue.back() = updated;
diff --git a/include/astar/solver.h b/include/astar/solver.h
index 81ea67e..77e8b2f 100644
--- a/include/astar/solver.h
+++ b/include/astar/solver.h
@@ -68,8 +68,7 @@ class AStarSolver
         Distance distance_func_;
         Estimator cost_func_;
         SNode last_;
-        std::set<SNode,ByState> closed_set_;
-        HeapSet<SNode,ByState,ByCost> open_set_;
+        CachingHeapSet<SNode,ByState,ByCost,CloseOnPop::TRUE> states_;
 };
 
 template<class T, class G, class H>
@@ -85,7 +84,7 @@ template<class T, class G, class H>
 AStarSolver<T,G,H>::AStarSolver(const T& s, const T& g, const Generator& gen, const Distance& d, const Estimator& c) :
     goal_(g), generator_func_(gen), distance_func_(d), cost_func_(c)
 {
-    open_set_.push(make_snode(*this,s));
+    states_.push(make_snode(*this,s));
 }
 
 template<class T, class G, class H>
@@ -115,10 +114,8 @@ void AStarSolver<T,G,H>::print_solution(std::ostream& o) const
 template<class T, class G, class H>
 bool AStarSolver<T,G,H>::solve()
 {
-    while (!open_set_.empty()) {
-        auto it_inserted = closed_set_.insert(open_set_.pop());
-
-        auto snode = *it_inserted.first;
+    while (!states_.empty()) {
+        auto snode = states_.pop();
 
         if (snode->state_ == goal_) {
             last_ = snode;
@@ -126,10 +123,7 @@ bool AStarSolver<T,G,H>::solve()
         }
 
         for (auto& n : generator_func_(snode->state_)) {
-            auto new_node = make_snode(*this, n, snode);
-            if (closed_set_.find(new_node) == end(closed_set_)) {
-                open_set_.push(std::move(new_node));
-            }
+            states_.push(make_snode(*this, n, snode));
         }
     }