Refactored code, seems to work