Reorganizes source code
Finishes refactoring. Adds test. Fixes #2