Algorithm working, but not correct