Added extra logic to improve wall avoidance, some improvement achieved