Finished implementing algorithm, debug and test