Fixed assertion + refactored code making it more simple