Refactored the code some more, and added test case for part II (2)
Added more tests, got the code working but nasty
Added more tests