Finished algorithme - or atleast i think so :)