Algorithm fully implemented, cleaned up the code, and finished javadocing it