First fully working (with waypoint + exclusion masks) implementation. Danny Kim approved.