Temp
Initial waypoint implementation, passes basic test