Added in waypoints and functionality