Astar algorithm now works pretty quickly but convergence is still too slow