Fully working algorithm