Finish writing pathfinding... now to debug it.