Refactored algorithm to step over a TreeMap instead of two primitive
arrays