fix waypoint logic, further refactor/code cleanup

turns out i was only half done.