improve pathfinder algorithm to be more realistic

+ code cleanup