+-- searchCompressed :: CompressedMap -> Position -> [[Position]] -> [[Position]] -> [[Position]]
+-- -- searchCompressed _ _ _ (c:_) _ | DT.trace (show c) False = undefined
+-- searchCompressed _ _ found [] = found
+-- searchCompressed map goal found (current:agenda)
+-- | head current == goal = searchCompressed map goal (current:found) agenda
+-- | otherwise = searchCompressed map goal found (nextPositions ++ agenda)
+-- where neighbours0 = map M.! (head current)
+-- neighbours = neighbours0 ^.. folded . filtered ((`notElem` current) . _nextPos)
+-- nextPositions = fmap ((: current) . _nextPos) neighbours
+
+searchCompressed :: Mountain -> Int
+searchCompressed mountain = maybe 0 _trailCost result
+ where result = runReader searchMountain mountain
+
+searchMountain :: MountainContext (Maybe Agendum)
+searchMountain =
+ do agenda <- initAgenda
+ aStar agenda Nothing
+
+initAgenda :: MountainContext Agenda
+initAgenda =
+ do s <- asks _start
+ c <- estimateCost Q.Empty s
+ let agendum = Agendum { _current = s, _trail = Q.empty, _trailCost = 0, _cost = c}
+ let agenda = P.singleton c agendum
+ return agenda
+
+aStar :: Agenda -> (Maybe Agendum) -> MountainContext (Maybe Agendum)
+aStar agenda best
+ -- | trace ("Peeping " ++ (show $ fst $ P.findMin agenda) ++ ": " ++ (show reached) ++ " <- " ++ (show $ toList $ Q.take 1 $ _trail $ currentAgendum) ++ " :: " ++ (show newAgenda)) False = undefined
+ -- | DT.trace ("Peeping " ++ (show $ _current $ snd $ P.findMax agenda) ) False = undefined
+ -- | DT.trace ("Peeping " ++ (show $ snd $ P.findMax agenda) ) False = undefined
+ -- | DT.trace ("Peeping " ++ (show agenda) ) False = undefined
+ | P.null agenda = return best
+ | (fst $ P.findMax agenda) < maybe 0 _trailCost best = return best
+ | otherwise =
+ do let (_, currentAgendum) = P.findMax agenda
+ let reached = currentAgendum ^. current
+ nexts <- candidates currentAgendum
+ let newAgenda = foldl' (\q a -> P.insert (_cost a) a q) (P.deleteMax agenda) nexts
+ reachedGoal <- isGoal reached
+ let best' = updateBest reachedGoal best currentAgendum
+ -- let closed' = M.insert reached (currentAgendum ^. trailCost) closed
+ if reachedGoal -- || (reached `S.member` closed)
+ then aStar (P.deleteMax agenda) best' -- closed'
+ else aStar newAgenda best' -- closed'
+
+updateBest :: Bool -> Maybe Agendum -> Agendum -> Maybe Agendum
+updateBest False current _ = current
+updateBest True Nothing best
+ -- | DT.trace ("Nothing " ++ show best) False = undefined
+ | otherwise = Just best
+updateBest True (Just current) best
+ -- | DT.trace (show current ++ " " ++ show best) False = undefined
+ | (current ^. trailCost) > (best ^. trailCost) = Just current
+ | otherwise = Just best
+
+
+
+
+candidates :: Agendum -> MountainContext (Q.Seq Agendum)
+candidates agendum =
+ do let here = agendum ^. current
+ let previous = agendum ^. trail
+ let prevCost = agendum ^. trailCost
+ ts <- asks _paths
+ let succs = Q.fromList $ ts M.! here
+ -- succs <- successors candidate
+ let nonloops = Q.filter (\s -> (s ^. nextPos) `notElem` previous) succs
+ mapM (makeAgendum previous prevCost here) nonloops
+
+
+makeAgendum :: (Q.Seq Position) -> Int -> Position -> CompressedPath -> MountainContext Agendum
+makeAgendum previous prevCost here step =
+ do let newTrail = previous :|> here
+ predicted <- estimateCost newTrail $ step ^. nextPos
+ -- ts <- asks _trails
+ let incurred = prevCost + step ^. pathLen
+ return Agendum { _current = step ^. nextPos
+ , _trail = newTrail
+ , _trailCost = incurred
+ , _cost = incurred + predicted
+ }
+
+
+isGoal :: Position -> MountainContext Bool
+isGoal here =
+ do goal <- asks _goal
+ return $ here == goal
+
+estimateCost :: Q.Seq Position -> Position -> MountainContext Int
+estimateCost r e =
+ do ts <- asks _paths
+ let endCost = fromMaybe 0 $ maximumOf (folded . filtered ((`notElem` r) . _nextPos) . pathLen) $ ts M.! e
+ let res = S.fromList $ toList (r :|> e)
+ let otherPaths = concat $ M.elems $ ts `M.withoutKeys` res
+ let restCost = sumOf (folded . filtered ((`notElem` r) . _nextPos) . pathLen) otherPaths
+ return $ (restCost `div` 2) + endCost