Fixed bugs in wall following and utils