Revert "[APATH] handle cur/next waypoint"

This reverts commit 756285292b085b0289e545b9c68b530f77cb11f3.