Fixed doxygen
Created a closed stack, sort of. Also heuristics
New FILO queue and dijkstra/A* improvements