Made method more readable
Test now fails because broken. TODO fix
Fixed test and a little logical error in a method
Removed system out prints