Got basic algorithm working correctly