Update algorithm, now works for simple test cases