Add unit tests and partially rewrite method to pass tests.

Add unit tests and partially rewrite method to pass tests. #200