Completed provisional pathfinding algorithm
Tweaked route finding
Removed debug print statements