Working waypoint stuff
Might work better