some optimizations, rework and partial implementation of waypoint navigation