Larger refactor of original source code, fixes #9