now includes KD-tree search and heuristics for prioritization