Should be correct algorithm now