Finished implementing algorithm