Custom queue implementation and refactored some tests
Javadoc updated