first steps to changing the current path to target
small change to recursivebug algorithm
new waypoint is chosen by full new path lenght, not only distance to target