Got the basic algorithm working