findPath :: (Ord a, Ord n, Eq a, Num n) => Pos a -> Pos a -> g -> Cost g a n -> Heuristic a n -> Neighbours g a -> Maybe (Path a)
findPath start end grid cost heuristic neighbours = findPath' (MinQueue.singleton (Node 0 0 start)) (Map.singleton start (start, 0))
  where
    findPath' open closed = do
        minNode@(Node f g pos) <- MinQueue.getMin open
        let neighbourNodes = filterNeighbours pos f closed
            closed' = foldr (\p c -> Map.insert p (pos, f + cost pos p grid) c) closed neighbourNodes
            open' = foldr (\p c -> MinQueue.insert (Node (f + cost pos p grid) (heuristic pos p) p) c) (MinQueue.deleteMin open) neighbourNodes
        if pos == end then
          return $ flattenPath closed' [end]
        else
          findPath' open' closed' -- Keep going till we find a solution.

    filterNeighbours pos f closed = filter filterFunc $ neighbours pos grid
      where
        filterFunc nPos = case Map.lookup nPos closed of
                            Nothing -> True
                            Just (cameFrom, score) -> f + cost pos nPos grid < score

    flattenPath closed path@(p:ps) = case Map.lookup p closed of
                                      Nothing -> []
                                      Just (cameFrom, _) -> if cameFrom == start then
                                                              cameFrom : path
                                                            else
                                                              flattenPath closed (cameFrom : path)