simplify and fix code, move clearance check to is_blocked