Major refactoring of code into smaller parts. Also some issue fixes. 

@binnisb, would you mind review this and comment on further improvements before merging?