Implemented waypoint counting hack, maybe works