Fixed up pathfinding, optimized