Some algorithm I wrote