Use indexing instead of .get
Avoid allocations by using TreeMap::find_with()