<SYSTEM_TASK:>
Solve the following problem using Python, implementing the functions described below, one line at a time
<END_TASK>
<USER_TASK:>
Description:

def cmd_snap_wp(self, args):
        '''snap waypoints to KML'''
        threshold = 10.0
        if len(args) > 0:
            threshold = float(args[0])
        wpmod = self.module('wp')
        wploader = wpmod.wploader
        changed = False
        for i in range(1,wploader.count()):
            w = wploader.wp(i)
            if not wploader.is_location_command(w.command):
                continue
            lat = w.x
            lon = w.y
            best = None
            best_dist = (threshold+1)*3
            for (snap_lat,snap_lon) in self.snap_points:
                dist = mp_util.gps_distance(lat, lon, snap_lat, snap_lon)
                if dist < best_dist:
                    best_dist = dist
                    best = (snap_lat, snap_lon)
            if best is not None and best_dist <= threshold:
                if w.x != best[0] or w.y != best[1]:
                    w.x = best[0]
                    w.y = best[1]
                    print("Snapping WP %u to %f %f" % (i, w.x, w.y))
                    wploader.set(w, i)
                    changed = True
            elif best is not None:
                if best_dist <= (threshold+1)*3:
                    print("Not snapping wp %u dist %.1f" % (i, best_dist))
        if changed:
            wpmod.send_all_waypoints()