Completed algorithm skeleton, need to now test!