I haven't read through this in complete detail but it generally looks like the correct algorithm.