removed unneeded WP_Distance check
remove unneeded return for Navigate
removed unneeded rate_D calcs
removed unused functions