Clean Up Initial Implementation

Also added a simple test case