Couple fixes, going towards algo described in paper