diff --git a/monad-dijkstra.cabal b/monad-dijkstra.cabal
--- a/monad-dijkstra.cabal
+++ b/monad-dijkstra.cabal
@@ -1,7 +1,7 @@
 name:                monad-dijkstra
-version:             0.1.1.1
-synopsis:            Monad transformer for weighted graph searches using Dijkstra's or A* algorithm
-description:         Please see README.md
+version:             0.1.1.2
+synopsis:            A monad transformer for weighted graph searches
+description:         A monad transformer for weighted graph searches using Dijkstra's or A* algorithm.
 homepage:            https://github.com/ennocramer/monad-dijkstra
 license:             BSD3
 license-file:        LICENSE
@@ -16,6 +16,7 @@
   hs-source-dirs:      src
   exposed-modules:     Control.Monad.Search
   build-depends:       base >= 4.7 && < 5
+                     , containers >= 0.5.6.2 && < 0.6.1
                      , transformers >= 0.4.2.0 && < 0.5.6
                      , mtl >= 2.2.0 && < 2.3
                      , free >= 4.12.0 && < 5.2
diff --git a/src/Control/Monad/Search.hs b/src/Control/Monad/Search.hs
--- a/src/Control/Monad/Search.hs
+++ b/src/Control/Monad/Search.hs
@@ -61,34 +61,64 @@
     , winner
     ) where
 
-import           Control.Applicative         ( Alternative(..) )
-import           Control.Monad               ( MonadPlus(..) )
-import           Control.Monad.Trans.Free    ( FreeF(Free, Pure), FreeT
-                                             , runFreeT, wrap )
-import           Control.Monad.Trans.State   ( evalStateT, gets, modify )
+import           Control.Applicative             ( Alternative(..) )
+import           Control.Monad                   ( MonadPlus(..) )
 
-import           Control.Monad.Trans.Class   ( MonadTrans, lift )
-import           Control.Monad.IO.Class      ( MonadIO )
-import           Control.Monad.Reader        ( MonadReader, ReaderT(..)
-                                             , runReaderT )
-import qualified Control.Monad.Writer.Lazy   as Lazy ( MonadWriter, WriterT(..)
-                                                     , runWriterT )
-import qualified Control.Monad.Writer.Strict as Strict ( WriterT(..)
-                                                       , runWriterT )
-import qualified Control.Monad.State.Lazy    as Lazy ( MonadState, StateT(..)
-                                                     , runStateT )
-import qualified Control.Monad.State.Strict  as Strict ( StateT(..), runStateT )
-import qualified Control.Monad.RWS.Lazy      as Lazy ( MonadRWS, RWST(..)
-                                                     , runRWST )
-import qualified Control.Monad.RWS.Strict    as Strict ( RWST(..), runRWST )
-import           Control.Monad.Except        ( ExceptT(..), MonadError
-                                             , runExceptT )
-import           Control.Monad.Cont          ( MonadCont )
-import           Data.Functor.Identity       ( Identity, runIdentity )
-import           Data.Maybe                  ( catMaybes, listToMaybe )
+import           Control.Monad.Cont              ( MonadCont )
+import           Control.Monad.Except            ( ExceptT(..), MonadError
+                                                 , runExceptT )
+import           Control.Monad.IO.Class          ( MonadIO )
 
-import qualified Data.OrdPSQ                 as PSQ
+import qualified Control.Monad.RWS.Lazy          as Lazy ( MonadRWS, RWST(..)
+                                                         , runRWST )
+import qualified Control.Monad.RWS.Strict        as Strict ( RWST(..), runRWST )
+import           Control.Monad.Reader            ( MonadReader, ReaderT(..)
+                                                 , runReaderT )
 
+import qualified Control.Monad.State.Lazy        as Lazy ( MonadState
+                                                         , StateT(..)
+                                                         , runStateT )
+import qualified Control.Monad.State.Strict      as Strict ( StateT(..)
+                                                           , runStateT )
+import           Control.Monad.Trans.Class       ( MonadTrans, lift )
+import           Control.Monad.Trans.Free        ( FreeF(Free, Pure), FreeT
+                                                 , runFreeT, wrap )
+import           Control.Monad.Trans.Free.Church ( FT, fromFT )
+import           Control.Monad.Trans.State       ( evalStateT, gets, modify )
+
+import qualified Control.Monad.Writer.Lazy       as Lazy ( MonadWriter
+                                                         , WriterT(..)
+                                                         , runWriterT )
+import qualified Control.Monad.Writer.Strict     as Strict ( WriterT(..)
+                                                           , runWriterT )
+
+import           Data.Functor.Identity           ( Identity, runIdentity )
+import qualified Data.IntPSQ                     as PSQ
+import qualified Data.IntMap.Strict              as Map
+import           Data.Maybe                      ( catMaybes, listToMaybe )
+import qualified Data.IntSet                     as Set
+
+newtype Scopemap = Scopemap { unScopemap :: Map.IntMap Set.IntSet }
+
+singleton  :: Int -> Int -> Scopemap
+singleton k = Scopemap . Map.singleton k . Set.singleton
+
+insert :: Int -> Int -> Scopemap -> Scopemap
+insert k v = Scopemap . Map.alter fn k . unScopemap
+  where
+    fn = Just . maybe (Set.singleton v) (Set.insert v)
+
+delete :: Int -> Int -> Scopemap -> Scopemap
+delete k v = Scopemap . Map.update fn k . unScopemap
+  where
+    fn = (\s -> if Set.null s then Nothing else Just s) . Set.delete v
+
+list :: Int -> Scopemap -> [Int]
+list k = maybe [] Set.toList . Map.lookup k . unScopemap
+
+listAll :: Scopemap -> [Int]
+listAll = Set.toList . foldr (Set.union . snd) Set.empty . Map.toList . unScopemap
+
 -- | The Search monad
 type Search c = SearchT c Identity
 
