Finished fleshing out algorithm, still untested