Completed algorithm and added javadoc comments