Completed implementation of algorithm and added javadoc comments