L'approche ici n'est pas bonne : tu fais une double boucle (d'ailleurs faire deux `for` ou un seul comme avant c'est équivalent) mais je vois pas vraiment pourquoi. Si tu regardes bien ton algo, tu vas ajouter chaque noeud RPS n fois où n est la taille de `closeNodes`. 
Il faudrait faire en plusieurs temps : 
- d'abord remettre à jour la liste `closeNodes` en fonction des nouvelles coordonnées que tu viens de recevoir
- ensuite insérer tous les RPS avec les distances calculées dans la liste `closeNodes`
- trier cette liste
- la tronquer pour ne garder que les `n` premiers éléments

Mieux vaut faire par étapes simplement et améliorer après.