Improvements, fixed some bugs/oversights, added tests (JUnit)