Large refactor (added A* search of map)