Refactoring for testing. Added lots of tests.

Added some tests and refactored base on michaels input on code review