Code based on Cormen et al Intro to Algorithms