Finished algorithm, still not working as expected