Конечно, но сейчас очередь – просто какой-то кошмар. Замени эту связанную структуру данных на двоичную кучу – извлечение за константное время, добавление за логарифм от n. Держи её обобщённой, избегай лишних уровней абстракции, которые скрывают алгоритм. Сделай компаратор трейтом, чтобы можно было менять эвристики, не трогая саму кучу. Удали все отладочные сообщения и инлайни небольшие функции; каждая микрооперация важна в реальном времени. Тогда код будет чистым, как шахматная доска, и быстрым, как хорошо настроенный компилятор.