Improved pathfinding, added path correction
Debugged pathfinding some more
Improved pathfinding algorithm