Finished algorithm implementation and added javadoc comments