diff --git a/src/bin/routefinder.rs b/src/bin/routefinder.rs
index 6acf050..b1bce2e 100644
--- a/src/bin/routefinder.rs
+++ b/src/bin/routefinder.rs
@@ -120,7 +120,8 @@ impl<Q: SearchQueue<SearchNode>> SearchEnv<Q> {
     fn search(&mut self) -> Option<f32> {
         self.reset();
         eprintln!("");
-        while let Some(node) = self.queue.q_next() {
+        while !self.is_done() {
+            let node = self.queue.q_next().unwrap();
             // check whether node should be re-expanded
             {
                 let scoord = self.meta[node.last.0].sys_data.coords;
@@ -268,16 +269,37 @@ impl<T> Default for DFS<T> {
     fn default() -> Self { DFS(Vec::new()) }
 }
 
-struct AStar<T>(BinaryHeap<Reverse<T>>);
-impl<T: Ord> SearchQueue<T> for AStar<T> {
-    fn q_next(&mut self) -> Option<T> { self.0.pop().map(|x| x.0) }
-    fn q_empty(&self) -> bool { self.0.is_empty() }
-    fn q_push(&mut self, item: T) { self.0.push(Reverse(item)); }
-    fn q_clear(&mut self) { self.0.clear(); }
-    fn q_len(&self) -> usize { self.0.len() }
+struct AStar<T: SearchState>{
+    q: BinaryHeap<Reverse<T>>,
+    seen: HashMap<T::Id, f32>,
 }
-impl<T: Ord> Default for AStar<T> {
-    fn default() -> Self { AStar(BinaryHeap::new()) }
+impl<T: Ord + SearchState> SearchQueue<T> for AStar<T> where T::Id: Eq + Hash {
+    fn q_next(&mut self) -> Option<T> { self.q.pop().map(|x| x.0) }
+    fn q_empty(&self) -> bool { self.q.is_empty() }
+    fn q_push(&mut self, item: T) {
+        let id = item.id();
+        if let Some(odist) = self.seen.get(&id) {
+            if *odist < item.cost() {
+                return;
+            }
+        }
+        self.seen.insert(id, item.cost());
+        self.q.push(Reverse(item));
+    }
+    fn q_clear(&mut self) { 
+        self.q.clear(); 
+        self.seen.clear();
+    }
+    fn q_len(&self) -> usize { self.q.len() }
+}
+impl<T: Ord+SearchState> Default for AStar<T> {
+    fn default() -> Self {
+        Self {
+            q: BinaryHeap::new(),
+            seen: HashMap::new(),
+        }
+    }
+
 }
 
 
@@ -327,7 +349,7 @@ fn main() -> anyhow::Result<()> {
         base_jump: opts.jump_range,
         init_system,
         target_system,
-        queue: BFS1::default(),
+        queue: AStar::default(),
         jumponium_cost_factor: opts.cost_factor,
         visits: 0,
     };