Block avoidance algorithm complete. Still needs filter for corner cases.