partialy fixed A_find_voisin
works if there are no obstacles
Need to get algo to restart properly