Refactored substition code and several added unit tests.