Algorithm done, needs testing