Got Astar working, VI implemented but will stack overflow.