Improved pathfinding, added path correction