@@ -110,7 +140,7 @@
     deriving Functor
 
 -- | The SearchT monad transformer
-newtype SearchT c m a = SearchT { unSearchT :: FreeT (SearchF c) m a }
+newtype SearchT c m a = SearchT { unSearchT :: FT (SearchF c) m a }
     deriving (Functor, Applicative, Monad, MonadTrans, MonadIO, MonadReader r, Lazy.MonadWriter w, Lazy.MonadState s, MonadError e, MonadCont)
 
 instance (Ord c, Monoid c, Monad m) => Alternative (SearchT c m) where
@@ -128,9 +158,10 @@
                        }
 
 -- | State used during evaluation of SearchT
-data St c m a = St { stNum   :: !Int
-                   , stScope :: !Int
-                   , stQueue :: !(PSQ.OrdPSQ Int c (Cand c m a))
+data St c m a = St { stNum    :: !Int
+                   , stScope  :: !Int
+                   , stActive :: !Scopemap
+                   , stQueue  :: !(PSQ.IntPSQ c (Cand c m a))
                    }
 
 -- | Generate all solutions in order of increasing cost.
@@ -152,28 +183,34 @@
             Free Abandon -> return Nothing
             Free (Cost c e p) ->
                 let newCost = candCost `mappend` c
-                    newPriority = newCost `mappend` e
+                    newPriority = max prio $ newCost `mappend` e
                 in do
-                    updateQueue $
-                        PSQ.insert num
-                                   newPriority
-                                   cand { candCost = newCost, candPath = p }
-                    return Nothing
+                    reschedule <- gets (maybe False
+                                              (\(_, x, _) -> x <= newPriority) .
+                                            PSQ.findMin . stQueue)
+                    let cand' = cand { candCost = newCost, candPath = p }
+                    if reschedule
+                        then do
+                            updateQueue $ PSQ.insert num newPriority cand'
+                            return Nothing
+                        else step num newPriority cand'
             Free (Alt lhs rhs) -> do
                 num' <- nextNum
+                addScopes candScope num'
                 updateQueue $ PSQ.insert num' prio cand { candPath = rhs }
                 step num prio cand { candPath = lhs }
             Free (Enter p) -> do
                 scope <- nextScope
+                addScope scope num
                 step num
                      prio
                      cand { candScope = scope : candScope, candPath = p }
-            Free (Exit p) ->
+            Free (Exit p) -> do
+                delScope (head candScope) num
                 step num prio cand { candScope = tail candScope, candPath = p }
             Free (Collapse p) -> do
-                updateQueue $ PSQ.fromList .
-                    filter (\(_, _, c) -> not $ hasScope (head candScope) c) .
-                        PSQ.toList
+                cs <- listScope $ listToMaybe candScope
+                updateQueue $ \q -> foldr PSQ.delete q cs
                 step num prio cand { candPath = p }
 
     nextNum = do
@@ -184,13 +221,21 @@
         modify $ \s -> s { stScope = stScope s + 1 }
         gets stScope
 
-    hasScope s Cand{..} = s `elem` candScope
+    addScope scope c = updateActive $ insert scope c
 
+    addScopes scopes c = updateActive $ \sm -> foldr (`insert` c) sm scopes
+
+    delScope scope c = updateActive $ delete scope c
+
+    listScope scope = gets $ maybe listAll list scope . stActive
+
     updateQueue f = modify $ \s -> s { stQueue = f (stQueue s) }
 
-    state = St 0 0 queue
+    updateActive f = modify $ \s -> s { stActive = f (stActive s) }
 
-    queue = PSQ.singleton 0 mempty (Cand mempty [ 0 ] (unSearchT m))
+    state = St 0 0 (singleton 0 0) queue
+
+    queue = PSQ.singleton 0 mempty (Cand mempty [ 0 ] (fromFT $ unSearchT m))
 
 -- | Generate only the best solutions.
 runSearchBestT :: (Ord c, Monoid c, Monad m) => SearchT c m a -> m (Maybe (c, a))
