-initAgenda :: Position -> CityContext Agenda
-initAgenda pos =
- do c <- estimateCost pos
- -- return $ P.singleton c Agendum { _current = pos, _trail = Q.empty, _trailCost = 0, _cost = c}
- return $ P.singleton c Agendum { _current = Q.singleton pos, _trail = Q.singleton pos, _trailCost = 0, _cost = c}
-
-aStar :: Agenda -> ExploredStates -> CityContext (Maybe Agendum)
-aStar agenda closed
- -- | trace ("Peeping " ++ (show $ fst $ P.findMin agenda) ++ ": " ++ (show reached) ++ " <- " ++ (show $ toList $ Q.take 1 $ _trail $ currentAgendum) ++ " :: " ++ (show newAgenda)) False = undefined
- -- | trace ("Peeping " ++ (show $ _current $ snd $ P.findMin agenda) ) False = undefined
- -- | trace ("Peeping " ++ (show $ snd $ P.findMin agenda) ) False = undefined
- -- | trace ("Peeping " ++ (show agenda) ) False = undefined
- | P.null agenda = return Nothing
- | otherwise =
- do let (_, currentAgendum) = P.findMin agenda
- let reached = currentAgendum ^. current
- nexts <- candidates currentAgendum closed
- let newAgenda = foldl' (\q a -> P.insert (_cost a) a q) (P.deleteMin agenda) nexts
- reachedGoal <- isGoal reached
- if reachedGoal
- then return (Just currentAgendum)
- else if reached `S.member` closed
- then aStar (P.deleteMin agenda) closed
- else aStar newAgenda (S.insert reached closed)
-
-candidates :: Agendum -> ExploredStates -> CityContext (Q.Seq Agendum)
-candidates agendum closed =
+ initAgenda :: Position -> CityContext (Agenda a)
+ initAgenda pos =
+ do c <- estimateCost pos
+ let dAgendum = Agendum { _current = (D, pos), _trail = Q.empty, _trailCost = 0, _cost = c}
+ dNexts <- candidates dAgendum S.empty
+ let rAgendum = Agendum { _current = (R, pos), _trail = Q.empty, _trailCost = 0, _cost = c}
+ rNexts <- candidates rAgendum S.empty
+ let nexts = dNexts >< rNexts
+ let agenda = foldl' (\q a -> P.insert (_cost a) a q) P.empty nexts
+ return agenda
+
+ aStar :: (Agenda a) -> ExploredStates -> CityContext (Maybe (Agendum a))
+ aStar agenda closed
+ -- | trace ("Peeping " ++ (show $ fst $ P.findMin agenda) ++ ": " ++ (show reached) ++ " <- " ++ (show $ toList $ Q.take 1 $ _trail $ currentAgendum) ++ " :: " ++ (show newAgenda)) False = undefined
+ -- | trace ("Peeping " ++ (show $ _current $ snd $ P.findMin agenda) ) False = undefined
+ -- | trace ("Peeping " ++ (show $ snd $ P.findMin agenda) ) False = undefined
+ -- | trace ("Peeping " ++ (show agenda) ) False = undefined
+ | P.null agenda = return Nothing
+ | otherwise =
+ do let (_, currentAgendum) = P.findMin agenda
+ let reached = currentAgendum ^. current
+ nexts <- candidates currentAgendum closed
+ let newAgenda = foldl' (\q a -> P.insert (_cost a) a q) (P.deleteMin agenda) nexts
+ reachedGoal <- isGoal reached
+ if reachedGoal
+ then return (Just currentAgendum)
+ else if reached `S.member` closed
+ then aStar (P.deleteMin agenda) closed
+ else aStar newAgenda (S.insert reached closed)
+
+ candidates :: (Agendum a) -> ExploredStates -> CityContext (Q.Seq (Agendum a))
+ candidates agendum closed =