Added A* search logic; additional pathfinding / graph helper functionality