Initial buggy implementation of waypoint AI