Working on the algorithm for Mode B