Finished algorithm. YEAH