Finished algorithm. Now I just need to tweak the parameters...