Bugfixes + enhanced waypoints