Turned A_star into threaded version