PathFinding

Tidied code a little.