Updated algo B with algo C's improvements. Should work on all trees now.