Some Pathfinding stuff

The stalling crap is commented out