Refractor code. Add test file (NOT FINISHED)