Preliminary algorithm sort-of working