Refactored code. Needs testing/possible debugging.