Refactored code developed at chalmers
More refactoring