Debugged and cleanup up Pathfinding