Cleaned PathFinding code